Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: sac_model_registration.h 2532 2011-09-20 20:39:18Z bouffa $ 00037 * 00038 */ 00039 00040 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_ 00041 #define PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_ 00042 00043 #include <boost/unordered_map.hpp> 00044 #include <Eigen/Core> 00045 #include "pcl/sample_consensus/sac_model.h" 00046 #include "pcl/sample_consensus/model_types.h" 00047 #include <pcl/common/eigen.h> 00048 #include <pcl/common/centroid.h> 00049 00050 namespace pcl 00051 { 00056 template <typename PointT> 00057 class SampleConsensusModelRegistration : public SampleConsensusModel<PointT> 00058 { 00059 using SampleConsensusModel<PointT>::input_; 00060 using SampleConsensusModel<PointT>::indices_; 00061 00062 public: 00063 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud; 00064 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00065 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00066 00067 typedef boost::shared_ptr<SampleConsensusModelRegistration> Ptr; 00068 00072 SampleConsensusModelRegistration (const PointCloudConstPtr &cloud) : SampleConsensusModel<PointT> (cloud) 00073 { 00074 // Call our own setInputCloud 00075 setInputCloud (cloud); 00076 } 00077 00082 SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, 00083 const std::vector<int> &indices) : 00084 SampleConsensusModel<PointT> (cloud, indices) 00085 { 00086 computeOriginalIndexMapping (); 00087 computeSampleDistanceThreshold (cloud, indices); 00088 } 00089 00093 inline virtual void 00094 setInputCloud (const PointCloudConstPtr &cloud) 00095 { 00096 SampleConsensusModel<PointT>::setInputCloud (cloud); 00097 computeOriginalIndexMapping (); 00098 computeSampleDistanceThreshold (cloud); 00099 } 00100 00104 inline void 00105 setInputTarget (const PointCloudConstPtr &target) 00106 { 00107 target_ = target; 00108 indices_tgt_.reset (new std::vector<int>); 00109 // Cache the size and fill the target indices 00110 unsigned int target_size = target->size (); 00111 indices_tgt_->resize (target_size); 00112 00113 for (unsigned int i = 0; i < target_size; ++i) 00114 (*indices_tgt_)[i] = i; 00115 computeOriginalIndexMapping (); 00116 } 00117 00122 inline void 00123 setInputTarget (const PointCloudConstPtr &target, const std::vector<int> &indices_tgt) 00124 { 00125 target_ = target; 00126 indices_tgt_.reset (new std::vector<int> (indices_tgt)); 00127 computeOriginalIndexMapping (); 00128 } 00129 00134 bool 00135 computeModelCoefficients (const std::vector<int> &samples, 00136 Eigen::VectorXf &model_coefficients); 00137 00142 void 00143 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00144 std::vector<double> &distances); 00145 00151 void 00152 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00153 const double threshold, 00154 std::vector<int> &inliers); 00155 00162 virtual int 00163 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00164 const double threshold); 00165 00171 void 00172 optimizeModelCoefficients (const std::vector<int> &inliers, 00173 const Eigen::VectorXf &model_coefficients, 00174 Eigen::VectorXf &optimized_coefficients); 00175 00176 void 00177 projectPoints (const std::vector<int> &inliers, 00178 const Eigen::VectorXf &model_coefficients, 00179 PointCloud &projected_points, bool copy_data_fields = true) 00180 {}; 00181 00182 bool 00183 doSamplesVerifyModel (const std::set<int> &indices, 00184 const Eigen::VectorXf &model_coefficients, 00185 const double threshold) 00186 { 00187 //PCL_ERROR ("[pcl::SampleConsensusModelRegistration::doSamplesVerifyModel] called!\n"); 00188 return (false); 00189 } 00190 00192 inline pcl::SacModel 00193 getModelType () const { return (SACMODEL_REGISTRATION); } 00194 00195 protected: 00199 inline bool 00200 isModelValid (const Eigen::VectorXf &model_coefficients) 00201 { 00202 // Needs a valid model coefficients 00203 if (model_coefficients.size () != 16) 00204 return (false); 00205 00206 return (true); 00207 } 00208 00213 bool 00214 isSampleGood (const std::vector<int> &samples) const; 00215 00220 inline void 00221 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud) 00222 { 00223 // Compute the principal directions via PCA 00224 Eigen::Vector4f xyz_centroid; 00225 compute3DCentroid (*cloud, xyz_centroid); 00226 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00227 computeCovarianceMatrixNormalized (*cloud, xyz_centroid, covariance_matrix); 00228 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00229 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00230 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00231 00232 // Compute the distance threshold for sample selection 00233 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 00234 sample_dist_thresh_ *= sample_dist_thresh_; 00235 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 00236 } 00237 00242 inline void 00243 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, 00244 const std::vector<int> &indices) 00245 { 00246 // Compute the principal directions via PCA 00247 Eigen::Vector4f xyz_centroid; 00248 compute3DCentroid (*cloud, indices, xyz_centroid); 00249 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00250 computeCovarianceMatrixNormalized (*cloud, indices, xyz_centroid, covariance_matrix); 00251 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00252 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00253 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00254 00255 // Compute the distance threshold for sample selection 00256 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 00257 sample_dist_thresh_ *= sample_dist_thresh_; 00258 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 00259 } 00260 00261 private: 00262 00274 void 00275 estimateRigidTransformationSVD (const pcl::PointCloud<PointT> &cloud_src, 00276 const std::vector<int> &indices_src, 00277 const pcl::PointCloud<PointT> &cloud_tgt, 00278 const std::vector<int> &indices_tgt, 00279 Eigen::VectorXf &transform); 00280 00282 void 00283 computeOriginalIndexMapping () 00284 { 00285 if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ()) 00286 return; 00287 for (size_t i = 0; i < indices_->size (); ++i) 00288 correspondences_[(*indices_)[i]] = (*indices_tgt_)[i]; 00289 } 00290 00292 PointCloudConstPtr target_; 00293 00295 boost::shared_ptr <std::vector<int> > indices_tgt_; 00296 00298 boost::unordered_map<int, int> correspondences_; 00299 00301 double sample_dist_thresh_; 00302 public: 00303 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00304 }; 00305 } 00306 00307 #include "pcl/sample_consensus/impl/sac_model_registration.hpp" 00308 00309 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_