Point Cloud Library (PCL)
1.7.0
|
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 00047 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00055 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00063 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00074 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00088 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00129 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00149 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00175 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00275 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00314 ////////////////////////////////////////////////////////////////////////////////////////////// 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; 00322 /// @remark iterative weighted least squares or sac might give better results 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 00349 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00396 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00430 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00447 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00460 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00473 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00508 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00559 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00582 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00600 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00623 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00727 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00753 ////////////////////////////////////////////////////////////////////////////////////////////// 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