4.3.4 C++ 直线拟合的实现

4.3.4 C++ 直线拟合的实现

参考教程：

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

Eigen打印输出_打印eigen矩阵-CSDN博客

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]]