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_circle.hpp 3027 2011-11-01 04:04:27Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ 00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ 00040 00041 #include "pcl/sample_consensus/sac_model_circle.h" 00042 #include "pcl/common/concatenate.h" 00043 #include <unsupported/Eigen/NonLinearOptimization> 00044 00046 template <typename PointT> bool 00047 pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const std::vector<int> &samples) const 00048 { 00049 // Get the values at the two points 00050 Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y); 00051 Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y); 00052 Eigen::Array2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y); 00053 00054 // Compute the segment values (in 2d) between p1 and p0 00055 p1 -= p0; 00056 // Compute the segment values (in 2d) between p2 and p0 00057 p2 -= p0; 00058 00059 Eigen::Array2d dy1dy2 = p1 / p2; 00060 00061 return (dy1dy2[0] != dy1dy2[1]); 00062 } 00063 00065 template <typename PointT> bool 00066 pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) 00067 { 00068 // Need 3 samples 00069 if (samples.size () != 3) 00070 { 00071 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", (unsigned long)samples.size ()); 00072 return (false); 00073 } 00074 00075 model_coefficients.resize (3); 00076 00077 Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y); 00078 Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y); 00079 Eigen::Vector2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y); 00080 00081 Eigen::Vector2d u = (p0 + p1) / 2.0; 00082 Eigen::Vector2d v = (p1 + p2) / 2.0; 00083 00084 Eigen::Vector2d p1p0dif = p1 - p0; 00085 Eigen::Vector2d p2p1dif = p2 - p1; 00086 Eigen::Vector2d uvdif = u - v; 00087 00088 Eigen::Vector2d m (- p1p0dif[0] / p1p0dif[1], - p2p1dif[0] / p2p1dif[1]); 00089 00090 // Center (x, y) 00091 model_coefficients[0] = (m[0] * u[0] - m[1] * v[0] - uvdif[1] ) / (m[0] - m[1]); 00092 model_coefficients[1] = (m[0] * m[1] * uvdif[0] + m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1]); 00093 00094 // Radius 00095 model_coefficients[2] = sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) + 00096 (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1])); 00097 return (true); 00098 } 00099 00101 template <typename PointT> void 00102 pcl::SampleConsensusModelCircle2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00103 { 00104 // Check if the model is valid given the user constraints 00105 if (!isModelValid (model_coefficients)) 00106 { 00107 distances.clear (); 00108 return; 00109 } 00110 distances.resize (indices_->size ()); 00111 00112 // Iterate through the 3d points and calculate the distances from them to the sphere 00113 for (size_t i = 0; i < indices_->size (); ++i) 00114 // Calculate the distance from the point to the circle as the difference between 00115 // dist(point,circle_origin) and circle_radius 00116 distances[i] = fabs (sqrt ( 00117 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * 00118 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + 00119 00120 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * 00121 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) 00122 ) - model_coefficients[2]); 00123 } 00124 00126 template <typename PointT> void 00127 pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance ( 00128 const Eigen::VectorXf &model_coefficients, const double threshold, 00129 std::vector<int> &inliers) 00130 { 00131 // Check if the model is valid given the user constraints 00132 if (!isModelValid (model_coefficients)) 00133 { 00134 inliers.clear (); 00135 return; 00136 } 00137 int nr_p = 0; 00138 inliers.resize (indices_->size ()); 00139 00140 // Iterate through the 3d points and calculate the distances from them to the sphere 00141 for (size_t i = 0; i < indices_->size (); ++i) 00142 { 00143 // Calculate the distance from the point to the sphere as the difference between 00144 // dist(point,sphere_origin) and sphere_radius 00145 float distance = fabs (sqrt ( 00146 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * 00147 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + 00148 00149 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * 00150 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) 00151 ) - model_coefficients[2]); 00152 if (distance < threshold) 00153 { 00154 // Returns the indices of the points whose distances are smaller than the threshold 00155 inliers[nr_p] = (*indices_)[i]; 00156 nr_p++; 00157 } 00158 } 00159 inliers.resize (nr_p); 00160 } 00161 00163 template <typename PointT> int 00164 pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistance ( 00165 const Eigen::VectorXf &model_coefficients, const double threshold) 00166 { 00167 // Check if the model is valid given the user constraints 00168 if (!isModelValid (model_coefficients)) 00169 return (0); 00170 int nr_p = 0; 00171 00172 // Iterate through the 3d points and calculate the distances from them to the sphere 00173 for (size_t i = 0; i < indices_->size (); ++i) 00174 { 00175 // Calculate the distance from the point to the sphere as the difference between 00176 // dist(point,sphere_origin) and sphere_radius 00177 float distance = fabs (sqrt ( 00178 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * 00179 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + 00180 00181 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * 00182 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) 00183 ) - model_coefficients[2]); 00184 if (distance < threshold) 00185 nr_p++; 00186 } 00187 return (nr_p); 00188 } 00189 00191 template <typename PointT> void 00192 pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients ( 00193 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 00194 { 00195 boost::mutex::scoped_lock lock (tmp_mutex_); 00196 00197 const int n_unknowns = 3; // 3 unknowns 00198 // Needs a set of valid model coefficients 00199 if (model_coefficients.size () != n_unknowns) 00200 { 00201 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00202 optimized_coefficients = model_coefficients; 00203 return; 00204 } 00205 00206 // Need at least 3 samples 00207 if (inliers.size () <= 3) 00208 { 00209 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ()); 00210 optimized_coefficients = model_coefficients; 00211 return; 00212 } 00213 00214 tmp_inliers_ = &inliers; 00215 00216 int m = inliers.size (); 00217 Eigen::VectorXd x(n_unknowns); 00218 for(int d = 0; d < n_unknowns; d++) 00219 x[d] = model_coefficients[d]; 00220 00221 OptimizationFunctor functor(n_unknowns, m, this); 00222 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor); 00223 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm(num_diff); 00224 int info = lm.minimize (x); 00225 00226 // Compute the L2 norm of the residuals 00227 PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g\n", 00228 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], x[0], x[1], x[2]); 00229 00230 optimized_coefficients = Eigen::Vector3f (x[0], x[1], x[2]); 00231 } 00232 00234 template <typename PointT> void 00235 pcl::SampleConsensusModelCircle2D<PointT>::projectPoints ( 00236 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, 00237 PointCloud &projected_points, bool copy_data_fields) 00238 { 00239 // Needs a valid set of model coefficients 00240 if (model_coefficients.size () != 3) 00241 { 00242 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00243 return; 00244 } 00245 00246 projected_points.header = input_->header; 00247 projected_points.is_dense = input_->is_dense; 00248 00249 // Copy all the data fields from the input cloud to the projected one? 00250 if (copy_data_fields) 00251 { 00252 // Allocate enough space and copy the basics 00253 projected_points.points.resize (input_->points.size ()); 00254 projected_points.width = input_->width; 00255 projected_points.height = input_->height; 00256 00257 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00258 // Iterate over each point 00259 for (size_t i = 0; i < projected_points.points.size (); ++i) 00260 // Iterate over each dimension 00261 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00262 00263 // Iterate through the 3d points and calculate the distances from them to the plane 00264 for (size_t i = 0; i < inliers.size (); ++i) 00265 { 00266 float dx = input_->points[inliers[i]].x - model_coefficients[0]; 00267 float dy = input_->points[inliers[i]].y - model_coefficients[1]; 00268 float a = sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) ); 00269 00270 projected_points.points[inliers[i]].x = a * dx + model_coefficients[0]; 00271 projected_points.points[inliers[i]].y = a * dy + model_coefficients[1]; 00272 } 00273 } 00274 else 00275 { 00276 // Allocate enough space and copy the basics 00277 projected_points.points.resize (inliers.size ()); 00278 projected_points.width = inliers.size (); 00279 projected_points.height = 1; 00280 00281 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00282 // Iterate over each point 00283 for (size_t i = 0; i < inliers.size (); ++i) 00284 // Iterate over each dimension 00285 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00286 00287 // Iterate through the 3d points and calculate the distances from them to the plane 00288 for (size_t i = 0; i < inliers.size (); ++i) 00289 { 00290 float dx = input_->points[inliers[i]].x - model_coefficients[0]; 00291 float dy = input_->points[inliers[i]].y - model_coefficients[1]; 00292 float a = sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) ); 00293 00294 projected_points.points[i].x = a * dx + model_coefficients[0]; 00295 projected_points.points[i].y = a * dy + model_coefficients[1]; 00296 } 00297 } 00298 } 00299 00301 template <typename PointT> bool 00302 pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel ( 00303 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) 00304 { 00305 // Needs a valid model coefficients 00306 if (model_coefficients.size () != 3) 00307 { 00308 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00309 return (false); 00310 } 00311 00312 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00313 // Calculate the distance from the point to the sphere as the difference between 00314 //dist(point,sphere_origin) and sphere_radius 00315 if (fabs (sqrt ( 00316 ( input_->points[*it].x - model_coefficients[0] ) * 00317 ( input_->points[*it].x - model_coefficients[0] ) + 00318 ( input_->points[*it].y - model_coefficients[1] ) * 00319 ( input_->points[*it].y - model_coefficients[1] ) 00320 ) - model_coefficients[2]) > threshold) 00321 return (false); 00322 00323 return (true); 00324 } 00325 00327 template <typename PointT> bool 00328 pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) 00329 { 00330 // Needs a valid model coefficients 00331 if (model_coefficients.size () != 3) 00332 { 00333 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); 00334 return (false); 00335 } 00336 00337 if (radius_min_ != -DBL_MAX && model_coefficients[2] < radius_min_) 00338 return (false); 00339 if (radius_max_ != DBL_MAX && model_coefficients[2] > radius_max_) 00340 return (false); 00341 00342 return (true); 00343 } 00344 00345 #define PCL_INSTANTIATE_SampleConsensusModelCircle2D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle2D<T>; 00346 00347 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ 00348