SVD를 사용하여 최상의 피팅 평면 방정식을 계산할 수 없습니다.

Question

점 집합에 가장 적합한 평면을 찾으려고합니다. 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 값은 올바르지 않습니다. 내가 어디로 잘못 가고 있는지 모르겠습니다. 어떤 도움이라도 대단히 감사합니다.

Answer 1

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; 
}