Point Cloud Library (PCL)
1.3.1
|
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