Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009-2010, 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_cylinder.hpp 3027 2011-11-01 04:04:27Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ 00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ 00040 00041 #include "pcl/sample_consensus/sac_model_cylinder.h" 00042 #include "pcl/common/concatenate.h" 00043 #include <unsupported/Eigen/NonLinearOptimization> 00044 00046 template <typename PointT, typename PointNT> bool 00047 pcl::SampleConsensusModelCylinder<PointT, PointNT>::isSampleGood(const std::vector<int> &samples) const 00048 { 00049 return true; 00050 } 00051 00053 template <typename PointT, typename PointNT> bool 00054 pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients ( 00055 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) 00056 { 00057 // Need 2 samples 00058 if (samples.size () != 2) 00059 { 00060 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", (unsigned long)samples.size ()); 00061 return (false); 00062 } 00063 00064 if (!normals_) 00065 { 00066 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n"); 00067 return (false); 00068 } 00069 00070 Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0); 00071 Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0); 00072 00073 Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0); 00074 Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0); 00075 Eigen::Vector4f w = n1 + p1 - p2; 00076 00077 double a = n1.dot (n1); 00078 double b = n1.dot (n2); 00079 double c = n2.dot (n2); 00080 double d = n1.dot (w); 00081 double e = n2.dot (w); 00082 double denominator = a*c - b*b; 00083 double sc, tc; 00084 // Compute the line parameters of the two closest points 00085 if (denominator < 1e-8) // The lines are almost parallel 00086 { 00087 sc = 0.0; 00088 tc = (b > c ? d / b : e / c); // Use the largest denominator 00089 } 00090 else 00091 { 00092 sc = (b*e - c*d) / denominator; 00093 tc = (a*e - b*d) / denominator; 00094 } 00095 00096 // point_on_axis, axis_direction 00097 Eigen::Vector4f line_pt = p1 + n1 + sc * n1; 00098 Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt; 00099 line_dir.normalize (); 00100 00101 model_coefficients.resize (7); 00102 // model_coefficients.template head<3> () = line_pt.template head<3> (); 00103 model_coefficients[0] = line_pt[0]; 00104 model_coefficients[1] = line_pt[1]; 00105 model_coefficients[2] = line_pt[2]; 00106 // model_coefficients.template segment<3> (3) = line_dir.template head<3> (); 00107 model_coefficients[3] = line_dir[0]; 00108 model_coefficients[4] = line_dir[1]; 00109 model_coefficients[5] = line_dir[2]; 00110 // cylinder radius 00111 model_coefficients[6] = sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)); 00112 00113 if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_) 00114 return (false); 00115 00116 return (true); 00117 } 00118 00120 template <typename PointT, typename PointNT> void 00121 pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel ( 00122 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00123 { 00124 // Check if the model is valid given the user constraints 00125 if (!isModelValid (model_coefficients)) 00126 { 00127 distances.clear (); 00128 return; 00129 } 00130 00131 distances.resize (indices_->size ()); 00132 00133 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00134 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00135 double ptdotdir = line_pt.dot (line_dir); 00136 double dirdotdir = 1.0 / line_dir.dot (line_dir); 00137 // Iterate through the 3d points and calculate the distances from them to the sphere 00138 for (size_t i = 0; i < indices_->size (); ++i) 00139 { 00140 // Aproximate the distance from the point to the cylinder as the difference between 00141 // dist(point,cylinder_axis) and cylinder radius 00142 // @note need to revise this. 00143 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); 00144 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); 00145 00146 double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); 00147 00148 // Calculate the point's projection on the cylinder axis 00149 double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; 00150 Eigen::Vector4f pt_proj = line_pt + k * line_dir; 00151 Eigen::Vector4f dir = pt - pt_proj; 00152 dir.normalize (); 00153 00154 // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector 00155 double d_normal = fabs (getAngle3D (n, dir)); 00156 d_normal = (std::min) (d_normal, M_PI - d_normal); 00157 00158 distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); 00159 } 00160 } 00161 00163 template <typename PointT, typename PointNT> void 00164 pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance ( 00165 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00166 { 00167 // Check if the model is valid given the user constraints 00168 if (!isModelValid (model_coefficients)) 00169 { 00170 inliers.clear (); 00171 return; 00172 } 00173 00174 int nr_p = 0; 00175 inliers.resize (indices_->size ()); 00176 00177 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00178 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00179 double ptdotdir = line_pt.dot (line_dir); 00180 double dirdotdir = 1.0 / line_dir.dot (line_dir); 00181 // Iterate through the 3d points and calculate the distances from them to the sphere 00182 for (size_t i = 0; i < indices_->size (); ++i) 00183 { 00184 // Aproximate the distance from the point to the cylinder as the difference between 00185 // dist(point,cylinder_axis) and cylinder radius 00186 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); 00187 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); 00188 double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); 00189 00190 // Calculate the point's projection on the cylinder axis 00191 double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; 00192 Eigen::Vector4f pt_proj = line_pt + k * line_dir; 00193 Eigen::Vector4f dir = pt - pt_proj; 00194 dir.normalize (); 00195 00196 // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector 00197 double d_normal = fabs (getAngle3D (n, dir)); 00198 d_normal = (std::min) (d_normal, M_PI - d_normal); 00199 00200 if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) 00201 { 00202 // Returns the indices of the points whose distances are smaller than the threshold 00203 inliers[nr_p] = (*indices_)[i]; 00204 nr_p++; 00205 } 00206 } 00207 inliers.resize (nr_p); 00208 } 00209 00211 template <typename PointT, typename PointNT> int 00212 pcl::SampleConsensusModelCylinder<PointT, PointNT>::countWithinDistance ( 00213 const Eigen::VectorXf &model_coefficients, const double threshold) 00214 { 00215 // Check if the model is valid given the user constraints 00216 if (!isModelValid (model_coefficients)) 00217 return (0); 00218 00219 int nr_p = 0; 00220 00221 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00222 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00223 double ptdotdir = line_pt.dot (line_dir); 00224 double dirdotdir = 1.0 / line_dir.dot (line_dir); 00225 // Iterate through the 3d points and calculate the distances from them to the sphere 00226 for (size_t i = 0; i < indices_->size (); ++i) 00227 { 00228 // Aproximate the distance from the point to the cylinder as the difference between 00229 // dist(point,cylinder_axis) and cylinder radius 00230 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); 00231 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); 00232 double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); 00233 00234 // Calculate the point's projection on the cylinder axis 00235 double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; 00236 Eigen::Vector4f pt_proj = line_pt + k * line_dir; 00237 Eigen::Vector4f dir = pt - pt_proj; 00238 dir.normalize (); 00239 00240 // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector 00241 double d_normal = fabs (getAngle3D (n, dir)); 00242 d_normal = (std::min) (d_normal, M_PI - d_normal); 00243 00244 if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) 00245 nr_p++; 00246 } 00247 return (nr_p); 00248 } 00249 00251 template <typename PointT, typename PointNT> void 00252 pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients ( 00253 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 00254 { 00255 boost::mutex::scoped_lock lock (tmp_mutex_); 00256 00257 const int n_unknowns = 7; // 4 unknowns 00258 // Needs a set of valid model coefficients 00259 if (model_coefficients.size () != n_unknowns) 00260 { 00261 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00262 optimized_coefficients = model_coefficients; 00263 return; 00264 } 00265 00266 if (inliers.empty ()) 00267 { 00268 PCL_DEBUG ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n"); 00269 optimized_coefficients = model_coefficients; 00270 return; 00271 } 00272 00273 tmp_inliers_ = &inliers; 00274 int m = inliers.size (); 00275 00276 Eigen::VectorXd x(n_unknowns); 00277 for(int d = 0; d < n_unknowns; d++) 00278 x[d] = model_coefficients[d]; 00279 00280 OptimizationFunctor functor(n_unknowns, m, this); 00281 Eigen::NumericalDiff<OptimizationFunctor > num_diff(functor); 00282 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm(num_diff); 00283 int info = lm.minimize(x); 00284 00285 // Compute the L2 norm of the residuals 00286 PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n", 00287 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], 00288 model_coefficients[4], model_coefficients[5], model_coefficients[6], x[0], x[1], x[2], x[3], x[4], x[5], x[6]); 00289 00290 optimized_coefficients.resize (n_unknowns); 00291 for (int d = 0; d < n_unknowns; ++d) 00292 optimized_coefficients[d] = x[d]; 00293 } 00294 00296 template <typename PointT, typename PointNT> void 00297 pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints ( 00298 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) 00299 { 00300 // Needs a valid set of model coefficients 00301 if (model_coefficients.size () != 7) 00302 { 00303 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00304 return; 00305 } 00306 00307 projected_points.header = input_->header; 00308 projected_points.is_dense = input_->is_dense; 00309 00310 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00311 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00312 double ptdotdir = line_pt.dot (line_dir); 00313 double dirdotdir = 1.0 / line_dir.dot (line_dir); 00314 00315 // Copy all the data fields from the input cloud to the projected one? 00316 if (copy_data_fields) 00317 { 00318 // Allocate enough space and copy the basics 00319 projected_points.points.resize (input_->points.size ()); 00320 projected_points.width = input_->width; 00321 projected_points.height = input_->height; 00322 00323 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00324 // Iterate over each point 00325 for (size_t i = 0; i < projected_points.points.size (); ++i) 00326 // Iterate over each dimension 00327 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00328 00329 // Iterate through the 3d points and calculate the distances from them to the cylinder 00330 for (size_t i = 0; i < inliers.size (); ++i) 00331 { 00332 Eigen::Vector4f p (input_->points[inliers[i]].x, 00333 input_->points[inliers[i]].y, 00334 input_->points[inliers[i]].z, 00335 1); 00336 00337 double k = (p.dot (line_dir) - ptdotdir) * dirdotdir; 00338 00339 pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap (); 00340 pp = line_pt + k * line_dir; 00341 00342 Eigen::Vector4f dir = p - pp; 00343 dir.normalize (); 00344 00345 // Calculate the projection of the point onto the cylinder 00346 pp += dir * model_coefficients[6]; 00347 } 00348 } 00349 else 00350 { 00351 // Allocate enough space and copy the basics 00352 projected_points.points.resize (inliers.size ()); 00353 projected_points.width = inliers.size (); 00354 projected_points.height = 1; 00355 00356 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00357 // Iterate over each point 00358 for (size_t i = 0; i < inliers.size (); ++i) 00359 // Iterate over each dimension 00360 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00361 00362 // Iterate through the 3d points and calculate the distances from them to the plane 00363 for (size_t i = 0; i < inliers.size (); ++i) 00364 { 00365 pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap (); 00366 pcl::Vector4fMapConst p = input_->points[inliers[i]].getVector4fMap (); 00367 00368 double k = (p.dot (line_dir) - ptdotdir) * dirdotdir; 00369 // Calculate the projection of the point on the line 00370 pp = line_pt + k * line_dir; 00371 00372 Eigen::Vector4f dir = p - pp; 00373 dir.normalize (); 00374 00375 // Calculate the projection of the point onto the cylinder 00376 pp += dir * model_coefficients[6]; 00377 } 00378 } 00379 } 00380 00382 template <typename PointT, typename PointNT> bool 00383 pcl::SampleConsensusModelCylinder<PointT, PointNT>::doSamplesVerifyModel ( 00384 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) 00385 { 00386 // Needs a valid model coefficients 00387 if (model_coefficients.size () != 7) 00388 { 00389 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00390 return (false); 00391 } 00392 00393 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00394 { 00395 // Aproximate the distance from the point to the cylinder as the difference between 00396 // dist(point,cylinder_axis) and cylinder radius 00397 // @note need to revise this. 00398 Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0); 00399 if (fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) 00400 return (false); 00401 } 00402 00403 return (true); 00404 } 00405 00407 template <typename PointT, typename PointNT> double 00408 pcl::SampleConsensusModelCylinder<PointT, PointNT>::pointToLineDistance ( 00409 const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) 00410 { 00411 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00412 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00413 return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir)); 00414 } 00415 00417 template <typename PointT, typename PointNT> void 00418 pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPointToCylinder ( 00419 const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) 00420 { 00421 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00422 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); 00423 00424 double k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir); 00425 pt_proj = line_pt + k * line_dir; 00426 00427 Eigen::Vector4f dir = pt - pt_proj; 00428 dir.normalize (); 00429 00430 // Calculate the projection of the point onto the cylinder 00431 pt_proj += dir * model_coefficients[6]; 00432 } 00433 00435 template <typename PointT, typename PointNT> bool 00436 pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) 00437 { 00438 // Needs a valid model coefficients 00439 if (model_coefficients.size () != 7) 00440 { 00441 PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isModelValid] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00442 return (false); 00443 } 00444 00445 // Check against template, if given 00446 if (eps_angle_ > 0.0) 00447 { 00448 // Obtain the cylinder direction 00449 Eigen::Vector4f coeff; 00450 coeff[0] = model_coefficients[3]; 00451 coeff[1] = model_coefficients[4]; 00452 coeff[2] = model_coefficients[5]; 00453 coeff[3] = 0; 00454 00455 Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); 00456 double angle_diff = fabs (getAngle3D (axis, coeff)); 00457 angle_diff = (std::min) (angle_diff, M_PI - angle_diff); 00458 // Check whether the current cylinder model satisfies our angle threshold criterion with respect to the given axis 00459 if (angle_diff > eps_angle_) 00460 return (false); 00461 } 00462 00463 if (radius_min_ != -DBL_MAX && model_coefficients[6] < radius_min_) 00464 return (false); 00465 if (radius_max_ != DBL_MAX && model_coefficients[6] > radius_max_) 00466 return (false); 00467 00468 return (true); 00469 } 00470 00471 #define PCL_INSTANTIATE_SampleConsensusModelCylinder(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCylinder<PointT, PointNT>; 00472 00473 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ 00474