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