当前位置：首页 > news >正文

C++ Eigen最小二乘拟合三维直线

news 2025/8/9 23:32:06

1. 基本类型

    /*** @brief 线*/struct Line3d {Eigen::Vector3d point {};Eigen::Vector3d direction {};};

2. 拟合结果可为空类型宏定义，用于std::optional与boost::optional平替


#ifdef OPTIONAL_BOOST
#include <boost/optional.hpp>
#ifndef M_OPTIONAL
#define M_OPTIONAL M_OPTIONAL
#endif // !M_OPTIONAL#ifndef OPTIONAL_NONE
#define OPTIONAL_NONE boost::none
#endif // !OPTIONAL_NONE
#else
#include <optional>
#ifndef M_OPTIONAL
#define M_OPTIONAL std::optional
#endif // !M_OPTIONAL#ifndef OPTIONAL_NONE
#define OPTIONAL_NONE std::nullopt
#endif // !OPTIONAL_NONE
#endif // !OPTIONAL_BOOST

3. 三维直线拟合函数

M_OPTIONAL<Line3d> FitLine3dPCA(const std::vector<Eigen::Vector3d>& points)
{if (points.size() < 2) {return { OPTIONAL_NONE };}// 计算质心Eigen::Vector3d centroid = Eigen::Vector3d::Zero();for (const auto& p : points) {centroid += p;}centroid /= points.size();// 构建协方差矩阵Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();for (const auto& p : points) {Eigen::Vector3d centered = p - centroid;cov += centered * centered.transpose();}cov /= points.size();// 求协方差矩阵的特征向量（最大特征值对应的方向）Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov);if (solver.info() != Eigen::Success) {return { OPTIONAL_NONE };}Eigen::Vector3d direction = solver.eigenvectors().col(2).normalized();Line3d line;line.point = centroid;line.direction = direction;return line;
}

4. 读取点文件工具函数

4.1. 点格式示例

1,3.3430063665158043,-2.2194078290652977,0.3354974815183671
2,1.7449288088835502,-0.8618028071806091,4.60509480423679
3,0.7857717528889129,-0.40685814532015363,-0.3132400527103719
4,0.4627448446856639,2.5927289953742285,2.009015614598561
5,3.3417530022332964,1.2860073754736252,2.8298107704812754
6,2.991151484336692,4.202162573792802,3.8695985120956156
7,1.0810646673514561,5.491595123760361,1.264199175914956
8,4.9517287623536,5.930930911561477,3.4499954792885714
9,-1.6884482187454348,3.841324915056982,3.6498221054770283
10,1.1753280668817334,1.9011403492990624,2.0105584013510045

4.2. 读取函数实现，这里以Qt为例

std::vector<Eigen::Vector3d> ReadXYZs(const QString& path)
{QFile file(path);if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {qDebug() << "Open file failed!";return {};}std::vector<Eigen::Vector3d> points;{QTextStream in(&file);while (!in.atEnd()) {QString line = in.readLine();QStringList list = line.split(",");if (list.size() != 4) {continue;}points.push_back({ list[1].toDouble(), list[2].toDouble(), list[3].toDouble() });}file.close();qDebug() << "point count：" << points.size();}return points;
}

5. 拟合示例

int main(int argc, char* argv[])
{auto&& points = ReadXYZs(":/data/perturbed_line_points.csv");auto&& t = FitLine3dPCA(points);if (!t.has_value()) {qDebug() << "拟合失败";return 0;}auto&& line = t.value();std::cout << "\n\nFitLine3dPCA:\n"<< "point: (" << line.point.transpose() << ")\n"<< "direction: (" << line.direction.transpose() << ")\n\n\n";return 0;
}