jjzjj

LIOSAM中关于gtsam的笔记——IMU预积分和MapOptimization

Mr.Naruto 2023-04-08 原文

对于LIOSAM中的ImuPreIntegration.cpp中出现gtsam的部分代码，主要是实现基于因子图的位姿估计。参考资料不多，主要看看GTSAM官网的examples学习代码。

Imufactorexample2.cpp和imufactorsexample.cpp两个文件。介绍了IMU位姿估计的主要方法。下面主要理顺ImuPreIntegration.cpp中如何使用gtsam。

首先是这个类：IMUPreintegration

1、初始化阶段

定义先验因子

对于位姿因子而言，需要三个量：

key，这里是X0

初始化位姿，prevPose_

噪声

gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);

然后添加到因子图中。以此类推，添加速度，偏置。最后初始化。并且使用ISAM优化器更新一次。

prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
// initial bias
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);
// add values
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
optimizer.update(graphFactors, graphValues);

GTSAM中有很多优化器，具体每一种优化器都有什么特点有待研究。

这个流程相在官网的Examples中ImuFactorsExample.cpp有解释。主要就是个框架。

初始化完成后，开始计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态

 注意这里有两个优化器，功能不同，详见知乎卢涛代码分析。这里说流程

下面这一步是往里面加每个时刻的IMU信息

imuIntegratorOpt_->integrateMeasurement

添加IMU预积分因子，imu_factor。这里X(key)还没有初始化，一开始也是比较疑惑。

然后添加imu偏置因子，BetweenFactor

添加位姿因子，注意，这个是从激光里程计来的

gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);

 做预积分，得到当前的状态

最后放到isam优化器中，做更新，算出当前的速度，位姿。

后面还有一步重传播，跟前面的结构差不多。

对于MapOptimization.cpp

 有一个里程计因子

if (cloudKeyPoses3D->points.empty())
        {
            noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
            gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
            initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
        }else{
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
            gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
        }

mdashMapOptimizationxff0cxff0xff

有关LIOSAM中关于gtsam的笔记——IMU预积分和MapOptimization的更多相关文章

  1. LC滤波器设计学习笔记（一）滤波电路入门 - 2

    目录前言滤波电路科普主要分类实际情况单位的概念常用评价参数函数型滤波器简单分析滤波电路构成低通滤波器RC低通滤波器RL低通滤波器高通滤波器RC高通滤波器RL高通滤波器部分摘自《LC滤波器设计与制作》，侵权删。前言最近需要学习放大电路和滤波电路，但是由于只在之前做音乐频谱分析仪的时候简单了解过一点点运放，所以也是相当从零开始学习了。滤波电路科普主要分类滤波器：主要是从不同频率的成分中提取出特定频率的信号。有源滤波器：由RC元件与运算放大器组成的滤波器。可滤除某一次或多次谐波，最普通易于采用的无源滤波器结构是将电感与电容串联，可对主要次谐波（3、5、7）构成低阻抗旁路。无源滤波器：无源滤波器，又称

  2. Unity Shader 学习笔记（5）Shader变体、Shader属性定义技巧、自定义材质面板 - 2

    写在之前Shader变体、Shader属性定义技巧、自定义材质面板，这三个知识点任何一个单拿出来都是一套知识体系，不能一概而论，本文章目的在于将学习和实际工作中遇见的问题进行总结，类似于网络笔记之用，方便后续回顾查看，如有以偏概全、不祥不尽之处，还望海涵。1、Shader变体先看一段代码......Properties{ [KeywordEnum(on,off)]USL_USE_COL("IsUseColorMixTex?",int)=0 [Toggle(IS_RED_ON)]_IsRed("IsRed?",int)=0}......//中间省略，后续会有完整代码 #pragmamulti_c

  3. Tcl脚本入门笔记详解（一） - 2

    TCL脚本语言简介•TCL（ToolCommandLanguage）是一种解释执行的脚本语言（ScriptingLanguage）,它提供了通用的编程能力：支持变量、过程和控制结构；同时TCL还拥有一个功能强大的固有的核心命令集。TCL经常被用于快速原型开发，脚本编程，GUI和测试等方面。•实际上包含了两个部分：一个语言和一个库。首先，Tcl是一种简单的脚本语言，主要使用于发布命令给一些互交程序如文本编辑器、调试器和shell。由于TCL的解释器是用C\C++语言的过程库实现的，因此在某种意义上我们又可以把TCL看作C库，这个库中有丰富的用于扩展TCL命令的C\C++过程和函数，所以，Tcl是

  4. 计算机网络笔记：TCP三次握手和四次挥手过程 - 2

    TCP是面向连接的协议，连接的建立和释放是每一次面向连接的通信中必不可少的过程。TCP连接的管理就是使连接的建立和释放都能正常地进行。三次握手TCP连接的建立—三次握手建立TCP连接①若主机A中运行了一个客户进程，当它需要主机B的服务时，就发起TCP连接请求，并在所发送的分段中用SYN=1表示连接请求，并产生一个随机发送序号x，如果连接成功，A将以x作为其发送序号的初始值：seq=x。主机B收到A的连接请求报文，就完成了第一次握手。客户端发送SYN=1表示连接请求客户端发送一个随机发送序号x，如果连接成功，A将以x作为其发送序号的初始值：seq=x②主机B如果同意建立连接，则向主机A发送确认报

  5. ruby-on-rails - 在 seeds.rb 中关闭验证 - 2

    如何在seeds.rb中关闭Rails3.2.3中的验证？我这样做了u1=User.createemail:'my@email.com',password:'123',validate:false但它说无法批量分配protected属性:验证。我知道这是什么意思。那么我该如何摆脱这个错误呢？ 最佳答案 你可以做到u1=User.new(email:'my@email.com',password:'123').save(validate:false) 关于ruby-on-rails-在se

  6. 华为数通笔记VXLAN&BGP EVPN - 2

    VXLAN简介定义RFC定义了VLAN扩展方案VXLAN（VirtualeXtensibleLocalAreaNetwork，虚拟扩展局域网）。VXLAN采用MACinUDP（UserDatagramProtocol）封装方式，是NVO3（NetworkVirtualizationoverLayer3）中的一种网络虚拟化技术。目的随着网络技术的发展，云计算凭借其在系统利用率高、人力/管理成本低、灵活性/可扩展性强等方面表现出的优势，已经成为目前企业IT建设的新趋势。而服务器虚拟化作为云计算的核心技术之一，得到了越来越多的应用。服务器虚拟化技术的广泛部署，极大地增加了数据中心的计算密度；同时，为

  7. [蓝桥杯单片机]学习笔记——串口通信的基本原理与应用 - 2

    目录一、原理部分1、什么是串行通信（1）并行通信与串行通信（2）串行通信的制式（3）串行通信的主要方式  2、配置串口（1）SCON和PCON：串行口1的控制寄存器（2）SBUF：串行口数据缓冲寄存器 （3）AUXR：辅助寄存器​编辑（4）ES、PS：与串行口1中断相关的寄存器（5）波特率设置  3、串口框架编写二、程序案例一、原理部分1、什么是串行通信（1）并行通信与串行通信微控制器与外部设备的数据通信，根据连线结构和传送方式的不同，可以分为两种：并行通信和串行通信。并行通信：数据的各位同时发送与接收，每个数据位使用一条导线，这种方式传输快，但是需要多条导线进行信号传输。串行通信：数据一位一

  8. 【微服务笔记23】使用Spring Cloud微服务组件从0到1搭建一个微服务工程 - 2

    这篇文章，主要介绍如何使用SpringCloud微服务组件从0到1搭建一个微服务工程。目录一、从0到1搭建微服务工程1.1、基础环境说明（1）使用组件（2）微服务依赖1.2、搭建注册中心（1）引入依赖（2）配置文件（3）启动类1.3、搭建配置中心（1）引入依赖（2）配置文件（3）启动类1.4、搭建API网关（1）引入依赖（2）配置文件（3）启动类1.5、搭建服务提供者（1）引入依赖（2）配置文件（3）启动类1.6、搭建服务消费者（1）引入依赖（2）配置文件（3）启动类1.7、运行测试一、从0到1搭建微服务工程1.1、基础环境说明（1）使用组件这里主要是使用的SpringCloudNetflix

  9. 论文笔记：InternImage—基于可变形卷积的视觉大模型，超越ViT视觉大模型，COCO 新纪录 64.5 mAP！ - 2

    目录文章信息写在前面Background&MotivationMethodDCNV2DCNV3模型架构Experiment分类检测文章信息Title：InternImage:ExploringLarge-ScaleVisionFoundationModelswithDeformableConvolutionsPaperLink：https://arxiv.org/abs/2211.05778CodeLink：https://github.com/OpenGVLab/InternImage写在前面拿到文章之后先看了一眼在ImageNet1k上的结果，确实很高，超越了同等大小下的VAN、RepLK

  10. ruby-on-rails - Rails 4 中关于匹配关键字在 Rails 3 中工作的路由问题 - 2

    在rails3中，匹配关键字有效，但在rails4中，匹配关键字不适用于路由我如何在rails4中定义这些路由此代码段在rails3中运行match'admin',:to=>'access#menu'match'show/:id',:to=>'public#show'match':controller(/:action(/:id(.:format)))'我需要rails4的通用公式，就像在rails3中一样match':controller(/:action(/:id(.:format)))' 最佳答案 Rails4删除了通用匹配，

随机推荐