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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:37