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) 2009, 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_IMPL_SAC_MODEL_LINE_H_ 00042 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ 00043 00044 #include <pcl/sample_consensus/sac_model_line.h> 00045 #include <pcl/common/centroid.h> 00046 #include <pcl/common/concatenate.h> 00047 00048 ////////////////////////////////////////////////////////////////////////// 00049 template <typename PointT> bool 00050 pcl::SampleConsensusModelLine<PointT>::isSampleGood (const std::vector<int> &samples) const 00051 { 00052 if ( 00053 (input_->points[samples[0]].x != input_->points[samples[1]].x) 00054 && 00055 (input_->points[samples[0]].y != input_->points[samples[1]].y) 00056 && 00057 (input_->points[samples[0]].z != input_->points[samples[1]].z)) 00058 return (true); 00059 00060 return (true); 00061 } 00062 00063 ////////////////////////////////////////////////////////////////////////// 00064 template <typename PointT> bool 00065 pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients ( 00066 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) 00067 { 00068 // Need 2 samples 00069 if (samples.size () != 2) 00070 { 00071 PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); 00072 return (false); 00073 } 00074 00075 if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () && 00076 fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () && 00077 fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ()) 00078 { 00079 return (false); 00080 } 00081 00082 model_coefficients.resize (6); 00083 model_coefficients[0] = input_->points[samples[0]].x; 00084 model_coefficients[1] = input_->points[samples[0]].y; 00085 model_coefficients[2] = input_->points[samples[0]].z; 00086 00087 model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0]; 00088 model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1]; 00089 model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2]; 00090 00091 model_coefficients.template tail<3> ().normalize (); 00092 return (true); 00093 } 00094 00095 ////////////////////////////////////////////////////////////////////////// 00096 template <typename PointT> void 00097 pcl::SampleConsensusModelLine<PointT>::getDistancesToModel ( 00098 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00099 { 00100 // Needs a valid set of model coefficients 00101 if (!isModelValid (model_coefficients)) 00102 return; 00103 00104 distances.resize (indices_->size ()); 00105 00106 // Obtain the line point and direction 00107 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00108 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00109 line_dir.normalize (); 00110 00111 // Iterate through the 3d points and calculate the distances from them to the line 00112 for (size_t i = 0; i < indices_->size (); ++i) 00113 { 00114 // Calculate the distance from the point to the line 00115 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00116 // Need to estimate sqrt here to keep MSAC and friends general 00117 distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ()); 00118 } 00119 } 00120 00121 ////////////////////////////////////////////////////////////////////////// 00122 template <typename PointT> void 00123 pcl::SampleConsensusModelLine<PointT>::selectWithinDistance ( 00124 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00125 { 00126 // Needs a valid set of model coefficients 00127 if (!isModelValid (model_coefficients)) 00128 return; 00129 00130 double sqr_threshold = threshold * threshold; 00131 00132 int nr_p = 0; 00133 inliers.resize (indices_->size ()); 00134 error_sqr_dists_.resize (indices_->size ()); 00135 00136 // Obtain the line point and direction 00137 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00138 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00139 line_dir.normalize (); 00140 00141 // Iterate through the 3d points and calculate the distances from them to the line 00142 for (size_t i = 0; i < indices_->size (); ++i) 00143 { 00144 // Calculate the distance from the point to the line 00145 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00146 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); 00147 00148 if (sqr_distance < sqr_threshold) 00149 { 00150 // Returns the indices of the points whose squared distances are smaller than the threshold 00151 inliers[nr_p] = (*indices_)[i]; 00152 error_sqr_dists_[nr_p] = sqr_distance; 00153 ++nr_p; 00154 } 00155 } 00156 inliers.resize (nr_p); 00157 error_sqr_dists_.resize (nr_p); 00158 } 00159 00160 ////////////////////////////////////////////////////////////////////////// 00161 template <typename PointT> int 00162 pcl::SampleConsensusModelLine<PointT>::countWithinDistance ( 00163 const Eigen::VectorXf &model_coefficients, const double threshold) 00164 { 00165 // Needs a valid set of model coefficients 00166 if (!isModelValid (model_coefficients)) 00167 return (0); 00168 00169 double sqr_threshold = threshold * threshold; 00170 00171 int nr_p = 0; 00172 00173 // Obtain the line point and direction 00174 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00175 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00176 line_dir.normalize (); 00177 00178 // Iterate through the 3d points and calculate the distances from them to the line 00179 for (size_t i = 0; i < indices_->size (); ++i) 00180 { 00181 // Calculate the distance from the point to the line 00182 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00183 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); 00184 00185 if (sqr_distance < sqr_threshold) 00186 nr_p++; 00187 } 00188 return (nr_p); 00189 } 00190 00191 ////////////////////////////////////////////////////////////////////////// 00192 template <typename PointT> void 00193 pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients ( 00194 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 00195 { 00196 // Needs a valid set of model coefficients 00197 if (!isModelValid (model_coefficients)) 00198 { 00199 optimized_coefficients = model_coefficients; 00200 return; 00201 } 00202 00203 // Need at least 2 points to estimate a line 00204 if (inliers.size () <= 2) 00205 { 00206 PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); 00207 optimized_coefficients = model_coefficients; 00208 return; 00209 } 00210 00211 optimized_coefficients.resize (6); 00212 00213 // Compute the 3x3 covariance matrix 00214 Eigen::Vector4f centroid; 00215 compute3DCentroid (*input_, inliers, centroid); 00216 Eigen::Matrix3f covariance_matrix; 00217 computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix); 00218 optimized_coefficients[0] = centroid[0]; 00219 optimized_coefficients[1] = centroid[1]; 00220 optimized_coefficients[2] = centroid[2]; 00221 00222 // Extract the eigenvalues and eigenvectors 00223 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00224 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00225 pcl::eigen33 (covariance_matrix, eigen_values); 00226 pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector); 00227 //pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00228 00229 optimized_coefficients.template tail<3> ().matrix () = eigen_vector; 00230 } 00231 00232 ////////////////////////////////////////////////////////////////////////// 00233 template <typename PointT> void 00234 pcl::SampleConsensusModelLine<PointT>::projectPoints ( 00235 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) 00236 { 00237 // Needs a valid model coefficients 00238 if (!isModelValid (model_coefficients)) 00239 return; 00240 00241 // Obtain the line point and direction 00242 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00243 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00244 00245 projected_points.header = input_->header; 00246 projected_points.is_dense = input_->is_dense; 00247 00248 // Copy all the data fields from the input cloud to the projected one? 00249 if (copy_data_fields) 00250 { 00251 // Allocate enough space and copy the basics 00252 projected_points.points.resize (input_->points.size ()); 00253 projected_points.width = input_->width; 00254 projected_points.height = input_->height; 00255 00256 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00257 // Iterate over each point 00258 for (size_t i = 0; i < projected_points.points.size (); ++i) 00259 // Iterate over each dimension 00260 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00261 00262 // Iterate through the 3d points and calculate the distances from them to the line 00263 for (size_t i = 0; i < inliers.size (); ++i) 00264 { 00265 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); 00266 // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; 00267 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); 00268 00269 Eigen::Vector4f pp = line_pt + k * line_dir; 00270 // Calculate the projection of the point on the line (pointProj = A + k * B) 00271 projected_points.points[inliers[i]].x = pp[0]; 00272 projected_points.points[inliers[i]].y = pp[1]; 00273 projected_points.points[inliers[i]].z = pp[2]; 00274 } 00275 } 00276 else 00277 { 00278 // Allocate enough space and copy the basics 00279 projected_points.points.resize (inliers.size ()); 00280 projected_points.width = static_cast<uint32_t> (inliers.size ()); 00281 projected_points.height = 1; 00282 00283 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00284 // Iterate over each point 00285 for (size_t i = 0; i < inliers.size (); ++i) 00286 // Iterate over each dimension 00287 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00288 00289 // Iterate through the 3d points and calculate the distances from them to the line 00290 for (size_t i = 0; i < inliers.size (); ++i) 00291 { 00292 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); 00293 // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; 00294 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); 00295 00296 Eigen::Vector4f pp = line_pt + k * line_dir; 00297 // Calculate the projection of the point on the line (pointProj = A + k * B) 00298 projected_points.points[i].x = pp[0]; 00299 projected_points.points[i].y = pp[1]; 00300 projected_points.points[i].z = pp[2]; 00301 } 00302 } 00303 } 00304 00305 ////////////////////////////////////////////////////////////////////////// 00306 template <typename PointT> bool 00307 pcl::SampleConsensusModelLine<PointT>::doSamplesVerifyModel ( 00308 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) 00309 { 00310 // Needs a valid set of model coefficients 00311 if (!isModelValid (model_coefficients)) 00312 return (false); 00313 00314 // Obtain the line point and direction 00315 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00316 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00317 line_dir.normalize (); 00318 00319 double sqr_threshold = threshold * threshold; 00320 // Iterate through the 3d points and calculate the distances from them to the line 00321 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00322 { 00323 // Calculate the distance from the point to the line 00324 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00325 if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold) 00326 return (false); 00327 } 00328 00329 return (true); 00330 } 00331 00332 #define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>; 00333 00334 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ 00335