mls.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2009-2011, Willow Garage, Inc.
00006  *
00007  * All rights reserved.
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions
00011  * are met:
00012  *
00013  * * Redistributions of source code must retain the above copyright
00014  *   notice, this list of conditions and the following disclaimer.
00015  * * Redistributions in binary form must reproduce the above
00016  *   copyright notice, this list of conditions and the following
00017  *   disclaimer in the documentation and/or other materials provided
00018  *   with the distribution.
00019  * * Neither the name of Willow Garage, Inc. nor the names of its
00020  *   contributors may be used to endorse or promote products derived
00021  *   from this software without specific prior written permission.
00022  *
00023  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  * POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: mls.hpp 6439 2012-07-17 07:13:58Z aichim $
00037  *
00038  */
00039 
00040 #ifndef PCL_SURFACE_IMPL_MLS_H_
00041 #define PCL_SURFACE_IMPL_MLS_H_
00042 
00043 #include <pcl/point_traits.h>
00044 #include <pcl/surface/mls.h>
00045 #include <pcl/common/io.h>
00046 #include <pcl/common/centroid.h>
00047 #include <pcl/common/eigen.h>
00048 #include <pcl/common/geometry.h>
00049 
00051 template <typename PointInT, typename PointOutT> void
00052 pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
00053 {
00054   // Check if normals have to be computed/saved
00055   if (compute_normals_)
00056   {
00057     normals_.reset (new NormalCloud);
00058     // Copy the header
00059     normals_->header = input_->header;
00060     // Clear the fields in case the method exits before computation
00061     normals_->width = normals_->height = 0;
00062     normals_->points.clear ();
00063   }
00064 
00065 
00066   // Copy the header
00067   output.header = input_->header;
00068   output.width = output.height = 0;
00069   output.points.clear ();
00070 
00071   if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
00072   {
00073     PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
00074     return;
00075   }
00076 
00077   if (!initCompute ())
00078     return;
00079 
00080 
00081   // Initialize the spatial locator
00082   if (!tree_)
00083   {
00084     KdTreePtr tree;
00085     if (input_->isOrganized ())
00086       tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
00087     else
00088       tree.reset (new pcl::search::KdTree<PointInT> (false));
00089     setSearchMethod (tree);
00090   }
00091 
00092   // Send the surface dataset to the spatial locator
00093   tree_->setInputCloud (input_, indices_);
00094 
00095   // Initialize random number generator if necessary
00096   switch (upsample_method_)
00097   {
00098     case (RANDOM_UNIFORM_DENSITY):
00099     {
00100       boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0)));
00101       float tmp = static_cast<float> (search_radius_ / 2.0f);
00102       boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp);
00103       rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib);
00104       break;
00105     }
00106     case (VOXEL_GRID_DILATION):
00107     {
00108       mls_results_.resize (input_->size ());
00109       break;
00110     }
00111     default:
00112       break;
00113   }
00114 
00115   // Perform the actual surface reconstruction
00116   performProcessing (output);
00117 
00118   if (compute_normals_)
00119   {
00120     normals_->height = 1;
00121     normals_->width = static_cast<uint32_t> (normals_->size ());
00122 
00123     // TODO!!! MODIFY TO PER-CLOUD COPYING - much faster than per-point
00124     for (unsigned int i = 0; i < output.size (); ++i)
00125     {
00126       typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
00127       pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
00128       pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
00129       pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
00130       pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature));
00131     }
00132 
00133   }
00134 
00135   // Set proper widths and heights for the clouds
00136   output.height = 1;
00137   output.width = static_cast<uint32_t> (output.size ());
00138 
00139   deinitCompute ();
00140 }
00141 
00143 template <typename PointInT, typename PointOutT> void
00144 pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
00145                                                                      const PointCloudIn &input,
00146                                                                      const std::vector<int> &nn_indices,
00147                                                                      std::vector<float> &nn_sqr_dists,
00148                                                                      PointCloudOut &projected_points,
00149                                                                      NormalCloud &projected_points_normals)
00150 {
00151   // Compute the plane coefficients
00152   //pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature);
00153   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00154   Eigen::Vector4f xyz_centroid;
00155 
00156   // Estimate the XYZ centroid
00157   pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
00158   //pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
00159 
00160   pcl::computeCovarianceMatrix (input, nn_indices, xyz_centroid, covariance_matrix);
00161   // Compute the 3x3 covariance matrix
00162 
00163   EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
00164   EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
00165   Eigen::Vector4f model_coefficients;
00166   pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
00167   model_coefficients.head<3> () = eigen_vector;
00168   model_coefficients[3] = 0;
00169   model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00170 
00171   // Projected query point
00172   Eigen::Vector3f point = input[(*indices_)[index]].getVector3fMap ();
00173   float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
00174   point -= distance * model_coefficients.head<3> ();
00175 
00176   float curvature = covariance_matrix.trace ();
00177   // Compute the curvature surface change
00178   if (curvature != 0)
00179     curvature = fabsf (eigen_value / curvature);
00180 
00181 
00182   // Get a copy of the plane normal easy access
00183   Eigen::Vector3d plane_normal = model_coefficients.head<3> ().cast<double> ();
00184   // Vector in which the polynomial coefficients will be put
00185   Eigen::VectorXd c_vec;
00186   // Local coordinate system (Darboux frame)
00187   Eigen::Vector3d v (0.0f, 0.0f, 0.0f), u (0.0f, 0.0f, 0.0f);
00188 
00189 
00190 
00191   // Perform polynomial fit to update point and normal
00193   if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
00194   {
00195     // Update neighborhood, since point was projected, and computing relative
00196     // positions. Note updating only distances for the weights for speed
00197     std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
00198     for (size_t ni = 0; ni < nn_indices.size (); ++ni)
00199     {
00200       de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
00201       de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
00202       de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
00203       nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
00204     }
00205 
00206     // Allocate matrices and vectors to hold the data used for the polynomial fit
00207     Eigen::VectorXd weight_vec (nn_indices.size ());
00208     Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
00209     Eigen::VectorXd f_vec (nn_indices.size ());
00210     Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
00211     Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);
00212 
00213     // Get local coordinate system (Darboux frame)
00214     v = plane_normal.unitOrthogonal ();
00215     u = plane_normal.cross (v);
00216 
00217     // Go through neighbors, transform them in the local coordinate system,
00218     // save height and the evaluation of the polynome's terms
00219     double u_coord, v_coord, u_pow, v_pow;
00220     for (size_t ni = 0; ni < nn_indices.size (); ++ni)
00221     {
00222       // (re-)compute weights
00223       weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
00224 
00225       // transforming coordinates
00226       u_coord = de_meaned[ni].dot (u);
00227       v_coord = de_meaned[ni].dot (v);
00228       f_vec (ni) = de_meaned[ni].dot (plane_normal);
00229 
00230       // compute the polynomial's terms at the current point
00231       int j = 0;
00232       u_pow = 1;
00233       for (int ui = 0; ui <= order_; ++ui)
00234       {
00235         v_pow = 1;
00236         for (int vi = 0; vi <= order_ - ui; ++vi)
00237         {
00238           P (j++, ni) = u_pow * v_pow;
00239           v_pow *= v_coord;
00240         }
00241         u_pow *= u_coord;
00242       }
00243     }
00244 
00245     // Computing coefficients
00246     P_weight = P * weight_vec.asDiagonal ();
00247     P_weight_Pt = P_weight * P.transpose ();
00248     c_vec = P_weight * f_vec;
00249     P_weight_Pt.llt ().solveInPlace (c_vec);
00250   }
00251 
00252   switch (upsample_method_)
00253   {
00254     case (NONE):
00255     {
00256       Eigen::Vector3d normal = plane_normal;
00257 
00258       if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
00259       {
00260         point += (c_vec[0] * plane_normal).cast<float> ();
00261 
00262         // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
00263         if (compute_normals_)
00264           normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
00265       }
00266 
00267       PointOutT aux;
00268       aux.x = point[0];
00269       aux.y = point[1];
00270       aux.z = point[2];
00271       projected_points.push_back (aux);
00272 
00273       if (compute_normals_)
00274       {
00275         pcl::Normal aux_normal;
00276         aux_normal.normal_x = static_cast<float> (normal[0]);
00277         aux_normal.normal_y = static_cast<float> (normal[1]);
00278         aux_normal.normal_z = static_cast<float> (normal[2]);
00279         aux_normal.curvature = curvature;
00280         projected_points_normals.push_back (aux_normal);
00281       }
00282 
00283       break;
00284     }
00285 
00286     case (SAMPLE_LOCAL_PLANE):
00287     {
00288       // Uniformly sample a circle around the query point using the radius and step parameters
00289       for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
00290         for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
00291           if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
00292           {
00293             PointOutT projected_point;
00294             pcl::Normal projected_normal;
00295             projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec, 
00296                                       static_cast<int> (nn_indices.size ()),
00297                                       projected_point, projected_normal);
00298 
00299             projected_points.push_back (projected_point);
00300             if (compute_normals_)
00301               projected_points_normals.push_back (projected_normal);
00302           }
00303       break;
00304     }
00305 
00306     case (RANDOM_UNIFORM_DENSITY):
00307     {
00308       // Compute the local point density and add more samples if necessary
00309       int num_points_to_add = static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
00310 
00311       // Just add the query point, because the density is good
00312       if (num_points_to_add <= 0)
00313       {
00314         // Just add the current point
00315         Eigen::Vector3d normal = plane_normal;
00316         if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
00317         {
00318           // Projection onto MLS surface along Darboux normal to the height at (0,0)
00319           point += (c_vec[0] * plane_normal).cast<float> ();
00320           // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
00321           if (compute_normals_)
00322             normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
00323         }
00324         PointOutT aux;
00325         aux.x = point[0];
00326         aux.y = point[1];
00327         aux.z = point[2];
00328         projected_points.push_back (aux);
00329 
00330         if (compute_normals_)
00331         {
00332           pcl::Normal aux_normal;
00333           aux_normal.normal_x = static_cast<float> (normal[0]);
00334           aux_normal.normal_y = static_cast<float> (normal[1]);
00335           aux_normal.normal_z = static_cast<float> (normal[2]);
00336           aux_normal.curvature = curvature;
00337           projected_points_normals.push_back (aux_normal);
00338         }
00339       }
00340       else
00341       {
00342         // Sample the local plane
00343         for (int num_added = 0; num_added < num_points_to_add;)
00344         {
00345           float u_disp = (*rng_uniform_distribution_) (),
00346                 v_disp = (*rng_uniform_distribution_) ();
00347           // Check if inside circle; if not, try another coin flip
00348           if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4)
00349             continue;
00350 
00351 
00352           PointOutT projected_point;
00353           pcl::Normal projected_normal;
00354           projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec, 
00355                                     static_cast<int> (nn_indices.size ()),
00356                                     projected_point, projected_normal);
00357 
00358           projected_points.push_back (projected_point);
00359           if (compute_normals_)
00360             projected_points_normals.push_back (projected_normal);
00361 
00362           num_added ++;
00363         }
00364       }
00365       break;
00366     }
00367 
00368     case (VOXEL_GRID_DILATION):
00369     {
00370       // Take all point pairs and sample space between them in a grid-fashion
00371       // \note consider only point pairs with increasing indices
00372       MLSResult result (plane_normal, u, v, c_vec, static_cast<int> (nn_indices.size ()), curvature);
00373       mls_results_[index] = result;
00374       break;
00375     }
00376   }
00377 }
00378 
00379 
00380 
00381 template <typename PointInT, typename PointOutT> void
00382 pcl::MovingLeastSquares<PointInT, PointOutT>::projectPointToMLSSurface (float &u_disp, float &v_disp,
00383                                                                         Eigen::Vector3d &u, Eigen::Vector3d &v,
00384                                                                         Eigen::Vector3d &plane_normal,
00385                                                                         float &curvature,
00386                                                                         Eigen::Vector3f &query_point,
00387                                                                         Eigen::VectorXd &c_vec,
00388                                                                         int num_neighbors,
00389                                                                         PointOutT &result_point,
00390                                                                         pcl::Normal &result_normal)
00391 {
00392   double n_disp = 0.0f;
00393   double d_u = 0.0f, d_v = 0.0f;
00394 
00395   // HARDCODED 5*nr_coeff_ to guarantee that the computed polynomial had a proper point set basis
00396   if (polynomial_fit_ && num_neighbors >= 5*nr_coeff_ && pcl_isfinite (c_vec[0]))
00397   {
00398     // Compute the displacement along the normal using the fitted polynomial
00399     // and compute the partial derivatives needed for estimating the normal
00400     int j = 0;
00401     float u_pow = 1.0f, v_pow = 1.0f, u_pow_prev = 1.0f, v_pow_prev = 1.0f;
00402     for (int ui = 0; ui <= order_; ++ui)
00403     {
00404       v_pow = 1;
00405       for (int vi = 0; vi <= order_ - ui; ++vi)
00406       {
00407         // Compute displacement along normal
00408         n_disp += u_pow * v_pow * c_vec[j++];
00409 
00410         // Compute partial derivatives
00411         if (ui >= 1)
00412           d_u += c_vec[j-1] * ui * u_pow_prev * v_pow;
00413         if (vi >= 1)
00414           d_v += c_vec[j-1] * vi * u_pow * v_pow_prev;
00415 
00416         v_pow_prev = v_pow;
00417         v_pow *= v_disp;
00418       }
00419       u_pow_prev = u_pow;
00420       u_pow *= u_disp;
00421     }
00422   }
00423 
00424   Eigen::Vector3d normal = plane_normal - d_u * u - d_v * v;
00425   normal.normalize ();
00426   result_point.x = static_cast<float> (query_point[0] + u[0] * u_disp + v[0] * v_disp + normal[0] * n_disp);
00427   result_point.y = static_cast<float> (query_point[1] + u[1] * u_disp + v[1] * v_disp + normal[1] * n_disp);
00428   result_point.z = static_cast<float> (query_point[2] + u[2] * u_disp + v[2] * v_disp + normal[2] * n_disp);
00429 
00430   result_normal.normal_x = static_cast<float> (normal[0]);
00431   result_normal.normal_y = static_cast<float> (normal[1]);
00432   result_normal.normal_z = static_cast<float> (normal[2]);
00433   result_normal.curvature = curvature;
00434 }
00435 
00436 
00438 template <typename PointInT, typename PointOutT> void
00439 pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
00440 {
00441   // Compute the number of coefficients
00442   nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
00443 
00444   // Allocate enough space to hold the results of nearest neighbor searches
00445   // \note resize is irrelevant for a radiusSearch ().
00446   std::vector<int> nn_indices;
00447   std::vector<float> nn_sqr_dists;
00448 
00449   // For all points
00450   for (size_t cp = 0; cp < indices_->size (); ++cp)
00451   {
00452     // Get the initial estimates of point positions and their neighborhoods
00453     if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists))
00454       continue;
00455 
00456     // Check the number of nearest neighbors for normal estimation (and later
00457     // for polynomial fit as well)
00458     if (nn_indices.size () < 3)
00459       continue;
00460 
00461 
00462     PointCloudOut projected_points;
00463     NormalCloud projected_points_normals;
00464     // Get a plane approximating the local surface's tangent and project point onto it
00465     computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals);
00466 
00467     // Append projected points to output
00468     output.insert (output.end (), projected_points.begin (), projected_points.end ());
00469     if (compute_normals_)
00470       normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
00471   }
00472 
00473  
00474   
00475   // For the voxel grid upsampling method, generate the voxel grid and dilate it
00476   // Then, project the newly obtained points to the MLS surface
00477   if (upsample_method_ == VOXEL_GRID_DILATION)
00478   {
00479     MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
00480     
00481     for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
00482       voxel_grid.dilate ();
00483     
00484     
00485     for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
00486     {
00487       // Get 3D position of point
00488       Eigen::Vector3f pos;
00489       voxel_grid.getPosition (m_it->first, pos);
00490 
00491       PointInT p;
00492       p.x = pos[0];
00493       p.y = pos[1];
00494       p.z = pos[2];
00495 
00496       std::vector<int> nn_indices;
00497       std::vector<float> nn_dists;
00498       tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
00499       int input_index = nn_indices.front ();
00500 
00501       // If the closest point did not have a valid MLS fitting result
00502       // OR if it is too far away from the sampled point
00503       if (mls_results_[input_index].valid == false)
00504         continue;
00505       
00506       Eigen::Vector3f add_point = p.getVector3fMap (),
00507                       input_point = input_->points[input_index].getVector3fMap ();
00508       
00509       Eigen::Vector3d aux = mls_results_[input_index].u;
00510       Eigen::Vector3f u = aux.cast<float> ();
00511       aux = mls_results_[input_index].v;
00512       Eigen::Vector3f v = aux.cast<float> ();
00513       
00514       float u_disp = (add_point - input_point).dot (u),
00515             v_disp = (add_point - input_point).dot (v);
00516       
00517       PointOutT result_point;
00518       pcl::Normal result_normal;
00519       projectPointToMLSSurface (u_disp, v_disp,
00520                                 mls_results_[input_index].u, mls_results_[input_index].v,
00521                                 mls_results_[input_index].plane_normal,
00522                                 mls_results_[input_index].curvature,
00523                                 input_point,
00524                                 mls_results_[input_index].c_vec,
00525                                 mls_results_[input_index].num_neighbors,
00526                                 result_point, result_normal);
00527       
00528       float d_before = (pos - input_point).norm (),
00529             d_after = (result_point.getVector3fMap () - input_point). norm();
00530       if (d_after > d_before)
00531         continue;
00532 
00533       output.push_back (result_point);
00534       if (compute_normals_)
00535         normals_->push_back (result_normal);
00536     }
00537   }
00538 }
00539 
00540 
00541 
00542 
00543 
00544 
00546 template <typename PointInT, typename PointOutT>
00547 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSResult::MLSResult (Eigen::Vector3d &a_plane_normal,
00548                                                                     Eigen::Vector3d &a_u,
00549                                                                     Eigen::Vector3d &a_v,
00550                                                                     Eigen::VectorXd a_c_vec,
00551                                                                     int a_num_neighbors,
00552                                                                     float &a_curvature) :
00553   plane_normal (a_plane_normal), u (a_u), v (a_v), c_vec (a_c_vec), num_neighbors (a_num_neighbors), 
00554   curvature (a_curvature), valid (true)
00555 {
00556 }
00557 
00559 template <typename PointInT, typename PointOutT>
00560 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,
00561                                                                           IndicesPtr &indices,
00562                                                                           float voxel_size) :
00563   voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
00564 {
00565   pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
00566 
00567   Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
00568   double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
00569   // Put initial cloud in voxel grid
00570   data_size_ = static_cast<uint64_t> (1.5 * max_size / voxel_size_);
00571   for (unsigned int i = 0; i < indices->size (); ++i)
00572     if (pcl_isfinite (cloud->points[(*indices)[i]].x))
00573     {
00574       Eigen::Vector3i pos;
00575       getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
00576 
00577       uint64_t index_1d;
00578       getIndexIn1D (pos, index_1d);
00579       Leaf leaf;
00580       voxel_grid_[index_1d] = leaf;
00581     }
00582 }
00583 
00585 template <typename PointInT, typename PointOutT> void
00586 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::dilate ()
00587 {
00588   HashMap new_voxel_grid = voxel_grid_;
00589   for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
00590   {
00591     Eigen::Vector3i index;
00592     getIndexIn3D (m_it->first, index);
00593 
00595     for (int x = -1; x <= 1; ++x)
00596       for (int y = -1; y <= 1; ++y)
00597         for (int z = -1; z <= 1; ++z)
00598           if (x != 0 || y != 0 || z != 0)
00599           {
00600             Eigen::Vector3i new_index;
00601             new_index = index + Eigen::Vector3i (x, y, z);
00602 
00603             uint64_t index_1d;
00604             getIndexIn1D (new_index, index_1d);
00605             Leaf leaf;
00606             new_voxel_grid[index_1d] = leaf;
00607           }
00608   }
00609   voxel_grid_ = new_voxel_grid;
00610 }
00611 
00612 
00613 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
00614 
00615 #endif    // PCL_SURFACE_IMPL_MLS_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:42