Point Cloud Library (PCL)  1.3.1
sac_model_plane.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines