1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2012-, Open Perception, Inc. 6 * 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 13 * * Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * * Redistributions in binary form must reproduce the above 16 * copyright notice, this list of conditions and the following 17 * disclaimer in the documentation and/or other materials provided 18 * with the distribution. 19 * * Neither the name of the copyright holder(s) nor the names of its 20 * contributors may be used to endorse or promote products derived 21 * from this software without specific prior written permission. 22 * 23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 * POSSIBILITY OF SUCH DAMAGE. 35 * 36 * 37 */ 38 39 #pragma once 40 41 #include <pcl/sample_consensus/sac_model_registration.h> 42 #include <pcl/memory.h> 43 #include <pcl/pcl_macros.h> 44 45 namespace pcl 46 { 47 /** \brief SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection using distances between 2D pixels 48 * \author Radu B. Rusu 49 * \ingroup sample_consensus 50 */ 51 template <typename PointT> 52 class SampleConsensusModelRegistration2D : public pcl::SampleConsensusModelRegistration<PointT> 53 { 54 public: 55 using pcl::SampleConsensusModelRegistration<PointT>::model_name_; 56 using pcl::SampleConsensusModelRegistration<PointT>::input_; 57 using pcl::SampleConsensusModelRegistration<PointT>::target_; 58 using pcl::SampleConsensusModelRegistration<PointT>::indices_; 59 using pcl::SampleConsensusModelRegistration<PointT>::indices_tgt_; 60 using pcl::SampleConsensusModelRegistration<PointT>::error_sqr_dists_; 61 using pcl::SampleConsensusModelRegistration<PointT>::correspondences_; 62 using pcl::SampleConsensusModelRegistration<PointT>::sample_dist_thresh_; 63 using pcl::SampleConsensusModelRegistration<PointT>::computeOriginalIndexMapping; 64 using pcl::SampleConsensusModel<PointT>::isModelValid; 65 66 using PointCloud = typename pcl::SampleConsensusModel<PointT>::PointCloud; 67 using PointCloudPtr = typename pcl::SampleConsensusModel<PointT>::PointCloudPtr; 68 using PointCloudConstPtr = typename pcl::SampleConsensusModel<PointT>::PointCloudConstPtr; 69 70 using Ptr = shared_ptr<SampleConsensusModelRegistration2D<PointT> >; 71 using ConstPtr = shared_ptr<const SampleConsensusModelRegistration2D<PointT> >; 72 73 /** \brief Constructor for base SampleConsensusModelRegistration2D. 74 * \param[in] cloud the input point cloud dataset 75 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 76 */ 77 SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud, 78 bool random = false) 79 : pcl::SampleConsensusModelRegistration<PointT> (cloud, random) 80 , projection_matrix_ (Eigen::Matrix3f::Identity ()) 81 { 82 // Call our own setInputCloud 83 setInputCloud (cloud); 84 model_name_ = "SampleConsensusModelRegistration2D"; 85 sample_size_ = 3; 86 model_size_ = 16; 87 } 88 89 /** \brief Constructor for base SampleConsensusModelRegistration2D. 90 * \param[in] cloud the input point cloud dataset 91 * \param[in] indices a vector of point indices to be used from \a cloud 92 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 93 */ 94 SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud, 95 const Indices &indices, 96 bool random = false) 97 : pcl::SampleConsensusModelRegistration<PointT> (cloud, indices, random) 98 , projection_matrix_ (Eigen::Matrix3f::Identity ()) 99 { 100 computeOriginalIndexMapping (); 101 computeSampleDistanceThreshold (cloud, indices); 102 model_name_ = "SampleConsensusModelRegistration2D"; 103 sample_size_ = 3; 104 model_size_ = 16; 105 } 106 107 /** \brief Empty destructor */ ~SampleConsensusModelRegistration2D()108 virtual ~SampleConsensusModelRegistration2D () {} 109 110 /** \brief Compute all distances from the transformed points to their correspondences 111 * \param[in] model_coefficients the 4x4 transformation matrix 112 * \param[out] distances the resultant estimated distances 113 */ 114 void 115 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 116 std::vector<double> &distances) const; 117 118 /** \brief Select all the points which respect the given model coefficients as inliers. 119 * \param[in] model_coefficients the 4x4 transformation matrix 120 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 121 * \param[out] inliers the resultant model inliers 122 */ 123 void 124 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 125 const double threshold, 126 Indices &inliers); 127 128 /** \brief Count all the points which respect the given model coefficients as inliers. 129 * 130 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 131 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 132 * \return the resultant number of inliers 133 */ 134 virtual std::size_t 135 countWithinDistance (const Eigen::VectorXf &model_coefficients, 136 const double threshold) const; 137 138 /** \brief Set the camera projection matrix. 139 * \param[in] projection_matrix the camera projection matrix 140 */ 141 inline void setProjectionMatrix(const Eigen::Matrix3f & projection_matrix)142 setProjectionMatrix (const Eigen::Matrix3f &projection_matrix) 143 { projection_matrix_ = projection_matrix; } 144 145 /** \brief Get the camera projection matrix. */ 146 inline Eigen::Matrix3f getProjectionMatrix()147 getProjectionMatrix () const 148 { return (projection_matrix_); } 149 150 protected: 151 using SampleConsensusModel<PointT>::sample_size_; 152 using SampleConsensusModel<PointT>::model_size_; 153 154 /** \brief Check if a sample of indices results in a good sample of points 155 * indices. 156 * \param[in] samples the resultant index samples 157 */ 158 bool 159 isSampleGood (const Indices &samples) const; 160 161 /** \brief Computes an "optimal" sample distance threshold based on the 162 * principal directions of the input cloud. 163 */ 164 inline void computeSampleDistanceThreshold(const PointCloudConstPtr &)165 computeSampleDistanceThreshold (const PointCloudConstPtr&) 166 { 167 //// Compute the principal directions via PCA 168 //Eigen::Vector4f xyz_centroid; 169 //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero (); 170 171 //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid); 172 173 //// Check if the covariance matrix is finite or not. 174 //for (int i = 0; i < 3; ++i) 175 // for (int j = 0; j < 3; ++j) 176 // if (!std::isfinite (covariance_matrix.coeffRef (i, j))) 177 // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 178 179 //Eigen::Vector3f eigen_values; 180 //pcl::eigen33 (covariance_matrix, eigen_values); 181 182 //// Compute the distance threshold for sample selection 183 //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 184 //sample_dist_thresh_ *= sample_dist_thresh_; 185 //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 186 } 187 188 /** \brief Computes an "optimal" sample distance threshold based on the 189 * principal directions of the input cloud. 190 */ 191 inline void computeSampleDistanceThreshold(const PointCloudConstPtr &,const Indices &)192 computeSampleDistanceThreshold (const PointCloudConstPtr&, 193 const Indices&) 194 { 195 //// Compute the principal directions via PCA 196 //Eigen::Vector4f xyz_centroid; 197 //Eigen::Matrix3f covariance_matrix; 198 //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid); 199 200 //// Check if the covariance matrix is finite or not. 201 //for (int i = 0; i < 3; ++i) 202 // for (int j = 0; j < 3; ++j) 203 // if (!std::isfinite (covariance_matrix.coeffRef (i, j))) 204 // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 205 206 //Eigen::Vector3f eigen_values; 207 //pcl::eigen33 (covariance_matrix, eigen_values); 208 209 //// Compute the distance threshold for sample selection 210 //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 211 //sample_dist_thresh_ *= sample_dist_thresh_; 212 //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 213 } 214 215 private: 216 /** \brief Camera projection matrix. */ 217 Eigen::Matrix3f projection_matrix_; 218 219 public: 220 PCL_MAKE_ALIGNED_OPERATOR_NEW 221 }; 222 } 223 224 #include <pcl/sample_consensus/impl/sac_model_registration_2d.hpp> 225