ROS-3DSLAM（三）lvi-sam源代码阅读1

最新推荐文章于 2023-11-30 11:18:30 发布

handsome_hhh

最新推荐文章于 2023-11-30 11:18:30 发布

阅读量1.2k

点赞数 6

分类专栏： ROS源代码分析文章标签： c++ 算法

本文链接：https://blog.csdn.net/qq_38170211/article/details/120688614

版权

ROS源代码分析专栏收录该内容

17 篇文章 22 订阅

订阅专栏

2021SC@SDUSC

（三）lvi-sam源代码阅读1

lvi-sam项目的代码主要分为两大部分：lidar_odometry（雷达里程计系统）和visual_odometry（视觉历程计系统）。

lidar_odometry

雷达里程计文件夹下包含4个.cpp文件和1个.h头文件

在这里插入图片描述

imuPreintegration.cpp

头文件部分引用了gtsam库中的很多头文件，首先分析gtsam库。

#include <gtsam/geometry/Rot3.h>
.
.
.
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>

gtsam库

GTSAM是用于计算机视觉和多传感器融合方面用于平滑和建图的C++库，采用因子图和贝叶斯网络的方式最大化后验概率。

对gtsam库的具体学习将围绕项目中使用到的相关函数等展开。

using gtsam::symbol_shorthand::X; // Pose3姿态 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel速度导数   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias陀螺仪残差  (ax,ay,az,gx,gy,gz)

成员变量定义

//定义了两个发布器，IMU和里程计；两个订阅器，IMU里程计和IMU路径
ros::Subscriber subImu;
ros::Subscriber subOdometry;
ros::Publisher pubImuOdometry;
ros::Publisher pubImuPath;


// map -> odom
tf::Transform map_to_odom;
tf::TransformBroadcaster tfMap2Odom;
// odom -> base_link
tf::TransformBroadcaster tfOdom2BaseLink;

bool systemInitialized = false;
// 噪声协方差
gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
gtsam::Vector noiseModelBetweenBias;

// imu 预积分
// //imuIntegratorOpt_负责预积分两个激光里程计之间的imu数据，作为约束加入因子图，并且优化出bias
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
//imuIntegratorImu_用来根据新的激光里程计到达后已经优化好的bias，预测从当前帧开始，下一帧激光里程计到达之前的imu里程计增量
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
// imu 数据队列
//imuQueOpt用来给imuIntegratorOpt_提供数据来源，不要的就弹出(从队头开始出发，比当前激光里程计数据早的imu通通积分，用一个扔一个)；

std::deque<sensor_msgs::Imu> imuQueOpt;
//imuQueImu用来给imuIntegratorImu_提供数据来源，不要的就弹出(弹出当前激光里程计之前的imu数据,预积分用完一个弹一个)； 
std::deque<sensor_msgs::Imu> imuQueImu;
// imu 因子图优化过程中的状态变量
gtsam::Pose3 prevPose_;
gtsam::Vector3 prevVel_;
gtsam::NavState prevState_;
gtsam::imuBias::ConstantBias prevBias_;
// imu 状态
gtsam::NavState prevStateOdom;
gtsam::imuBias::ConstantBias prevBiasOdom;
// ISAM2优化器
bool doneFirstOpt = false;
double lastImuT_imu = -1;
double lastImuT_opt = -1;

gtsam::ISAM2 optimizer;
gtsam::NonlinearFactorGraph graphFactors;//总的因子图模型
gtsam::Values graphValues;//因子图模型中的值

const double delta_t = 0;

int key = 1;
int imuPreintegrationResetId = 0;


// imu-lidar位姿变换
//这点要注意，tixiaoshan这里命名的很垃圾，这只是一个平移变换，???
//同样头文件的imuConverter中，也只有一个旋转变换。这里绝对不可以理解为把imu数据转到lidar下的变换矩阵。
//事实上，作者后续是把imu数据先用imuConverter旋转到雷达系下（但其实还差了个平移）。
//作者真正是把雷达数据又根据lidar2Imu反向平移了一下，和转换以后差了个平移的imu数据在“中间系”对齐，
//之后算完又从中间系通过imu2Lidar挪回了雷达系进行publish。


gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));;

构造函数

IMUPreintegration()
{
    //nh ： ros::NodeHandle nh;   在utility.h中定义的
    // 订阅imu原始数据，用下面因子图优化的结果，施加两帧之间的 imu预计分量 ，预测每一时刻（imu频率）的imu里程计
    subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
    // 订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）
    subOdometry = nh.subscribe<nav_msgs::Odometry>(PROJECT_NAME + "/lidar/mapping/odometry", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

// 发布 imu 里程计
    pubImuOdometry = nh.advertise<nav_msgs::Odometry> ("odometry/imu", 2000);
    pubImuPath     = nh.advertise<nav_msgs::Path>     (PROJECT_NAME + "/lidar/imu/path", 1);

    //初始化 Transform：用来表示只包括旋转和和平移的变化
    //参数：Matrix3*3 m_basis;    Vector3 m_origin; //表示一个坐标原点到 另一个坐标原点之间的 位移关系 
    //Matrix3*3 表示三维空间中的旋转（成员变量：Vector3 m_el[3] 向量数组 存储矩阵中的数据，每个向量存储矩阵的一行） 

    map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
    // imu预积分的噪声协方差
    // （shared_ptr<>内的类不需要手动释放。）
    boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
    p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
    p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
    p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
    gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
    // 噪声先验
    priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
    priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s
    priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
    // 激光里程计scan-to-map优化过程中发生退化，则选择一个较大的协方差
    correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-2); // meter
    noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
    // imu预积分器，用于预测每一时刻（imu频率）的imu里程计（转到lidar系了，与激光里程计同一个系）
    imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
    // imu预积分器，用于因子图优化
    imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization        
}

前置知识学习

贝叶斯网络

参考博客：https://blog.csdn.net/weixin_37781153/article/details/113658259

贝叶斯网络基本性质

贝叶斯网络主要用来描述随机变量之间的条件依赖，用点表示随机变量(random variables)，用边表示条件依赖(conditional dependencies)。

网络拓扑结构为有向无环图（DAG）。

边权：条件概率 P(H/E)

在这里插入图片描述

性质（1）：有向无环图（贝叶斯网络）数学描述： G=(I,E)
I: 图形中所有点的集合。
E: 所有线段的集合。

性质（2）：联合概率
在这里插入图片描述

贝叶斯网络的三种形式

1）：head-to-head

P(a,b,c) = P(a)*P(b)*P(c|a,b)成立，即在c未知的条件下，a、b被阻断(blocked)，是独立的，称之为head-to-head条件独立。

2）：tail-to-tail

考虑c未知，跟c已知这两种情况：
1. 在c未知的时候，有：P(a,b,c)=P©*P(a|c)*P(b|c)，此时，没法得出P(a,b) = P(a)P(b)，即c未知时，a、b不独立。
2. 在c已知的时候，有：P(a,b|c)=P(a,b,c)/P©，然后将P(a,b,c)=P©*P(a|c)*P(b|c)带入式子中，得到：P(a,b|c)=P(a,b,c)/P© = P©*P(a|c)*P(b|c) / P© = P(a|c)*P(b|c)，即c已知时，a、b独立。
3）：head-to-tail
1. 在c未知的时候，有：P(a,b,c)=P©*P(a|c)*P(b|c)，此时，没法得出P(a,b) = P(a)P(b).
2. 在c已知的时候，有：P(a,b|c)=P(a,b,c)/P©，然后将P(a,b,c)=P©*P(a|c)*P(b|c)带入式子中，得到：P(a,b|c)=P(a,b,c)/P© = P©*P(a|c)*P(b|c) / P© = P(a|c)*P(b|c)，即c已知时，a、b独立。
综合1）2）3）可知，在xi给定的条件下，xi+1的分布和x1,x2…xi-1条件独立。意味着啥呢？意味着：xi+1的分布状态只和xi有关，和其他变量条件独立。通俗点说，当前状态只跟上一状态有关，跟上上或上上之前的状态无关。这种顺次演变的随机过程，就叫做马尔科夫链（Markov chain）。

因子图

参考博客：https://blog.csdn.net/weixin_37781153/article/details/113658259

因子图定义（factor graph）

具有多变量的全局函数因子分解，得到几个局部函数的乘积，以此为基础得到的一个双向图叫做因子图.

在这里插入图片描述

图表示（举例说明）

g(x1,x2,x3,x4,x5)=fA(x1)fB(x2)fC(x1,x2,x3)fD(x3,x4)fE(x3,x5)

其中fA,fB,fC,fD,fE为各函数，表示变量之间的关系，可以是条件概率也可以是其他关系。其对应的因子图为：

在这里插入图片描述

IMU预积分

参考博客：

https://zhuanlan.zhihu.com/p/38009126

https://zhuanlan.zhihu.com/p/90213963

IMU概念

IMU（Inertial measurement unit 惯性测量单元）。

概括的说，IMU主要用来测量：加速度 $a$ 是沿三个轴 $a_x,a_y,a_z$ 方向的线加速度，而角速度 $w$ 就是这三个方向的角速度 $w_x,w_y,w_z$ 。
在IMU内部，除了通常的白噪声，还有个特别的量零偏bias，在这是传感器内部机械、温度等各种物理因素产生的传感器内部误差的综合参数。IMU的加速度计和陀螺仪的每个轴都用彼此相互独立的参数建模，角速度、加速度的测量值和真值之间的连续域上的关系可以写作：

在这里插入图片描述

在这里插入图片描述
表示旋转四元数，从世界坐标系到IMU坐标系， $g^w$ 表示世界坐标系下的重力矢量，b，n分别表示加速度计和陀螺仪的偏置和噪声。

从上面的公式可以看出，我们读的数据都不是客观事实，是在客观事实的基础上叠加上传感器的误差，也就是偏置和噪声。

a 和 g 分别表示 acc 加速度和 gyro 陀螺仪，w 表示在世界坐标系，b 表示IMU体坐标系。

IMU预积分

位移，速度和姿态（position, velocity, quaternion, i.e. PVQ）对时间的导数可以写成：

在这里插入图片描述

从第 i 时刻的PVQ 对 IMU的测量值进行积分得到第 j 时刻的PVQ：

在这里插入图片描述

先抛开公式里的具体推导不谈，因为IMU的采样频率高，通常为100Hz - 1000Hz，数据量非常大，在做优化的时候，不可能将如此多的数据都放到状态变量中，因此通常的做法是每隔一段时间提取一个数据。也就是上式中假如 i 是第一秒提取的IMU数据，j 是第二秒提取的IMU数据。

基本过程就是：已知第 i 秒的PVQ；第 i 秒和第 j 秒中间所有数据（如100个）以及我们已知的运动学知识积分，从第 i 秒一点一点积分得到第 j 秒的PVQ。

但是这样在做后端优化的过程中，当进行迭代求解计算来更新和调整PVQ的值时，一旦（比如第 1 秒）的PVQ进行了调整，每一个中间过程以及后面所有的轨迹都要重新再积分算一遍，如果是100Hz，两秒之间有100个采集数据，就要计算100次积分。

预积分的目的就是尝试将这100次积分过程变成只有1次积分，或者说用1个值来代替100个值，通过预积分模型的应用可以大大节省了计算量。

在这里插入图片描述

将在这里插入图片描述
从
中分离出来，改变了公式中的积分项：

在这里插入图片描述

每次优化迭代的过程中，调整姿态都是相对于世界坐标系调整。

在这里插入图片描述

上图的三个项称为IMU预积分，分别对应了位置，速度和姿态。预积分量仅和IMU测量值有关，它将一段时间内的IMU数据直接积分起来就得到了预积分量。

预积分的离散形式：

使用mid-point方法，即两个相邻时刻k到 k+1的位姿是用两个时刻的测量值
$a, w$
的平均值来计算：

在这里插入图片描述

总结来说，IMU预积分是三个积分值，对应了位置，速度和姿态。通过IMU的测量值a加速度和w角速度，以及PVQ的初始值，计算出后续的PVQ值；IMU预积分就是用来加速这一过程的。

预积分误差

通过将积分模型转化为预积分模型有效地减小了计算量，但是同时丢失了一些东西。当我们用1个结果代替（如100个）数据点的时候，我们就不知道这1个结果的不确定度了。

在转化之前，这100个数据点每一个数据点的不确定度我们是知道的（因为IMU数据作为测量值的噪声方差我们能够标定），但是这100个数据积分形成的预积分量的方差需要在得到IMU预积分的结果之后，还要推导预积分量的协方差，需要知道IMU噪声和预积分量之间的线性递推关系。

handsome_hhh

关注

6
点赞
踩
19

收藏

觉得还不错? 一键收藏
0
评论
ROS-3DSLAM（三）lvi-sam源代码阅读1

2021SC@SDUSC（三）lvi-sam源代码阅读1lvi-sam项目的代码主要分为两大部分：lidar_odometry（雷达里程计系统）和visual_odometry（视觉历程计系统）。lidar_odometry雷达里程计文件夹下包含4个.cpp文件和1个.h头文件imuPreintegration.cpp头文件部分引用了gtsam库中的很多头文件，首先分析gtsam库。#include <gtsam/geometry/Rot3.h>...#include &
复制链接

扫一扫

专栏目录