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