从零入门激光SLAM（二十）——IESKF代码实现

2024-05-24 22:36
文章标签 代码 实现 入门 slam 激光 二十 ieskf

本文主要是介绍从零入门激光SLAM（二十）——IESKF代码实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：772356582，欢迎大家加入讨论。

源码是高博大佬的，网址如下 

slam_in_autonomous_driving/src/ch8 at master · gaoxiang12/slam_in_autonomous_driving · GitHub

下面对代码的逻辑和主要函数进行解析

一、IESKF结构

  • 参数配置/设置状态变量

    struct Options {Options() = default;/// IEKF配置int num_iterations_ = 3;  // 迭代次数double quit_eps_ = 1e-3;  // 终止迭代的dx大小/// IMU 测量与零偏参数double imu_dt_ = 0.01;         // IMU测量间隔double gyro_var_ = 1e-5;       // 陀螺测量标准差double acce_var_ = 1e-2;       // 加计测量标准差double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走标准差double bias_acce_var_ = 1e-4;  // 加计零偏游走标准差/// RTK 观测参数double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声double gnss_height_noise_ = 0.1;                // GNSS高度噪声double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声/// 其他配置bool update_bias_gyro_ = true;  // 是否更新biasbool update_bias_acce_ = true;  // 是否更新bias};// nominal stateSO3 R_;VecT p_ = VecT::Zero();VecT v_ = VecT::Zero();VecT bg_ = VecT::Zero();VecT ba_ = VecT::Zero();VecT g_{0, 0, -9.8};// error stateVec18T dx_ = Vec18T::Zero();// covarianceMat18T cov_ = Mat18T::Identity();// noiseMotionNoiseT Q_ = MotionNoiseT::Zero();GnssNoiseT gnss_noise_ = GnssNoiseT::Zero();Options options_;
  • 设置初始条件 
    void SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba,const VecT& gravity = VecT(0, 0, -9.8)) {BuildNoise(options);options_ = options;bg_ = init_bg;ba_ = init_ba;g_ = gravity;cov_ = 1e-4 * Mat18T::Identity();cov_.template block<3, 3>(6, 6) = 0.1 * math::kDEG2RAD * Mat3T::Identity();}
//构建噪声模型
void BuildNoise(const Options& options) {double ev = options.acce_var_;double et = options.gyro_var_;double eg = options.bias_gyro_var_;double ea = options.bias_acce_var_;double ev2 = ev;  // * ev;double et2 = et;  // * et;double eg2 = eg;  // * eg;double ea2 = ea;  // * ea;// set QQ_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;
}

  • 使用IMU预测

    bool IESKF<S>::Predict(const IMU& imu) {/// Predict 部分与ESKF完全一样，不再解释assert(imu.timestamp_ >= current_time_);double dt = imu.timestamp_ - current_time_;if (dt > (5 * options_.imu_dt_) || dt < 0) {LOG(INFO) << "skip this imu because dt_ = " << dt;current_time_ = imu.timestamp_;return false;}VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);R_ = new_R;v_ = new_v;p_ = new_p;Mat18T F = Mat18T::Identity();F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;F.template block<3, 3>(3, 12) = -R_.matrix() * dt;F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;cov_ = F * cov_ * F.transpose() + Q_;current_time_ = imu.timestamp_;return true;
}

  • 迭代观测模型

    using CustomObsFunc = std::function<void(const SE3& input_pose, Eigen::Matrix<S, 18, 18>& HT_Vinv_H,Eigen::Matrix<S, 18, 1>& HT_Vinv_r)>;
// 使用自定义观测函数更新滤波器
bool IESKF<S>::UpdateUsingCustomObserve(IESKF::CustomObsFunc obs) {// H阵由用户给定// 保存当前的旋转矩阵SO3 start_R = R_;Eigen::Matrix<S, 18, 1> HTVr;Eigen::Matrix<S, 18, 18> HTVH;Eigen::Matrix<S, 18, Eigen::Dynamic> K;Mat18T Pk, Qk;//进入残差更新循环for (int iter = 0; iter < options_.num_iterations_; ++iter) {// 调用用户提供的观测函数//GetNominalSE3()获取当前名义状态位姿
//SE3 GetNominalSE3() const { return SE3(R_, p_); }//HTVH卡尔曼更新步骤中用于修正预测的协方差矩阵//HTVr卡尔曼更新步骤中用于修正预测的状态变量obs(GetNominalSE3(), HTVH, HTVr);// 投影协方差矩阵PMat18T J = Mat18T::Identity();J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat((R_.inverse() * start_R).log());Pk = J * cov_ * J.transpose();// 卡尔曼更新Qk = (Pk.inverse() + HTVH).inverse();   // 计算更新后的协方差矩阵Qkdx_ = Qk * HTVr; // 计算状态增量dx_// LOG(INFO) << "iter " << iter << " dx = " << dx_.transpose() << ", dxn: " << dx_.norm();//将增量dx_合并到名义变量中Update();// 检查增量的范数是否小于终止阈值if (dx_.norm() < options_.quit_eps_) {break;}}// 更新协方差矩阵update Pcov_ = (Mat18T::Identity() - Qk * HTVH) * Pk;// 再次投影协方差矩阵PMat18T J = Mat18T::Identity();Vec3d dtheta = (R_.inverse() * start_R).log();J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dtheta);cov_ = J * cov_ * J.inverse();// 重置状态增量dx_.setZero();return true;
}

详情请见...

 从零入门激光SLAM（二十）——IESKF代码解释 - 古月居 (guyuehome.com)

这篇关于从零入门激光SLAM（二十）——IESKF代码实现的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/999725。

相关文章

Spring Security 从入门到进阶系列教程

Spring Security 从入门到进阶系列教程

Spring Security 入门系列 《保护 Web 应用的安全》 《Spring-Security-入门（一）：登录与退出》 《Spring-Security-入门（二）：基于数据库验证》 《Spring-Security-入门（三）：密码加密》 《Spring-Security-入门（四）：自定义-Filter》 《Spring-Security-入门（五）：在 Sprin
阅读更多...
无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

利用康拓展开将一个排列映射成一个自然数，然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu
阅读更多...
【C++】_list常用方法解析及模拟实现

【C++】_list常用方法解析及模拟实现

相信自己的力量，只要对自己始终保持信心，尽自己最大努力去完成任何事，就算事情最终结果是失败了，努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一：什么是list？ •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二：list常用接口 •🌰1.默认成员函数 🔥构造函数（⭐） 🔥析构函数 •🌰2.list对象
阅读更多...
【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯，先赞后看哦~🎈🎈 🏆 作者简介：景天科技苑 🏆《头衔》：大厂架构师，华为云开发者社区专家博主，阿里云开发者社区专家博主，CSDN全栈领域优质创作者，掘金优秀博主，51CTO博客专家等。 🏆《博客》：Python全栈，前后端开发，小程序开发，人工智能，js逆向，App逆向，网络系统安全，数据分析，Django，fastapi
阅读更多...
活用c4d官方开发文档查询代码

活用c4d官方开发文档查询代码

当你问AI助手比如豆包，如何用python禁止掉xpresso标签时候，它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来
阅读更多...
让树莓派智能语音助手实现定时提醒功能

让树莓派智能语音助手实现定时提醒功能

最初的时候是想直接在rasa 的chatbot上实现，因为rasa本身是带有remindschedule模块的。不过经过一番折腾后，忽然发现，chatbot上实现的定时，语音助手不一定会有响应。因为，我目前语音助手的代码设置了长时间无应答会结束对话，这样一来，chatbot定时提醒的触发就不会被语音助手获悉。那怎么让语音助手也具有定时提醒功能呢？ 我最后选择的方法是用threading.Time
阅读更多...
Android实现任意版本设置默认的锁屏壁纸和桌面壁纸（两张壁纸可不一致）

Android实现任意版本设置默认的锁屏壁纸和桌面壁纸（两张壁纸可不一致）

客户有些需求需要设置默认壁纸和锁屏壁纸  在默认情况下 这两个壁纸是相同的  如果需要默认的锁屏壁纸和桌面壁纸不一样 需要额外修改 Android13实现 替换默认桌面壁纸： 将图片文件替换frameworks/base/core/res/res/drawable-nodpi/default_wallpaper.*  （注意不能是bmp格式） 替换默认锁屏壁纸： 将图片资源放入vendo
阅读更多...
C#实战|大乐透选号器[6]：实现实时显示已选择的红蓝球数量

C#实战|大乐透选号器[6]：实现实时显示已选择的红蓝球数量

哈喽，你好啊，我是雷工。 关于大乐透选号器在前面已经记录了5篇笔记，这是第6篇； 接下来实现实时显示当前选中红球数量，蓝球数量； 以下为练习笔记。 01 效果演示 当选择和取消选择红球或蓝球时，在对应的位置显示实时已选择的红球、蓝球的数量； 02 标签名称 分别设置Label标签名称为：lblRedCount、lblBlueCount
阅读更多...
poj 1258 Agri-Net（最小生成树模板代码）

poj 1258 Agri-Net（最小生成树模板代码）

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码：效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2