Point Cloud Library (PCL)  1.3.1
mls.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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: mls.hpp 3031 2011-11-01 04:25:15Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SURFACE_IMPL_MLS_H_
00039 #define PCL_SURFACE_IMPL_MLS_H_
00040 
00041 #include "pcl/surface/mls.h"
00042 #include <pcl/common/io.h>
00043 #include <pcl/common/centroid.h>
00044 #include <pcl/common/eigen.h>
00045 
00047 template <typename PointInT, typename NormalOutT> void
00048 pcl::MovingLeastSquares<PointInT, NormalOutT>::reconstruct (PointCloudIn &output)
00049 {
00050   // check if normals have to be computed/saved
00051   if (normals_)
00052   {
00053     // Copy the header
00054     normals_->header = input_->header;
00055     // Clear the fields in case the method exits before computation
00056     normals_->width = normals_->height = 0;
00057     normals_->points.clear ();
00058   }
00059 
00060   // Copy the header
00061   output.header = input_->header;
00062   
00063   if (!initCompute ()) 
00064   {
00065     output.width = output.height = 0;
00066     output.points.clear ();
00067     return;
00068   }
00069 
00070   // Initialize the spatial locator
00071   if (!tree_)
00072   {
00073     KdTreePtr tree;
00074     if (input_->isOrganized ())
00075       tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
00076     else
00077       tree.reset (new pcl::search::KdTree<PointInT> (false));
00078     setSearchMethod (tree);
00079   }
00080 
00081   // Send the surface dataset to the spatial locator
00082   tree_->setInputCloud (input_, indices_);
00083 
00084   // Perform the actual surface reconstruction
00085   performReconstruction (output);
00086 
00087   deinitCompute ();
00088 }
00089 
00091 template <typename PointInT, typename NormalOutT> void
00092 pcl::MovingLeastSquares<PointInT, NormalOutT>::performReconstruction (PointCloudIn &output)
00093 {
00094   if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
00095   {
00096     PCL_ERROR ("[pcl::%s::performReconstruction] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
00097     output.width = output.height = 0;
00098     output.points.clear ();
00099     if (normals_)
00100     {
00101       normals_->width = normals_->height = 0;
00102       normals_->points.clear ();
00103     }
00104     return;
00105   }
00106 
00107   // Compute the number of coefficients
00108   nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
00109 
00110   // Allocate enough space to hold the results of nearest neighbor searches
00111   // \note resize is irrelevant for a radiusSearch ().
00112   std::vector<int> nn_indices;
00113   std::vector<float> nn_sqr_dists;
00114   
00115   // Use original point positions for fitting
00116   // \note no up/down/adapting-sampling or hole filling possible like this
00117   output.points.resize (indices_->size ());
00118   // Check if fake indices were used, otherwise the output loses its organized structure
00119   if (!fake_indices_)
00120     pcl::copyPointCloud (*input_, *indices_, output);
00121   else
00122     output = *input_;
00123 
00124   // Resize the output normal dataset
00125   if (normals_)
00126   {
00127     normals_->points.resize (output.points.size ());
00128     normals_->width    = output.width;
00129     normals_->height   = output.height;
00130     normals_->is_dense = output.is_dense;
00131   }
00132 
00133   // For all points
00134   for (size_t cp = 0; cp < indices_->size (); ++cp)
00135   {
00136     // Get the initial estimates of point positions and their neighborhoods
00138 
00139     // Search for the nearest neighbors
00140     if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
00141     {
00142       if (normals_)
00143         normals_->points[cp].normal[0] = normals_->points[cp].normal[1] = normals_->points[cp].normal[2] = normals_->points[cp].curvature = std::numeric_limits<float>::quiet_NaN ();
00144       continue;
00145     }
00146 
00147     // Check the number of nearest neighbors for normal estimation (and later
00148     // for polynomial fit as well)
00149     int k = nn_indices.size ();
00150     if (k < 3)
00151       continue;
00152 
00153     // Get a plane approximating the local surface's tangent and project point onto it
00155 
00156     // Compute the plane coefficients
00157     Eigen::Vector4f model_coefficients;
00158     //pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature);
00159     EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00160     Eigen::Vector4f xyz_centroid;
00161 
00162     // Estimate the XYZ centroid
00163     pcl::compute3DCentroid (*input_, nn_indices, xyz_centroid);
00164 
00165     // Compute the 3x3 covariance matrix
00166     pcl::computeCovarianceMatrix (*input_, nn_indices, xyz_centroid, covariance_matrix);
00167 
00168     // Get the plane normal
00169     EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00170     EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00171     pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00172 
00173     // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
00174     model_coefficients[0] = eigen_vectors (0, 0);
00175     model_coefficients[1] = eigen_vectors (1, 0);
00176     model_coefficients[2] = eigen_vectors (2, 0);
00177     model_coefficients[3] = 0;
00178     // Hessian form (D = nc . p_plane (centroid here) + p)
00179     model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00180 
00181     float curvature = 0;
00182     // Compute the curvature surface change
00183     float eig_sum = eigen_values.sum ();
00184     if (eig_sum != 0)
00185       curvature = fabs (eigen_values[0] / eig_sum);
00186 
00187     // Projected point
00188     Eigen::Vector3f point = output.points[cp].getVector3fMap ();
00189     float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
00190     point -= distance * model_coefficients.head<3> ();
00191 
00192     // Perform polynomial fit to update point and normal
00194     if (polynomial_fit_ && k >= nr_coeff_)
00195     {
00196       // For easy change between float and double
00197       typedef Eigen::Vector3d Evector3;
00198       typedef Eigen::VectorXd Evector;
00199       typedef Eigen::MatrixXd Ematrix;
00200       // Get a copy of the plane normal easy access
00201       Evector3 plane_normal = model_coefficients.head<3> ().cast<double> ();
00202 
00203       // Update neighborhood, since point was projected, and computing relative
00204       // positions. Note updating only distances for the weights for speed
00205       std::vector<Evector3> de_meaned (k);
00206       for (int ni = 0; ni < k; ++ni)
00207       {
00208         de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
00209         de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
00210         de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
00211         nn_sqr_dists[ni] = de_meaned[ni].dot (de_meaned[ni]);
00212       }
00213 
00214       // Allocate matrices and vectors to hold the data used for the polynomial
00215       // fit
00216       Evector weight_vec_ (k);
00217       Ematrix P_ (nr_coeff_, k);
00218       Evector f_vec_ (k);
00219       Evector c_vec_;
00220       Ematrix P_weight_; // size will be (nr_coeff_, k);
00221       Ematrix P_weight_Pt_ (nr_coeff_, nr_coeff_);
00222 
00223       // Get local coordinate system (Darboux frame)
00224       Evector3 v = plane_normal.unitOrthogonal ();
00225       Evector3 u = plane_normal.cross (v);
00226 
00227       // Go through neighbors, transform them in the local coordinate system,
00228       // save height and the evaluation of the polynome's terms
00229       double u_coord, v_coord, u_pow, v_pow;
00230       for (int ni = 0; ni < k; ++ni)
00231       {
00232         // (re-)compute weights
00233         weight_vec_ (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
00234 
00235         // transforming coordinates
00236         u_coord = de_meaned[ni].dot (u);
00237         v_coord = de_meaned[ni].dot (v);
00238         f_vec_(ni) = de_meaned[ni].dot (plane_normal);
00239 
00240         // compute the polynomial's terms at the current point
00241         int j = 0;
00242         u_pow = 1;
00243         for (int ui = 0; ui <= order_; ++ui)
00244         {
00245           v_pow = 1;
00246           for (int vi = 0; vi <= order_ - ui; ++vi)
00247           {
00248             P_ (j++, ni) = u_pow * v_pow;
00249             v_pow *= v_coord;
00250           }
00251           u_pow *= u_coord;
00252         }
00253       }
00254 
00255       // Computing coefficients
00256       P_weight_ = P_ * weight_vec_.asDiagonal ();
00257       P_weight_Pt_ = P_weight_ * P_.transpose ();
00258       c_vec_ = P_weight_ * f_vec_;
00259       P_weight_Pt_.llt ().solveInPlace (c_vec_);
00260 
00261       // Projection onto MLS surface along Darboux normal to the height at (0,0)
00262       if (pcl_isfinite (c_vec_[0]))
00263       {
00264         point += (c_vec_[0] * plane_normal).cast<float> ();
00265 
00266         // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec_[order_+1] and c_vec_[1]
00267         if (normals_)
00268         {
00269           Evector3 n_a = u + plane_normal * c_vec_[order_ + 1];
00270           Evector3 n_b = v + plane_normal * c_vec_[1];
00271           model_coefficients.head<3> () = n_a.cross (n_b).cast<float> ();
00272           model_coefficients.head<3> ().normalize ();
00273         }
00274       }
00275     }
00276 
00277     // Save results to output cloud
00279     output.points[cp].x = point[0];
00280     output.points[cp].y = point[1];
00281     output.points[cp].z = point[2];
00282     if (normals_)
00283     {
00284       normals_->points[cp].normal[0] = model_coefficients[0];
00285       normals_->points[cp].normal[1] = model_coefficients[1];
00286       normals_->points[cp].normal[2] = model_coefficients[2];
00287       normals_->points[cp].curvature = curvature;
00288     }
00289   }
00290 }
00291 
00292 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
00293 
00294 #endif    // PCL_SURFACE_IMPL_MLS_H_
00295 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines