三维空间点 直线拟合

使用最小二乘法来找到一个直线方程，使得到点的距离平方和最小。

直线拟合步骤

1. 计算点的质心。
2. 构建协方差矩阵。
3. 对协方差矩阵进行特征值分解。
4. 特征值对应的特征向量即为方向向量。

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

// 定义一个三维点的结构体
struct Point3D {
    double x, y, z;
};

// 计算三维点的质心
Eigen::Vector3d computeCentroid(const std::vector<Point3D>& points) {
    Eigen::Vector3d centroid(0.0, 0.0, 0.0);
    for (const auto& point : points) {
        centroid[0] += point.x;
        centroid[1] += point.y;
        centroid[2] += point.z;
    }
    centroid /= points.size();
    return centroid;
}

// 函数：使用最小二乘法拟合直线
void fitLine(const std::vector<Point3D>& points, Eigen::Vector3d& pointOnLine, Eigen::Vector3d& direction) {
    // 计算质心
    Eigen::Vector3d centroid = computeCentroid(points);

    // 构建协方差矩阵
    Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero();
    for (const auto& point : points) {
        Eigen::Vector3d centeredPoint(point.x - centroid[0], point.y - centroid[1], point.z - centroid[2]);
        covariance += centeredPoint * centeredPoint.transpose();
    }

    // 计算协方差矩阵的特征值和特征向量
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(covariance);
    direction = eigenSolver.eigenvectors().col(2).normalized();

    // 直线上的一点为质心
    pointOnLine = centroid;
}

int main() {
    // 定义一组三维点
    std::vector<Point3D> points = {
        {1.0, 2.0, 3.0},
        {2.0, 3.0, 4.0},
        {3.0, 4.0, 5.0},
        {4.0, 5.0, 6.0},
        {5.0, 6.0, 7.0}
    };

    // 直线上的一点
    Eigen::Vector3d pointOnLine;

    // 直线的方向向量
    Eigen::Vector3d direction;

    // 拟合直线
    fitLine(points, pointOnLine, direction);

    // 输出结果
    std::cout << "直线上一点: (" << pointOnLine[0] << ", " << pointOnLine[1] << ", " << pointOnLine[2] << ")" << std::endl;
    std::cout << "直线方向向量: (" << direction[0] << ", " << direction[1] << ", " << direction[2] << ")" << std::endl;

    return 0;
}

代码说明
1、定义 Point3D 结构体：
     表示一个三维点，包含 x、y、z 三个坐标。
2、computeCentroid 函数：
     计算给定点集的质心。
3、fitLine 函数：
     计算点集的质心。
     构建协方差矩阵。
     使用 Eigen 的 SelfAdjointEigenSolver 计算协方差矩阵的特征值和特征向量。
     方向向量为特征值最大对应的特征向量。
     直线上一点为质心。
4、main 函数：
     定义一组三维点并调用 fitLine 函数进行直线拟合。
     输出拟合得到的直线上一点和方向向量。