4.3.2 C++ 平面拟合的实现

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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.rhkb.cn/news/396278.html

如若内容造成侵权/违法违规/事实不符,请联系长河编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

文件操作与IO(下)

✨个人主页&#xff1a; 不漫游-CSDN博客 目录 前言 流对象 InputStream OutputStream 运用 在控制台进行输入并写入文件 进行普通文件的复制 前言 之前的文章文件操作与IO&#xff08;上&#xff09;已经介绍了文件系统的相关操作&#xff0c;这次的主角是文件内容的相关…

SpringBoot 框架学习笔记(七):Thymeleaf、拦截器 和 文件上传实现(解决了文件重名 和 按日期分目录存放问题)

1 Thymeleaf 1.1 基本介绍 &#xff08;1&#xff09;官方文档&#xff1a;Tutorial: Using Thymeleaf &#xff08;2&#xff09;Thymeleaf 是什么 Thymeleaf 是一个跟 Velocity、FreeMarker 类似的模板引擎&#xff0c;可完全替代 JSPThymeleaf 是一个 java 类库&#xf…

.net core webapi 自定义异常过滤器

1.定义统一返回格式 namespace webapi;/// <summary> /// 统一数据响应格式 /// </summary> public class Results<T> {/// <summary>/// 自定义的响应码&#xff0c;可以和http响应码一致&#xff0c;也可以不一致/// </summary>public int Co…

vue 打包时候的分包

export default defineConfig({plugins: [vue()],resolve: {alias: {: fileURLToPath(new URL(./src/, import.meta.url))}},// 分包&#xff0c;node_modules中的单独打包成名字为vendor的js文件build: {rollupOptions: {manualChunks(id) {if (id.includes(node_modules)) {r…

EF8 学习过程中的问题和解决方案

一、varchar类型字段如果为null 无法使用contains来判断是否包含字符串 1. 有问题的代码&#xff1a; contractList _dbcontext.contractHeads.Where(u > u.code.Contains(queryStr) || u.name.Contains(queryStr) || u.companyName.Contains(queryStr) || u.customerNa…

uniapp开启数据压缩的坑-SpringBoot-gzip

1、服务器配置 服务端开启的数据压缩配置 server:port: ${port:8881}servlet:# 应用上下文路径context-path: /orderserverundertow:threads:io: 4worker: 500buffer-size: 2048# 开启Gzip压缩&#xff0c;compression:# 开启压缩enabled: true# 对json格式内容进行压缩mime-…

KCTF 闯关游戏:1 ~ 7 关

前言 看雪CTF平台是一个专注于网络安全技术竞赛的在线平台&#xff0c;它提供了一个供网络安全爱好者和技术专家进行技术交流、学习和竞技的环境。CTF&#xff08;Capture The Flag&#xff0c;夺旗赛&#xff09;是网络安全领域内的一种流行竞赛形式&#xff0c;起源于1996年…

嵌入式全栈开发学习笔记---数据结构(排序算法)

目录 排序的分类 稳定排序与不稳定排序 内部排序和外部排序 算法的复杂性 常见的排序算法 直接插入排序 希尔排序 快速排序 简单选择排序 堆排序 归并排序 基数排序 常见的排序总结 到目前为止&#xff0c;数据结构的线性结构和树状结构就都讲完了&#xff0c;本节…

使用 MongoDB 构建 AI:Flagler Health 的 AI 旅程如何彻底改变患者护理

Flagler Health 致力于为慢性病患者提供支持&#xff0c;为其匹配合适的医生以提供合适的护理。 通常&#xff0c;身患严重病痛的患者面临的选择有限&#xff0c;他们往往需要长期服用阿片类药物&#xff0c;或寻求成本高昂的侵入性外科手术干预。遗憾的是&#xff0c;后一种方…

SQL语句创建数据库(增删查改)

SQL语句 一.数据库的基础1.1 什么是数据库1.2 基本使用1.2.1 连接服务器1.2.2 使用案例 1.2 SQL分类 二.库的操作2.1 创建数据库2.2 创建数据库示例2.3 字符集和校验规则2.3.1 查看系统默认字符集以及校验规则2.3.2查看数据库支持的字符集2.3.3查看数据库支持的字符集校验规则2…

Android系统Android.bp文件详解

文章目录 1. 基本语法结构2. 常见模块类型3. 模块属性常见属性包括&#xff1a; 4. 具体示例5. 高级功能5.1. 条件编译5.2. 变量定义与使用5.3. 模块继承 6. 总结 Android.bp 是 Android 构建系统&#xff08;Android Build System&#xff09;中的配置文件&#xff0c;用于描述…

go之命令行工具urfave-cli

一、urfave/cli urfave/cli 是一个声明性的、简单、快速且有趣的包&#xff0c;用于用 Go 构建命令行工具。 二、快速使用 2.1 引入依赖 go get github.com/urfave/cli/v2 2.2 demo package mainimport ("fmt""log""os""github.com/ur…

OpenCV图像滤波(9)getGaussianKernel()函数的使用

操作系统&#xff1a;ubuntu22.04 OpenCV版本&#xff1a;OpenCV4.9 IDE:Visual Studio Code 编程语言&#xff1a;C11 功能描述 cv::getGaussianKernel() 是 OpenCV 中的一个函数&#xff0c;用于生成一维高斯核。这种核通常用于实现高斯模糊滤波器&#xff0c;该滤波器可以…

备考CISSP,看这一篇就够了!(附备考资料下载)

作者在2023年发布过一篇博文《不报辅导班一次性通过CISSP经验分享》&#xff0c;后台收到很多备考小伙伴的私信咨询&#xff0c;我就基于大家经常问的问题整理了此文章为大家答疑解惑&#xff0c;同时附上备考过程中作者收集到的全部资源&#xff08;见文末&#xff09;&#x…

EasyCVR视频汇聚平台云计算技术核心优势:高效、灵活与可扩展性深度解读

随着科技的飞速发展和社会的不断进步&#xff0c;视频监控已经成为现代社会治安防控、企业管理等场景安全管理中不可或缺的一部分。在这一背景下&#xff0c;EasyCVR视频汇聚平台凭借其强大的云计算技术&#xff0c;展现出了卓越的性能和广泛的应用前景。本文将深入解析EasyCVR…

Rust学习----Rust安装

如何安装Rust&#xff1f; 1.官网&#xff1a;https://www.rust-lang.org/zh-CN/ 2.Linux or Max: curl https://sh.rustup.rs -sSf | sh 3.Windows按官网指导安装。 4.Windows Subsystem for Linux&#xff1a; curl --proto https --tlsv1.2 -sSf https://sh.rustup.rs…

JavaDS —— 位图(BitSet)与 布隆过滤器

位图 引入问题&#xff1a;给40亿个不重复的无符号整数&#xff0c;没排过序。给一个无符号整数&#xff0c;如何快速判断一个数是否在这40亿个数中。 首先要注意 40 亿个数据如果使用 整型&#xff08;int) 来存放的话&#xff0c;就是要 40 亿个整型&#xff0c;一个整型有…

redis面试(十一)锁超时

boolean res lock.tryLock(100, 10, TimeUnit.SECONDS); RedissonLock里面有这样一个方法tryLock()&#xff0c;意思是尝试获取锁的结果。 最大等待时间100s&#xff0c;并且获取到锁之后&#xff0c;10s之内没有释放的话&#xff0c;锁会自动失效。 尝试获取锁超时 time …

【vue3|第20期】vue3中Vue Router路由器工作模式

日期&#xff1a;2024年8月6日 作者&#xff1a;Commas 签名&#xff1a;(ง •_•)ง 积跬步以致千里,积小流以成江海…… 注释&#xff1a;如果您觉得有所帮助&#xff0c;帮忙点个赞&#xff0c;也可以关注我&#xff0c;我们一起成长&#xff1b;如果有不对的地方&#xff…

LiveNVR监控流媒体Onvif/RTSP常见问题-页面上传SSL证书配置开启 HTTPS 服务?什么时候必须要开启HTTPS服务?

LiveNVR常见问题-页面上传SSL证书配置开启 HTTPS 服务&#xff1f;什么时候必须要开启HTTPS服务&#xff1f; 1、配置开启HTTPS1.1、准备https证书1.2、配置HTTPS端口1.3、配置证书路径1.3、 页面上传SSL证书 2、验证HTTPS服务3、为什么要开启HTTPS4、RTSP/HLS/FLV/RTMP拉流Onv…