00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
00051 template <typename PointInT, typename PointOutT> void
00052 pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
00053 {
00054
00055 if (compute_normals_)
00056 {
00057 normals_.reset (new NormalCloud);
00058
00059 normals_->header = input_->header;
00060
00061 normals_->width = normals_->height = 0;
00062 normals_->points.clear ();
00063 }
00064
00065
00066
00067 output.header = input_->header;
00068 output.width = output.height = 0;
00069 output.points.clear ();
00070
00071 if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
00072 {
00073 PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
00074 return;
00075 }
00076
00077 if (!initCompute ())
00078 return;
00079
00080
00081
00082 if (!tree_)
00083 {
00084 KdTreePtr tree;
00085 if (input_->isOrganized ())
00086 tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
00087 else
00088 tree.reset (new pcl::search::KdTree<PointInT> (false));
00089 setSearchMethod (tree);
00090 }
00091
00092
00093 tree_->setInputCloud (input_, indices_);
00094
00095
00096 switch (upsample_method_)
00097 {
00098 case (RANDOM_UNIFORM_DENSITY):
00099 {
00100 boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0)));
00101 float tmp = static_cast<float> (search_radius_ / 2.0f);
00102 boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp);
00103 rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib);
00104 break;
00105 }
00106 case (VOXEL_GRID_DILATION):
00107 {
00108 mls_results_.resize (input_->size ());
00109 break;
00110 }
00111 default:
00112 break;
00113 }
00114
00115
00116 performProcessing (output);
00117
00118 if (compute_normals_)
00119 {
00120 normals_->height = 1;
00121 normals_->width = static_cast<uint32_t> (normals_->size ());
00122
00123
00124 for (unsigned int i = 0; i < output.size (); ++i)
00125 {
00126 typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
00127 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
00128 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
00129 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
00130 pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature));
00131 }
00132
00133 }
00134
00135
00136 output.height = 1;
00137 output.width = static_cast<uint32_t> (output.size ());
00138
00139 deinitCompute ();
00140 }
00141
00143 template <typename PointInT, typename PointOutT> void
00144 pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
00145 const PointCloudIn &input,
00146 const std::vector<int> &nn_indices,
00147 std::vector<float> &nn_sqr_dists,
00148 PointCloudOut &projected_points,
00149 NormalCloud &projected_points_normals)
00150 {
00151
00152
00153 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00154 Eigen::Vector4f xyz_centroid;
00155
00156
00157 pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
00158
00159
00160 pcl::computeCovarianceMatrix (input, nn_indices, xyz_centroid, covariance_matrix);
00161
00162
00163 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
00164 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
00165 Eigen::Vector4f model_coefficients;
00166 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
00167 model_coefficients.head<3> () = eigen_vector;
00168 model_coefficients[3] = 0;
00169 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00170
00171
00172 Eigen::Vector3f point = input[(*indices_)[index]].getVector3fMap ();
00173 float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
00174 point -= distance * model_coefficients.head<3> ();
00175
00176 float curvature = covariance_matrix.trace ();
00177
00178 if (curvature != 0)
00179 curvature = fabsf (eigen_value / curvature);
00180
00181
00182
00183 Eigen::Vector3d plane_normal = model_coefficients.head<3> ().cast<double> ();
00184
00185 Eigen::VectorXd c_vec;
00186
00187 Eigen::Vector3d v (0.0f, 0.0f, 0.0f), u (0.0f, 0.0f, 0.0f);
00188
00189
00190
00191
00193 if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
00194 {
00195
00196
00197 std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
00198 for (size_t ni = 0; ni < nn_indices.size (); ++ni)
00199 {
00200 de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
00201 de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
00202 de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
00203 nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
00204 }
00205
00206
00207 Eigen::VectorXd weight_vec (nn_indices.size ());
00208 Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
00209 Eigen::VectorXd f_vec (nn_indices.size ());
00210 Eigen::MatrixXd P_weight;
00211 Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);
00212
00213
00214 v = plane_normal.unitOrthogonal ();
00215 u = plane_normal.cross (v);
00216
00217
00218
00219 double u_coord, v_coord, u_pow, v_pow;
00220 for (size_t ni = 0; ni < nn_indices.size (); ++ni)
00221 {
00222
00223 weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
00224
00225
00226 u_coord = de_meaned[ni].dot (u);
00227 v_coord = de_meaned[ni].dot (v);
00228 f_vec (ni) = de_meaned[ni].dot (plane_normal);
00229
00230
00231 int j = 0;
00232 u_pow = 1;
00233 for (int ui = 0; ui <= order_; ++ui)
00234 {
00235 v_pow = 1;
00236 for (int vi = 0; vi <= order_ - ui; ++vi)
00237 {
00238 P (j++, ni) = u_pow * v_pow;
00239 v_pow *= v_coord;
00240 }
00241 u_pow *= u_coord;
00242 }
00243 }
00244
00245
00246 P_weight = P * weight_vec.asDiagonal ();
00247 P_weight_Pt = P_weight * P.transpose ();
00248 c_vec = P_weight * f_vec;
00249 P_weight_Pt.llt ().solveInPlace (c_vec);
00250 }
00251
00252 switch (upsample_method_)
00253 {
00254 case (NONE):
00255 {
00256 Eigen::Vector3d normal = plane_normal;
00257
00258 if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
00259 {
00260 point += (c_vec[0] * plane_normal).cast<float> ();
00261
00262
00263 if (compute_normals_)
00264 normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
00265 }
00266
00267 PointOutT aux;
00268 aux.x = point[0];
00269 aux.y = point[1];
00270 aux.z = point[2];
00271 projected_points.push_back (aux);
00272
00273 if (compute_normals_)
00274 {
00275 pcl::Normal aux_normal;
00276 aux_normal.normal_x = static_cast<float> (normal[0]);
00277 aux_normal.normal_y = static_cast<float> (normal[1]);
00278 aux_normal.normal_z = static_cast<float> (normal[2]);
00279 aux_normal.curvature = curvature;
00280 projected_points_normals.push_back (aux_normal);
00281 }
00282
00283 break;
00284 }
00285
00286 case (SAMPLE_LOCAL_PLANE):
00287 {
00288
00289 for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
00290 for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
00291 if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
00292 {
00293 PointOutT projected_point;
00294 pcl::Normal projected_normal;
00295 projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
00296 static_cast<int> (nn_indices.size ()),
00297 projected_point, projected_normal);
00298
00299 projected_points.push_back (projected_point);
00300 if (compute_normals_)
00301 projected_points_normals.push_back (projected_normal);
00302 }
00303 break;
00304 }
00305
00306 case (RANDOM_UNIFORM_DENSITY):
00307 {
00308
00309 int num_points_to_add = static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
00310
00311
00312 if (num_points_to_add <= 0)
00313 {
00314
00315 Eigen::Vector3d normal = plane_normal;
00316 if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
00317 {
00318
00319 point += (c_vec[0] * plane_normal).cast<float> ();
00320
00321 if (compute_normals_)
00322 normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
00323 }
00324 PointOutT aux;
00325 aux.x = point[0];
00326 aux.y = point[1];
00327 aux.z = point[2];
00328 projected_points.push_back (aux);
00329
00330 if (compute_normals_)
00331 {
00332 pcl::Normal aux_normal;
00333 aux_normal.normal_x = static_cast<float> (normal[0]);
00334 aux_normal.normal_y = static_cast<float> (normal[1]);
00335 aux_normal.normal_z = static_cast<float> (normal[2]);
00336 aux_normal.curvature = curvature;
00337 projected_points_normals.push_back (aux_normal);
00338 }
00339 }
00340 else
00341 {
00342
00343 for (int num_added = 0; num_added < num_points_to_add;)
00344 {
00345 float u_disp = (*rng_uniform_distribution_) (),
00346 v_disp = (*rng_uniform_distribution_) ();
00347
00348 if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4)
00349 continue;
00350
00351
00352 PointOutT projected_point;
00353 pcl::Normal projected_normal;
00354 projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
00355 static_cast<int> (nn_indices.size ()),
00356 projected_point, projected_normal);
00357
00358 projected_points.push_back (projected_point);
00359 if (compute_normals_)
00360 projected_points_normals.push_back (projected_normal);
00361
00362 num_added ++;
00363 }
00364 }
00365 break;
00366 }
00367
00368 case (VOXEL_GRID_DILATION):
00369 {
00370
00371
00372 MLSResult result (plane_normal, u, v, c_vec, static_cast<int> (nn_indices.size ()), curvature);
00373 mls_results_[index] = result;
00374 break;
00375 }
00376 }
00377 }
00378
00379
00380
00381 template <typename PointInT, typename PointOutT> void
00382 pcl::MovingLeastSquares<PointInT, PointOutT>::projectPointToMLSSurface (float &u_disp, float &v_disp,
00383 Eigen::Vector3d &u, Eigen::Vector3d &v,
00384 Eigen::Vector3d &plane_normal,
00385 float &curvature,
00386 Eigen::Vector3f &query_point,
00387 Eigen::VectorXd &c_vec,
00388 int num_neighbors,
00389 PointOutT &result_point,
00390 pcl::Normal &result_normal)
00391 {
00392 double n_disp = 0.0f;
00393 double d_u = 0.0f, d_v = 0.0f;
00394
00395
00396 if (polynomial_fit_ && num_neighbors >= 5*nr_coeff_ && pcl_isfinite (c_vec[0]))
00397 {
00398
00399
00400 int j = 0;
00401 float u_pow = 1.0f, v_pow = 1.0f, u_pow_prev = 1.0f, v_pow_prev = 1.0f;
00402 for (int ui = 0; ui <= order_; ++ui)
00403 {
00404 v_pow = 1;
00405 for (int vi = 0; vi <= order_ - ui; ++vi)
00406 {
00407
00408 n_disp += u_pow * v_pow * c_vec[j++];
00409
00410
00411 if (ui >= 1)
00412 d_u += c_vec[j-1] * ui * u_pow_prev * v_pow;
00413 if (vi >= 1)
00414 d_v += c_vec[j-1] * vi * u_pow * v_pow_prev;
00415
00416 v_pow_prev = v_pow;
00417 v_pow *= v_disp;
00418 }
00419 u_pow_prev = u_pow;
00420 u_pow *= u_disp;
00421 }
00422 }
00423
00424 Eigen::Vector3d normal = plane_normal - d_u * u - d_v * v;
00425 normal.normalize ();
00426 result_point.x = static_cast<float> (query_point[0] + u[0] * u_disp + v[0] * v_disp + normal[0] * n_disp);
00427 result_point.y = static_cast<float> (query_point[1] + u[1] * u_disp + v[1] * v_disp + normal[1] * n_disp);
00428 result_point.z = static_cast<float> (query_point[2] + u[2] * u_disp + v[2] * v_disp + normal[2] * n_disp);
00429
00430 result_normal.normal_x = static_cast<float> (normal[0]);
00431 result_normal.normal_y = static_cast<float> (normal[1]);
00432 result_normal.normal_z = static_cast<float> (normal[2]);
00433 result_normal.curvature = curvature;
00434 }
00435
00436
00438 template <typename PointInT, typename PointOutT> void
00439 pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
00440 {
00441
00442 nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
00443
00444
00445
00446 std::vector<int> nn_indices;
00447 std::vector<float> nn_sqr_dists;
00448
00449
00450 for (size_t cp = 0; cp < indices_->size (); ++cp)
00451 {
00452
00453 if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists))
00454 continue;
00455
00456
00457
00458 if (nn_indices.size () < 3)
00459 continue;
00460
00461
00462 PointCloudOut projected_points;
00463 NormalCloud projected_points_normals;
00464
00465 computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals);
00466
00467
00468 output.insert (output.end (), projected_points.begin (), projected_points.end ());
00469 if (compute_normals_)
00470 normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
00471 }
00472
00473
00474
00475
00476
00477 if (upsample_method_ == VOXEL_GRID_DILATION)
00478 {
00479 MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
00480
00481 for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
00482 voxel_grid.dilate ();
00483
00484
00485 for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
00486 {
00487
00488 Eigen::Vector3f pos;
00489 voxel_grid.getPosition (m_it->first, pos);
00490
00491 PointInT p;
00492 p.x = pos[0];
00493 p.y = pos[1];
00494 p.z = pos[2];
00495
00496 std::vector<int> nn_indices;
00497 std::vector<float> nn_dists;
00498 tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
00499 int input_index = nn_indices.front ();
00500
00501
00502
00503 if (mls_results_[input_index].valid == false)
00504 continue;
00505
00506 Eigen::Vector3f add_point = p.getVector3fMap (),
00507 input_point = input_->points[input_index].getVector3fMap ();
00508
00509 Eigen::Vector3d aux = mls_results_[input_index].u;
00510 Eigen::Vector3f u = aux.cast<float> ();
00511 aux = mls_results_[input_index].v;
00512 Eigen::Vector3f v = aux.cast<float> ();
00513
00514 float u_disp = (add_point - input_point).dot (u),
00515 v_disp = (add_point - input_point).dot (v);
00516
00517 PointOutT result_point;
00518 pcl::Normal result_normal;
00519 projectPointToMLSSurface (u_disp, v_disp,
00520 mls_results_[input_index].u, mls_results_[input_index].v,
00521 mls_results_[input_index].plane_normal,
00522 mls_results_[input_index].curvature,
00523 input_point,
00524 mls_results_[input_index].c_vec,
00525 mls_results_[input_index].num_neighbors,
00526 result_point, result_normal);
00527
00528 float d_before = (pos - input_point).norm (),
00529 d_after = (result_point.getVector3fMap () - input_point). norm();
00530 if (d_after > d_before)
00531 continue;
00532
00533 output.push_back (result_point);
00534 if (compute_normals_)
00535 normals_->push_back (result_normal);
00536 }
00537 }
00538 }
00539
00540
00541
00542
00543
00544
00546 template <typename PointInT, typename PointOutT>
00547 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSResult::MLSResult (Eigen::Vector3d &a_plane_normal,
00548 Eigen::Vector3d &a_u,
00549 Eigen::Vector3d &a_v,
00550 Eigen::VectorXd a_c_vec,
00551 int a_num_neighbors,
00552 float &a_curvature) :
00553 plane_normal (a_plane_normal), u (a_u), v (a_v), c_vec (a_c_vec), num_neighbors (a_num_neighbors),
00554 curvature (a_curvature), valid (true)
00555 {
00556 }
00557
00559 template <typename PointInT, typename PointOutT>
00560 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,
00561 IndicesPtr &indices,
00562 float voxel_size) :
00563 voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
00564 {
00565 pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
00566
00567 Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
00568 double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
00569
00570 data_size_ = static_cast<uint64_t> (1.5 * max_size / voxel_size_);
00571 for (unsigned int i = 0; i < indices->size (); ++i)
00572 if (pcl_isfinite (cloud->points[(*indices)[i]].x))
00573 {
00574 Eigen::Vector3i pos;
00575 getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
00576
00577 uint64_t index_1d;
00578 getIndexIn1D (pos, index_1d);
00579 Leaf leaf;
00580 voxel_grid_[index_1d] = leaf;
00581 }
00582 }
00583
00585 template <typename PointInT, typename PointOutT> void
00586 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::dilate ()
00587 {
00588 HashMap new_voxel_grid = voxel_grid_;
00589 for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
00590 {
00591 Eigen::Vector3i index;
00592 getIndexIn3D (m_it->first, index);
00593
00595 for (int x = -1; x <= 1; ++x)
00596 for (int y = -1; y <= 1; ++y)
00597 for (int z = -1; z <= 1; ++z)
00598 if (x != 0 || y != 0 || z != 0)
00599 {
00600 Eigen::Vector3i new_index;
00601 new_index = index + Eigen::Vector3i (x, y, z);
00602
00603 uint64_t index_1d;
00604 getIndexIn1D (new_index, index_1d);
00605 Leaf leaf;
00606 new_voxel_grid[index_1d] = leaf;
00607 }
00608 }
00609 voxel_grid_ = new_voxel_grid;
00610 }
00611
00612
00613 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
00614
00615 #endif // PCL_SURFACE_IMPL_MLS_H_