计算点到三维平面点投影点属于高数中的知识,最开始百度一下都没找到合适的,无奈翻出高数书,在课后习题中找到答案。
抽象出来
利用Eigen库很容易就能解出。
Eigen库求解代码:
// 计算点到平面投影点
void calPointToPlanePoint()
{
double pt[3] = { -1,2,0 };
double plane[4] = { 1,2,-1,1 };
MatrixXf A(3,3);
VectorXf b(3);
A << plane[0], plane[1], plane[2], plane[1], -plane[0], 0, 0, plane[2], -plane[1];
b(0) = -plane[3]; b(1) = plane[1]*pt[0]-plane[0]*pt[1]; b(2) = plane[2]*pt[1]-plane[1]*pt[2];
//SVD
cout << "matrix A:\n" << A << endl;
cout << "vector b:\n" << b << endl;
VectorXf x;
x = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);
cout << "The least-squares solution is:\n"
<< x << endl;
}