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 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 PointT> void
pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
int index,
const std::vector<int> &nn_indices,
double search_radius,
int polynomial_order,
boost::function<double(const double)> weight_func)
{
// Compute the plane coefficients
EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
Eigen::Vector4d xyz_centroid;
// Estimate the XYZ centroid
pcl::compute3DCentroid (cloud, nn_indices, xyz_centroid);
// Compute the 3x3 covariance matrix
pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix);
EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
Eigen::Vector4d model_coefficients (0, 0, 0, 0);
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
model_coefficients.head<3> ().matrix () = eigen_vector;
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Projected query point
valid = true;
query_point = cloud.points[index].getVector3fMap ().template cast<double> ();
double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
mean = query_point - distance * model_coefficients.head<3> ();
curvature = covariance_matrix.trace ();
// Compute the curvature surface change
if (curvature != 0)
curvature = std::abs (eigen_value / curvature);
// Get a copy of the plane normal easy access
plane_normal = model_coefficients.head<3> ();
// Local coordinate system (Darboux frame)
v_axis = plane_normal.unitOrthogonal ();
u_axis = plane_normal.cross (v_axis);
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
num_neighbors = static_cast<int> (nn_indices.size ());
order = polynomial_order;
if (order > 1)
{
int nr_coeff = (order + 1) * (order + 2) / 2;
if (num_neighbors >= nr_coeff)
{
// Note: The max_sq_radius parameter is only used if weight_func was not defined
double max_sq_radius = 1;
if (weight_func == 0)
{
max_sq_radius = search_radius * search_radius;
weight_func = boost::bind (&pcl::MLSResult::computeMLSWeight, this, _1, max_sq_radius);
}
// Allocate matrices and vectors to hold the data used for the polynomial fit
Eigen::VectorXd weight_vec (num_neighbors);
Eigen::MatrixXd P (nr_coeff, num_neighbors);
Eigen::VectorXd f_vec (num_neighbors);
Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
Eigen::MatrixXd P_weight_Pt (nr_coeff, 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, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
for (size_t ni = 0; ni < (size_t) num_neighbors; ++ni)
{
de_meaned[ni][0] = cloud.points[nn_indices[ni]].x - mean[0];
de_meaned[ni][1] = cloud.points[nn_indices[ni]].y - mean[1];
de_meaned[ni][2] = cloud.points[nn_indices[ni]].z - mean[2];
weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
}
// 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 < (size_t) num_neighbors; ++ni)
{
// Transforming coordinates
u_coord = de_meaned[ni].dot (u_axis);
v_coord = de_meaned[ni].dot (v_axis);
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);
}
}
}
