PCL 法向量估计源码学习
一、思路：
二、源码
#ifndef PCL_FEATURES_IMPL_NORMAL_3D_H_
#define PCL_FEATURES_IMPL_NORMAL_3D_H_
#include
///
template void
pcl::NormalEstimation::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector nn_indices (k_);
std::vector nn_dists (k_);
output.is_dense = true;
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
// 如果云设置为密集，则不检查每个点的NaN/Inf值，从而节省几个周期
if (input_->is_dense)
{
// Iterating over the entire index vector
// 迭代索引，计算每个索引对应点的 法向量
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
// 判断点云领域内是否有点，并且计算点云的法向量 和 曲率
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
!computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN ();
output.is_dense = false;
continue;
}
// 设置视角点 可以理解为法相的朝向问题
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
else
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
!computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN ();
output.is_dense = false;
continue;
}
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
}
#define PCL_INSTANTIATE_NormalEstimation(T,NT) template class PCL_EXPORTS pcl::NormalEstimation;
#endif // PCL_FEATURES_IMPL_NORMAL_3D_H_
1、 computePointNormal计算协方差矩阵和曲率
template inline bool
computePointNormal (const pcl::PointCloud &cloud, const std::vector &indices,
Eigen::Vector4f &plane_parameters, float &curvature)
{
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// 计算每个点的协方差矩阵和质心
if (indices.size () < 3 ||
computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0)
{
plane_parameters.setConstant (std::numeric_limits::quiet_NaN ());
curvature = std::numeric_limits::quiet_NaN ();
return false;
}
// Get the plane normal and surface curvature
// 通过协方差矩阵和质心来计算点云的 局部平面 的法相 nx ny nz 和 局部曲率
solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
return true;
}
inline bool
computePointNormal (const pcl::PointCloud &cloud, const std::vector &indices,
float &nx, float &ny, float &nz, float &curvature)
{
if (indices.size () < 3 ||
computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0)
{
nx = ny = nz = curvature = std::numeric_limits::quiet_NaN ();
return false;
}
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
return true;
}
1.1 computeMeanAndCovarianceMatrix 计算协方差矩阵
//
template inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud,
Eigen::Matrix &covariance_matrix,
Eigen::Matrix ¢roid)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix accu = Eigen::Matrix::Zero ();
size_t point_count;
if (cloud.is_dense)
{
point_count = cloud.size ();
// For each point in the cloud
for (size_t i = 0; i < point_count; ++i)
{
accu [0] += cloud[i].x * cloud[i].x;
accu [1] += cloud[i].x * cloud[i].y;
accu [2] += cloud[i].x * cloud[i].z;
accu [3] += cloud[i].y * cloud[i].y; // 4
accu [4] += cloud[i].y * cloud[i].z; // 5
accu [5] += cloud[i].z * cloud[i].z; // 8
accu [6] += cloud[i].x;
accu [7] += cloud[i].y;
accu [8] += cloud[i].z;
}
}
else
{
point_count = 0;
for (size_t i = 0; i < cloud.size (); ++i)
{
if (!isFinite (cloud[i]))
continue;
accu [0] += cloud[i].x * cloud[i].x;
accu [1] += cloud[i].x * cloud[i].y;
accu [2] += cloud[i].x * cloud[i].z;
accu [3] += cloud[i].y * cloud[i].y;
accu [4] += cloud[i].y * cloud[i].z;
accu [5] += cloud[i].z * cloud[i].z;
accu [6] += cloud[i].x;
accu [7] += cloud[i].y;
accu [8] += cloud[i].z;
++point_count;
}
}
accu /= static_cast (point_count);
if (point_count != 0)
{
//centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
centroid[3] = 1;
covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
}
return (static_cast (point_count));
}
1.2 solvePlaneParameters计算平面法向量和曲率
//
inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
const Eigen::Vector4f &point,
Eigen::Vector4f &plane_parameters, float &curvature)
{
solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);
plane_parameters[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
plane_parameters[3] = -1 * plane_parameters.dot (point);
}
//
inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
float &nx, float &ny, float &nz, float &curvature)
{
// Avoid getting hung on Eigen's optimizers
// for (int i = 0; i < 9; ++i)
// if (!pcl_isfinite (covariance_matrix.coeff (i)))
// {
// //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
// nx = ny = nz = curvature = std::numeric_limits::quiet_NaN ();
// return;
// }
// Extract the smallest eigenvalue and its eigenvector
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
nx = eigen_vector [0];
ny = eigen_vector [1];
nz = eigen_vector [2];
// Compute the curvature surface change
float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
if (eig_sum != 0)
curvature = fabsf (eigen_value / eig_sum);
else
curvature = 0;
}
2、 flipNormalTowardsViewpoint法向量的方向问题
/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
* \param point a given point
* \param vp_x the X coordinate of the viewpoint
* \param vp_y the X coordinate of the viewpoint
* \param vp_z the X coordinate of the viewpoint
* \param normal the plane normal to be flipped
* \ingroup features
*/
template inline void
flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
Eigen::Matrix& normal)
{
Eigen::Matrix vp (vp_x - point.x, vp_y - point.y, vp_z - point.z, 0);
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = vp.dot (normal);
// Flip the plane normal
if (cos_theta < 0)
{
normal *= -1;
normal[3] = 0.0f;
// Hessian form (D = nc . p_plane (centroid here) + p)
normal[3] = -1 * normal.dot (point.getVector4fMap ());
}
}
/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
* \param point a given point
* \param vp_x the X coordinate of the viewpoint
* \param vp_y the X coordinate of the viewpoint
* \param vp_z the X coordinate of the viewpoint
* \param normal the plane normal to be flipped
* \ingroup features
*/
template inline void
flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
Eigen::Matrix& normal)
{
Eigen::Matrix vp (vp_x - point.x, vp_y - point.y, vp_z - point.z);
// Flip the plane normal
if (vp.dot (normal) < 0)
normal *= -1;
}
/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
* \param point a given point
* \param vp_x the X coordinate of the viewpoint
* \param vp_y the X coordinate of the viewpoint
* \param vp_z the X coordinate of the viewpoint
* \param nx the resultant X component of the plane normal
* \param ny the resultant Y component of the plane normal
* \param nz the resultant Z component of the plane normal
* \ingroup features
*/
template inline void
flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
float &nx, float &ny, float &nz)
{
// See if we need to flip any plane normals
vp_x -= point.x;
vp_y -= point.y;
vp_z -= point.z;
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
// Flip the plane normal
if (cos_theta < 0)
{
nx *= -1;
ny *= -1;
nz *= -1;
}
}
2.2 设置参数
inline void
setViewPoint (float vpx, float vpy, float vpz)
{
vpx_ = vpx;
vpy_ = vpy;
vpz_ = vpz;
use_sensor_origin_ = false;
}
/** \brief Get the viewpoint.
* \param [out] vpx x-coordinate of the view point
* \param [out] vpy y-coordinate of the view point
* \param [out] vpz z-coordinate of the view point
* \note this method returns the currently used viewpoint for normal flipping.
* If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates.
* If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0)
*/
inline void
getViewPoint (float &vpx, float &vpy, float &vpz)
{
vpx = vpx_;
vpy = vpy_;
vpz = vpz_;
}
inline void
useSensorOriginAsViewPoint ()
{
use_sensor_origin_ = true;
if (input_)
{
vpx_ = input_->sensor_origin_.coeff (0);
vpy_ = input_->sensor_origin_.coeff (1);
vpz_ = input_->sensor_origin_.coeff (2);
}
else
{
vpx_ = 0;
vpy_ = 0;
vpz_ = 0;
}
}