坑爹的,csdn没法编辑公式,我只能在word上写好截图了。
因此,如果能构造AX=0,则最小二乘解就直接求ATA的最小特征解就可以了,这里举一个平面拟合的例子:
假如有一束点云,如何从点云里把平面求出来。啥也不说了,上代码吧:
Bool ComplanationFit(Geometries::Coordinate *pPoints, Int32 nPointcount, Double tolerance)
{
if ((pPoints == NULL) || (nPointcount < 4) || (tolerance < 0.0))
{
return false;
}
/*
* 解XTX的特征值
*/
Eigen::MatrixXd X = Eigen::MatrixXd::Zero(nPointcount, 4);
for (Int32 i = 0; i < nPointcount; i++)
{
Coordinate c = pPoints[i];
X(i, 0) = c.x;
X(i, 1) = c.y;
X(i, 2) = c.z;
X(i, 3) = 1.0;
}
Eigen::MatrixXd XTX = X.transpose() * X;
Eigen::EigenSolver<Eigen::MatrixXd> es(XTX);
std::complex<Double> d[4];
d[0] = es.eigenvalues()[0];
d[1] = es.eigenvalues()[1];
d[2] = es.eigenvalues()[2];
d[3] = es.eigenvalues()[3];
int iMin = 0; double dMax = DBL_MAX;
for (int i = 0; i < 4; i++)
{
if (d[i].real() < dMax)
{
iMin = i;
dMax = d[i].real();
}
}
Eigen::VectorXcd V = es.eigenvectors().col(iMin);
//平面方程
double dA = V(0).real();
double dB = V(1).real();
double dC = V(2).real();
double dD = V(3).real();
//投射到该平面
for (int i = 0; i < nPointcount; i++)
{
double x = pPoints[i].x;
double y = pPoints[i].y;
double z = pPoints[i].z;
double t = (dA * x + dB * y + dC * z + dD) / (dA * dA + dB * dB + dC * dC);
pPoints[i].x = x - dA * t;
pPoints[i].y = y - dB * t;
pPoints[i].z = z - dC * t;
}
return true;
}