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: sac_model_line.hpp 2328 2011-08-31 08:11:00Z rusu $ 00038 * 00039 */ 00040 00041 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ 00042 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ 00043 00044 #include <pcl/sample_consensus/sac_model_stick.h> 00045 #include <pcl/common/centroid.h> 00046 #include <pcl/common/concatenate.h> 00047 00048 ////////////////////////////////////////////////////////////////////////// 00049 template <typename PointT> bool 00050 pcl::SampleConsensusModelStick<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 (false); 00061 } 00062 00063 ////////////////////////////////////////////////////////////////////////// 00064 template <typename PointT> bool 00065 pcl::SampleConsensusModelStick<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::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); 00072 return (false); 00073 } 00074 00075 model_coefficients.resize (7); 00076 model_coefficients[0] = input_->points[samples[0]].x; 00077 model_coefficients[1] = input_->points[samples[0]].y; 00078 model_coefficients[2] = input_->points[samples[0]].z; 00079 00080 model_coefficients[3] = input_->points[samples[1]].x; 00081 model_coefficients[4] = input_->points[samples[1]].y; 00082 model_coefficients[5] = input_->points[samples[1]].z; 00083 00084 // model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0]; 00085 // model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1]; 00086 // model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2]; 00087 00088 // model_coefficients.template segment<3> (3).normalize (); 00089 // We don't care about model_coefficients[6] which is the width (radius) of the stick 00090 00091 return (true); 00092 } 00093 00094 ////////////////////////////////////////////////////////////////////////// 00095 template <typename PointT> void 00096 pcl::SampleConsensusModelStick<PointT>::getDistancesToModel ( 00097 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00098 { 00099 // Needs a valid set of model coefficients 00100 if (!isModelValid (model_coefficients)) 00101 return; 00102 00103 float sqr_threshold = static_cast<float> (radius_max_ * radius_max_); 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 float sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); 00117 00118 if (sqr_distance < sqr_threshold) 00119 // Need to estimate sqrt here to keep MSAC and friends general 00120 distances[i] = sqrt (sqr_distance); 00121 else 00122 // Penalize outliers by doubling the distance 00123 distances[i] = 2 * sqrt (sqr_distance); 00124 } 00125 } 00126 00127 ////////////////////////////////////////////////////////////////////////// 00128 template <typename PointT> void 00129 pcl::SampleConsensusModelStick<PointT>::selectWithinDistance ( 00130 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00131 { 00132 // Needs a valid set of model coefficients 00133 if (!isModelValid (model_coefficients)) 00134 return; 00135 00136 float sqr_threshold = static_cast<float> (threshold * threshold); 00137 00138 int nr_p = 0; 00139 inliers.resize (indices_->size ()); 00140 error_sqr_dists_.resize (indices_->size ()); 00141 00142 // Obtain the line point and direction 00143 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00144 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00145 Eigen::Vector4f line_dir = line_pt2 - line_pt1; 00146 //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00147 //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); 00148 line_dir.normalize (); 00149 //float norm = line_dir.squaredNorm (); 00150 00151 // Iterate through the 3d points and calculate the distances from them to the line 00152 for (size_t i = 0; i < indices_->size (); ++i) 00153 { 00154 // Calculate the distance from the point to the line 00155 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00156 Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1; 00157 //float u = dir.dot (line_dir); 00158 00159 // If the point falls outside of the segment, ignore it 00160 //if (u < 0.0f || u > 1.0f) 00161 // continue; 00162 00163 float sqr_distance = dir.cross3 (line_dir).squaredNorm (); 00164 if (sqr_distance < sqr_threshold) 00165 { 00166 // Returns the indices of the points whose squared distances are smaller than the threshold 00167 inliers[nr_p] = (*indices_)[i]; 00168 error_sqr_dists_[nr_p] = static_cast<double> (sqr_distance); 00169 ++nr_p; 00170 } 00171 } 00172 inliers.resize (nr_p); 00173 error_sqr_dists_.resize (nr_p); 00174 } 00175 00176 /////////////////////////////////////////////////////////////////////////// 00177 template <typename PointT> int 00178 pcl::SampleConsensusModelStick<PointT>::countWithinDistance ( 00179 const Eigen::VectorXf &model_coefficients, const double threshold) 00180 { 00181 // Needs a valid set of model coefficients 00182 if (!isModelValid (model_coefficients)) 00183 return (0); 00184 00185 float sqr_threshold = static_cast<float> (threshold * threshold); 00186 00187 int nr_i = 0, nr_o = 0; 00188 00189 // Obtain the line point and direction 00190 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00191 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00192 Eigen::Vector4f line_dir = line_pt2 - line_pt1; 00193 line_dir.normalize (); 00194 00195 //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); 00196 //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00197 00198 // Iterate through the 3d points and calculate the distances from them to the line 00199 for (size_t i = 0; i < indices_->size (); ++i) 00200 { 00201 // Calculate the distance from the point to the line 00202 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00203 Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1; 00204 //float u = dir.dot (line_dir); 00205 00206 // If the point falls outside of the segment, ignore it 00207 //if (u < 0.0f || u > 1.0f) 00208 // continue; 00209 00210 float sqr_distance = dir.cross3 (line_dir).squaredNorm (); 00211 // Use a larger threshold (4 times the radius) to get more points in 00212 if (sqr_distance < sqr_threshold) 00213 nr_i++; 00214 else if (sqr_distance < 4 * sqr_threshold) 00215 nr_o++; 00216 } 00217 00218 return (nr_i - nr_o < 0 ? 0 : nr_i - nr_o); 00219 } 00220 00221 ////////////////////////////////////////////////////////////////////////// 00222 template <typename PointT> void 00223 pcl::SampleConsensusModelStick<PointT>::optimizeModelCoefficients ( 00224 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 00225 { 00226 // Needs a valid set of model coefficients 00227 if (!isModelValid (model_coefficients)) 00228 { 00229 optimized_coefficients = model_coefficients; 00230 return; 00231 } 00232 00233 // Need at least 2 points to estimate a line 00234 if (inliers.size () <= 2) 00235 { 00236 PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); 00237 optimized_coefficients = model_coefficients; 00238 return; 00239 } 00240 00241 optimized_coefficients.resize (7); 00242 00243 // Compute the 3x3 covariance matrix 00244 Eigen::Vector4f centroid; 00245 Eigen::Matrix3f covariance_matrix; 00246 00247 computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, centroid); 00248 00249 optimized_coefficients[0] = centroid[0]; 00250 optimized_coefficients[1] = centroid[1]; 00251 optimized_coefficients[2] = centroid[2]; 00252 00253 // Extract the eigenvalues and eigenvectors 00254 Eigen::Vector3f eigen_values; 00255 Eigen::Vector3f eigen_vector; 00256 pcl::eigen33 (covariance_matrix, eigen_values); 00257 pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector); 00258 00259 optimized_coefficients.template segment<3> (3).matrix () = eigen_vector; 00260 } 00261 00262 ////////////////////////////////////////////////////////////////////////// 00263 template <typename PointT> void 00264 pcl::SampleConsensusModelStick<PointT>::projectPoints ( 00265 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) 00266 { 00267 // Needs a valid model coefficients 00268 if (!isModelValid (model_coefficients)) 00269 return; 00270 00271 // Obtain the line point and direction 00272 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00273 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00274 00275 projected_points.header = input_->header; 00276 projected_points.is_dense = input_->is_dense; 00277 00278 // Copy all the data fields from the input cloud to the projected one? 00279 if (copy_data_fields) 00280 { 00281 // Allocate enough space and copy the basics 00282 projected_points.points.resize (input_->points.size ()); 00283 projected_points.width = input_->width; 00284 projected_points.height = input_->height; 00285 00286 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00287 // Iterate over each point 00288 for (size_t i = 0; i < projected_points.points.size (); ++i) 00289 // Iterate over each dimension 00290 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00291 00292 // Iterate through the 3d points and calculate the distances from them to the line 00293 for (size_t i = 0; i < inliers.size (); ++i) 00294 { 00295 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); 00296 // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; 00297 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); 00298 00299 Eigen::Vector4f pp = line_pt + k * line_dir; 00300 // Calculate the projection of the point on the line (pointProj = A + k * B) 00301 projected_points.points[inliers[i]].x = pp[0]; 00302 projected_points.points[inliers[i]].y = pp[1]; 00303 projected_points.points[inliers[i]].z = pp[2]; 00304 } 00305 } 00306 else 00307 { 00308 // Allocate enough space and copy the basics 00309 projected_points.points.resize (inliers.size ()); 00310 projected_points.width = static_cast<uint32_t> (inliers.size ()); 00311 projected_points.height = 1; 00312 00313 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00314 // Iterate over each point 00315 for (size_t i = 0; i < inliers.size (); ++i) 00316 // Iterate over each dimension 00317 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00318 00319 // Iterate through the 3d points and calculate the distances from them to the line 00320 for (size_t i = 0; i < inliers.size (); ++i) 00321 { 00322 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); 00323 // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; 00324 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); 00325 00326 Eigen::Vector4f pp = line_pt + k * line_dir; 00327 // Calculate the projection of the point on the line (pointProj = A + k * B) 00328 projected_points.points[i].x = pp[0]; 00329 projected_points.points[i].y = pp[1]; 00330 projected_points.points[i].z = pp[2]; 00331 } 00332 } 00333 } 00334 00335 ////////////////////////////////////////////////////////////////////////// 00336 template <typename PointT> bool 00337 pcl::SampleConsensusModelStick<PointT>::doSamplesVerifyModel ( 00338 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) 00339 { 00340 // Needs a valid set of model coefficients 00341 if (!isModelValid (model_coefficients)) 00342 return (false); 00343 00344 // Obtain the line point and direction 00345 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00346 Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); 00347 //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00348 line_dir.normalize (); 00349 00350 float sqr_threshold = static_cast<float> (threshold * threshold); 00351 // Iterate through the 3d points and calculate the distances from them to the line 00352 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00353 { 00354 // Calculate the distance from the point to the line 00355 // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) 00356 if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold) 00357 return (false); 00358 } 00359 00360 return (true); 00361 } 00362 00363 #define PCL_INSTANTIATE_SampleConsensusModelStick(T) template class PCL_EXPORTS pcl::SampleConsensusModelStick<T>; 00364 00365 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ 00366