C++最小二乘法拟合平面

、`bool AlgFitPlane::FitPlane3D(std::vector& points, float& A, float& B, float& C, float& D)
{
if (points.size() < 3)
{
return false;
}

//判断是否共线
if (AlgCheckCollinear::Check(points))
{
	return false;
}

Eigen::MatrixXd Am = Eigen::MatrixXd::Zero(4, 4);
Eigen::VectorXd Bm = Eigen::VectorXd::Zero(4, 1);

Am(3, 3) = points.size();
for (std::size_t i = 0; i < points.size(); i++)
{
	Am(0, 0) += pow(points[i].x_, 2);
	Am(0, 1) += points[i].x_ * points[i].y_;
	Am(0, 2) += points[i].x_ * points[i].z_;
	Am(0, 3) += points[i].x_;

	Am(1, 1) += pow(points[i].y_, 2);
	Am(1, 2) += points[i].y_ * points[i].z_;
	Am(1, 3) += points[i].y_;

	Am(2, 2) += pow(points[i].z_, 2);
	Am(2, 3) += points[i].z_;
}

Am(1, 0) = Am(0, 1);
Am(2, 0) = Am(0, 2);
Am(2, 1) = Am(1, 2);
Am(3, 0) = Am(0, 3);
Am(3, 1) = Am(1, 3);
Am(3, 2) = Am(2, 3);

Eigen::BDCSVD<Eigen::MatrixXd> Cm = Am.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
//Eigen::JacobiSVD<Eigen::MatrixXd> Cm = Am.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

Eigen::Matrix4d V = Cm.matrixV();
Eigen::MatrixXd U = Cm.matrixU();

Eigen::Vector4d normal;
normal << V(0, 3), V(1, 3), V(2, 3), V(3, 3);

A = normal(0, 0) / normal.head<3>().norm();
B = normal(1, 0) / normal.head<3>().norm();
C = normal(2, 0) / normal.head<3>().norm();
D = normal(3, 0) / normal.head<3>().norm();

}`