4.3.4 直线拟合的实现
参考教程:
gaoxiang12/slam_in_autonomous_driving: 《自动驾驶中的SLAM技术》对应开源代码 (github.com)
1. 编写 Line fitting
1.1 创建文件夹
通过终端创建一个名为Line_fitting
的文件夹以保存我们的VSCode
项目,在/Line_fitting
目录下打开vscode
rosnoetic@rosnoetic-VirtualBox:~$ mkdir -p Line_fitting
rosnoetic@rosnoetic-VirtualBox:~$ cd Line_fitting/
rosnoetic@rosnoetic-VirtualBox:~/Line_fitting$ code .
1.2 编写直线拟合程序
新建文件line_fitting.cpp
注意,这里需要计算整个 SVD 的 V 矩阵,因为要取最大的奇异值。其他计算和数学描述方面一致。同样地,先设定直线的真值参数,然后对直线上的采样点添加噪声,再由采样点估计出直线参数。
在line_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 FitLine(std::vector<Eigen::Matrix<S, 3, 1>>& data, Eigen::Matrix<S, 3, 1>& origin, Eigen::Matrix<S, 3, 1>& dir,
double eps = 0.2) {
if (data.size() < 2) {
return false;
}
origin = std::accumulate(data.begin(), data.end(), Eigen::Matrix<S, 3, 1>::Zero().eval()) / data.size();
Eigen::MatrixXd Y(data.size(), 3);
for (int i = 0; i < data.size(); ++i) {
Y.row(i) = (data[i] - origin).transpose();
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(Y, Eigen::ComputeFullV);
dir = svd.matrixV().col(0);
// check eps
for (const auto& d : data) {
if (dir.template cross(d - origin).template squaredNorm() > eps) {
return false;
}
}
return true;
}
int main(int argc, char argv) {
Eigen::Vector3d true_line_origin(0.1, 0.2, 0.3);
Eigen::Vector3d true_line_dir(0.4,0.5,0.6);
true_line_dir.normalize();
// 随机生成直线点,利用参数方程
std::vector<Eigen::Vector3d> points;
// 生成平面中的点的数量
int FLAGS_num_tested_points_line = 100;
double FLAGS_noise_sigma = 0.01;
// 随机生成仿真平面点
cv::RNG rng;
for (int i = 0; i < FLAGS_num_tested_points_line; ++i) {
double t = rng.uniform(-1.0, 1.0);
Eigen::Vector3d p = true_line_origin + true_line_dir * t;
p += Eigen::Vector3d(rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma));
points.emplace_back(p);
}
Eigen::Vector3d esti_origin, esti_dir;
FitLine(points, esti_origin, esti_dir);
esti_origin = esti_origin.transpose();
esti_dir = esti_dir.transpose();
std::cout << esti_origin.format(HeavyFmt) << std::endl;
std::cout << esti_dir.format(HeavyFmt) << std::endl;
return 0;
}
2. 新建 CMakeLists.txt 文件
新建CMakeLists.txt
文件
在CMakeLists.txt
中添加如下内容:
# 声明要求的cmake最低版本
cmake_minimum_required(VERSION 2.8 )
# 声明一个cmake工程
project(Line_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(line_fitting line_fitting.cpp)
# 链接OpenCV库
target_link_libraries(line_fitting ${OpenCV_LIBS})
3. 编译并执行
ctrl+alt+T
打开终端,执行如下指令进行cmake
编译
rosnoetic@rosnoetic-VirtualBox:~$ cd Line_fitting/
rosnoetic@rosnoetic-VirtualBox:~/Line_fitting$ mkdir build
rosnoetic@rosnoetic-VirtualBox:~/Line_fitting$ cd build/
rosnoetic@rosnoetic-VirtualBox:~/Line_fitting/build$ cmake ..
接着make
对工程进行编译
rosnoetic@rosnoetic-VirtualBox:~/Line_fitting/build$ make
进一步的调用可执行文件line_fitting
:
rosnoetic@rosnoetic-VirtualBox:~/Line_fitting/build$ ./line_fitting
[[0.102905539253127];
[0.204955074214233];
[ 0.30563328060192]]
[[0.452940430780007];
[0.569854682370444];
[0.685646123846188]]
标签:直线,Eigen,4.3,rosnoetic,fitting,拟合,line,Line,dir
From: https://www.cnblogs.com/windandchimes/p/18352189