点云中点法向量

计算步骤：

找到点pi相邻点集合S所有点Vi,然后去中心化,并构造协方差矩阵,公式如下:

二维点云该点曲率计算方法：

三维点云该点曲率计算方法：

$\frac{\lambda _{1}}{\lambda _{1}+\lambda_{2}+\lambda_{3} }$

最小特征值对应的特征向量就是点的法向量

Eigen::Vector2d ComputeNormal(std::vector<Eigen::Vector2d> &nearPoints)
{Eigen::Vector2d normal;//计算激光点法向量，NICP计算法向量的方法Eigen::Vector2d average;//周围点的几何中心average.setZero();//置0for(auto v : nearPoints)//遍历每个点{average += v / nearPoints.size();//求其周围点的几何中心}Eigen::Matrix2d covariance;//协方差矩阵covariance.setZero();//置0for(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();//置0for(auto v : nearPoints)//遍历每个点{center += v / nearPoints.size();//求其周围点的几何中心}Eigen::Matrix3d covMat;//协方差矩阵covMat.setZero();//置0for(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 planebool 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