Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: sac_model_line.hpp 2405 2011-09-07 00:40:59Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ 00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ 00040 00041 #include "pcl/sample_consensus/sac_model_line.h" 00042 #include "pcl/common/centroid.h" 00043 #include "pcl/common/concatenate.h" 00044 00046 template <typename PointT> bool 00047 pcl::SampleConsensusModelLine<PointT>::isSampleGood (const std::vector<int> &samples) const 00048 { 00049 if ( 00050 (input_->points[samples[0]].x != input_->points[samples[1]].x) 00051 && 00052 (input_->points[samples[0]].y != input_->points[samples[1]].y) 00053 && 00054 (input_->points[samples[0]].z != input_->points[samples[1]].z)) 00055 return (true); 00056 00057 return (true); 00058 } 00059 00061 template <typename PointT> bool 00062 pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients ( 00063 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) 00064 { 00065 // Need 2 samples 00066 if (samples.size () != 2) 00067 { 00068 PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", (unsigned long)samples.size ()); 00069 return (false); 00070 } 00071 00072 model_coefficients.resize (6); 00073 model_coefficients[0] = input_->points[samples[0]].x; 00074 model_coefficients[1] = input_->points[samples[0]].y; 00075 model_coefficients[2] = input_->points[samples[0]].z; 00076 00077 model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0]; 00078 model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1]; 00079 model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2]; 00080 00081 model_coefficients.template tail<3> ().normalize (); 00082 return (true); 00083 } 00084 00086 template <typename PointT> void 00087 pcl::SampleConsensusModelLine<PointT>::getDistancesToModel ( 00088 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00089 { 00090 // Needs a valid set of model coefficients 00091 if (!isModelValid (model_coefficients)) 00092 return; 00093 00094 distances.resize (indices_->size ()); 00095 00096 // Obtain the line point and direction 00097 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00098 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00099 line_dir.normalize (); 00100 00101 // Iterate through the 3d points and calculate the distances from them to the line 00102 for (size_t i = 0; i < indices_->size (); ++i) 00103 { 00104 // Calculate the distance from the point to the line 00105 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00106 // Need to estimate sqrt here to keep MSAC and friends general 00107 distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ()); 00108 } 00109 } 00110 00112 template <typename PointT> void 00113 pcl::SampleConsensusModelLine<PointT>::selectWithinDistance ( 00114 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00115 { 00116 // Needs a valid set of model coefficients 00117 if (!isModelValid (model_coefficients)) 00118 return; 00119 00120 double sqr_threshold = threshold * threshold; 00121 00122 int nr_p = 0; 00123 inliers.resize (indices_->size ()); 00124 00125 // Obtain the line point and direction 00126 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00127 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00128 line_dir.normalize (); 00129 00130 // Iterate through the 3d points and calculate the distances from them to the line 00131 for (size_t i = 0; i < indices_->size (); ++i) 00132 { 00133 // Calculate the distance from the point to the line 00134 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00135 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); 00136 00137 if (sqr_distance < sqr_threshold) 00138 { 00139 // Returns the indices of the points whose squared distances are smaller than the threshold 00140 inliers[nr_p] = (*indices_)[i]; 00141 nr_p++; 00142 } 00143 } 00144 inliers.resize (nr_p); 00145 } 00146 00148 template <typename PointT> int 00149 pcl::SampleConsensusModelLine<PointT>::countWithinDistance ( 00150 const Eigen::VectorXf &model_coefficients, const double threshold) 00151 { 00152 // Needs a valid set of model coefficients 00153 if (!isModelValid (model_coefficients)) 00154 return (0); 00155 00156 double sqr_threshold = threshold * threshold; 00157 00158 int nr_p = 0; 00159 00160 // Obtain the line point and direction 00161 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00162 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00163 line_dir.normalize (); 00164 00165 // Iterate through the 3d points and calculate the distances from them to the line 00166 for (size_t i = 0; i < indices_->size (); ++i) 00167 { 00168 // Calculate the distance from the point to the line 00169 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00170 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); 00171 00172 if (sqr_distance < sqr_threshold) 00173 nr_p++; 00174 } 00175 return (nr_p); 00176 } 00177 00179 template <typename PointT> void 00180 pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients ( 00181 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 00182 { 00183 // Needs a valid set of model coefficients 00184 if (!isModelValid (model_coefficients)) 00185 { 00186 optimized_coefficients = model_coefficients; 00187 return; 00188 } 00189 00190 // Need at least 2 points to estimate a line 00191 if (inliers.size () <= 2) 00192 { 00193 PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ()); 00194 optimized_coefficients = model_coefficients; 00195 return; 00196 } 00197 00198 optimized_coefficients.resize (6); 00199 00200 // Compute the 3x3 covariance matrix 00201 Eigen::Vector4f centroid; 00202 compute3DCentroid (*input_, inliers, centroid); 00203 Eigen::Matrix3f covariance_matrix; 00204 computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix); 00205 optimized_coefficients[0] = centroid[0]; 00206 optimized_coefficients[1] = centroid[1]; 00207 optimized_coefficients[2] = centroid[2]; 00208 00209 // Extract the eigenvalues and eigenvectors 00210 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00211 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00212 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00213 00214 optimized_coefficients.template tail<3> () = eigen_vectors.col (2).normalized (); 00215 } 00216 00218 template <typename PointT> void 00219 pcl::SampleConsensusModelLine<PointT>::projectPoints ( 00220 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) 00221 { 00222 // Needs a valid model coefficients 00223 if (!isModelValid (model_coefficients)) 00224 return; 00225 00226 // Obtain the line point and direction 00227 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00228 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00229 00230 projected_points.header = input_->header; 00231 projected_points.is_dense = input_->is_dense; 00232 00233 // Copy all the data fields from the input cloud to the projected one? 00234 if (copy_data_fields) 00235 { 00236 // Allocate enough space and copy the basics 00237 projected_points.points.resize (input_->points.size ()); 00238 projected_points.width = input_->width; 00239 projected_points.height = input_->height; 00240 00241 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00242 // Iterate over each point 00243 for (size_t i = 0; i < projected_points.points.size (); ++i) 00244 // Iterate over each dimension 00245 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00246 00247 // Iterate through the 3d points and calculate the distances from them to the line 00248 for (size_t i = 0; i < inliers.size (); ++i) 00249 { 00250 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); 00251 // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; 00252 double k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); 00253 00254 Eigen::Vector4f pp = line_pt + k * line_dir; 00255 // Calculate the projection of the point on the line (pointProj = A + k * B) 00256 projected_points.points[inliers[i]].x = pp[0]; 00257 projected_points.points[inliers[i]].y = pp[1]; 00258 projected_points.points[inliers[i]].z = pp[2]; 00259 } 00260 } 00261 else 00262 { 00263 // Allocate enough space and copy the basics 00264 projected_points.points.resize (inliers.size ()); 00265 projected_points.width = inliers.size (); 00266 projected_points.height = 1; 00267 00268 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00269 // Iterate over each point 00270 for (size_t i = 0; i < inliers.size (); ++i) 00271 // Iterate over each dimension 00272 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00273 00274 // Iterate through the 3d points and calculate the distances from them to the line 00275 for (size_t i = 0; i < inliers.size (); ++i) 00276 { 00277 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); 00278 // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; 00279 double k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); 00280 00281 Eigen::Vector4f pp = line_pt + k * line_dir; 00282 // Calculate the projection of the point on the line (pointProj = A + k * B) 00283 projected_points.points[i].x = pp[0]; 00284 projected_points.points[i].y = pp[1]; 00285 projected_points.points[i].z = pp[2]; 00286 } 00287 } 00288 } 00289 00291 template <typename PointT> bool 00292 pcl::SampleConsensusModelLine<PointT>::doSamplesVerifyModel ( 00293 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) 00294 { 00295 // Needs a valid set of model coefficients 00296 if (!isModelValid (model_coefficients)) 00297 return (false); 00298 00299 // Obtain the line point and direction 00300 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00301 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00302 line_dir.normalize (); 00303 00304 double sqr_threshold = threshold * threshold; 00305 // Iterate through the 3d points and calculate the distances from them to the line 00306 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00307 { 00308 // Calculate the distance from the point to the line 00309 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00310 if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold) 00311 return (false); 00312 } 00313 00314 return (true); 00315 } 00316 00317 #define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>; 00318 00319 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ 00320