template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Check if input was set
if (!normals_)
{
ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (normals_->points.size () != surface_->points.size ())
{
ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
size_t data_size = indices_->size ();
// Reset the whole thing
hist_f1_.setZero (data_size, nr_bins_f1_);
hist_f2_.setZero (data_size, nr_bins_f2_);
hist_f3_.setZero (data_size, nr_bins_f3_);
int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
// Iterating over the entire index vector
for (size_t idx = 0; idx < data_size; ++idx)
{
int p_idx = (*indices_)[idx];
searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
// Estimate the FPFH signature at each patch
computePointSPFHSignature (*surface_, *normals_, p_idx, nn_indices,
hist_f1_, hist_f2_, hist_f3_);
}
fpfh_histogram_.setZero (nr_bins);
// Iterating over the entire index vector
for (size_t idx = 0; idx < data_size; ++idx)
{
searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// Copy into the resultant cloud
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
fpfh_histogram_.setZero ();
}
}
template <typename PointInT, typename PointOutT> void
pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
continue;
}
computePointNormal (*surface_, nn_indices,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
flipNormalTowardsViewpoint (surface_->points[idx], vpx_, vpy_, vpz_,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
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 ((*indices_)[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
int index = (*indices_)[cp];
computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[index]);
// Copy all information from the input cloud to the output points (not doing any interpolation)
for (size_t pp = 0; pp < projected_points.size (); ++pp)
copyMissingFields (input_->points[(*indices_)[cp]], projected_points[pp]);
// 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 ());
}
// Perform the distinct-cloud or voxel-grid upsampling
performUpsampling (output);
}
template <typename PointInT, typename PointOutT> void
pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
computePointMomentInvariants (*surface_, nn_indices,
output.points[idx].j1, output.points[idx].j2, output.points[idx].j3);
}
}
template <typename PointInT, typename NormalOutT> void
pcl::MovingLeastSquaresOMP<PointInT, NormalOutT>::performReconstruction (PointCloudIn &output)
{
// Compute the number of coefficients
nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
#pragma omp parallel for schedule (dynamic, threads_)
// For all points
for (int cp = 0; cp < (int) indices_->size (); ++cp)
{
// 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;
// Get the initial estimates of point positions and their neighborhoods
if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
{
if (normals_)
normals_->points[cp].normal[0] = normals_->points[cp].normal[1] = normals_->points[cp].normal[2] = normals_->points[cp].curvature = std::numeric_limits<float>::quiet_NaN ();
continue;
}
// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
if (nn_indices.size () < 3)
continue;
Eigen::Vector4f model_coefficients;
// Get a plane approximating the local surface's tangent and project point onto it
computeMLSPointNormal (output.points[cp], *input_, nn_indices, nn_sqr_dists,
model_coefficients);
// Save results to output cloud
if (normals_)
{
normals_->points[cp].normal[0] = model_coefficients[0];
normals_->points[cp].normal[1] = model_coefficients[1];
normals_->points[cp].normal[2] = model_coefficients[2];
normals_->points[cp].curvature = model_coefficients[3];
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Check if input was set
if (!normals_)
{
ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (normals_->points.size () != surface_->points.size ())
{
ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset (%zu) differs from the number of points in the dataset containing the normals (%zu)!",
getClassName ().c_str (), surface_->points.size (), normals_->points.size ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
// Estimate the principal curvatures at each patch
computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2],
output.points[idx].pc1, output.points[idx].pc2);
}
}
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 NormalOutT> void
pcl::MovingLeastSquares<PointInT, NormalOutT>::performReconstruction (PointCloudIn &output)
{
if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
{
PCL_ERROR ("[pcl::%s::performReconstruction] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
output.width = output.height = 0;
output.points.clear ();
if (normals_)
{
normals_->width = normals_->height = 0;
normals_->points.clear ();
}
return;
}
// 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;
// Use original point positions for fitting
// \note no up/down/adapting-sampling or hole filling possible like this
output.points.resize (indices_->size ());
// Check if fake indices were used, otherwise the output loses its organized structure
if (!fake_indices_)
pcl::copyPointCloud (*input_, *indices_, output);
else
output = *input_;
// Resize the output normal dataset
if (normals_)
{
normals_->points.resize (output.points.size ());
normals_->width = output.width;
normals_->height = output.height;
normals_->is_dense = output.is_dense;
}
// For all points
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
// Get the initial estimates of point positions and their neighborhoods
///////////////////////////////////////////////////////////////////////
// Search for the nearest neighbors
if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
{
if (normals_)
normals_->points[cp].normal[0] = normals_->points[cp].normal[1] = normals_->points[cp].normal[2] = normals_->points[cp].curvature = std::numeric_limits<float>::quiet_NaN ();
continue;
}
// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
int k = nn_indices.size ();
if (k < 3)
continue;
// Get a plane approximating the local surface's tangent and project point onto it
//////////////////////////////////////////////////////////////////////////////////
// Compute the plane coefficients
Eigen::Vector4f model_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);
// Compute the 3x3 covariance matrix
pcl::computeCovarianceMatrix (*input_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
// The normalization is not necessary, since the eigenvectors from libeigen are already normalized
model_coefficients[0] = eigen_vectors (0, 0);
model_coefficients[1] = eigen_vectors (1, 0);
model_coefficients[2] = eigen_vectors (2, 0);
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
float curvature = 0;
// Compute the curvature surface change
float eig_sum = eigen_values.sum ();
if (eig_sum != 0)
curvature = fabs (eigen_values[0] / eig_sum);
// Projected point
Eigen::Vector3f point = output.points[cp].getVector3fMap ();
float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
point -= distance * model_coefficients.head<3> ();
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
if (polynomial_fit_ && k >= nr_coeff_)
{
// For easy change between float and double
typedef Eigen::Vector3d Evector3;
typedef Eigen::VectorXd Evector;
typedef Eigen::MatrixXd Ematrix;
// Get a copy of the plane normal easy access
Evector3 plane_normal = model_coefficients.head<3> ().cast<double> ();
// Update neighborhood, since point was projected, and computing relative
// positions. Note updating only distances for the weights for speed
std::vector<Evector3> de_meaned (k);
for (int ni = 0; ni < k; ++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] = de_meaned[ni].dot (de_meaned[ni]);
}
// Allocate matrices and vectors to hold the data used for the polynomial
// fit
Evector weight_vec_ (k);
Ematrix P_ (nr_coeff_, k);
Evector f_vec_ (k);
Evector c_vec_;
Ematrix P_weight_; // size will be (nr_coeff_, k);
Ematrix P_weight_Pt_ (nr_coeff_, nr_coeff_);
// Get local coordinate system (Darboux frame)
Evector3 v = plane_normal.unitOrthogonal ();
Evector3 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 (int ni = 0; ni < k; ++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_);
// Projection onto MLS surface along Darboux normal to the height at (0,0)
if (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 (normals_)
{
Evector3 n_a = u + plane_normal * c_vec_[order_ + 1];
Evector3 n_b = v + plane_normal * c_vec_[1];
model_coefficients.head<3> () = n_a.cross (n_b).cast<float> ();
model_coefficients.head<3> ().normalize ();
}
}
}
// Save results to output cloud
///////////////////////////////
output.points[cp].x = point[0];
output.points[cp].y = point[1];
output.points[cp].z = point[2];
if (normals_)
{
normals_->points[cp].normal[0] = model_coefficients[0];
normals_->points[cp].normal[1] = model_coefficients[1];
normals_->points[cp].normal[2] = model_coefficients[2];
normals_->points[cp].curvature = curvature;
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
// The RF is formed as this x_axis | y_axis | normal
Eigen::Map<Eigen::Vector3f> x_axis (rf);
Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
Eigen::Map<Eigen::Vector3f> normal (rf + 6);
// Find every point within specified search_radius_
std::vector<int> nn_indices;
std::vector<float> nn_dists;
const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
if (neighb_cnt == 0)
{
for (size_t i = 0; i < desc.size (); ++i)
desc[i] = std::numeric_limits<float>::quiet_NaN ();
memset (rf, 0, sizeof (rf[0]) * 9);
return (false);
}
float minDist = std::numeric_limits<float>::max ();
int minIndex = -1;
for (size_t i = 0; i < nn_indices.size (); i++)
{
if (nn_dists[i] < minDist)
{
minDist = nn_dists[i];
minIndex = nn_indices[i];
}
}
// Get origin point
Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
// Get origin normal
// Use pre-computed normals
normal = normals[minIndex].getNormalVector3fMap ();
// Compute and store the RF direction
x_axis[0] = rnd ();
x_axis[1] = rnd ();
x_axis[2] = rnd ();
if (!pcl::utils::equal (normal[2], 0.0f))
x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
else if (!pcl::utils::equal (normal[1], 0.0f))
x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
else if (!pcl::utils::equal (normal[0], 0.0f))
x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
x_axis.normalize ();
// Check if the computed x axis is orthogonal to the normal
assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
// Store the 3rd frame vector
y_axis = normal.cross (x_axis);
// For each point within radius
for (size_t ne = 0; ne < neighb_cnt; ne++)
{
if (pcl::utils::equal (nn_dists[ne], 0.0f))
continue;
// Get neighbours coordinates
Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
/// ----- Compute current neighbour polar coordinates -----
/// Get distance between the neighbour and the origin
float r = sqrt (nn_dists[ne]);
/// Project point into the tangent plane
Eigen::Vector3f proj;
pcl::geometry::project (neighbour, origin, normal, proj);
proj -= origin;
/// Normalize to compute the dot product
proj.normalize ();
/// Compute the angle between the projection and the x axis in the interval [0,360]
Eigen::Vector3f cross = x_axis.cross (proj);
float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
/// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
Eigen::Vector3f no = neighbour - origin;
no.normalize ();
float theta = normal.dot (no);
theta = pcl::rad2deg (acos (std::min (1.0f, std::max (-1.0f, theta))));
// Bin (j, k, l)
size_t j = 0;
size_t k = 0;
size_t l = 0;
// Compute the Bin(j, k, l) coordinates of current neighbour
for (size_t rad = 1; rad < radius_bins_+1; rad++)
{
if (r <= radii_interval_[rad])
{
j = rad-1;
break;
}
}
for (size_t ang = 1; ang < elevation_bins_+1; ang++)
{
if (theta <= theta_divisions_[ang])
{
k = ang-1;
break;
}
}
for (size_t ang = 1; ang < azimuth_bins_+1; ang++)
{
if (phi <= phi_divisions_[ang])
{
l = ang-1;
break;
}
}
// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
std::vector<float> neighbour_distances;
int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
// point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
if (point_density == 0)
continue;
float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
(k*radius_bins_) +
j];
assert (w >= 0.0);
if (w == std::numeric_limits<float>::infinity ())
PCL_ERROR ("Shape Context Error INF!\n");
if (w != w)
PCL_ERROR ("Shape Context Error IND!\n");
/// Accumulate w into correspondant Bin(j,k,l)
desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
} // end for each neighbour
// 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
memset (rf, 0, sizeof (rf[0]) * 9);
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
{
const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
std::vector<int> n_indices;
std::vector<float> n_sqr_distances;
searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);
Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
double distance = 0.0;
double sum = 0.0;
int valid_nn_points = 0;
for (size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
{
Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap ();
if (pt.head<3> () == central_point.head<3> ())
continue;
// Difference between current point and origin
vij.row (valid_nn_points) = (pt - central_point).cast<double> ();
vij (valid_nn_points, 3) = 0;
distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);
// Multiply vij * vij'
cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());
sum += distance;
valid_nn_points++;
}
if (valid_nn_points < 5)
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
cov_m /= sum;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
const double& e1c = solver.eigenvalues ()[0];
const double& e2c = solver.eigenvalues ()[1];
const double& e3c = solver.eigenvalues ()[2];
if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
// Disambiguation
Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
v1.head<3> () = solver.eigenvectors ().col (2);
v3.head<3> () = solver.eigenvectors ().col (0);
int plusNormal = 0, plusTangentDirection1=0;
for (int ne = 0; ne < valid_nn_points; ne++)
{
double dp = vij.row (ne).dot (v1);
if (dp >= 0)
plusTangentDirection1++;
dp = vij.row (ne).dot (v3);
if (dp >= 0)
plusNormal++;
}
//TANGENT
plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
if (plusTangentDirection1 == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v1) > 0)
plusTangentDirection1 ++;
if (plusTangentDirection1 < points/2+1)
v1 *= - 1;
} else if (plusTangentDirection1 < 0)
v1 *= - 1;
//Normal
plusNormal = 2*plusNormal - valid_nn_points;
if (plusNormal == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v3) > 0)
plusNormal ++;
if (plusNormal < points/2+1)
v3 *= - 1;
} else if (plusNormal < 0)
v3 *= - 1;
rf.row (0) = v1.head<3> ().cast<float> ();
rf.row (2) = v3.head<3> ().cast<float> ();
rf.row (1) = rf.row (2).cross (rf.row (0));
return (0.0f);
}
template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
{
pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
frames_->points[index].x_axis[1],
frames_->points[index].x_axis[2]);
//const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
frames_->points[index].z_axis[1],
frames_->points[index].z_axis[2]);
// Find every point within specified search_radius_
std::vector<int> nn_indices;
std::vector<float> nn_dists;
const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
// For each point within radius
for (size_t ne = 0; ne < neighb_cnt; ne++)
{
if (pcl::utils::equal(nn_dists[ne], 0.0f))
continue;
// Get neighbours coordinates
Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
// ----- Compute current neighbour polar coordinates -----
// Get distance between the neighbour and the origin
float r = std::sqrt (nn_dists[ne]);
// Project point into the tangent plane
Eigen::Vector3f proj;
pcl::geometry::project (neighbour, origin, normal, proj);
proj -= origin;
// Normalize to compute the dot product
proj.normalize ();
// Compute the angle between the projection and the x axis in the interval [0,360]
Eigen::Vector3f cross = x_axis.cross (proj);
float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
/// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
Eigen::Vector3f no = neighbour - origin;
no.normalize ();
float theta = normal.dot (no);
theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
/// Bin (j, k, l)
size_t j = 0;
size_t k = 0;
size_t l = 0;
/// Compute the Bin(j, k, l) coordinates of current neighbour
for (size_t rad = 1; rad < radius_bins_ + 1; rad++)
{
if (r <= radii_interval_[rad])
{
j = rad - 1;
break;
}
}
for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
{
if (theta <= theta_divisions_[ang])
{
k = ang - 1;
break;
}
}
for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
{
if (phi <= phi_divisions_[ang])
{
l = ang - 1;
break;
}
}
/// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
std::vector<float> neighbour_didtances;
float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
/// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
(k*radius_bins_) +
j];
assert (w >= 0.0);
if (w == std::numeric_limits<float>::infinity ())
PCL_ERROR ("Shape Context Error INF!\n");
if (w != w)
PCL_ERROR ("Shape Context Error IND!\n");
/// Accumulate w into correspondant Bin(j,k,l)
desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
} // end for each neighbour
}
template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
// Allocate enough space to hold the results
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudTarget input_corresp;
input_corresp.points.resize (indices_->size ());
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
// If the guessed transformation is non identity
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Resize the vector of distances between correspondences
std::vector<float> previous_correspondence_distances (indices_->size ());
correspondence_distances_.resize (indices_->size ());
while (!converged_) // repeat until convergence
{
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// And the previous set of distances
previous_correspondence_distances = correspondence_distances_;
int cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// Iterating over the entire index vector and find all correspondences
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
{
PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
source_indices[cnt] = (*indices_)[idx];
target_indices[cnt] = nn_indices[0];
cnt++;
}
// Save the nn_dists[0] to a global vector of distances
correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], (float)dist_threshold);
}
// Resize to the actual number of valid correspondences
source_indices.resize (cnt); target_indices.resize (cnt);
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
sac.setMaxIterations (1000);
// Compute the set of inliers
if (!sac.computeModel ())
{
source_indices_good = source_indices;
target_indices_good = target_indices;
}
else
{
std::vector<int> inliers;
// Get the inliers
sac.getInliers (inliers);
source_indices_good.resize (inliers.size ());
target_indices_good.resize (inliers.size ());
boost::unordered_map<int, int> source_to_target;
for (unsigned int i = 0; i < source_indices.size(); ++i)
source_to_target[source_indices[i]] = target_indices[i];
// Copy just the inliers
std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
for (size_t i = 0; i < inliers.size (); ++i)
target_indices_good[i] = source_to_target[inliers[i]];
}
}
// Check whether we have enough correspondences
cnt = (int)source_indices_good.size ();
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %lu points [100.0%%], RANSAC rejected: %lu [%f%%].\n", getClassName ().c_str (), cnt, (cnt * 100.0) / indices_->size (), (unsigned long)indices_->size (), (unsigned long)source_indices.size () - cnt, (source_indices.size () - cnt) * 100.0 / source_indices.size ());
// Estimate the transform
//rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);
// Tranform the data
transformPointCloud (output, output, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
nr_iterations_++;
// Update the vizualization of icp convergence
if (update_visualizer_ != 0)
update_visualizer_(output, source_indices_good, *target_, target_indices_good );
// Various/Different convergence termination criteria
// 1. Number of iterations has reached the maximum user imposed number of iterations (via
// setMaximumIterations)
// 2. The epsilon (difference) between the previous transformation and the current estimated transformation
// is smaller than an user imposed value (via setTransformationEpsilon)
// 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via
// setEuclideanFitnessEpsilon)
if (nr_iterations_ >= max_iterations_ ||
fabs ((transformation_ - previous_transformation_).sum ()) < transformation_epsilon_ ||
fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
)
{
converged_ = true;
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
getClassName ().c_str (), nr_iterations_, max_iterations_, fabs ((transformation_ - previous_transformation_).sum ()));
PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
PCL_DEBUG ("fabs ((transformation_ - previous_transformation_).sum ()) (%f) < transformation_epsilon_ (%f)\n",
fabs ((transformation_ - previous_transformation_).sum ()), transformation_epsilon_);
PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
euclidean_fitness_epsilon_);
}
}
}
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);
}
}
}
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;
#ifdef _OPENMP
// (Maximum) number of threads
const unsigned int threads = threads_ == 0 ? 1 : threads_;
// Create temporaries for each thread in order to avoid synchronization
typename PointCloudOut::CloudVectorType projected_points (threads);
typename NormalCloud::CloudVectorType projected_points_normals (threads);
std::vector<PointIndices> corresponding_input_indices (threads);
#endif
// For all points
#ifdef _OPENMP
#pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
#endif
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
{
// 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;
// Get the initial estimates of point positions and their neighborhoods
if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
{
// Check the number of nearest neighbors for normal estimation (and later for polynomial fit as well)
if (nn_indices.size () >= 3)
{
// This thread's ID (range 0 to threads-1)
#ifdef _OPENMP
const int tn = omp_get_thread_num ();
// Size of projected points before computeMLSPointNormal () adds points
size_t pp_size = projected_points[tn].size ();
#else
PointCloudOut projected_points;
NormalCloud projected_points_normals;
#endif
// Get a plane approximating the local surface's tangent and project point onto it
const int index = (*indices_)[cp];
size_t mls_result_index = 0;
if (cache_mls_results_)
mls_result_index = index; // otherwise we give it a dummy location.
#ifdef _OPENMP
computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
// Copy all information from the input cloud to the output points (not doing any interpolation)
for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]);
#else
computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
// 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 ());
#endif
}
}
}
#ifdef _OPENMP
// Combine all threads' results into the output vectors
for (unsigned int tn = 0; tn < threads; ++tn)
{
output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
if (compute_normals_)
normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
}
#endif
// Perform the distinct-cloud or voxel-grid upsampling
performUpsampling (output);
}