4.3.2 C++ 平面拟合的实现

4.3.2 C++ 平面拟合的实现

参考教程：

gaoxiang12/slam_in_autonomous_driving: 《自动驾驶中的SLAM技术》对应开源代码 (github.com)

Eigen打印输出_打印eigen矩阵-优快云博客

1. 编写 Plane fitting

1.1 创建文件夹

通过终端创建一个名为Plane_fitting的文件夹以保存我们的VSCode项目，在/Plane_fitting目录下打开vscode

rosnoetic@rosnoetic-VirtualBox:~$ mkdir -p Plane_fitting

rosnoetic@rosnoetic-VirtualBox:~$ cd Plane_fitting/

rosnoetic@rosnoetic-VirtualBox:~/Plane_fitting$ code .

1.2 编写平面拟合程序

新建文件linear_fitting.cpp

使用快速 SVD 分解，仅计算 A 矩阵 SVD 结果的最后一列。在计算完成后，将点的具体取值代入本方程，要求它们的平方误差不超过预设的阈值。下面这段测试程序把随机生成的平面参数作为真值，在平面上取若干个点，再加入噪声，做平面拟合。

在linear_fitting.cpp粘贴如下代码并保存(Ctrl+S）

// 引入Eigen头文件与常用类型
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <iostream>

Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");

template <typename S>
bool FitPlane(std::vector<Eigen::Matrix<S, 3, 1>>& data, Eigen::Matrix<S, 4, 1>& plane_coeffs, double eps = 1e-