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