1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2011, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #pragma once 42 43 #include <pcl/memory.h> 44 #include <pcl/pcl_macros.h> 45 #include <pcl/pcl_base.h> 46 #include <pcl/sample_consensus/sac_model.h> 47 #include <pcl/sample_consensus/model_types.h> 48 #include <pcl/common/eigen.h> 49 #include <pcl/common/centroid.h> 50 #include <map> 51 #include <numeric> // for std::iota 52 53 namespace pcl 54 { 55 /** \brief SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection. 56 * \author Radu Bogdan Rusu 57 * \ingroup sample_consensus 58 */ 59 template <typename PointT> 60 class SampleConsensusModelRegistration : public SampleConsensusModel<PointT> 61 { 62 public: 63 using SampleConsensusModel<PointT>::model_name_; 64 using SampleConsensusModel<PointT>::input_; 65 using SampleConsensusModel<PointT>::indices_; 66 using SampleConsensusModel<PointT>::error_sqr_dists_; 67 using SampleConsensusModel<PointT>::isModelValid; 68 69 using PointCloud = typename SampleConsensusModel<PointT>::PointCloud; 70 using PointCloudPtr = typename SampleConsensusModel<PointT>::PointCloudPtr; 71 using PointCloudConstPtr = typename SampleConsensusModel<PointT>::PointCloudConstPtr; 72 73 using Ptr = shared_ptr<SampleConsensusModelRegistration<PointT> >; 74 using ConstPtr = shared_ptr<const SampleConsensusModelRegistration<PointT>>; 75 76 /** \brief Constructor for base SampleConsensusModelRegistration. 77 * \param[in] cloud the input point cloud dataset 78 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 79 */ 80 SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, 81 bool random = false) 82 : SampleConsensusModel<PointT> (cloud, random) 83 , target_ () 84 , sample_dist_thresh_ (0) 85 { 86 // Call our own setInputCloud 87 setInputCloud (cloud); 88 model_name_ = "SampleConsensusModelRegistration"; 89 sample_size_ = 3; 90 model_size_ = 16; 91 } 92 93 /** \brief Constructor for base SampleConsensusModelRegistration. 94 * \param[in] cloud the input point cloud dataset 95 * \param[in] indices a vector of point indices to be used from \a cloud 96 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 97 */ 98 SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, 99 const Indices &indices, 100 bool random = false) 101 : SampleConsensusModel<PointT> (cloud, indices, random) 102 , target_ () 103 , sample_dist_thresh_ (0) 104 { 105 computeOriginalIndexMapping (); 106 computeSampleDistanceThreshold (cloud, indices); 107 model_name_ = "SampleConsensusModelRegistration"; 108 sample_size_ = 3; 109 model_size_ = 16; 110 } 111 112 /** \brief Empty destructor */ ~SampleConsensusModelRegistration()113 ~SampleConsensusModelRegistration () {} 114 115 /** \brief Provide a pointer to the input dataset 116 * \param[in] cloud the const boost shared pointer to a PointCloud message 117 */ 118 inline void setInputCloud(const PointCloudConstPtr & cloud)119 setInputCloud (const PointCloudConstPtr &cloud) override 120 { 121 SampleConsensusModel<PointT>::setInputCloud (cloud); 122 computeOriginalIndexMapping (); 123 computeSampleDistanceThreshold (cloud); 124 } 125 126 /** \brief Set the input point cloud target. 127 * \param[in] target the input point cloud target 128 */ 129 inline void setInputTarget(const PointCloudConstPtr & target)130 setInputTarget (const PointCloudConstPtr &target) 131 { 132 target_ = target; 133 // Cache the size and fill the target indices 134 const index_t target_size = static_cast<index_t> (target->size ()); 135 indices_tgt_.reset (new Indices (target_size)); 136 std::iota (indices_tgt_->begin (), indices_tgt_->end (), 0); 137 computeOriginalIndexMapping (); 138 } 139 140 /** \brief Set the input point cloud target. 141 * \param[in] target the input point cloud target 142 * \param[in] indices_tgt a vector of point indices to be used from \a target 143 */ 144 inline void setInputTarget(const PointCloudConstPtr & target,const Indices & indices_tgt)145 setInputTarget (const PointCloudConstPtr &target, const Indices &indices_tgt) 146 { 147 target_ = target; 148 indices_tgt_.reset (new Indices (indices_tgt)); 149 computeOriginalIndexMapping (); 150 } 151 152 /** \brief Compute a 4x4 rigid transformation matrix from the samples given 153 * \param[in] samples the indices found as good candidates for creating a valid model 154 * \param[out] model_coefficients the resultant model coefficients 155 */ 156 bool 157 computeModelCoefficients (const Indices &samples, 158 Eigen::VectorXf &model_coefficients) const override; 159 160 /** \brief Compute all distances from the transformed points to their correspondences 161 * \param[in] model_coefficients the 4x4 transformation matrix 162 * \param[out] distances the resultant estimated distances 163 */ 164 void 165 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 166 std::vector<double> &distances) const override; 167 168 /** \brief Select all the points which respect the given model coefficients as inliers. 169 * \param[in] model_coefficients the 4x4 transformation matrix 170 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 171 * \param[out] inliers the resultant model inliers 172 */ 173 void 174 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 175 const double threshold, 176 Indices &inliers) override; 177 178 /** \brief Count all the points which respect the given model coefficients as inliers. 179 * 180 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 181 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 182 * \return the resultant number of inliers 183 */ 184 std::size_t 185 countWithinDistance (const Eigen::VectorXf &model_coefficients, 186 const double threshold) const override; 187 188 /** \brief Recompute the 4x4 transformation using the given inlier set 189 * \param[in] inliers the data inliers found as supporting the model 190 * \param[in] model_coefficients the initial guess for the optimization 191 * \param[out] optimized_coefficients the resultant recomputed transformation 192 */ 193 void 194 optimizeModelCoefficients (const Indices &inliers, 195 const Eigen::VectorXf &model_coefficients, 196 Eigen::VectorXf &optimized_coefficients) const override; 197 198 void 199 projectPoints (const Indices &, 200 const Eigen::VectorXf &, 201 PointCloud &, bool = true) const override 202 { 203 }; 204 205 bool doSamplesVerifyModel(const std::set<index_t> &,const Eigen::VectorXf &,const double)206 doSamplesVerifyModel (const std::set<index_t> &, 207 const Eigen::VectorXf &, 208 const double) const override 209 { 210 return (false); 211 } 212 213 /** \brief Return a unique id for this model (SACMODEL_REGISTRATION). */ 214 inline pcl::SacModel getModelType()215 getModelType () const override { return (SACMODEL_REGISTRATION); } 216 217 protected: 218 using SampleConsensusModel<PointT>::sample_size_; 219 using SampleConsensusModel<PointT>::model_size_; 220 221 /** \brief Check if a sample of indices results in a good sample of points 222 * indices. 223 * \param[in] samples the resultant index samples 224 */ 225 bool 226 isSampleGood (const Indices &samples) const override; 227 228 /** \brief Computes an "optimal" sample distance threshold based on the 229 * principal directions of the input cloud. 230 * \param[in] cloud the const boost shared pointer to a PointCloud message 231 */ 232 inline void computeSampleDistanceThreshold(const PointCloudConstPtr & cloud)233 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud) 234 { 235 // Compute the principal directions via PCA 236 Eigen::Vector4f xyz_centroid; 237 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero (); 238 239 computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid); 240 241 // Check if the covariance matrix is finite or not. 242 for (int i = 0; i < 3; ++i) 243 for (int j = 0; j < 3; ++j) 244 if (!std::isfinite (covariance_matrix.coeffRef (i, j))) 245 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 246 247 Eigen::Vector3f eigen_values; 248 pcl::eigen33 (covariance_matrix, eigen_values); 249 250 // Compute the distance threshold for sample selection 251 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 252 sample_dist_thresh_ *= sample_dist_thresh_; 253 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 254 } 255 256 /** \brief Computes an "optimal" sample distance threshold based on the 257 * principal directions of the input cloud. 258 * \param[in] cloud the const boost shared pointer to a PointCloud message 259 * \param indices 260 */ 261 inline void computeSampleDistanceThreshold(const PointCloudConstPtr & cloud,const Indices & indices)262 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, 263 const Indices &indices) 264 { 265 // Compute the principal directions via PCA 266 Eigen::Vector4f xyz_centroid; 267 Eigen::Matrix3f covariance_matrix; 268 computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid); 269 270 // Check if the covariance matrix is finite or not. 271 for (int i = 0; i < 3; ++i) 272 for (int j = 0; j < 3; ++j) 273 if (!std::isfinite (covariance_matrix.coeffRef (i, j))) 274 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 275 276 Eigen::Vector3f eigen_values; 277 pcl::eigen33 (covariance_matrix, eigen_values); 278 279 // Compute the distance threshold for sample selection 280 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 281 sample_dist_thresh_ *= sample_dist_thresh_; 282 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 283 } 284 285 /** \brief Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form 286 * solution of absolute orientation using unit quaternions 287 * \param[in] cloud_src the source point cloud dataset 288 * \param[in] indices_src the vector of indices describing the points of interest in cloud_src 289 * \param[in] cloud_tgt the target point cloud dataset 290 * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from 291 * indices_src 292 * \param[out] transform the resultant transformation matrix (as model coefficients) 293 * 294 * This method is an implementation of: Horn, B. “Closed-Form Solution of Absolute Orientation Using Unit Quaternions,” JOSA A, Vol. 4, No. 4, 1987 295 */ 296 void 297 estimateRigidTransformationSVD (const pcl::PointCloud<PointT> &cloud_src, 298 const Indices &indices_src, 299 const pcl::PointCloud<PointT> &cloud_tgt, 300 const Indices &indices_tgt, 301 Eigen::VectorXf &transform) const; 302 303 /** \brief Compute mappings between original indices of the input_/target_ clouds. */ 304 void computeOriginalIndexMapping()305 computeOriginalIndexMapping () 306 { 307 if (!indices_tgt_) 308 { 309 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_tgt_ is null.\n"); 310 return; 311 } 312 if (!indices_) 313 { 314 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is null.\n"); 315 return; 316 } 317 if (indices_->empty ()) 318 { 319 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is empty.\n"); 320 return; 321 } 322 if (indices_->size () != indices_tgt_->size ()) 323 { 324 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ and indices_tgt_ are not the same size (%zu vs %zu).\n", 325 indices_->size (), indices_tgt_->size ()); 326 return; 327 } 328 for (std::size_t i = 0; i < indices_->size (); ++i) 329 correspondences_[(*indices_)[i]] = (*indices_tgt_)[i]; 330 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Successfully computed mapping.\n"); 331 } 332 333 /** \brief A boost shared pointer to the target point cloud data array. */ 334 PointCloudConstPtr target_; 335 336 /** \brief A pointer to the vector of target point indices to use. */ 337 IndicesPtr indices_tgt_; 338 339 /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */ 340 std::map<index_t, index_t> correspondences_; 341 342 /** \brief Internal distance threshold used for the sample selection step. */ 343 double sample_dist_thresh_; 344 public: 345 PCL_MAKE_ALIGNED_OPERATOR_NEW 346 }; 347 } 348 349 #include <pcl/sample_consensus/impl/sac_model_registration.hpp> 350