点云中点法向量
计算步骤：
找到点pi相邻点集合S所有点Vi,然后去中心化,并构造协方差矩阵,公式如下:
二维点云该点曲率计算方法：
三维点云该点曲率计算方法：
最小特征值对应的特征向量就是点的法向量
Eigen::Vector2d ComputeNormal(std::vector<Eigen::Vector2d> &nearPoints)
{
Eigen::Vector2d normal;
//计算激光点法向量，NICP计算法向量的方法
Eigen::Vector2d average;//周围点的几何中心
average.setZero();//置0
for(auto v : nearPoints)//遍历每个点
{
average += v / nearPoints.size();//求其周围点的几何中心
}
Eigen::Matrix2d covariance;//协方差矩阵
covariance.setZero();//置0
for(auto v : nearPoints)//遍历每个点
{
covariance += (v - average) * (v - average).transpose() / nearPoints.size();//求协方差矩阵
}
Eigen::EigenSolver<Eigen::Matrix2d> eigen_solver(covariance);//转化为对角线矩阵
Eigen::Vector2d eigenValues = eigen_solver.pseudoEigenvalueMatrix().diagonal();//特征值，协方差所有特征值构成向量
Eigen::Matrix2d eigenVectors = eigen_solver.pseudoEigenvectors();//特征向量，协方差所有特征向量构成矩阵
normal = eigenValues(0) < eigenValues(1) ? eigenVectors.col(0) : eigenVectors.col(1);//计算法向量
return normal;
}
点拟合的直线
相邻点集合S拟合成一条直线。
计算步骤：
计算出点集合协方差矩阵M（也就是上面的计算公式），特征向量为E，特征值为V，那么中一个特征值就会明显比其他两个大，最大特征值所对应特征向量就是直线的方向。（特征值：两小一大，大方向）
Eigen::Vector3d ComputeLineNormal(std::vector<Eigen::Vector3d> &nearPoints)
{
//计算激光点直线法向量
Eigen::Vector3d center;//周围点的几何中心
center.setZero();//置0
for(auto v : nearPoints)//遍历每个点
{
center += v / nearPoints.size();//求其周围点的几何中心
}
Eigen::Matrix3d covMat;//协方差矩阵
covMat.setZero();//置0
for(auto v : nearPoints)//遍历每个点
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = v - center;
covariance += tmpZeroMean * tmpZeroMean.transpose();//求协方差矩阵
}
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); //最大特征值对应的特征向量->边缘线方向
// 最大特征值大于次大特征值的3倍认为是线特征
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 根据拟合出来的线特征方向，以平均点为中心构建两个虚拟点，代替一条直线
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
}
return unit_direction; //返回线的法向量
}点拟合平面
如果点集合S在一块平面上，
计算步骤：
计算出点集合协方差矩阵M（也就是上面的计算公式），特征向量为E，特征值为V，那么其中一个特征值就会明显比其他两个小，最小特征值所对应特征向量就是平面法向量。（特征值：两大一小，小方向）
//假设传进来的nearPoints有五个点
void ComputePlaneNormal(std::vector<Eigen::Vector3d> &nearPoints)
{
//平面方程 Ax + By + Cz + 1 =0 平面法向量(A,B,C)[待求] 已知5个点求解3个未知数，构建超定方程AX = b求解平面法向量;
Eigen::Matrix<double, 5, 3> matA0; //A = 5 * 3 系数矩阵方程
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = nearPoints[j][0];
matA0(j, 1) = nearPoints[j][1];
matA0(j, 2) = nearPoints[j][2];
}
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);//平面法向量求解
double negative_OA_dot_norm = 1 / norm.norm();
norm.normalize(); //法向量单位化 B.normalize() = B / B.norm() B 是个向量
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
// 根据求出来的平面方程进行校验，看看是不是符合平面约束
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well
//点到平面的距离公式： Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
//由于前面已经进行归一化，将系数除以分母，所以可以直接代点进去求距离
if (fabs(norm(0) * nearPoints[j][0] +norm(1) * nearPoints[j][1] + norm(2) * nearPoints[j][2] + negative_OA_dot_norm) > 0.2)
{
planeValid = false;
break;
}
}
}参考：点云配准方法---ICP升级版本NICP(Normal ICP) - 古月居
GitHub - HKUST-Aerial-Robotics/A-LOAM: Advanced implementation of LOAM
