Ошибка в вычислении наилучшего подходящего плоского уравнения с использованием SVD

Question

Ошибка в вычислении наилучшего подходящего плоского уравнения с использованием 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

Метод СВД:

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

Исходя из результатов, я считаю, что нормали рассчитываются очень хорошо (хотя направление обратное), но значение d это неверно. Я не уверен, где я иду не так. Любая помощь очень ценится.

Заранее спасибо..

0

c++eigen math plane svd

Решение

Другие решения

Других решений пока нет …

Источник

Accepted Answer

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

2