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