已經大半年沒更新博客了，記得去年開始寫與點雲相關的第一篇博客時，就信誓旦旦要堅守下去。這一篇技術博客姍姍來遲了些，正如2002年的第一場雪一樣，不過9102年武漢的第一場雪已經來過，近半年狀態的持續低迷，以及瑣事纏身根本無法潛心對一些技術進行專研，其實最初寫博客的初衷,其一，更多的是表明一種活到老學到老的人生態度，無論何種狀態下都要保持對知識技能的高度認可；其二，也是把自己在校期間的一些學習資源甚至是學習方法與網友們共享，如果以生產實際這個標准來衡量我的博客的話真心沒有多大的價值可言，更多的還是為初學者提供入門的基礎知識以及給予他們在科研、算法這條被玄幻籠罩的路上有堅持走下去的決心與力量。
廢話到此為止，這也表明今天這篇博客屬於“失蹤人口回歸”系列，法線在點雲里作為表述點雲特征的一種重要信息，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平面上的幾個點求解的。