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>::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>::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>::performUpsampling (PointCloudOut &output)
{
if (upsample_method_ == DISTINCT_CLOUD)
{
corresponding_input_indices_.reset (new PointIndices);
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> ();
MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
}
}
// 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)
{
corresponding_input_indices_.reset (new PointIndices);
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> ();
MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
}
}
}
