赞
踩
2021SC@SDUSC
lvi-sam项目的代码主要分为两大部分:lidar_odometry(雷达里程计系统)和visual_odometry(视觉历程计系统)。
雷达里程计文件夹下包含4个.cpp文件和1个.h头文件
头文件部分引用了gtsam库中的很多头文件,首先分析gtsam库。
#include <gtsam/geometry/Rot3.h>
.
.
.
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
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已知这两种情况:
3):head-to-tail
综合1)2)3)可知,在xi给定的条件下,xi+1的分布和x1,x2…xi-1条件独立。意味着啥呢?意味着:xi+1的分布状态只和xi有关,和其他变量条件独立。通俗点说,当前状态只跟上一状态有关,跟上上或上上之前的状态无关。这种顺次演变的随机过程,就叫做马尔科夫链(Markov chain)。
参考博客:https://blog.csdn.net/weixin_37781153/article/details/113658259
具有多变量的全局函数因子分解,得到几个局部函数的乘积,以此为基础得到的一个双向图叫做因子图.
图表示(举例说明)
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为各函数,表示变量之间的关系,可以是条件概率也可以是其他关系。其对应的因子图为:
参考博客:
https://zhuanlan.zhihu.com/p/38009126
https://zhuanlan.zhihu.com/p/90213963
IMU(Inertial measurement unit 惯性测量单元)。
概括的说,IMU主要用来测量:加速度
a
a
a是沿三个轴
a
x
,
a
y
,
a
z
a_x,a_y,a_z
ax,ay,az方向的线加速度,而角速度
w
w
w就是这三个方向的角速度
w
x
,
w
y
,
w
z
w_x,w_y,w_z
wx,wy,wz。
在IMU内部,除了通常的白噪声,还有个特别的量零偏bias,在这是传感器内部机械、温度等各种物理因素产生的传感器内部误差的综合参数。IMU的加速度计和陀螺仪的每个轴都用彼此相互独立的参数建模,角速度、加速度的测量值和真值之间的连续域上的关系可以写作:
表示旋转四元数,从世界坐标系到IMU坐标系,
g
w
g^w
gw表示世界坐标系下的重力矢量,b,n分别表示加速度计和陀螺仪的偏置和噪声。
从上面的公式可以看出,我们读的数据都不是客观事实,是在客观事实的基础上叠加上传感器的误差,也就是偏置和噪声。
a 和 g 分别表示 acc 加速度 和 gyro 陀螺仪,w 表示在世界坐标系,b 表示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
a,w
a,w
的平均值来计算:
总结来说,IMU预积分是三个积分值,对应了位置,速度和姿态。通过IMU的测量值a加速度和w角速度,以及PVQ的初始值,计算出后续的PVQ值;IMU预积分就是用来加速这一过程的。
通过将积分模型转化为预积分模型有效地减小了计算量,但是同时丢失了一些东西。当我们用1个结果代替(如100个)数据点的时候,我们就不知道这1个结果的不确定度了。
在转化之前,这100个数据点每一个数据点的不确定度我们是知道的(因为IMU数据作为测量值的噪声方差我们能够标定),但是这100个数据积分形成的预积分量的方差需要在得到IMU预积分的结果之后,还要推导预积分量的协方差,需要知道IMU噪声和预积分量之间的线性递推关系。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。