Point Cloud Library (PCL)
1.7.0
|
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 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_ 00042 #define PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_ 00043 00044 #include <pcl/sample_consensus/eigen.h> 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 #include <map> 00050 00051 namespace pcl 00052 { 00053 /** \brief SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection. 00054 * \author Radu Bogdan Rusu 00055 * \ingroup sample_consensus 00056 */ 00057 template <typename PointT> 00058 class SampleConsensusModelRegistration : public SampleConsensusModel<PointT> 00059 { 00060 public: 00061 using SampleConsensusModel<PointT>::input_; 00062 using SampleConsensusModel<PointT>::indices_; 00063 using SampleConsensusModel<PointT>::error_sqr_dists_; 00064 00065 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud; 00066 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00067 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00068 00069 typedef boost::shared_ptr<SampleConsensusModelRegistration> Ptr; 00070 00071 /** \brief Constructor for base SampleConsensusModelRegistration. 00072 * \param[in] cloud the input point cloud dataset 00073 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00074 */ 00075 SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, 00076 bool random = false) 00077 : SampleConsensusModel<PointT> (cloud, random) 00078 , target_ () 00079 , indices_tgt_ () 00080 , correspondences_ () 00081 , sample_dist_thresh_ (0) 00082 { 00083 // Call our own setInputCloud 00084 setInputCloud (cloud); 00085 } 00086 00087 /** \brief Constructor for base SampleConsensusModelRegistration. 00088 * \param[in] cloud the input point cloud dataset 00089 * \param[in] indices a vector of point indices to be used from \a cloud 00090 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00091 */ 00092 SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, 00093 const std::vector<int> &indices, 00094 bool random = false) 00095 : SampleConsensusModel<PointT> (cloud, indices, random) 00096 , target_ () 00097 , indices_tgt_ () 00098 , correspondences_ () 00099 , sample_dist_thresh_ (0) 00100 { 00101 computeOriginalIndexMapping (); 00102 computeSampleDistanceThreshold (cloud, indices); 00103 } 00104 00105 /** \brief Empty destructor */ 00106 virtual ~SampleConsensusModelRegistration () {} 00107 00108 /** \brief Provide a pointer to the input dataset 00109 * \param[in] cloud the const boost shared pointer to a PointCloud message 00110 */ 00111 inline virtual void 00112 setInputCloud (const PointCloudConstPtr &cloud) 00113 { 00114 SampleConsensusModel<PointT>::setInputCloud (cloud); 00115 computeOriginalIndexMapping (); 00116 computeSampleDistanceThreshold (cloud); 00117 } 00118 00119 /** \brief Set the input point cloud target. 00120 * \param[in] target the input point cloud target 00121 */ 00122 inline void 00123 setInputTarget (const PointCloudConstPtr &target) 00124 { 00125 target_ = target; 00126 indices_tgt_.reset (new std::vector<int>); 00127 // Cache the size and fill the target indices 00128 int target_size = static_cast<int> (target->size ()); 00129 indices_tgt_->resize (target_size); 00130 00131 for (int i = 0; i < target_size; ++i) 00132 (*indices_tgt_)[i] = i; 00133 computeOriginalIndexMapping (); 00134 } 00135 00136 /** \brief Set the input point cloud target. 00137 * \param[in] target the input point cloud target 00138 * \param[in] indices_tgt a vector of point indices to be used from \a target 00139 */ 00140 inline void 00141 setInputTarget (const PointCloudConstPtr &target, const std::vector<int> &indices_tgt) 00142 { 00143 target_ = target; 00144 indices_tgt_.reset (new std::vector<int> (indices_tgt)); 00145 computeOriginalIndexMapping (); 00146 } 00147 00148 /** \brief Compute a 4x4 rigid transformation matrix from the samples given 00149 * \param[in] samples the indices found as good candidates for creating a valid model 00150 * \param[out] model_coefficients the resultant model coefficients 00151 */ 00152 bool 00153 computeModelCoefficients (const std::vector<int> &samples, 00154 Eigen::VectorXf &model_coefficients); 00155 00156 /** \brief Compute all distances from the transformed points to their correspondences 00157 * \param[in] model_coefficients the 4x4 transformation matrix 00158 * \param[out] distances the resultant estimated distances 00159 */ 00160 void 00161 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00162 std::vector<double> &distances); 00163 00164 /** \brief Select all the points which respect the given model coefficients as inliers. 00165 * \param[in] model_coefficients the 4x4 transformation matrix 00166 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00167 * \param[out] inliers the resultant model inliers 00168 */ 00169 void 00170 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00171 const double threshold, 00172 std::vector<int> &inliers); 00173 00174 /** \brief Count all the points which respect the given model coefficients as inliers. 00175 * 00176 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00177 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00178 * \return the resultant number of inliers 00179 */ 00180 virtual int 00181 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00182 const double threshold); 00183 00184 /** \brief Recompute the 4x4 transformation using the given inlier set 00185 * \param[in] inliers the data inliers found as supporting the model 00186 * \param[in] model_coefficients the initial guess for the optimization 00187 * \param[out] optimized_coefficients the resultant recomputed transformation 00188 */ 00189 void 00190 optimizeModelCoefficients (const std::vector<int> &inliers, 00191 const Eigen::VectorXf &model_coefficients, 00192 Eigen::VectorXf &optimized_coefficients); 00193 00194 void 00195 projectPoints (const std::vector<int> &, 00196 const Eigen::VectorXf &, 00197 PointCloud &, bool = true) 00198 { 00199 }; 00200 00201 bool 00202 doSamplesVerifyModel (const std::set<int> &, 00203 const Eigen::VectorXf &, 00204 const double) 00205 { 00206 return (false); 00207 } 00208 00209 /** \brief Return an unique id for this model (SACMODEL_REGISTRATION). */ 00210 inline pcl::SacModel 00211 getModelType () const { return (SACMODEL_REGISTRATION); } 00212 00213 protected: 00214 /** \brief Check whether a model is valid given the user constraints. 00215 * \param[in] model_coefficients the set of model coefficients 00216 */ 00217 inline bool 00218 isModelValid (const Eigen::VectorXf &model_coefficients) 00219 { 00220 // Needs a valid model coefficients 00221 if (model_coefficients.size () != 16) 00222 return (false); 00223 00224 return (true); 00225 } 00226 00227 /** \brief Check if a sample of indices results in a good sample of points 00228 * indices. 00229 * \param[in] samples the resultant index samples 00230 */ 00231 virtual bool 00232 isSampleGood (const std::vector<int> &samples) const; 00233 00234 /** \brief Computes an "optimal" sample distance threshold based on the 00235 * principal directions of the input cloud. 00236 * \param[in] cloud the const boost shared pointer to a PointCloud message 00237 */ 00238 inline void 00239 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud) 00240 { 00241 // Compute the principal directions via PCA 00242 Eigen::Vector4f xyz_centroid; 00243 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero (); 00244 00245 computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid); 00246 00247 // Check if the covariance matrix is finite or not. 00248 for (int i = 0; i < 3; ++i) 00249 for (int j = 0; j < 3; ++j) 00250 if (!pcl_isfinite (covariance_matrix.coeffRef (i, j))) 00251 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 00252 00253 Eigen::Vector3f eigen_values; 00254 pcl::eigen33 (covariance_matrix, eigen_values); 00255 00256 // Compute the distance threshold for sample selection 00257 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 00258 sample_dist_thresh_ *= sample_dist_thresh_; 00259 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 00260 } 00261 00262 /** \brief Computes an "optimal" sample distance threshold based on the 00263 * principal directions of the input cloud. 00264 * \param[in] cloud the const boost shared pointer to a PointCloud message 00265 */ 00266 inline void 00267 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, 00268 const std::vector<int> &indices) 00269 { 00270 // Compute the principal directions via PCA 00271 Eigen::Vector4f xyz_centroid; 00272 Eigen::Matrix3f covariance_matrix; 00273 computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid); 00274 00275 // Check if the covariance matrix is finite or not. 00276 for (int i = 0; i < 3; ++i) 00277 for (int j = 0; j < 3; ++j) 00278 if (!pcl_isfinite (covariance_matrix.coeffRef (i, j))) 00279 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 00280 00281 Eigen::Vector3f eigen_values; 00282 pcl::eigen33 (covariance_matrix, eigen_values); 00283 00284 // Compute the distance threshold for sample selection 00285 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 00286 sample_dist_thresh_ *= sample_dist_thresh_; 00287 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 00288 } 00289 00290 /** \brief Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form 00291 * solution of absolute orientation using unit quaternions 00292 * \param[in] cloud_src the source point cloud dataset 00293 * \param[in] indices_src the vector of indices describing the points of interest in cloud_src 00294 * \param[in] cloud_tgt the target point cloud dataset 00295 * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from 00296 * indices_src 00297 * \param[out] transform the resultant transformation matrix (as model coefficients) 00298 * 00299 * This method is an implementation of: Horn, B. “Closed-Form Solution of Absolute Orientation Using Unit Quaternions,” JOSA A, Vol. 4, No. 4, 1987 00300 */ 00301 void 00302 estimateRigidTransformationSVD (const pcl::PointCloud<PointT> &cloud_src, 00303 const std::vector<int> &indices_src, 00304 const pcl::PointCloud<PointT> &cloud_tgt, 00305 const std::vector<int> &indices_tgt, 00306 Eigen::VectorXf &transform); 00307 00308 /** \brief Compute mappings between original indices of the input_/target_ clouds. */ 00309 void 00310 computeOriginalIndexMapping () 00311 { 00312 if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ()) 00313 return; 00314 for (size_t i = 0; i < indices_->size (); ++i) 00315 correspondences_[(*indices_)[i]] = (*indices_tgt_)[i]; 00316 } 00317 00318 /** \brief A boost shared pointer to the target point cloud data array. */ 00319 PointCloudConstPtr target_; 00320 00321 /** \brief A pointer to the vector of target point indices to use. */ 00322 boost::shared_ptr <std::vector<int> > indices_tgt_; 00323 00324 /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */ 00325 std::map<int, int> correspondences_; 00326 00327 /** \brief Internal distance threshold used for the sample selection step. */ 00328 double sample_dist_thresh_; 00329 public: 00330 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00331 }; 00332 } 00333 00334 #include <pcl/sample_consensus/impl/sac_model_registration.hpp> 00335 00336 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_