pcl最小二乘求点云平面度

Cat_AC

已于 2024-01-30 12:04:37 修改

阅读量711

点赞数 18

文章标签：平面

于 2024-01-26 15:50:53 首次发布

本文链接：https://blog.csdn.net/weixin_41674673/article/details/135866043

版权

平面方程

$z=ax+by+c$

N个点,坐标为

$(x_i,y_i,z_i)$

点到平面的距离

$d_i=\frac{ax_i+by_i+c-z_i}{\sqrt{a^2+b^2+1}}$

平面度

$F=d_{max}-d_{min}$

目标函数

$f(a,b,c) = \sum (ax_i+y_i+x-z_i)^2$

偏导为0

$\frac{\partial f }{\partial a}=\frac{\partial f }{\partial b}=\frac{\partial f }{\partial c}=0$

得到矩阵

$\begin{bmatrix} \sum x_i^2 & \sum x_iy_i & \sum x_i \\ \sum x_iy_i & \sum y_i^2&\sum y_i \\ \sum x_i& \sum y_i &N \end{bmatrix}\begin{bmatrix} a\\ b\\ c \end{bmatrix}=\begin{bmatrix} \sum x_iz_i \\ \sum z_iy_i \\ \sum z_i \end{bmatrix}$

令

$A=\begin{bmatrix} \sum x_i^2 & \sum x_iy_i & \sum x_i \\ \sum x_iy_i & \sum y_i^2&\sum y_i \\ \sum x_i& \sum y_i &N \end{bmatrix}$ , $X=\begin{bmatrix} a\\ b\\ c \end{bmatrix}$ , $b=\begin{bmatrix} \sum x_iz_i \\ \sum z_iy_i \\ \sum z_i \end{bmatrix}$

解下来就是解线性方程的问题了,得到的解X就是平面系数.

接下来求得所有点到该平面的距离(这里的距离要带方向,区分平面上下的点),将距平面上方一个最远的点和平面上方最远的点作差即可,也就是最大距离减去最小距离.

该方法只适合点云数量在千万级以下的,在千万级以上的需要另做处理.

附上部分代码.

float LS(pcl::PointCloud<PointT>::Ptr cloud) {
    pcl::console::TicToc time; time.tic();

    auto xyz_id = cloud->getMatrixXfMap(3, 4, 0);

    Eigen::Vector3f xyz_id_mean = xyz_id.rowwise().mean();                                 
    Eigen::MatrixXf xyz_id_centriod = xyz_id.colwise() - xyz_id_mean;

    Eigen::Vector3f xyz_all_Sum = xyz_id_centriod.rowwise().sum();                          //计算最小二乘法里矩阵A以及b的各元素值
    Eigen::Vector3f xyz_all_Pow2_Sum = xyz_id_centriod.array().square().rowwise().sum();
    auto xy_all_sum = xyz_id_centriod.row(0).dot(xyz_id_centriod.row(1).transpose());
    auto xz_all_sum = xyz_id_centriod.row(0).dot(xyz_id_centriod.row(2).transpose());
    auto yz_all_sum = xyz_id_centriod.row(1).dot(xyz_id_centriod.row(2).transpose());

    Eigen::Matrix3f A;
    A << xyz_all_Pow2_Sum(0), xy_all_sum, xyz_all_Sum(0),
        xy_all_sum, xyz_all_Pow2_Sum(1), xyz_all_Sum(1),
        xyz_all_Sum(0), xyz_all_Sum(1), cloud->size();
    Eigen::Vector3f B;
    B << xz_all_sum, yz_all_sum, xyz_all_Sum(2);
    Eigen::Vector3f X = A.lu().solve(B);                                                    

    Eigen::Vector4f plane;
    plane << X(0), X(1), -1, X(2);                                                          
    float dataMin = INT_MAX, dataMax = INT_MIN;

    for (int j = 0; j < cloud->size(); j++) {//这里可以用矩阵乘法代替for循环                                               
        Eigen::Vector4f point;
        point << xyz_id_centriod.col(j), 1;
        float distanceCur = (point.dot(plane)) / plane.block<3, 1>(0, 0).norm();
        dataMin = dataMin > distanceCur ? distanceCur : dataMin;
        dataMax = dataMax < distanceCur ? distanceCur : dataMax;
    }

    std::cout << "LS用时：" << time.toc() << "ms" << endl;
    return dataMax - dataMin;
}