template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::addProjectedPointNormal (int index, const Eigen::Vector3d &point, const Eigen::Vector3d &normal, double curvature, PointCloudOut &projected_points, NormalCloud &projected_points_normals, PointIndices &corresponding_input_indices) const { PointOutT aux; aux.x = static_cast<float> (point[0]); aux.y = static_cast<float> (point[1]); aux.z = static_cast<float> (point[2]); // Copy additional point information if available copyMissingFields (input_->points[index], aux); projected_points.push_back (aux); corresponding_input_indices.indices.push_back (index); if (compute_normals_) { pcl::Normal aux_normal; aux_normal.normal_x = static_cast<float> (normal[0]); aux_normal.normal_y = static_cast<float> (normal[1]); aux_normal.normal_z = static_cast<float> (normal[2]); aux_normal.curvature = curvature; projected_points_normals.push_back (aux_normal); } }
template <typename PointInT, typename PointOutT, typename NormalT> void pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output) { response_.reset (new pcl::PointCloud<float> (input_->width, input_->height)); const Normals &normals = *normals_; const PointCloudIn &input = *input_; pcl::PointCloud<float>& response = *response_; const int w = static_cast<int> (input_->width) - half_window_size_; const int h = static_cast<int> (input_->height) - half_window_size_; if (method_ == FOUR_CORNERS) { #ifdef _OPENMP #pragma omp parallel for num_threads (threads_) #endif for(int j = half_window_size_; j < h; ++j) { for(int i = half_window_size_; i < w; ++i) { if (!isFinite (input (i,j))) continue; const NormalT ¢er = normals (i,j); if (!isFinite (center)) continue; int count = 0; const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); // Get rid of isolated points if (!count) continue; float sn1 = squaredNormalsDiff (up, center); float sn2 = squaredNormalsDiff (down, center); float r1 = sn1 + sn2; float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center); float d = std::min (r1, r2); if (d < first_threshold_) continue; sn1 = sqrt (sn1); sn2 = sqrt (sn2); float b1 = normalsDiff (right, up) * sn1; b1+= normalsDiff (left, down) * sn2; float b2 = normalsDiff (right, down) * sn2; b2+= normalsDiff (left, up) * sn1; float B = std::min (b1, b2); float A = r2 - r1 - 2*B; response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; } } } else { #ifdef _OPENMP #pragma omp parallel for num_threads (threads_) #endif for(int j = half_window_size_; j < h; ++j) { for(int i = half_window_size_; i < w; ++i) { if (!isFinite (input (i,j))) continue; const NormalT ¢er = normals (i,j); if (!isFinite (center)) continue; int count = 0; const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count); const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count); const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count); const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count); // Get rid of isolated points if (!count) continue; std::vector<float> r (4,0); r[0] = squaredNormalsDiff (up, center); r[0]+= squaredNormalsDiff (down, center); r[1] = squaredNormalsDiff (upright, center); r[1]+= squaredNormalsDiff (downleft, center); r[2] = squaredNormalsDiff (right, center); r[2]+= squaredNormalsDiff (left, center); r[3] = squaredNormalsDiff (downright, center); r[3]+= squaredNormalsDiff (upleft, center); float d = *(std::min_element (r.begin (), r.end ())); if (d < first_threshold_) continue; std::vector<float> B (4,0); std::vector<float> A (4,0); std::vector<float> sumAB (4,0); B[0] = normalsDiff (upright, up) * normalsDiff (up, center); B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center); B[1] = normalsDiff (right, upright) * normalsDiff (upright, center); B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center); B[2] = normalsDiff (downright, right) * normalsDiff (downright, center); B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center); B[3] = normalsDiff (down, downright) * normalsDiff (downright, center); B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center); A[0] = r[1] - r[0] - B[0] - B[0]; A[1] = r[2] - r[1] - B[1] - B[1]; A[2] = r[3] - r[2] - B[2] - B[2]; A[3] = r[0] - r[3] - B[3] - B[3]; sumAB[0] = A[0] + B[0]; sumAB[1] = A[1] + B[1]; sumAB[2] = A[2] + B[2]; sumAB[3] = A[3] + B[3]; if ((*std::max_element (B.begin (), B.end ()) < 0) && (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) { std::vector<float> D (4,0); D[0] = B[0] * B[0] / A[0]; D[1] = B[1] * B[1] / A[1]; D[2] = B[2] * B[2] / A[2]; D[3] = B[3] * B[3] / A[3]; response (i,j) = *(std::min (D.begin (), D.end ())); } else response (i,j) = d; } } } // Non maximas suppression std::vector<int> indices = *indices_; std::sort (indices.begin (), indices.end (), boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2)); output.clear (); output.reserve (input_->size ()); std::vector<bool> occupency_map (indices.size (), false); const int width (input_->width); const int height (input_->height); const int occupency_map_size (indices.size ()); #ifdef _OPENMP #pragma omp parallel for shared (output) num_threads (threads_) #endif for (int i = 0; i < indices.size (); ++i) { int idx = indices[i]; if ((response_->points[idx] < second_threshold_) || occupency_map[idx]) continue; PointOutT p; p.getVector3fMap () = input_->points[idx].getVector3fMap (); p.intensity = response_->points [idx]; #ifdef _OPENMP #pragma omp critical #endif { output.push_back (p); keypoints_indices_->indices.push_back (idx); } const int x = idx % width; const int y = idx / width; const int u_end = std::min (width, x + half_window_size_); const int v_end = std::min (height, y + half_window_size_); for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) occupency_map[v*width + u] = true; } output.height = 1; output.width = static_cast<uint32_t> (output.size()); // we don not change the denseness output.is_dense = true; }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) { // Get the initial estimates of point positions and their neighborhoods if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists)) continue; // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () < 3) continue; PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals); // Append projected points to output output.insert (output.end (), projected_points.begin (), projected_points.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); BOOST_FOREACH (typename MLSVoxelGrid::HashMap::value_type voxel, voxel_grid.voxel_grid_) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (voxel.first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = p.getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); float d_before = (pos - input_point).norm (), d_after = (result_point.getVector3fMap () - input_point). norm(); if (d_after > d_before) continue; output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index, const PointCloudIn &input, const std::vector<int> &nn_indices, std::vector<float> &nn_sqr_dists, PointCloudOut &projected_points, NormalCloud &projected_points_normals) { // Compute the plane coefficients //pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature); EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; // Estimate the XYZ centroid pcl::compute3DCentroid (input, nn_indices, xyz_centroid); //pcl::compute3DCentroid (input, nn_indices, xyz_centroid); pcl::computeCovarianceMatrix (input, nn_indices, xyz_centroid, covariance_matrix); // Compute the 3x3 covariance matrix EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; Eigen::Vector4f model_coefficients; pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); model_coefficients.head<3> () = eigen_vector; model_coefficients[3] = 0; model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); // Projected query point Eigen::Vector3f point = input[(*indices_)[index]].getVector3fMap (); float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3]; point -= distance * model_coefficients.head<3> (); float curvature = covariance_matrix.trace (); // Compute the curvature surface change if (curvature != 0) curvature = fabsf (eigen_value / curvature); // Get a copy of the plane normal easy access Eigen::Vector3d plane_normal = model_coefficients.head<3> ().cast<double> (); // Vector in which the polynomial coefficients will be put Eigen::VectorXd c_vec; // Local coordinate system (Darboux frame) Eigen::Vector3d v (0.0f, 0.0f, 0.0f), u (0.0f, 0.0f, 0.0f); // Perform polynomial fit to update point and normal //////////////////////////////////////////////////// if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_) { // Update neighborhood, since point was projected, and computing relative // positions. Note updating only distances for the weights for speed std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ()); for (size_t ni = 0; ni < nn_indices.size (); ++ni) { de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0]; de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1]; de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2]; nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni])); } // Allocate matrices and vectors to hold the data used for the polynomial fit Eigen::VectorXd weight_vec (nn_indices.size ()); Eigen::MatrixXd P (nr_coeff_, nn_indices.size ()); Eigen::VectorXd f_vec (nn_indices.size ()); Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ()); Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_); // Get local coordinate system (Darboux frame) v = plane_normal.unitOrthogonal (); u = plane_normal.cross (v); // Go through neighbors, transform them in the local coordinate system, // save height and the evaluation of the polynome's terms double u_coord, v_coord, u_pow, v_pow; for (size_t ni = 0; ni < nn_indices.size (); ++ni) { // (re-)compute weights weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_); // transforming coordinates u_coord = de_meaned[ni].dot (u); v_coord = de_meaned[ni].dot (v); f_vec (ni) = de_meaned[ni].dot (plane_normal); // compute the polynomial's terms at the current point int j = 0; u_pow = 1; for (int ui = 0; ui <= order_; ++ui) { v_pow = 1; for (int vi = 0; vi <= order_ - ui; ++vi) { P (j++, ni) = u_pow * v_pow; v_pow *= v_coord; } u_pow *= u_coord; } } // Computing coefficients P_weight = P * weight_vec.asDiagonal (); P_weight_Pt = P_weight * P.transpose (); c_vec = P_weight * f_vec; P_weight_Pt.llt ().solveInPlace (c_vec); } switch (upsample_method_) { case (NONE): { Eigen::Vector3d normal = plane_normal; if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0])) { point += (c_vec[0] * plane_normal).cast<float> (); // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1] if (compute_normals_) normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v; } PointOutT aux; aux.x = point[0]; aux.y = point[1]; aux.z = point[2]; projected_points.push_back (aux); if (compute_normals_) { pcl::Normal aux_normal; aux_normal.normal_x = static_cast<float> (normal[0]); aux_normal.normal_y = static_cast<float> (normal[1]); aux_normal.normal_z = static_cast<float> (normal[2]); aux_normal.curvature = curvature; projected_points_normals.push_back (aux_normal); } break; } case (SAMPLE_LOCAL_PLANE): { // Uniformly sample a circle around the query point using the radius and step parameters for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_)) for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_)) if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_) { PointOutT projected_point; pcl::Normal projected_normal; projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec, static_cast<int> (nn_indices.size ()), projected_point, projected_normal); projected_points.push_back (projected_point); if (compute_normals_) projected_points_normals.push_back (projected_normal); } break; } case (RANDOM_UNIFORM_DENSITY): { // Compute the local point density and add more samples if necessary int num_points_to_add = static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ()))); // Just add the query point, because the density is good if (num_points_to_add <= 0) { // Just add the current point Eigen::Vector3d normal = plane_normal; if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0])) { // Projection onto MLS surface along Darboux normal to the height at (0,0) point += (c_vec[0] * plane_normal).cast<float> (); // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1] if (compute_normals_) normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v; } PointOutT aux; aux.x = point[0]; aux.y = point[1]; aux.z = point[2]; projected_points.push_back (aux); if (compute_normals_) { pcl::Normal aux_normal; aux_normal.normal_x = static_cast<float> (normal[0]); aux_normal.normal_y = static_cast<float> (normal[1]); aux_normal.normal_z = static_cast<float> (normal[2]); aux_normal.curvature = curvature; projected_points_normals.push_back (aux_normal); } } else { // Sample the local plane for (int num_added = 0; num_added < num_points_to_add;) { float u_disp = (*rng_uniform_distribution_) (), v_disp = (*rng_uniform_distribution_) (); // Check if inside circle; if not, try another coin flip if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4) continue; PointOutT projected_point; pcl::Normal projected_normal; projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec, static_cast<int> (nn_indices.size ()), projected_point, projected_normal); projected_points.push_back (projected_point); if (compute_normals_) projected_points_normals.push_back (projected_normal); num_added ++; } } break; } case (VOXEL_GRID_DILATION): { // Take all point pairs and sample space between them in a grid-fashion // \note consider only point pairs with increasing indices MLSResult result (plane_normal, u, v, c_vec, static_cast<int> (nn_indices.size ()), curvature); mls_results_[index] = result; break; } } }
template <typename PointInT, typename PointOutT, typename IntensityT> void pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output) { derivatives_cols_.resize (input_->width, input_->height); derivatives_rows_.resize (input_->width, input_->height); //Compute cloud intensities first derivatives along columns and rows //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan int w = static_cast<int> (input_->width) - 1; int h = static_cast<int> (input_->height) - 1; // j = 0 --> j-1 out of range ; use 0 // i = 0 --> i-1 out of range ; use 0 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5; derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5; // #ifdef _OPENMP // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) // #pragma omp parallel for num_threads (threads_) // #endif for(int i = 1; i < w; ++i) { derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5; } derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5; derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5; // #ifdef _OPENMP // //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_) // #pragma omp parallel for num_threads (threads_) // #endif for(int j = 1; j < h; ++j) { // i = 0 --> i-1 out of range ; use 0 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5; for(int i = 1; i < w; ++i) { // derivative with respect to rows derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5; // derivative with respect to cols derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5; } // i = w --> w+1 out of range ; use w derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5; } // j = h --> j+1 out of range use h derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5; derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5; // #ifdef _OPENMP // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) // #pragma omp parallel for num_threads (threads_) // #endif for(int i = 1; i < w; ++i) { derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5; } derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5; derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5; float highest_response_; switch (method_) { case HARRIS: responseHarris(*response_, highest_response_); break; case NOBLE: responseNoble(*response_, highest_response_); break; case LOWE: responseLowe(*response_, highest_response_); break; case TOMASI: responseTomasi(*response_, highest_response_); break; } if (!nonmax_) output = *response_; else { threshold_*= highest_response_; std::sort (indices_->begin (), indices_->end (), boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2)); output.clear (); output.reserve (response_->size()); std::vector<bool> occupency_map (response_->size (), false); int width (response_->width); int height (response_->height); const int occupency_map_size (occupency_map.size ()); #ifdef _OPENMP #pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_) #endif for (int idx = 0; idx < occupency_map_size; ++idx) { if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx])) continue; #ifdef _OPENMP #pragma omp critical #endif output.push_back (response_->at (indices_->at (idx))); int u_end = std::min (width, indices_->at (idx) % width + min_distance_); int v_end = std::min (height, indices_->at (idx) / width + min_distance_); for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u) for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v) occupency_map[v*input_->width+u] = true; } // if (refine_) // refineCorners (output); output.height = 1; output.width = static_cast<uint32_t> (output.size()); } // we don not change the denseness output.is_dense = input_->is_dense; }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output) { if (upsample_method_ == DISTINCT_CLOUD) { for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i { // Distinct cloud may have nan points, skip them if (!pcl_isfinite (distinct_cloud_->points[dp_i].x)) continue; // Get 3D position of point //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap (); std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> (); float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)), v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis)); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u_axis, mls_results_[input_index].v_axis, mls_results_[input_index].plane_normal, mls_results_[input_index].mean, mls_results_[input_index].curvature, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); // Copy additional point information if available copyMissingFields (input_->points[input_index], result_point); // Store the id of the original point corresponding_input_indices_->indices.push_back (input_index); output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (m_it->first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> (); float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)), v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis)); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u_axis, mls_results_[input_index].v_axis, mls_results_[input_index].plane_normal, mls_results_[input_index].mean, mls_results_[input_index].curvature, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); // Copy additional point information if available copyMissingFields (input_->points[input_index], result_point); // Store the id of the original point corresponding_input_indices_->indices.push_back (input_index); output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { // Compute the number of coefficients nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices; std::vector<float> nn_sqr_dists; typedef typename pcl::traits::fieldList<typename PointCloudIn::PointType>::type FieldListInput; typedef typename pcl::traits::fieldList<typename PointCloudOut::PointType>::type FieldListOutput; // For all points for (size_t cp = 0; cp < indices_->size (); ++cp) { // Get the initial estimates of point positions and their neighborhoods if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists)) continue; // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) if (nn_indices.size () < 3) continue; PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals); /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[(*indices_)[cp]], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { for (size_t pp = 0; pp < projected_points.size (); ++pp) pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( projected_points.points[pp], "rgb", rgb_input)); } // Append projected points to output output.insert (output.end (), projected_points.begin (), projected_points.end ()); if (compute_normals_) normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } if (upsample_method_ == DISTINCT_CLOUD) { for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i { // Distinct cloud may have nan points, skip them if (!pcl_isfinite (distinct_cloud_->points[dp_i].x)) continue; // Get 3D position of point //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap (); std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = distinct_cloud_->points[dp_i].getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[input_index], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( result_point, "rgb", rgb_input)); } output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } // For the voxel grid upsampling method, generate the voxel grid and dilate it // Then, project the newly obtained points to the MLS surface if (upsample_method_ == VOXEL_GRID_DILATION) { MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) voxel_grid.dilate (); for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it) { // Get 3D position of point Eigen::Vector3f pos; voxel_grid.getPosition (m_it->first, pos); PointInT p; p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; std::vector<int> nn_indices; std::vector<float> nn_dists; tree_->nearestKSearch (p, 1, nn_indices, nn_dists); int input_index = nn_indices.front (); // If the closest point did not have a valid MLS fitting result // OR if it is too far away from the sampled point if (mls_results_[input_index].valid == false) continue; Eigen::Vector3f add_point = p.getVector3fMap (), input_point = input_->points[input_index].getVector3fMap (); Eigen::Vector3d aux = mls_results_[input_index].u; Eigen::Vector3f u = aux.cast<float> (); aux = mls_results_[input_index].v; Eigen::Vector3f v = aux.cast<float> (); float u_disp = (add_point - input_point).dot (u), v_disp = (add_point - input_point).dot (v); PointOutT result_point; pcl::Normal result_normal; projectPointToMLSSurface (u_disp, v_disp, mls_results_[input_index].u, mls_results_[input_index].v, mls_results_[input_index].plane_normal, mls_results_[input_index].curvature, input_point, mls_results_[input_index].c_vec, mls_results_[input_index].num_neighbors, result_point, result_normal); float d_before = (pos - input_point).norm (), d_after = (result_point.getVector3fMap () - input_point). norm(); if (d_after > d_before) continue; /// Copy RGB information if available float rgb_input; bool rgb_exists_input; pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> ( input_->points[input_index], "rgb", rgb_exists_input, rgb_input)); if (rgb_exists_input) { pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> ( result_point, "rgb", rgb_input)); } output.push_back (result_point); if (compute_normals_) normals_->push_back (result_normal); } } }