Point Cloud Library (PCL)  1.3.1
grid_projection.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: grid_projection.hpp 2337 2011-09-01 15:37:10Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SURFACE_IMPL_GRID_PROJECTION_H_
00039 #define PCL_SURFACE_IMPL_GRID_PROJECTION_H_
00040 
00041 #include "pcl/surface/grid_projection.h"
00042 #include <pcl/common/common.h>
00043 #include <pcl/common/centroid.h>
00044 #include <pcl/common/vector_average.h>
00045 #include <pcl/Vertices.h>
00046 
00048 template <typename PointNT>
00049 pcl::GridProjection<PointNT>::GridProjection () : 
00050   leaf_size_(0.001), data_size_(0), max_binary_search_level_(10), k_(50), padding_size_(3), data_()
00051 {}
00052 
00054 template <typename PointNT>
00055 pcl::GridProjection<PointNT>::GridProjection (double resolution) :
00056   leaf_size_(resolution), data_size_(0), max_binary_search_level_(10), k_(50), padding_size_(3), data_()
00057 {}
00058 
00060 template <typename PointNT>
00061 pcl::GridProjection<PointNT>::~GridProjection ()
00062 {
00063   vector_at_data_point_.clear ();
00064   surface_.clear ();;
00065   cell_hash_map_.clear ();
00066   occupied_cell_list_.clear ();
00067   data_.reset ();
00068 }
00069 
00071 template <typename PointNT> void 
00072 pcl::GridProjection<PointNT>::scaleInputDataPoint (double scale_factor)
00073 {
00074   for (size_t i = 0; i < data_->points.size(); ++i)
00075     data_->points[i].getVector4fMap () /= scale_factor;
00076   max_p_ /=  scale_factor;
00077   min_p_ /= scale_factor;
00078 }
00079 
00081 template <typename PointNT> void
00082 pcl::GridProjection<PointNT>::getBoundingBox ()
00083 {
00084   pcl::getMinMax3D (*data_, min_p_, max_p_);
00085 
00086   Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
00087   double scale_factor = (std::max)((std::max)(bounding_box_size.x (), 
00088                                               bounding_box_size.y ()), 
00089                                               bounding_box_size.z ());
00090   if (scale_factor > 1)
00091     scaleInputDataPoint (scale_factor);
00092 
00093   // Round the max_p_, min_p_ so that they are aligned with the cells vertices
00094   int upper_right_index[3];
00095   int lower_left_index[3];
00096   for (size_t i = 0; i < 3; ++i) 
00097   { 
00098     upper_right_index[i] = max_p_(i) / leaf_size_ + 5;
00099     lower_left_index[i] = min_p_(i) / leaf_size_ - 5;
00100     max_p_(i) = upper_right_index[i] * leaf_size_;
00101     min_p_(i) = lower_left_index[i] * leaf_size_;
00102   }
00103   bounding_box_size = max_p_ - min_p_;
00104   PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n", 
00105       bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
00106   double max_size = 
00107     (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), 
00108                 bounding_box_size.z ());
00109 
00110   data_size_ = max_size / leaf_size_;
00111   PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n",
00112       min_p_.x (), min_p_.y (), min_p_.z ());
00113   PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n",
00114       max_p_.x (), max_p_.y (), max_p_.z ());
00115   PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_);
00116   PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_);
00117   occupied_cell_list_.resize (data_size_ * data_size_ * data_size_);
00118   gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
00119 }
00120 
00122 template <typename PointNT> void 
00123 pcl::GridProjection<PointNT>::getVertexFromCellCenter (
00124     const Eigen::Vector4f &cell_center, 
00125     std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &pts) const
00126 {
00127   assert (pts.size () == 8);
00128 
00129   float sz = leaf_size_ / 2.0;
00130 
00131   pts[0] = cell_center + Eigen::Vector4f (-sz,  sz, -sz, 0);
00132   pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0);
00133   pts[2] = cell_center + Eigen::Vector4f (sz,  -sz, -sz, 0);
00134   pts[3] = cell_center + Eigen::Vector4f (sz,   sz, -sz, 0);
00135   pts[4] = cell_center + Eigen::Vector4f (-sz,  sz,  sz, 0);
00136   pts[5] = cell_center + Eigen::Vector4f (-sz, -sz,  sz, 0);
00137   pts[6] = cell_center + Eigen::Vector4f (sz,  -sz,  sz, 0);
00138   pts[7] = cell_center + Eigen::Vector4f (sz,   sz,  sz, 0);
00139 }
00140 
00142 template <typename PointNT> void
00143 pcl::GridProjection<PointNT>::getDataPtsUnion (const Eigen::Vector3i &index, 
00144                                                std::vector <int> &pt_union_indices)
00145 {
00146   for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
00147   {
00148     for (int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
00149     {
00150       for (int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
00151       {
00152         Eigen::Vector3i cell_index_3d (i, j, k);
00153         int cell_index_1d = getIndexIn1D (cell_index_3d);
00154         if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
00155         {
00156           // Get the indices of the input points within the cell(i,j,k), which
00157           // is stored in the hash map
00158           pt_union_indices.insert (pt_union_indices.end (), 
00159                                    cell_hash_map_.at (cell_index_1d).data_indices.begin (), 
00160                                    cell_hash_map_.at (cell_index_1d).data_indices.end ());
00161         }
00162       }
00163     }
00164   }
00165 }
00166 
00168 template <typename PointNT> void
00169 pcl::GridProjection<PointNT>::createSurfaceForCell (const Eigen::Vector3i &index, 
00170                                                     std::vector <int> &pt_union_indices)
00171 {
00172   // 8 vertices of the cell
00173   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
00174 
00175   // 4 end points that shared by 3 edges connecting the upper left front points 
00176   Eigen::Vector4f pts[4];
00177   Eigen::Vector3f vector_at_pts[4];
00178 
00179   // Given the index of cell, caluate the coordinates of the eight vertices of the cell
00180   // index the index of the cell in (x,y,z) 3d format
00181   Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
00182   getCellCenterFromIndex (index, cell_center);
00183   getVertexFromCellCenter (cell_center, vertices);
00184 
00185   // Get the indices of the cells which stores the 4 end points.
00186   Eigen::Vector3i indices[4];
00187   indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
00188   indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
00189   indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
00190   indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
00191 
00192   // Get the coordinate of the 4 end points, and the corresponding vectors 
00193   for (size_t i = 0; i < 4; ++i)
00194   {
00195     pts[i] = vertices[I_SHIFT_PT[i]];
00196     unsigned int index_1d = getIndexIn1D (indices[i]);
00197     if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () || 
00198         occupied_cell_list_[index_1d] == 0)
00199       return;
00200     else
00201       vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
00202   }
00203 
00204   // Go through the 3 edges, test whether they are intersected by the surface 
00205   for (size_t i = 0; i < 3; ++i)
00206   {
00207     std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
00208     std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
00209     for (size_t j = 0; j < 2; ++j)
00210     {
00211       end_pts[j] = pts[I_SHIFT_EDGE[i][j]];
00212       vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
00213     }
00214     if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
00215     {
00216       // Indices of cells that contains points which will be connected to
00217       // create a polygon
00218       Eigen::Vector3i polygon[4];
00219       Eigen::Vector4f polygon_pts[4];
00220       int polygon_indices_1d[4];
00221       bool is_all_in_hash_map = true;
00222       switch (i)
00223       {
00224         case 0:
00225           polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
00226           polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
00227           polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
00228           polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
00229           break;
00230         case 1:
00231           polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
00232           polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
00233           polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
00234           polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
00235           break;
00236         case 2:
00237           polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
00238           polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
00239           polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
00240           polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
00241           break;
00242         default:
00243           break;
00244       }
00245       for (size_t k = 0; k < 4; k++)
00246       {
00247         polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
00248         if (!occupied_cell_list_[polygon_indices_1d[k]])
00249         {
00250           is_all_in_hash_map = false;
00251           break;
00252         }
00253       }
00254       if (is_all_in_hash_map)
00255       {
00256         for (size_t k = 0; k < 4; k++)
00257         {
00258           polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
00259           surface_.push_back (polygon_pts[k]);
00260         }
00261       }
00262     }
00263   }
00264 }
00265 
00267 template <typename PointNT> void
00268 pcl::GridProjection<PointNT>::getProjection (const Eigen::Vector4f &p, 
00269                                              std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
00270 {
00271   const double projection_distance = leaf_size_ * 3;
00272   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
00273   std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
00274   end_pt[0] = p;
00275   getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
00276   end_pt_vect[0].normalize();
00277   
00278   double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
00279 
00280   // Find another point which is projection_distance away from the p, do a
00281   // binary search between these two points, to find the projected point on the
00282   // surface
00283   if (dSecond > 0)
00284     end_pt[1] = end_pt[0] + Eigen::Vector4f (end_pt_vect[0][0] * projection_distance, 
00285                                              end_pt_vect[0][1] * projection_distance, 
00286                                              end_pt_vect[0][2] * projection_distance, 0);
00287   else
00288     end_pt[1] = end_pt[0] - Eigen::Vector4f (end_pt_vect[0][0] * projection_distance, 
00289                                              end_pt_vect[0][1] * projection_distance, 
00290                                              end_pt_vect[0][2] * projection_distance, 0);
00291   getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
00292   if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
00293   {
00294     Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
00295     findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
00296   }
00297   else
00298     projection = p;
00299 }
00300 
00302 template <typename PointNT> void 
00303 pcl::GridProjection<PointNT>::getProjectionWithPlaneFit (const Eigen::Vector4f &p, 
00304                                                          std::vector<int> (&pt_union_indices),
00305                                                          Eigen::Vector4f &projection)
00306 {
00307   // Compute the plane coefficients
00308   Eigen::Vector4f model_coefficients;
00310   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00311   Eigen::Vector4f xyz_centroid;
00312 
00313   // Estimate the XYZ centroid
00314   pcl::compute3DCentroid (*data_, pt_union_indices, xyz_centroid);
00315 
00316   // Compute the 3x3 covariance matrix
00317   pcl::computeCovarianceMatrix (*data_, pt_union_indices, xyz_centroid, covariance_matrix);
00318 
00319   // Get the plane normal
00320   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00321   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00322   pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00323 
00324   // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
00325   model_coefficients[0] = eigen_vectors (0, 0);
00326   model_coefficients[1] = eigen_vectors (1, 0);
00327   model_coefficients[2] = eigen_vectors (2, 0);
00328   model_coefficients[3] = 0;
00329   // Hessian form (D = nc . p_plane (centroid here) + p)
00330   model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00331 
00332   // Projected point
00333   Eigen::Vector3f point (p.x (), p.y (), p.z ());     //= Eigen::Vector3f::MapAligned (&output.points[cp].x, 3);
00334   float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3];
00335   point -= distance * model_coefficients.head < 3 > ();
00336 
00337   projection = Eigen::Vector4f (point[0], point[1], point[2], 0);
00338 }
00339 
00341 template <typename PointNT> void
00342 pcl::GridProjection<PointNT>::getVectorAtPoint (const Eigen::Vector4f &p, 
00343                                                 std::vector <int> &pt_union_indices, 
00344                                                 Eigen::Vector3f &vo)
00345 {
00346   std::vector <double> pt_union_dist (pt_union_indices.size ());
00347   std::vector <double> pt_union_weight (pt_union_indices.size ());
00348   Eigen::Vector3f out_vector (0, 0, 0);
00349   double sum = 0.0;
00350   double mag = 0.0;
00351 
00352   for (size_t i = 0; i < pt_union_indices.size (); ++i)
00353   {
00354     pt_union_dist[i] = (data_->points[pt_union_indices[i]].getVector4fMap () - p).squaredNorm ();
00355     pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
00356     mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_);
00357     sum += pt_union_weight[i];
00358   }
00359 
00360   pcl::VectorAverage3f vector_average;
00361 
00362   Eigen::Vector3f v (data_->points[pt_union_indices[0]].normal[0],
00363               data_->points[pt_union_indices[0]].normal[1],
00364               data_->points[pt_union_indices[0]].normal[2]);
00365 
00366   for (size_t i = 0; i < pt_union_weight.size (); ++i)
00367   {
00368     pt_union_weight[i] /= sum;
00369     Eigen::Vector3f vec (data_->points[pt_union_indices[i]].normal[0],
00370                   data_->points[pt_union_indices[i]].normal[1],
00371                   data_->points[pt_union_indices[i]].normal[2]);
00372     if (vec.dot (v) < 0)
00373       vec = -vec;
00374     vector_average.add (vec, pt_union_weight[i]);
00375   }
00376   out_vector = vector_average.getMean ();
00377   // vector_average.getEigenVector1(out_vector);
00378 
00379   out_vector.normalize ();
00380   double d1 = getD1AtPoint (p, out_vector, pt_union_indices);
00381   out_vector = out_vector * sum;
00382   vo = ((d1 > 0) ? -1 : 1) * out_vector;
00383 }
00384 
00386 template <typename PointNT> void
00387 pcl::GridProjection<PointNT>::getVectorAtPointKNN (const Eigen::Vector4f &p, 
00388                                                    std::vector <int> &k_indices, 
00389                                                    std::vector <float> &k_squared_distances,
00390                                                    Eigen::Vector3f &vo)
00391 {
00392   Eigen::Vector3f out_vector (0, 0, 0);
00393   std::vector <float> k_weight;
00394   k_weight.resize (k_);
00395   float sum = 0.0;
00396   for (int i = 0; i < k_; i++)
00397   {
00398     //k_weight[i] = pow (M_E, -pow (k_squared_distances[i], 2.0) / gaussian_scale_);
00399     k_weight[i] = std::pow (static_cast<float>(M_E), static_cast<float>(-pow (static_cast<float>(k_squared_distances[i]), 2.0f) / gaussian_scale_));
00400     sum += k_weight[i];
00401   }
00402   pcl::VectorAverage3f vector_average;
00403   for (int i = 0; i < k_; i++)
00404   {
00405     k_weight[i] /= sum;
00406     Eigen::Vector3f vec (data_->points[k_indices[i]].normal[0],
00407                          data_->points[k_indices[i]].normal[1], 
00408                          data_->points[k_indices[i]].normal[2]);
00409     vector_average.add (vec, k_weight[i]);
00410   }
00411   vector_average.getEigenVector1 (out_vector);
00412   out_vector.normalize ();
00413   double d1 = getD1AtPoint (p, out_vector, k_indices);
00414   out_vector = out_vector * sum;
00415   vo = ((d1 > 0) ? -1 : 1) * out_vector;
00416 
00417 }
00418 
00420 template <typename PointNT> double
00421 pcl::GridProjection<PointNT>::getMagAtPoint (const Eigen::Vector4f &p, 
00422                                              const std::vector <int> &pt_union_indices)
00423 {
00424   std::vector <double> pt_union_dist (pt_union_indices.size ());
00425   std::vector <double> pt_union_weight (pt_union_indices.size ());
00426   double sum = 0.0;
00427   for (size_t i = 0; i < pt_union_indices.size (); ++i)
00428   {
00429     pt_union_dist[i] = (data_->points[pt_union_indices[i]].getVector4fMap () - p).norm ();
00430     sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_);
00431   }
00432   return (sum);
00433 }
00434 
00436 template <typename PointNT> double
00437 pcl::GridProjection<PointNT>::getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 
00438                                             const std::vector <int> &pt_union_indices)
00439 {
00440   double sz = 0.01 * leaf_size_;
00441   Eigen::Vector3f v = vec * sz;
00442 
00443   double forward  = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
00444   double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices);
00445   return ((forward - backward) / (0.02 * leaf_size_));
00446 }
00447 
00449 template <typename PointNT> double
00450 pcl::GridProjection<PointNT>::getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, 
00451                                             const std::vector <int> &pt_union_indices)
00452 {
00453   double sz = 0.01 * leaf_size_;
00454   Eigen::Vector3f v = vec * sz;
00455 
00456   double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
00457   double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices);
00458   return ((forward - backward) / (0.02 * leaf_size_));
00459 }
00460 
00462 template <typename PointNT> bool
00463 pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 
00464                                              std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts, 
00465                                              std::vector <int> &pt_union_indices)
00466 {
00467   assert (end_pts.size () == 2);
00468   assert (vect_at_end_pts.size () == 2);
00469 
00470   double length[2];
00471   for (size_t i = 0; i < 2; ++i)
00472   {
00473     length[i] = vect_at_end_pts[i].norm ();
00474     vect_at_end_pts[i].normalize ();
00475   }
00476   double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
00477   if (dot_prod < 0)
00478   {
00479     double ratio = length[0] / (length[0] + length[1]);
00480     Eigen::Vector4f start_pt = end_pts[0] + (end_pts[1] - end_pts[0]) * ratio;
00481     Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
00482     findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
00483     
00484     Eigen::Vector3f vec;
00485     getVectorAtPoint (intersection_pt, pt_union_indices, vec);
00486     vec.normalize ();
00487 
00488     double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
00489     if (d2 < 0)
00490       return (true);
00491   }
00492   return (false);
00493 }
00494 
00496 template <typename PointNT> void
00497 pcl::GridProjection<PointNT>::findIntersection (int level, 
00498                                                 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts, 
00499                                                 const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
00500                                                 const Eigen::Vector4f &start_pt,
00501                                                 std::vector <int> &pt_union_indices,
00502                                                 Eigen::Vector4f &intersection)
00503 {
00504   assert (end_pts.size () == 2);
00505   assert (vect_at_end_pts.size () == 2);
00506 
00507   Eigen::Vector3f vec;
00508   getVectorAtPoint (start_pt, pt_union_indices, vec);
00509   double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
00510   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
00511   std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
00512   if ((abs (d1) < 10e-3) || (level == max_binary_search_level_))
00513   {
00514     intersection = start_pt;
00515     return;
00516   }
00517   else
00518   {
00519     vec.normalize ();
00520     if (vec.dot (vect_at_end_pts[0]) < 0)
00521     {
00522       Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
00523       new_end_pts[0] = end_pts[0];
00524       new_end_pts[1] = start_pt;
00525       new_vect_at_end_pts[0] = vect_at_end_pts[0];
00526       new_vect_at_end_pts[1] = vec;
00527       findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
00528       return;
00529     }
00530     if (vec.dot (vect_at_end_pts[1]) < 0)
00531     {
00532       Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
00533       new_end_pts[0] = start_pt;
00534       new_end_pts[1] = end_pts[1];
00535       new_vect_at_end_pts[0] = vec;
00536       new_vect_at_end_pts[1] = vect_at_end_pts[1];
00537       findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
00538       return;
00539     }
00540     intersection = start_pt;
00541     return;
00542   }
00543 }
00544 
00545 
00547 template <typename PointNT> void
00548 pcl::GridProjection<PointNT>::fillPad (const Eigen::Vector3i &index)
00549 {
00550   for (int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
00551   {
00552     for (int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
00553     {
00554       for (int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
00555       {
00556         Eigen::Vector3i cell_index_3d (i, j, k);
00557         unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
00558         if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
00559         {
00560           cell_hash_map_[cell_index_1d].data_indices.resize (0);
00561           getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
00562         }
00563       }
00564     }
00565   }
00566 }
00567 
00568 
00570 template <typename PointNT> void
00571 pcl::GridProjection<PointNT>::storeVectAndSurfacePoint (int index_1d, 
00572                                                         const Eigen::Vector3i &index_3d, 
00573                                                         std::vector <int> &pt_union_indices,
00574                                                         const Leaf &cell_data)
00575 {
00576   // Get point on grid
00577   Eigen::Vector4f grid_pt (cell_data.pt_on_surface.x () - leaf_size_ / 2.0, 
00578                            cell_data.pt_on_surface.y () + leaf_size_ / 2.0, 
00579                            cell_data.pt_on_surface.z () + leaf_size_ / 2.0, 0);
00580 
00581   // Save the vector and the point on the surface
00582   getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt);
00583   getProjection (cell_data.pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface);
00584 }
00585 
00587 template <typename PointNT> void
00588 pcl::GridProjection<PointNT>::storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &index_3d, 
00589                                                            const Leaf &cell_data)
00590 {
00591   Eigen::Vector4f cell_center = cell_data.pt_on_surface;
00592   Eigen::Vector4f grid_pt (cell_center.x () - leaf_size_ / 2, 
00593                            cell_center.y () + leaf_size_ / 2, 
00594                            cell_center.z () + leaf_size_ / 2, 0);
00595 
00596   std::vector <int> k_indices;
00597   k_indices.resize (k_);
00598   std::vector <float> k_squared_distances;
00599   k_squared_distances.resize (k_);
00600 
00601   PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z ();
00602   tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances);
00603 
00604   getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt);
00605   getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface);
00606 }
00607 
00609 template <typename PointNT> void
00610 pcl::GridProjection<PointNT>::performReconstruction (pcl::PolygonMesh &output)
00611 {
00612   data_.reset (new pcl::PointCloud<PointNT> (*input_));
00613   getBoundingBox ();
00614 
00615   // Store the point cloud data into the voxel grid, and at the same time
00616   // create a hash map to store the information for each cell
00617   cell_hash_map_.max_load_factor (2.0);
00618   cell_hash_map_.rehash (data_->points.size () / cell_hash_map_.max_load_factor ());
00619 
00620   // Go over all points and insert them into the right leaf
00621   for (size_t cp = 0; cp < data_->points.size (); ++cp)
00622   {
00623     // Check if the point is invalid
00624     if (!pcl_isfinite (data_->points[cp].x) || 
00625         !pcl_isfinite (data_->points[cp].y) || 
00626         !pcl_isfinite (data_->points[cp].z))
00627       continue;
00628 
00629     Eigen::Vector3i index_3d;
00630     getCellIndex (data_->points[cp].getVector4fMap (), index_3d);
00631     int index_1d = getIndexIn1D (index_3d);
00632     if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
00633     {
00634       Leaf cell_data;
00635       cell_data.data_indices.push_back (cp);
00636       getCellCenterFromIndex (index_3d, cell_data.pt_on_surface);
00637       cell_hash_map_[index_1d] = cell_data;
00638       occupied_cell_list_[index_1d] = 1;
00639     }
00640     else
00641     {
00642       Leaf cell_data = cell_hash_map_.at (index_1d);
00643       cell_data.data_indices.push_back (cp);
00644       cell_hash_map_[index_1d] = cell_data;
00645     }
00646   }
00647 
00648   Eigen::Vector3i index;
00649   int numOfFilledPad = 0;
00650 
00651   for (int i = 0; i < data_size_; ++i)
00652   {
00653     for (int j = 0; j < data_size_; ++j)
00654     {
00655       for (int k = 0; k < data_size_; ++k)
00656       {
00657         index[0] = i;
00658         index[1] = j;
00659         index[2] = k;
00660         if (occupied_cell_list_[getIndexIn1D (index)])
00661         {
00662           fillPad (index);
00663           numOfFilledPad++;
00664         }
00665       }
00666     }
00667   }
00668 
00669   // Update the hashtable and store the vector and point
00670   BOOST_FOREACH (typename HashMap::value_type entry, cell_hash_map_)
00671   {
00672     getIndexIn3D (entry.first, index);
00673     std::vector <int> pt_union_indices;
00674     getDataPtsUnion (index, pt_union_indices);
00675 
00676     // Needs at least 10 points (?)
00677     // NOTE: set as parameter later
00678     if (pt_union_indices.size () > 10)
00679     {
00680       storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
00681       //storeVectAndSurfacePointKNN(entry.first, index, entry.second);
00682       occupied_cell_list_[entry.first] = 1;
00683     }
00684   }
00685 
00686   // Go through the hash table another time to extract surface
00687   BOOST_FOREACH (typename HashMap::value_type entry, cell_hash_map_)
00688   {
00689     getIndexIn3D (entry.first, index);
00690     std::vector <int> pt_union_indices;
00691     getDataPtsUnion (index, pt_union_indices);
00692 
00693     // Needs at least 10 points (?)
00694     // NOTE: set as parameter later
00695     if (pt_union_indices.size () > 10)
00696       createSurfaceForCell (index, pt_union_indices);
00697   }
00698 
00699   // The mesh surface is held in surface_. Copy it to the output format
00700   output.header = input_->header;
00701 
00702   pcl::PointCloud<pcl::PointXYZ> cloud;
00703   cloud.width = surface_.size ();
00704   cloud.height = 1;
00705   cloud.is_dense = true;
00706 
00707   cloud.points.resize (surface_.size ());
00708   // Copy the data from surface_ to cloud
00709   for (size_t i = 0; i < cloud.points.size (); ++i)
00710   {
00711     cloud.points[i].x = surface_[i].x ();
00712     cloud.points[i].y = surface_[i].y ();
00713     cloud.points[i].z = surface_[i].z ();
00714   }
00715   pcl::toROSMsg (cloud, output.cloud);
00716 
00717   output.polygons.resize (surface_.size () / 4);
00718   // Copy the data from surface_ to polygons
00719   for (size_t i = 0; i < output.polygons.size (); ++i)
00720   {
00721     pcl::Vertices v;
00722     v.vertices.resize (4);
00723     for (int j = 0; j < 4; ++j)
00724       v.vertices[j] = i*4+j;
00725      output.polygons[i] = v;
00726   }
00727 }
00728 
00729 #define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection<T>;
00730 
00731 #endif    // PCL_SURFACE_IMPL_GRID_PROJECTION_H_
00732 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines