c++ - 使用SVD计算最佳拟合平面方程时出现错误

我试图为一组点找到最合适的平面，然后使用SVD计算ax+by+cz+d=0给定的平面方程。

我已经实现了SVD，并且能够成功获取飞机的法线，但是我无法计算d。

经过一些挖掘后，我将等式中返回的质心替换为计算d，但是得到的值不正确。我确定这是一个不正确的值，因为我正在将其与RANSAC方法进行比较。

我的代码的实现如下

    pcl::ModelCoefficients normal_extractor::plane_est_svd(pcl::PointCloud<pcl::PointXYZ>::ConstPtr point_cloud)
{
    Eigen::MatrixXd points_3D(3,point_cloud->width);
    //assigning the points from point cloud to matrix
    for (int i=0;i<point_cloud->width;i++)
    {
        points_3D(0,i) = point_cloud->at(i).x;
        points_3D(1,i) = point_cloud->at(i).y;
        points_3D(2,i) = point_cloud->at(i).z;
    }
    // calcaulating the centroid of the pointcloud
    Eigen::MatrixXd centroid = points_3D.rowwise().mean();
    //std::cout<<"The centroid of the pointclouds is given by:\t"<<centroid<<std::endl;
    //subtract the centroid from points
    points_3D.row(0).array() -= centroid(0);
    points_3D.row(1).array() -= centroid(1);
    points_3D.row(2).array() -= centroid(2);
    //calculate the SVD of points_3D matrix
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(points_3D,Eigen::ComputeFullU);
    Eigen::MatrixXd U_MAT = svd.matrixU();
    //std::cout<<"U matrix transpose is:"<<U_MAT<<std::endl<<std::endl<<"U matrix is:"<<svd.matrixU()<<std::endl;
    /*********************************************************************************************
     * caculating d by sybstituting the centroid back in the quation
     *      aCx+bCy+cCz = -d
     ********************************************************************************************/
    //double d = -((U_MAT(0,2)*points_3D(0,1))+ (U_MAT(1,2)*points_3D(1,1)) + (U_MAT(1,2)*points_3D(1,2)));
    double d = -((U_MAT(0,2)*centroid(0))+ (U_MAT(1,2)*centroid(1)) + (U_MAT(1,2)*centroid(2)));

    pcl::ModelCoefficients normals;
    normals.values.push_back(U_MAT(0,2));
    normals.values.push_back(U_MAT(1,2));
    normals.values.push_back(U_MAT(2,2));
    normals.values.push_back(d);
    return(normals);

}

我得到的结果是

RANSAC方法：

    a = -0.0584306  b = 0.0358117   c = 0.997649    d = -0.161604

SVD方法：

    a = 0.0584302   b = -0.0357721  c = -0.99765    d = 0.00466139

从结果来看，我相信法线计算得很好（尽管方向相反），但是d的值不正确。我不确定我要去哪里。任何帮助都非常感谢。

提前致谢..

最佳答案

1201ProgramAlarm是正确的，在U_MAT(1,2)*centroid(2)中有一个错字。

为了避免这种错字，最好写：

d = -centroid.dot(U_MAT).col(2);

您还可以简化：

points_3D.colwise() -= centroid;

供以后参考，这是一个独立的示例：

#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

int main()
{
  int n = 10;
  // generate n points in the plane centered in p and spanned bu the u,v vectors.
  MatrixXd points_3D(3,n);
  Vector3d u = Vector3d::Random().normalized();
  Vector3d v = Vector3d::Random().normalized();
  Vector3d p = Vector3d::Random();
  points_3D = p.rowwise().replicate(n) + u*VectorXd::Random(n).transpose() + v*VectorXd::Random(n).transpose();
  MatrixXd initial_points = points_3D;

  Vector3d centroid = points_3D.rowwise().mean();
  points_3D.colwise()-=centroid;
  JacobiSVD<MatrixXd> svd(points_3D,ComputeFullU);
  Vector3d normal = svd.matrixU().col(2);
  double d = -normal.dot(centroid);

  cout << "Plane equation: " << normal.transpose() << " " << d << endl;
  cout << "Distances: " << (normal.transpose() * initial_points).array() + d << endl;
}

关于c++ - 使用SVD计算最佳拟合平面方程时出现错误，我们在Stack Overflow上找到一个类似的问题：https://stackoverflow.com/questions/41373372/