求取平面参数

最新推荐文章于 2024-08-01 20:59:33 发布

妄想出头的工业炼药师

最新推荐文章于 2024-08-01 20:59:33 发布

阅读量167

点赞数

分类专栏：算法文章标签：人工智能自动驾驶算法

本文链接：https://blog.csdn.net/z15f34/article/details/131057700

版权

算法专栏收录该内容

18 篇文章 0 订阅

订阅专栏

平面方程AX+BY+CZ+D=0

变换一下-> A/DX + B/DY+C/DZ= -1

该方程满足Ax=b

其中A为 $\begin{bmatrix} x\\ y\\ z \end{bmatrix}$ , x为 $\begin{bmatrix} \frac{A}{D}\\ \frac{B}{D}\\ \frac{C}{D} \end{bmatrix}$ ,b为-1单位阵

求解代码如下:

/* comment
plane equation: Ax + By + Cz + D = 0
convert to: A/D*x + B/D*y + C/D*z = -1
solve: A0*x0 = b0
where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T
normvec:  normalized x0
*/
template<typename T>
bool esti_normvector(Matrix<T, 3, 1> &normvec, const PointVector &point, const T &threshold, const int &point_num)
{
    MatrixXf A(point_num, 3);
    MatrixXf b(point_num, 1);
    b.setOnes();
    b *= -1.0f;

    for (int j = 0; j < point_num; j++)
    {
        A(j,0) = point[j].x;
        A(j,1) = point[j].y;
        A(j,2) = point[j].z;
    }
    normvec = A.colPivHouseholderQr().solve(b);
    
    for (int j = 0; j < point_num; j++)
    {
        if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
        {
            return false;
        }
    }

    normvec.normalize();
    return true;
}

template<typename T>
bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
{
    Matrix<T, NUM_MATCH_POINTS, 3> A;
    Matrix<T, NUM_MATCH_POINTS, 1> b;
    b.setOnes();
    b *= -1.0f;

    for (int j = 0; j < NUM_MATCH_POINTS; j++)
    {
        A(j,0) = point[j].x;
        A(j,1) = point[j].y;
        A(j,2) = point[j].z;
    }

    Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);

    T n = normvec.norm();
    pca_result(0) = normvec(0) / n;
    pca_result(1) = normvec(1) / n;
    pca_result(2) = normvec(2) / n;
    pca_result(3) = 1.0 / n;

    for (int j = 0; j < NUM_MATCH_POINTS; j++)
    {
        if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
        {
            return false;
        }
    }

    // for (int j = 0; j < NUM_MATCH_POINTS; j++)
    // {
    //     if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
    //     {
    //         return false;
    //     }
    // }

    // T n = normvec.norm();
    // pca_result(0) = normvec(0) / n;
    // pca_result(1) = normvec(1) / n;
    // pca_result(2) = normvec(2) / n;
    // pca_result(3) = 1.0 / n; 
    return true;
}