点云法向量估计方法
已经大半年没更新博客了，今天来更新一下博客。法线在点云里作为表述点云特征的一种重要信息，pcl提供了成熟的接口。当然自己去实现，算法难度也不大，为了熟悉pcl源码，博主之前对pcl源码求取法线的代码进行了剥离，同时对算法的实现流程做了一个简答的表述，让法线从此不再神秘。
点云法向量的估计流程
1.对于第一步就比较常规了，建议大家直接使用pcl的kdtree或者octree进行最近邻搜索
2.求协方差矩阵，这里贴一段代码（当然也可以自己实现，难度不大）
//协方差矩阵求解方法 unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud<PointXYZ> &cloud, Eigen::Matrix<double, 3, 3> &covariance_matrix, Eigen::Matrix<double, 4, 1> ¢roid) { // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::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<double> (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<unsigned int> (point_count)); }
3.特征值以及对应的特征向量的求解（贴一段代码）
说明一下eigen也提供了相关的方法，博主这里对pcl的源码进行了整理，二者所求结果不尽一致，孰优孰劣大家可以在日常使用中去进行检验。
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
//#include <pcl/common/centroid.h>
//#include <pcl/common/eigen.h>
using namespace Eigen;
using namespace std;
//using namespace pcl;
void computeRoots2(const Matrix3d::Scalar& b, const Matrix3d::Scalar& c, Eigen::Vector3d& roots)
{
roots(0) = Matrix3d::Scalar(0);
Matrix3d::Scalar d = Matrix3d::Scalar(b * b - 4.0 * c);
if (d < 0.0) // no real roots ! THIS SHOULD NOT HAPPEN!
d = 0.0;
Matrix3d::Scalar sd = ::std::sqrt(d);
roots(2) = 0.5f * (b + sd);
roots(1) = 0.5f * (b - sd);
}
void computeRoots(const Matrix3d& m, Eigen::Vector3d& roots)
{
typedef Matrix3d::Scalar Scalar;
// The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
// eigenvalues are the roots to this equation, all guaranteed to be
// real-valued, because the matrix is symmetric.
Scalar c0 = m(0, 0) * m(1, 1) * m(2, 2)
+ Scalar(2) * m(0, 1) * m(0, 2) * m(1, 2)
- m(0, 0) * m(1, 2) * m(1, 2)
- m(1, 1) * m(0, 2) * m(0, 2)
- m(2, 2) * m(0, 1) * m(0, 1);
Scalar c1 = m(0, 0) * m(1, 1) -
m(0, 1) * m(0, 1) +
m(0, 0) * m(2, 2) -
m(0, 2) * m(0, 2) +
m(1, 1) * m(2, 2) -
m(1, 2) * m(1, 2);
Scalar c2 = m(0, 0) + m(1, 1) + m(2, 2);
if (fabs(c0) < Eigen::NumTraits < Scalar > ::epsilon()) // one root is 0 -> quadratic equation
computeRoots2(c2, c1, roots);
else
{
const Scalar s_inv3 = Scalar(1.0 / 3.0);
const Scalar s_sqrt3 = std::sqrt(Scalar(3.0));
// Construct the parameters used in classifying the roots of the equation
// and in solving the equation for the roots in closed form.
Scalar c2_over_3 = c2 * s_inv3;
Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
if (a_over_3 > Scalar(0))
a_over_3 = Scalar(0);
Scalar half_b = Scalar(0.5) * (c0 + c2_over_3 * (Scalar(2) * c2_over_3 * c2_over_3 - c1));
Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
if (q > Scalar(0))
q = Scalar(0);
// Compute the eigenvalues by solving for the roots of the polynomial.
Scalar rho = std::sqrt(-a_over_3);
Scalar theta = std::atan2(std::sqrt(-q), half_b) * s_inv3;
Scalar cos_theta = std::cos(theta);
Scalar sin_theta = std::sin(theta);
roots(0) = c2_over_3 + Scalar(2) * rho * cos_theta;
roots(1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
roots(2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
// Sort in increasing order.
if (roots(0) >= roots(1))
std::swap(roots(0), roots(1));
if (roots(1) >= roots(2))
{
std::swap(roots(1), roots(2));
if (roots(0) >= roots(1))
std::swap(roots(0), roots(1));
}
if (roots(0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
computeRoots2(c2, c1, roots);
}
}
void eigen33zx(const Matrix3d& mat, Matrix3d& evecs, Eigen::Vector3d& evals)
{
typedef Matrix3d::Scalar Scalar;
// typedef Matrix3d::Scalar Scalar;
// Scale the matrix so its entries are in [-1,1]. The scaling is applied
// only when at least one matrix entry has magnitude larger than 1.
Scalar scale = mat.cwiseAbs().maxCoeff();
if (scale <= std::numeric_limits < Scalar > ::min())
scale = Scalar(1.0);
Matrix3d scaledMat = mat / scale;
// Compute the eigenvalues
computeRoots(scaledMat, evals);
if ((evals(2) - evals(0)) <= Eigen::NumTraits < Scalar > ::epsilon())
{
// all three equal
evecs.setIdentity();
}
else if ((evals(1) - evals(0)) <= Eigen::NumTraits < Scalar > ::epsilon())
{
// first and second equal
Matrix3d tmp;
tmp = scaledMat;
tmp.diagonal().array() -= evals(2);
Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));
Scalar len1 = vec1.squaredNorm();
Scalar len2 = vec2.squaredNorm();
Scalar len3 = vec3.squaredNorm();
if (len1 >= len2 && len1 >= len3)
evecs.col(2) = vec1 / std::sqrt(len1);
else if (len2 >= len1 && len2 >= len3)
evecs.col(2) = vec2 / std::sqrt(len2);
else
evecs.col(2) = vec3 / std::sqrt(len3);
evecs.col(1) = evecs.col(2).unitOrthogonal();
evecs.col(0) = evecs.col(1).cross(evecs.col(2));
}
else if ((evals(2) - evals(1)) <= Eigen::NumTraits < Scalar > ::epsilon())
{
// second and third equal
Matrix3d tmp;
tmp = scaledMat;
tmp.diagonal().array() -= evals(0);
Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));
Scalar len1 = vec1.squaredNorm();
Scalar len2 = vec2.squaredNorm();
Scalar len3 = vec3.squaredNorm();
if (len1 >= len2 && len1 >= len3)
evecs.col(0) = vec1 / std::sqrt(len1);
else if (len2 >= len1 && len2 >= len3)
evecs.col(0) = vec2 / std::sqrt(len2);
else
evecs.col(0) = vec3 / std::sqrt(len3);
evecs.col(1) = evecs.col(0).unitOrthogonal();
evecs.col(2) = evecs.col(0).cross(evecs.col(1));
}
else
{
Matrix3d tmp;
tmp = scaledMat;
tmp.diagonal().array() -= evals(2);
Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));
Scalar len1 = vec1.squaredNorm();
Scalar len2 = vec2.squaredNorm();
Scalar len3 = vec3.squaredNorm();
#ifdef _WIN32
Scalar *mmax = new Scalar[3];
#else
Scalar mmax[3];
#endif
unsigned int min_el = 2;
unsigned int max_el = 2;
if (len1 >= len2 && len1 >= len3)
{
mmax[2] = len1;
evecs.col(2) = vec1 / std::sqrt(len1);
}
else if (len2 >= len1 && len2 >= len3)
{
mmax[2] = len2;
evecs.col(2) = vec2 / std::sqrt(len2);
}
else
{
mmax[2] = len3;
evecs.col(2) = vec3 / std::sqrt(len3);
}
tmp = scaledMat;
tmp.diagonal().array() -= evals(1);
vec1 = tmp.row(0).cross(tmp.row(1));
vec2 = tmp.row(0).cross(tmp.row(2));
vec3 = tmp.row(1).cross(tmp.row(2));
len1 = vec1.squaredNorm();
len2 = vec2.squaredNorm();
len3 = vec3.squaredNorm();
if (len1 >= len2 && len1 >= len3)
{
mmax[1] = len1;
evecs.col(1) = vec1 / std::sqrt(len1);
min_el = len1 <= mmax[min_el] ? 1 : min_el;
max_el = len1 > mmax[max_el] ? 1 : max_el;
}
else if (len2 >= len1 && len2 >= len3)
{
mmax[1] = len2;
evecs.col(1) = vec2 / std::sqrt(len2);
min_el = len2 <= mmax[min_el] ? 1 : min_el;
max_el = len2 > mmax[max_el] ? 1 : max_el;
}
else
{
mmax[1] = len3;
evecs.col(1) = vec3 / std::sqrt(len3);
min_el = len3 <= mmax[min_el] ? 1 : min_el;
max_el = len3 > mmax[max_el] ? 1 : max_el;
}
tmp = scaledMat;
tmp.diagonal().array() -= evals(0);
vec1 = tmp.row(0).cross(tmp.row(1));
vec2 = tmp.row(0).cross(tmp.row(2));
vec3 = tmp.row(1).cross(tmp.row(2));
len1 = vec1.squaredNorm();
len2 = vec2.squaredNorm();
len3 = vec3.squaredNorm();
if (len1 >= len2 && len1 >= len3)
{
mmax[0] = len1;
evecs.col(0) = vec1 / std::sqrt(len1);
min_el = len3 <= mmax[min_el] ? 0 : min_el;
max_el = len3 > mmax[max_el] ? 0 : max_el;
}
else if (len2 >= len1 && len2 >= len3)
{
mmax[0] = len2;
evecs.col(0) = vec2 / std::sqrt(len2);
min_el = len3 <= mmax[min_el] ? 0 : min_el;
max_el = len3 > mmax[max_el] ? 0 : max_el;
}
else
{
mmax[0] = len3;
evecs.col(0) = vec3 / std::sqrt(len3);
min_el = len3 <= mmax[min_el] ? 0 : min_el;
max_el = len3 > mmax[max_el] ? 0 : max_el;
}
unsigned mid_el = 3 - min_el - max_el;
evecs.col(min_el) = evecs.col((min_el + 1) % 3).cross(evecs.col((min_el + 2) % 3)).normalized();
evecs.col(mid_el) = evecs.col((mid_el + 1) % 3).cross(evecs.col((mid_el + 2) % 3)).normalized();
#ifdef _WIN32
delete[] mmax;
#endif
}
// Rescale back to the original size.
evals *= scale;
}
4.根据3中的结果即可实现法向量的估计
上面的代码抽取的pcl的源码，接口较多，不过也间接说明了底层算法人员的不易，其实每一个成熟稳定的接口都来自于底层算法人员长期努力的结果，这里向那些开源的算法库致敬。
随便写的有点混乱，我还是来一个例子，对于近邻搜索这一块，网上例子铺天盖地，先略过。
Matrix3d A;
A << 6, -3,0, -3,6, 0, 0, 0, 0;
cout << "Here is a 3x3 matrix, A:" << endl << A << endl << endl;
Eigen:: Vector3d eigen_values;
// pcl::eigen33(A, eigen_values);//pcl的接口
Matrix3d eigenVectors1;
eigen33zx(A, eigenVectors1, eigen_values);
cout << eigenVectors1 << endl;
cout << eigen_values << endl;
上面的矩阵A是一个协方差矩阵，是博主为了检验该方法是否准确，自己随便用XoY平面上的几个点求解的。
