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) 2012-, Open Perception, 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 the copyright holder(s) 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 * 00037 */ 00038 00039 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_ 00040 #define PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_ 00041 00042 #include <pcl/sample_consensus/sac_model_registration.h> 00043 00044 namespace pcl 00045 { 00046 /** \brief SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection using distances between 2D pixels 00047 * \author Radu B. Rusu 00048 * \ingroup sample_consensus 00049 */ 00050 template <typename PointT> 00051 class SampleConsensusModelRegistration2D : public pcl::SampleConsensusModelRegistration<PointT> 00052 { 00053 public: 00054 using pcl::SampleConsensusModelRegistration<PointT>::input_; 00055 using pcl::SampleConsensusModelRegistration<PointT>::target_; 00056 using pcl::SampleConsensusModelRegistration<PointT>::indices_; 00057 using pcl::SampleConsensusModelRegistration<PointT>::indices_tgt_; 00058 using pcl::SampleConsensusModelRegistration<PointT>::error_sqr_dists_; 00059 using pcl::SampleConsensusModelRegistration<PointT>::correspondences_; 00060 using pcl::SampleConsensusModelRegistration<PointT>::sample_dist_thresh_; 00061 using pcl::SampleConsensusModelRegistration<PointT>::computeOriginalIndexMapping; 00062 00063 typedef typename pcl::SampleConsensusModel<PointT>::PointCloud PointCloud; 00064 typedef typename pcl::SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00065 typedef typename pcl::SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00066 00067 typedef boost::shared_ptr<SampleConsensusModelRegistration2D> Ptr; 00068 typedef boost::shared_ptr<const SampleConsensusModelRegistration2D> ConstPtr; 00069 00070 /** \brief Constructor for base SampleConsensusModelRegistration2D. 00071 * \param[in] cloud the input point cloud dataset 00072 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00073 */ 00074 SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud, 00075 bool random = false) 00076 : pcl::SampleConsensusModelRegistration<PointT> (cloud, random) 00077 , projection_matrix_ (Eigen::Matrix3f::Identity ()) 00078 { 00079 // Call our own setInputCloud 00080 setInputCloud (cloud); 00081 } 00082 00083 /** \brief Constructor for base SampleConsensusModelRegistration2D. 00084 * \param[in] cloud the input point cloud dataset 00085 * \param[in] indices a vector of point indices to be used from \a cloud 00086 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00087 */ 00088 SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud, 00089 const std::vector<int> &indices, 00090 bool random = false) 00091 : pcl::SampleConsensusModelRegistration<PointT> (cloud, indices, random) 00092 , projection_matrix_ (Eigen::Matrix3f::Identity ()) 00093 { 00094 computeOriginalIndexMapping (); 00095 computeSampleDistanceThreshold (cloud, indices); 00096 } 00097 00098 /** \brief Empty destructor */ 00099 virtual ~SampleConsensusModelRegistration2D () {} 00100 00101 /** \brief Compute all distances from the transformed points to their correspondences 00102 * \param[in] model_coefficients the 4x4 transformation matrix 00103 * \param[out] distances the resultant estimated distances 00104 */ 00105 void 00106 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00107 std::vector<double> &distances); 00108 00109 /** \brief Select all the points which respect the given model coefficients as inliers. 00110 * \param[in] model_coefficients the 4x4 transformation matrix 00111 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00112 * \param[out] inliers the resultant model inliers 00113 */ 00114 void 00115 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00116 const double threshold, 00117 std::vector<int> &inliers); 00118 00119 /** \brief Count all the points which respect the given model coefficients as inliers. 00120 * 00121 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00122 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00123 * \return the resultant number of inliers 00124 */ 00125 virtual int 00126 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00127 const double threshold); 00128 00129 /** \brief Set the camera projection matrix. 00130 * \param[in] projection_matrix the camera projection matrix 00131 */ 00132 inline void 00133 setProjectionMatrix (const Eigen::Matrix3f &projection_matrix) 00134 { projection_matrix_ = projection_matrix; } 00135 00136 /** \brief Get the camera projection matrix. */ 00137 inline Eigen::Matrix3f 00138 getProjectionMatrix () const 00139 { return (projection_matrix_); } 00140 00141 protected: 00142 /** \brief Check if a sample of indices results in a good sample of points 00143 * indices. 00144 * \param[in] samples the resultant index samples 00145 */ 00146 bool 00147 isSampleGood (const std::vector<int> &samples) const; 00148 00149 /** \brief Computes an "optimal" sample distance threshold based on the 00150 * principal directions of the input cloud. 00151 * \param[in] cloud the const boost shared pointer to a PointCloud message 00152 */ 00153 inline void 00154 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud) 00155 { 00156 //// Compute the principal directions via PCA 00157 //Eigen::Vector4f xyz_centroid; 00158 //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero (); 00159 00160 //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid); 00161 00162 //// Check if the covariance matrix is finite or not. 00163 //for (int i = 0; i < 3; ++i) 00164 // for (int j = 0; j < 3; ++j) 00165 // if (!pcl_isfinite (covariance_matrix.coeffRef (i, j))) 00166 // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 00167 00168 //Eigen::Vector3f eigen_values; 00169 //pcl::eigen33 (covariance_matrix, eigen_values); 00170 00171 //// Compute the distance threshold for sample selection 00172 //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 00173 //sample_dist_thresh_ *= sample_dist_thresh_; 00174 //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 00175 } 00176 00177 /** \brief Computes an "optimal" sample distance threshold based on the 00178 * principal directions of the input cloud. 00179 * \param[in] cloud the const boost shared pointer to a PointCloud message 00180 */ 00181 inline void 00182 computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, 00183 const std::vector<int> &indices) 00184 { 00185 //// Compute the principal directions via PCA 00186 //Eigen::Vector4f xyz_centroid; 00187 //Eigen::Matrix3f covariance_matrix; 00188 //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid); 00189 00190 //// Check if the covariance matrix is finite or not. 00191 //for (int i = 0; i < 3; ++i) 00192 // for (int j = 0; j < 3; ++j) 00193 // if (!pcl_isfinite (covariance_matrix.coeffRef (i, j))) 00194 // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); 00195 00196 //Eigen::Vector3f eigen_values; 00197 //pcl::eigen33 (covariance_matrix, eigen_values); 00198 00199 //// Compute the distance threshold for sample selection 00200 //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; 00201 //sample_dist_thresh_ *= sample_dist_thresh_; 00202 //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); 00203 } 00204 00205 private: 00206 /** \brief Camera projection matrix. */ 00207 Eigen::Matrix3f projection_matrix_; 00208 00209 public: 00210 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00211 }; 00212 } 00213 00214 #include <pcl/sample_consensus/impl/sac_model_registration_2d.hpp> 00215 00216 #endif // PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_ 00217