我正在嘗試爲一組點計算最佳擬合平面,並使用SVD計算由ax+by+cz+d=0
給出的平面方程。使用SVD計算最佳擬合平面方程時的誤差
我已經實現了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
的值不正確。我不知道我要去哪裏錯。任何幫助真的很感激。
在此先感謝..
如果' d'計算錯誤,請檢查您正在使用的公式。最後一項('U_MAT(1,2)*質心(2)')是否不正確(而不是'U_MAT(2,2)')? – 1201ProgramAlarm
當然是,非常感謝。出於某種原因,我有選擇性地對此視而不見。 @ 1201ProgramAlarm再次感謝你。 – spacemanspiff