4.3.2 C++ 平面拟合的实现
参考教程:
gaoxiang12/slam_in_autonomous_driving: 《自动驾驶中的SLAM技术》对应开源代码 (github.com)
Eigen打印输出_打印eigen矩阵-CSDN博客
1. 编写 Plane fitting
1.1 创建文件夹
通过终端创建一个名为Plane_fitting
的文件夹以保存我们的VSCode
项目,在/Plane_fitting
目录下打开vscode
rosnoetic@rosnoetic-VirtualBox:~$ mkdir -p Plane_fittingrosnoetic@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-2) {if (data.size() < 3) {return false;}Eigen::MatrixXd A(data.size(), 4);for (int i = 0; i < data.size(); ++i) {A.row(i).head<3>() = data[i].transpose();A.row(i)[3] = 1.0;}Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinV);plane_coeffs = svd.matrixV().col(3);// check error epsfor (int i = 0; i < data.size(); ++i) {double err = plane_coeffs.template head<3>().dot(data[i]) + plane_coeffs[3];if (err * err > eps) {return false;}}return true;
}int main(int argc, char argv) {Eigen::Vector4d true_plane_coeffs(0.1, 0.2, 0.3, 0.4);true_plane_coeffs.normalize();std::vector<Eigen::Vector3d> points;// 生成平面中的点的数量int FLAGS_num_tested_points_plane = 10;double FLAGS_noise_sigma = 0.01;// 随机生成仿真平面点cv::RNG rng;for (int i = 0; i < FLAGS_num_tested_points_plane; ++i) {// 先生成一个随机点,计算第四维,增加噪声,再归一化Eigen::Vector3d p(rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0));double n4 = -p.dot(true_plane_coeffs.head<3>()) / true_plane_coeffs[3];p = p / (n4 + std::numeric_limits<double>::min()); // 防止除零p += Eigen::Vector3d(rng.gaussian(FLAGS_noise_sigma),rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma));points.emplace_back(p);// 验证在平面上printf("res of p: %.2f\n",p.dot(true_plane_coeffs.head<3>()) + true_plane_coeffs[3] );}Eigen::Vector4d estimated_plane_coeffs;FitPlane(points, estimated_plane_coeffs);estimated_plane_coeffs = estimated_plane_coeffs.transpose();std::cout << estimated_plane_coeffs.format(HeavyFmt) << std::endl;return 0;}
2. 新建 CMakeLists.txt 文件
新建CMakeLists.txt
文件
在CMakeLists.txt
中添加如下内容:
# 声明要求的cmake最低版本
cmake_minimum_required(VERSION 2.8 )# 声明一个cmake工程
project(Plane_fitting)# 添加C++ 11支持
set(CMAKE_CXX_FLAGS "-std=c++11")# 寻找OpenCV库
find_package( OpenCV REQUIRED)
# 添加头文件
include_directories("/usr/include/eigen3" ${OpenCV_INCLUDE_DIRS})# 添加一个可执行文件
add_executable(linear_fitting linear_fitting.cpp)
# 链接OpenCV库
target_link_libraries(linear_fitting ${OpenCV_LIBS})
3. 编译并执行
ctrl+alt+T
打开终端,执行如下指令进行cmake
编译
rosnoetic@rosnoetic-VirtualBox:~$ cd Plane_fitting/rosnoetic@rosnoetic-VirtualBox:~/Plane_fitting$ mkdir buildrosnoetic@rosnoetic-VirtualBox:~/Plane_fitting$ cd build/rosnoetic@rosnoetic-VirtualBox:~/Plane_fitting/build$ cmake ..
接着make
对工程进行编译
rosnoetic@rosnoetic-VirtualBox:~/Plane_fitting/build$ make
进一步的调用可执行文件linear_fitting
:
rosnoetic@rosnoetic-VirtualBox:~/Plane_fitting/build$ ./linear_fitting res of p: -0.00
res of p: -0.00
res of p: -0.01
res of p: 0.00
res of p: 0.00
res of p: -0.00
res of p: 0.01
res of p: 0.00
res of p: 0.01
res of p: 0.00
[[0.186755046854881];[0.363655524983479];[0.546691644450618];[0.730756770417559]]