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