SVD方法拟合平面的C++实现

平面拟合有许多种思路。最直白的最小二乘方法从直线一般式

$ax+by+cz+d=0$

出发，但是需要假定某一个系数不为0，例如，假定c不为0，优化平面方程

$mx+ny+p=z$

中剩余的三个自由变量 $(m, n, p)$ 。但这其实有点本末倒置：在拟合平面前，怎么能保证某一个系数不会为0呢？

另一种SVD方法则避免了这个窘境。

SVD方法先求得平面的法向量，即 $(a, b, c)$

并有 $a^2+b^2+c^2=1$

再根据重心求得最后的 $d$ 。

具体的推导在此不再赘述，这里给出一份基于Eigen3的实现

#include <vector>
#include <Eigen/SVD>

Eigen::Vector4d PlaneFitting(const std::vector<Eigen::Vector3d> & plane_pts) {
    Eigen::Vector3d center = Eigen::Vector3d::Zero();
    for (const auto & pt : plane_pts) center += pt;
    center /= plane_pts.size();

    Eigen::MatrixXd A(plane_pts.size(), 3);
    for (int i = 0; i < plane_pts.size(); i++) {
        A(i, 0) = plane_pts[i][0] - center[0];
        A(i, 1) = plane_pts[i][1] - center[1];
        A(i, 2) = plane_pts[i][2] - center[2];
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinV);
    const float a = svd.matrixV()(0, 2);
    const float b = svd.matrixV()(1, 2);
    const float c = svd.matrixV()(2, 2);
    const float d = -(a * center[0] + b * center[1] + c * center[2]);
    return Eigen::Vector4d(a, b, c, d);
}