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_PLANE_H_ 00042 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_ 00043 00044 #include <pcl/sample_consensus/sac_model_plane.h> 00045 #include <pcl/common/centroid.h> 00046 #include <pcl/common/eigen.h> 00047 #include <pcl/common/concatenate.h> 00048 #include <pcl/point_types.h> 00049 00050 ////////////////////////////////////////////////////////////////////////// 00051 template <typename PointT> bool 00052 pcl::SampleConsensusModelPlane<PointT>::isSampleGood (const std::vector<int> &samples) const 00053 { 00054 // Need an extra check in case the sample selection is empty 00055 if (samples.empty ()) 00056 return (false); 00057 // Get the values at the two points 00058 pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); 00059 pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); 00060 pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); 00061 00062 Eigen::Array4f dy1dy2 = (p1-p0) / (p2-p0); 00063 00064 return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ); 00065 } 00066 00067 ////////////////////////////////////////////////////////////////////////// 00068 template <typename PointT> bool 00069 pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients ( 00070 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) 00071 { 00072 // Need 3 samples 00073 if (samples.size () != 3) 00074 { 00075 PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ()); 00076 return (false); 00077 } 00078 00079 pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); 00080 pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); 00081 pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); 00082 00083 // Compute the segment values (in 3d) between p1 and p0 00084 Eigen::Array4f p1p0 = p1 - p0; 00085 // Compute the segment values (in 3d) between p2 and p0 00086 Eigen::Array4f p2p0 = p2 - p0; 00087 00088 // Avoid some crashes by checking for collinearity here 00089 Eigen::Array4f dy1dy2 = p1p0 / p2p0; 00090 if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ) // Check for collinearity 00091 return (false); 00092 00093 // Compute the plane coefficients from the 3 given points in a straightforward manner 00094 // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1) 00095 model_coefficients.resize (4); 00096 model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1]; 00097 model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2]; 00098 model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0]; 00099 model_coefficients[3] = 0; 00100 00101 // Normalize 00102 model_coefficients.normalize (); 00103 00104 // ... + d = 0 00105 model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ())); 00106 00107 return (true); 00108 } 00109 00110 ////////////////////////////////////////////////////////////////////////// 00111 template <typename PointT> void 00112 pcl::SampleConsensusModelPlane<PointT>::getDistancesToModel ( 00113 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00114 { 00115 // Needs a valid set of model coefficients 00116 if (model_coefficients.size () != 4) 00117 { 00118 PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00119 return; 00120 } 00121 00122 distances.resize (indices_->size ()); 00123 00124 // Iterate through the 3d points and calculate the distances from them to the plane 00125 for (size_t i = 0; i < indices_->size (); ++i) 00126 { 00127 // Calculate the distance from the point to the plane normal as the dot product 00128 // D = (P-A).N/|N| 00129 /*distances[i] = fabs (model_coefficients[0] * input_->points[(*indices_)[i]].x + 00130 model_coefficients[1] * input_->points[(*indices_)[i]].y + 00131 model_coefficients[2] * input_->points[(*indices_)[i]].z + 00132 model_coefficients[3]);*/ 00133 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, 00134 input_->points[(*indices_)[i]].y, 00135 input_->points[(*indices_)[i]].z, 00136 1); 00137 distances[i] = fabs (model_coefficients.dot (pt)); 00138 } 00139 } 00140 00141 ////////////////////////////////////////////////////////////////////////// 00142 template <typename PointT> void 00143 pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance ( 00144 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00145 { 00146 // Needs a valid set of model coefficients 00147 if (model_coefficients.size () != 4) 00148 { 00149 PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00150 return; 00151 } 00152 00153 int nr_p = 0; 00154 inliers.resize (indices_->size ()); 00155 error_sqr_dists_.resize (indices_->size ()); 00156 00157 // Iterate through the 3d points and calculate the distances from them to the plane 00158 for (size_t i = 0; i < indices_->size (); ++i) 00159 { 00160 // Calculate the distance from the point to the plane normal as the dot product 00161 // D = (P-A).N/|N| 00162 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, 00163 input_->points[(*indices_)[i]].y, 00164 input_->points[(*indices_)[i]].z, 00165 1); 00166 00167 float distance = fabsf (model_coefficients.dot (pt)); 00168 00169 if (distance < threshold) 00170 { 00171 // Returns the indices of the points whose distances are smaller than the threshold 00172 inliers[nr_p] = (*indices_)[i]; 00173 error_sqr_dists_[nr_p] = static_cast<double> (distance); 00174 ++nr_p; 00175 } 00176 } 00177 inliers.resize (nr_p); 00178 error_sqr_dists_.resize (nr_p); 00179 } 00180 00181 ////////////////////////////////////////////////////////////////////////// 00182 template <typename PointT> int 00183 pcl::SampleConsensusModelPlane<PointT>::countWithinDistance ( 00184 const Eigen::VectorXf &model_coefficients, const double threshold) 00185 { 00186 // Needs a valid set of model coefficients 00187 if (model_coefficients.size () != 4) 00188 { 00189 PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00190 return (0); 00191 } 00192 00193 int nr_p = 0; 00194 00195 // Iterate through the 3d points and calculate the distances from them to the plane 00196 for (size_t i = 0; i < indices_->size (); ++i) 00197 { 00198 // Calculate the distance from the point to the plane normal as the dot product 00199 // D = (P-A).N/|N| 00200 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, 00201 input_->points[(*indices_)[i]].y, 00202 input_->points[(*indices_)[i]].z, 00203 1); 00204 if (fabs (model_coefficients.dot (pt)) < threshold) 00205 nr_p++; 00206 } 00207 return (nr_p); 00208 } 00209 00210 ////////////////////////////////////////////////////////////////////////// 00211 template <typename PointT> void 00212 pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients ( 00213 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) 00214 { 00215 // Needs a valid set of model coefficients 00216 if (model_coefficients.size () != 4) 00217 { 00218 PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00219 optimized_coefficients = model_coefficients; 00220 return; 00221 } 00222 00223 // Need at least 3 points to estimate a plane 00224 if (inliers.size () < 4) 00225 { 00226 PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); 00227 optimized_coefficients = model_coefficients; 00228 return; 00229 } 00230 00231 Eigen::Vector4f plane_parameters; 00232 00233 // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients 00234 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00235 Eigen::Vector4f xyz_centroid; 00236 00237 computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, xyz_centroid); 00238 00239 // Compute the model coefficients 00240 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 00241 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00242 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); 00243 00244 // Hessian form (D = nc . p_plane (centroid here) + p) 00245 optimized_coefficients.resize (4); 00246 optimized_coefficients[0] = eigen_vector [0]; 00247 optimized_coefficients[1] = eigen_vector [1]; 00248 optimized_coefficients[2] = eigen_vector [2]; 00249 optimized_coefficients[3] = 0; 00250 optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid); 00251 00252 // Make sure it results in a valid model 00253 if (!isModelValid (optimized_coefficients)) 00254 { 00255 optimized_coefficients = model_coefficients; 00256 } 00257 } 00258 00259 ////////////////////////////////////////////////////////////////////////// 00260 template <typename PointT> void 00261 pcl::SampleConsensusModelPlane<PointT>::projectPoints ( 00262 const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) 00263 { 00264 // Needs a valid set of model coefficients 00265 if (model_coefficients.size () != 4) 00266 { 00267 PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00268 return; 00269 } 00270 00271 projected_points.header = input_->header; 00272 projected_points.is_dense = input_->is_dense; 00273 00274 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); 00275 00276 // normalize the vector perpendicular to the plane... 00277 mc.normalize (); 00278 // ... and store the resulting normal as a local copy of the model coefficients 00279 Eigen::Vector4f tmp_mc = model_coefficients; 00280 tmp_mc[0] = mc[0]; 00281 tmp_mc[1] = mc[1]; 00282 tmp_mc[2] = mc[2]; 00283 00284 // Copy all the data fields from the input cloud to the projected one? 00285 if (copy_data_fields) 00286 { 00287 // Allocate enough space and copy the basics 00288 projected_points.points.resize (input_->points.size ()); 00289 projected_points.width = input_->width; 00290 projected_points.height = input_->height; 00291 00292 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00293 // Iterate over each point 00294 for (size_t i = 0; i < input_->points.size (); ++i) 00295 // Iterate over each dimension 00296 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i])); 00297 00298 // Iterate through the 3d points and calculate the distances from them to the plane 00299 for (size_t i = 0; i < inliers.size (); ++i) 00300 { 00301 // Calculate the distance from the point to the plane 00302 Eigen::Vector4f p (input_->points[inliers[i]].x, 00303 input_->points[inliers[i]].y, 00304 input_->points[inliers[i]].z, 00305 1); 00306 // use normalized coefficients to calculate the scalar projection 00307 float distance_to_plane = tmp_mc.dot (p); 00308 00309 pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap (); 00310 pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe 00311 } 00312 } 00313 else 00314 { 00315 // Allocate enough space and copy the basics 00316 projected_points.points.resize (inliers.size ()); 00317 projected_points.width = static_cast<uint32_t> (inliers.size ()); 00318 projected_points.height = 1; 00319 00320 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00321 // Iterate over each point 00322 for (size_t i = 0; i < inliers.size (); ++i) 00323 // Iterate over each dimension 00324 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i])); 00325 00326 // Iterate through the 3d points and calculate the distances from them to the plane 00327 for (size_t i = 0; i < inliers.size (); ++i) 00328 { 00329 // Calculate the distance from the point to the plane 00330 Eigen::Vector4f p (input_->points[inliers[i]].x, 00331 input_->points[inliers[i]].y, 00332 input_->points[inliers[i]].z, 00333 1); 00334 // use normalized coefficients to calculate the scalar projection 00335 float distance_to_plane = tmp_mc.dot (p); 00336 00337 pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap (); 00338 pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe 00339 } 00340 } 00341 } 00342 00343 ////////////////////////////////////////////////////////////////////////// 00344 template <typename PointT> bool 00345 pcl::SampleConsensusModelPlane<PointT>::doSamplesVerifyModel ( 00346 const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) 00347 { 00348 // Needs a valid set of model coefficients 00349 if (model_coefficients.size () != 4) 00350 { 00351 PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00352 return (false); 00353 } 00354 00355 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00356 { 00357 Eigen::Vector4f pt (input_->points[*it].x, 00358 input_->points[*it].y, 00359 input_->points[*it].z, 00360 1); 00361 if (fabs (model_coefficients.dot (pt)) > threshold) 00362 return (false); 00363 } 00364 00365 return (true); 00366 } 00367 00368 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>; 00369 00370 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_ 00371