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