首页 > 其他分享 >4.3.4 直线拟合的实现

4.3.4 直线拟合的实现

时间:2024-08-10 13:05:03浏览次数:9  
标签:直线 Eigen 4.3 rosnoetic fitting 拟合 line Line dir

4.3.4 直线拟合的实现

参考教程:

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

标签:直线,Eigen,4.3,rosnoetic,fitting,拟合,line,Line,dir
From: https://www.cnblogs.com/windandchimes/p/18352189

相关文章

  • Qt自定义TreeWidget,实现展开折叠按钮在右侧,且一条竖直线上对齐
    效果如下:图片随便找的,可能需要调下样式,代码复制可用,留给有需要的人。 #ifndefCustomTreeWidget_h__#defineCustomTreeWidget_h__#include<QTreeWidget>#include<QPushButton>classCCustomTreeWidget:publicQTreeWidget{ Q_OBJECTpublic: CCustomTreeW......
  • 4.3.2 图像去畸变
    4.3.2图像去畸变参考教程:相机标定(4)矫正畸变undistort()和initUndistortRectifyMap()-CSDN博客学习笔记--opencv图像去畸变_opencv畸变参数-CSDN博客下面我们将演示图像去畸变的过程,在OpenCV中提供了一个函数cv::undistort()用于对图像进行去畸变,为了加深我们的印象,我们......
  • 4.3 ES6 Class 类
    4.3ES6Class类分类 ES6教程概述在ES6中,class(类)作为对象的模板被引入,可以通过class关键字定义类。class的本质是function。它可以看作一个语法糖,让对象原型的写法更加清晰、更像面向对象编程的语法。基础用法类定义类表达式可以为匿名或命名。//匿名......
  • 与 scipy ODR 的正确拟合包括错误吗?
    我有一些数据点的x和y坐标都有错误。因此我需要使用python的scipy.odr.ODR工具来计算最佳拟合斜率和该斜率上的误差。但是,当我尝试添加错误时,ODR失败并仅返回初始猜测参数。我尝试运行它而没有错误,并且它按预期工作:#Importstatementsimportnumpyasnpi......
  • 某东,m端滑块,h5st4.2,4.3,4.7
    ​声明本文章中所有内容仅供学习交流使用,不用于其他任何目的,抓包内容、敏感网址、数据接口等均已做脱敏处理,严禁用于商业用途和非法用途,否则由此产生的一切后果均与作者无关!wxa15018601872       本文章未经许可禁止转载,禁止任何修改后二次传播,擅自使用本文讲解......
  • 【机器学习】过拟合和欠拟合、高偏差(High Bias)和高方差(High Variance)的区别、过拟合和
    引言在机器学习中,过拟合(Overfitting)是指模型在训练数据上学习得太好,以至于它捕捉到了数据中的噪声和随机波动,而不是潜在的真实关系,这导致模型在新的、未见过的数据上表现不佳;欠拟合(Underfitting)是指模型在训练数据上未能捕捉到足够的信息或模式,导致模型在训练集和测试集上......
  • 功能强大的电路设计与仿真软件Multisim 14.3安装教程
    一款功能强大的电路设计与仿真软件Multisim是一款强大的电子电路仿真软件,广泛应用于电子工程和教育领域。本教程全面细致地解析了原理图设计、电路仿真以及虚拟仪器测试等核心功能,通过友好易学的界面设计,为用户打造了一款高效便捷的电路设计和分析工具,助您轻松掌握电路设计的精髓......
  • 4.3.6.7 读取PCD文件并在rviz中展示
    4.3.6.7读取PCD文件并在rviz中展示参考教程:读取PCD文件的点云并在RVIZ显示_rviz显示pcd点云-CSDN博客读取pcd文件并在rviz中进行显示_rviz看不到pcd-CSDN博客Hinson-A/pcd2pgm_package:点云pcd文件转二维栅格地图(github.com)ROS-PCL读取pcd点云数据并在rviz中进行显示_r......
  • 前端使用 Konva 实现可视化设计器(19)- 连接线 - 直线、折线
    本章响应小伙伴的反馈,除了算法自动画连接线(仍需优化完善),实现了可以手动绘制直线、折线连接线功能。请大家动动小手,给我一个免费的Star吧~大家如果发现了Bug,欢迎来提Issue哟~github源码gitee源码示例地址模式切换前置工作连接线模式种类//src/Render/types.tse......
  • 最小二乘法拟合空间直线
    一、空间直线方程1.1一般方程空间直线可以看成成两个平面的交线,设两个平面方程分别为\(A_1x+B_1y+C_1z+D_1=0\)和\(A_2x+B_2y+C_2z+D_2=0\),则直线\(l\)的一般方程可以表示为:\(\left\{\begin{matrix}A_1x+B_1y+C_1+D_1=0\\A_2x+B_2y+C_2+D_2......