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) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * 00037 */ 00038 00039 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_ 00040 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_ 00041 00042 #include <pcl/sample_consensus/eigen.h> 00043 #include <pcl/sample_consensus/sac_model_circle3d.h> 00044 #include <pcl/common/concatenate.h> 00045 00046 ////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT> bool 00048 pcl::SampleConsensusModelCircle3D<PointT>::isSampleGood ( 00049 const std::vector<int> &samples) const 00050 { 00051 // Get the values at the three points 00052 Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z); 00053 Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z); 00054 Eigen::Vector3d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z); 00055 00056 // calculate vectors between points 00057 p1 -= p0; 00058 p2 -= p0; 00059 00060 if (p1.dot (p2) < 0.000001) 00061 return (true); 00062 else 00063 return (false); 00064 } 00065 00066 ////////////////////////////////////////////////////////////////////////// 00067 template <typename PointT> bool 00068 pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) 00069 { 00070 // Need 3 samples 00071 if (samples.size () != 3) 00072 { 00073 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); 00074 return (false); 00075 } 00076 00077 model_coefficients.resize (7); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ 00078 00079 Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z); 00080 Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z); 00081 Eigen::Vector3d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z); 00082 00083 00084 Eigen::Vector3d helper_vec01 = p0 - p1; 00085 Eigen::Vector3d helper_vec02 = p0 - p2; 00086 Eigen::Vector3d helper_vec10 = p1 - p0; 00087 Eigen::Vector3d helper_vec12 = p1 - p2; 00088 Eigen::Vector3d helper_vec20 = p2 - p0; 00089 Eigen::Vector3d helper_vec21 = p2 - p1; 00090 00091 Eigen::Vector3d common_helper_vec = helper_vec01.cross (helper_vec12); 00092 00093 double commonDividend = 2.0 * common_helper_vec.squaredNorm (); 00094 00095 double alpha = (helper_vec12.squaredNorm () * helper_vec01.dot (helper_vec02)) / commonDividend; 00096 double beta = (helper_vec02.squaredNorm () * helper_vec10.dot (helper_vec12)) / commonDividend; 00097 double gamma = (helper_vec01.squaredNorm () * helper_vec20.dot (helper_vec21)) / commonDividend; 00098 00099 Eigen::Vector3d circle_center = alpha * p0 + beta * p1 + gamma * p2; 00100 00101 Eigen::Vector3d circle_radiusVector = circle_center - p0; 00102 double circle_radius = circle_radiusVector.norm (); 00103 Eigen::Vector3d circle_normal = common_helper_vec.normalized (); 00104 00105 model_coefficients[0] = static_cast<float> (circle_center[0]); 00106 model_coefficients[1] = static_cast<float> (circle_center[1]); 00107 model_coefficients[2] = static_cast<float> (circle_center[2]); 00108 model_coefficients[3] = static_cast<float> (circle_radius); 00109 model_coefficients[4] = static_cast<float> (circle_normal[0]); 00110 model_coefficients[5] = static_cast<float> (circle_normal[1]); 00111 model_coefficients[6] = static_cast<float> (circle_normal[2]); 00112 00113 return (true); 00114 } 00115 00116 ////////////////////////////////////////////////////////////////////////// 00117 template <typename PointT> void 00118 pcl::SampleConsensusModelCircle3D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00119 { 00120 // Check if the model is valid given the user constraints 00121 if (!isModelValid (model_coefficients)) 00122 { 00123 distances.clear (); 00124 return; 00125 } 00126 distances.resize (indices_->size ()); 00127 00128 // Iterate through the 3d points and calculate the distances from them to the sphere 00129 for (size_t i = 0; i < indices_->size (); ++i) 00130 // Calculate the distance from the point to the circle: 00131 // 1. calculate intersection point of the plane in which the circle lies and the 00132 // line from the sample point with the direction of the plane normal (projected point) 00133 // 2. calculate the intersection point of the line from the circle center to the projected point 00134 // with the circle 00135 // 3. calculate distance from corresponding point on the circle to the sample point 00136 { 00137 // what i have: 00138 // P : Sample Point 00139 Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z); 00140 // C : Circle Center 00141 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); 00142 // N : Circle (Plane) Normal 00143 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); 00144 // r : Radius 00145 double r = model_coefficients[3]; 00146 00147 Eigen::Vector3d helper_vectorPC = P - C; 00148 // 1.1. get line parameter 00149 double lambda = (helper_vectorPC.dot (N)) / N.squaredNorm (); 00150 00151 // Projected Point on plane 00152 Eigen::Vector3d P_proj = P + lambda * N; 00153 Eigen::Vector3d helper_vectorP_projC = P_proj - C; 00154 00155 // K : Point on Circle 00156 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); 00157 Eigen::Vector3d distanceVector = P - K; 00158 00159 distances[i] = distanceVector.norm (); 00160 } 00161 } 00162 00163 ////////////////////////////////////////////////////////////////////////// 00164 template <typename PointT> void 00165 pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance ( 00166 const Eigen::VectorXf &model_coefficients, const double threshold, 00167 std::vector<int> &inliers) 00168 { 00169 // Check if the model is valid given the user constraints 00170 if (!isModelValid (model_coefficients)) 00171 { 00172 inliers.clear (); 00173 return; 00174 } 00175 int nr_p = 0; 00176 inliers.resize (indices_->size ()); 00177 00178 // Iterate through the 3d points and calculate the distances from them to the sphere 00179 for (size_t i = 0; i < indices_->size (); ++i) 00180 { 00181 // what i have: 00182 // P : Sample Point 00183 Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z); 00184 // C : Circle Center 00185 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); 00186 // N : Circle (Plane) Normal 00187 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); 00188 // r : Radius 00189 double r = model_coefficients[3]; 00190 00191 Eigen::Vector3d helper_vectorPC = P - C; 00192 // 1.1. get line parameter 00193 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); 00194 // Projected Point on plane 00195 Eigen::Vector3d P_proj = P + lambda * N; 00196 Eigen::Vector3d helper_vectorP_projC = P_proj - C; 00197 00198 // K : Point on Circle 00199 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); 00200 Eigen::Vector3d distanceVector = P - K; 00201 00202 if (distanceVector.norm () < threshold) 00203 { 00204 // Returns the indices of the points whose distances are smaller than the threshold 00205 inliers[nr_p] = (*indices_)[i]; 00206 nr_p++; 00207 } 00208 } 00209 inliers.resize (nr_p); 00210 } 00211 00212 ////////////////////////////////////////////////////////////////////////// 00213 template <typename PointT> int 00214 pcl::SampleConsensusModelCircle3D<PointT>::countWithinDistance ( 00215 const Eigen::VectorXf &model_coefficients, const double threshold) 00216 { 00217 // Check if the model is valid given the user constraints 00218 if (!isModelValid (model_coefficients)) 00219 return (0); 00220 int nr_p = 0; 00221 00222 // Iterate through the 3d points and calculate the distances from them to the sphere 00223 for (size_t i = 0; i < indices_->size (); ++i) 00224 { 00225 // what i have: 00226 // P : Sample Point 00227 Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z); 00228 // C : Circle Center 00229 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); 00230 // N : Circle (Plane) Normal 00231 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); 00232 // r : Radius 00233 double r = model_coefficients[3]; 00234 00235 Eigen::Vector3d helper_vectorPC = P - C; 00236 // 1.1. get line parameter 00237 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); 00238 00239 // Projected Point on plane 00240 Eigen::Vector3d P_proj = P + lambda * N; 00241 Eigen::Vector3d helper_vectorP_projC = P_proj - C; 00242 00243 // K : Point on Circle 00244 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); 00245 Eigen::Vector3d distanceVector = P - K; 00246 00247 if (distanceVector.norm () < threshold) 00248 nr_p++; 00249 } 00250 return (nr_p); 00251 } 00252 00253 ////////////////////////////////////////////////////////////////////////// 00254 template <typename PointT> void 00255 pcl::SampleConsensusModelCircle3D<PointT>::optimizeModelCoefficients ( 00256 const std::vector<int> &inliers, 00257 const Eigen::VectorXf &model_coefficients, 00258 Eigen::VectorXf &optimized_coefficients) 00259 { 00260 optimized_coefficients = model_coefficients; 00261 00262 // Needs a set of valid model coefficients 00263 if (model_coefficients.size () != 7) 00264 { 00265 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00266 return; 00267 } 00268 00269 // Need at least 3 samples 00270 if (inliers.size () <= 3) 00271 { 00272 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); 00273 return; 00274 } 00275 00276 tmp_inliers_ = &inliers; 00277 00278 OptimizationFunctor functor (static_cast<int> (inliers.size ()), this); 00279 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); 00280 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff); 00281 Eigen::VectorXd coeff; 00282 int info = lm.minimize (coeff); 00283 for (size_t i = 0; i < coeff.size (); ++i) 00284 optimized_coefficients[i] = static_cast<float> (coeff[i]); 00285 00286 // Compute the L2 norm of the residuals 00287 PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::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", 00288 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]); 00289 } 00290 00291 ////////////////////////////////////////////////////////////////////////// 00292 template <typename PointT> void 00293 pcl::SampleConsensusModelCircle3D<PointT>::projectPoints ( 00294 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, 00295 PointCloud &projected_points, bool copy_data_fields) 00296 { 00297 // Needs a valid set of model coefficients 00298 if (model_coefficients.size () != 7) 00299 { 00300 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00301 return; 00302 } 00303 00304 projected_points.header = input_->header; 00305 projected_points.is_dense = input_->is_dense; 00306 00307 // Copy all the data fields from the input cloud to the projected one? 00308 if (copy_data_fields) 00309 { 00310 // Allocate enough space and copy the basics 00311 projected_points.points.resize (input_->points.size ()); 00312 projected_points.width = input_->width; 00313 projected_points.height = input_->height; 00314 00315 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00316 // Iterate over each point 00317 for (size_t i = 0; i < projected_points.points.size (); ++i) 00318 // Iterate over each dimension 00319 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00320 00321 // Iterate through the 3d points and calculate the distances from them to the plane 00322 for (size_t i = 0; i < inliers.size (); ++i) 00323 { 00324 // what i have: 00325 // P : Sample Point 00326 Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z); 00327 // C : Circle Center 00328 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); 00329 // N : Circle (Plane) Normal 00330 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); 00331 // r : Radius 00332 double r = model_coefficients[3]; 00333 00334 Eigen::Vector3d helper_vectorPC = P - C; 00335 // 1.1. get line parameter 00336 //float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ; 00337 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); 00338 // Projected Point on plane 00339 Eigen::Vector3d P_proj = P + lambda * N; 00340 Eigen::Vector3d helper_vectorP_projC = P_proj - C; 00341 00342 // K : Point on Circle 00343 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); 00344 00345 projected_points.points[i].x = static_cast<float> (K[0]); 00346 projected_points.points[i].y = static_cast<float> (K[1]); 00347 projected_points.points[i].z = static_cast<float> (K[2]); 00348 } 00349 } 00350 else 00351 { 00352 // Allocate enough space and copy the basics 00353 projected_points.points.resize (inliers.size ()); 00354 projected_points.width = uint32_t (inliers.size ()); 00355 projected_points.height = 1; 00356 00357 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00358 // Iterate over each point 00359 for (size_t i = 0; i < inliers.size (); ++i) 00360 // Iterate over each dimension 00361 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00362 00363 // Iterate through the 3d points and calculate the distances from them to the plane 00364 for (size_t i = 0; i < inliers.size (); ++i) 00365 { 00366 // what i have: 00367 // P : Sample Point 00368 Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z); 00369 // C : Circle Center 00370 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); 00371 // N : Circle (Plane) Normal 00372 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); 00373 // r : Radius 00374 double r = model_coefficients[3]; 00375 00376 Eigen::Vector3d helper_vectorPC = P - C; 00377 // 1.1. get line parameter 00378 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); 00379 // Projected Point on plane 00380 Eigen::Vector3d P_proj = P + lambda * N; 00381 Eigen::Vector3d helper_vectorP_projC = P_proj - C; 00382 00383 // K : Point on Circle 00384 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); 00385 00386 projected_points.points[i].x = static_cast<float> (K[0]); 00387 projected_points.points[i].y = static_cast<float> (K[1]); 00388 projected_points.points[i].z = static_cast<float> (K[2]); 00389 } 00390 } 00391 } 00392 00393 ////////////////////////////////////////////////////////////////////////// 00394 template <typename PointT> bool 00395 pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel ( 00396 const std::set<int> &indices, 00397 const Eigen::VectorXf &model_coefficients, 00398 const double threshold) 00399 { 00400 // Needs a valid model coefficients 00401 if (model_coefficients.size () != 7) 00402 { 00403 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00404 return (false); 00405 } 00406 00407 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00408 { 00409 // Calculate the distance from the point to the sphere as the difference between 00410 //dist(point,sphere_origin) and sphere_radius 00411 00412 // what i have: 00413 // P : Sample Point 00414 Eigen::Vector3d P (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z); 00415 // C : Circle Center 00416 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); 00417 // N : Circle (Plane) Normal 00418 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); 00419 // r : Radius 00420 double r = model_coefficients[3]; 00421 Eigen::Vector3d helper_vectorPC = P - C; 00422 // 1.1. get line parameter 00423 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); 00424 // Projected Point on plane 00425 Eigen::Vector3d P_proj = P + lambda * N; 00426 Eigen::Vector3d helper_vectorP_projC = P_proj - C; 00427 00428 // K : Point on Circle 00429 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); 00430 Eigen::Vector3d distanceVector = P - K; 00431 00432 if (distanceVector.norm () > threshold) 00433 return (false); 00434 } 00435 return (true); 00436 } 00437 00438 ////////////////////////////////////////////////////////////////////////// 00439 template <typename PointT> bool 00440 pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) 00441 { 00442 // Needs a valid model coefficients 00443 if (model_coefficients.size () != 7) 00444 { 00445 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00446 return (false); 00447 } 00448 00449 if (radius_min_ != -DBL_MAX && model_coefficients[3] < radius_min_) 00450 return (false); 00451 if (radius_max_ != DBL_MAX && model_coefficients[3] > radius_max_) 00452 return (false); 00453 00454 return (true); 00455 } 00456 00457 #define PCL_INSTANTIATE_SampleConsensusModelCircle3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle3D<T>; 00458 00459 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE3D_HPP_ 00460