平面拟合 C++ Eigen

### 使用 C++ 和 Eigen 库实现平面拟合

#### 定义点类
为了方便处理三维空间中的点数据，可以定义一个简单的 `vec` 类来表示三维向量：

class vec {
public:
    float v[3];
    
    vec() {}
    
    vec(float x, float y, float z) {
        v[0] = x;
        v[1] = y;
        v[2] = z;
    }
    
    const float& operator[](int i) const {
        return v[i];
    }

    float& operator[](int i) {
        return v[i];
    }
};

此部分代码展示了如何创建一个能够存储三个浮点数值的简单容器，并提供索引访问功能[^3]。

#### 构建设计矩阵 A 和观测向量 b
对于给定的一组 n 个三维坐标 \((x_i,y_i,z_i)\)，构建设计矩阵 \(A\) 及其对应的观测向量 \(b\):

\[
A =
\begin{pmatrix}
x_1 & y_1 & 1 \\
x_2 & y_2 & 1 \\
... & ... & .\\
x_n & y_n & 1 \\
\end{pmatrix},
b=
\begin{pmatrix}
z_1 \\ z_2 \\ .. \\ z_n
\end{pmatrix}
\]

其中每一行代表了一个样本点的位置信息。通过这些输入参数，目标是最小化误差平方和得到最佳拟合平面方程系数 a,b,c:

\[ ax + by + c=z \]

#### 实现最小二乘法求解
使用 Eigen 库提供的函数可以直接解决上述问题。下面给出完整的程序清单：

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// 假设有若干个三维点的数据集 points[]
VectorXd fitPlane(const vector<vec>& points){
    int N = points.size();
    MatrixXd A(N, 3);
    VectorXd B(N);

    // Fill matrix and vectors with data from the input set of points.
    for(int i=0;i<N;++i){
        A(i,0)=points[i][0]; // X coordinate
        A(i,1)=points[i][1]; // Y coordinate
        A(i,2)=1.;           // Constant term
        B(i)=points[i][2];   // Z value as target variable
    }

    // Solve using least squares method provided by Eigen library
    VectorXd result=A.colPivHouseholderQr().solve(B); 

    cout << "Solution:\n" << result << endl;
    return result;
}

int main(){
    // Example usage
    vector<vec> samplePoints={
        vec(0.,0.,0.),  
        vec(1.,0.,1.),   
        vec(-1.,0,-1.)     
    };

    auto solution=fitPlane(samplePoints);
    double a=solution(0),b=solution(1),c=solution(2);

    printf("Fitted plane equation is %.2fx + %.2fy + %.2f = z\n",a,b,c);
    return 0;
}

这段代码首先导入必要的头文件并命名空间；接着声明了辅助性的 `fitPlane()` 函数接受一组三维点作为输入，并返回经过最小二乘拟合后的平面参数；最后，在 `main()` 中给出了具体的例子说明如何调用该函数完成一次实际的操作。