LVI-SAM imuPreintegration代码解析

写在前面：这个部分imu预积分，具体是啥，大家可以自行网络搜索，我的理解就是在进行优化的时候为了方便计算，不至于从头开始计算，于是提出了预积分的概念，然后相关公式大家可以在网上自行查阅。还有预积分的话，就是imu获取数据的频率是很高的，但是雷达有些慢。比如雷达获取1帧的数据，此时imu已经有了10帧数据，因此预积分也可以将这10帧imu数据变成1帧，然后与雷达的数据帧数对齐。下面是代码解析。

头文件

// 开头 包含很多库，然后这个gtsam是关于解决slam的一个库，这个暂时还没仔细看里面的内容
#include "utility.h"#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>

头文件是一些关于gtsam的文件，这个库是专门来解决slam或者sfm问题的。然后其中的具体内容没看，但是那个ISAM2是用来解决优化问题的，可以理解成来求解的。

变量

// 这里是一个变量的使用声明
using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 姿态
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot) 速度 这个dot表示导数的意思
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz) 偏差// 下面这4个都是在ros::里面的，说明下面的变量可能最终会在rviz里面进行展示// 输入ros::Subscriber subImu; // 订阅器ros::Subscriber subOdometry;// 输出ros::Publisher pubImuOdometry; // 发布器ros::Publisher pubImuPath;// map -> odom 转换关系，前面所示，不多讲tf::Transform map_to_odom;tf::TransformBroadcaster tfMap2Odom;// odom -> base_linktf::TransformBroadcaster tfOdom2BaseLink;bool systemInitialized = false; // 这个是系统初始化// 噪声协方差gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; // Diagonal对角线  先验位姿噪声gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; // 先验速度噪声gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; // 先验偏差噪声gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; // 修正噪声gtsam::Vector noiseModelBetweenBias;// 预积分// 负责预积分两个激光里程计之间的imu数据，作为约束加入因子图，并且优化出bias// 这个和优化有关gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;// 用来根据新的激光里程计到达后已经优化好的bias，预测从当前帧开始，下一帧激光里程计到达之前的imu里程计增量// 这个和imu预测有关gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;// 下面这两个双端队列，用来存储上面的两个指针（或者叫数据来源）// 给imuIntegratorOpt_提供数据来源，不要的就弹出（从队头开始出发，比当前激光里程计数据早的imu通通积分，用一个仍一个）std::deque<sensor_msgs::Imu> imuQueOpt;// 给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;bool doneFirstOpt = false; // 这个应该是判断是否是第一次来优化的double lastImuT_imu = -1;double lastImuT_opt = -1;// iSAM2优化器gtsam::ISAM2 optimizer;gtsam::NonlinearFactorGraph graphFactors; // 总的因子图模型gtsam::Values graphValues; // 因子图模型中的值const double delta_t = 0;int key = 1;int imuPreintegrationResetId = 0; // 这个应该和reset有关系// 这里是imu-lidar位姿变换// 这里不可以理解为把imu数据转到lidar下的变换矩阵// 作者把imu数据先用imuConverter旋转到雷达系下（但其实还差个平移）// 通过lidar2Imu将雷达数据反向平移了一下，和转换以后差了个平移的imu数据在“中间系”对齐// 之后算完又从中间系通过imu2lidar挪回了雷达系进行publishgtsam::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 == nodehandle 是个函数句柄// 订阅imu原始数据，用下面因子图优化的结果，施加两帧之间的imu预积分量，预测每一时刻（imu频率）的imu里程计subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay()); // 最后这个参数是ros里面信息的传输方式，这里用的是tcp传输，而且是无延迟的//订阅激光里程计，来自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);map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));// imu预积分的噪声协方差boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);// 加速计的   pow(x,y)是求x的y次方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 velocitiesgtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias 假设零初始偏差 // 噪声先验// 这里是噪声，猜测后面的sigma函数是有关这个噪声分布的，然后注释后面跟着每个数值的单位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, mpriorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/spriorBiasNoise  = 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); // meternoiseModelBetweenBias = (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预积分器，用于因子图优化 这里的Opt是optimization的缩写imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization        }

构造函数，里面用了下面的odometryhandler和imuhandler，其他的话没啥好解释的，就看里面的内容就可以。然后这个sigma（）函数的话，个人认为是和分布有关的函数。

    void resetOptimization(){//ISAM2的参数类 重新赋值gtsam::ISAM2Params optParameters;optParameters.relinearizeThreshold = 0.1; // relinearize 重新线性化optParameters.relinearizeSkip = 1;//优化器 根据参数重新生成优化器optimizer = gtsam::ISAM2(optParameters);gtsam::NonlinearFactorGraph newGraphFactors;graphFactors = newGraphFactors;gtsam::Values NewGraphValues;graphValues = NewGraphValues;}

重新设置图优化的参数，这里的话就关注一下图优化里面的内容，就是一个graphFactors graphValues的值就可以。

    void resetParams(){//将上一帧imu数据的时间戳更新成-1（代表还没有imu数据进来）lastImuT_imu = -1;//false代表需要重新进行一次odom优化（跟imuHandler联系起来）doneFirstOpt = false;//系统关闭systemInitialized = false;}

重新设置参数，这个没啥好解释的，然后里面的值的话用在下面函数的if判断条件里面，逻辑搞清楚即可。

    /*里程计回调函数每隔100帧激光里程计，重置iSAM2优化器，添加里程计、速度、偏置先验因子，执行优化计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前位姿，进行因子图优化，更新当前帧状态。优化之后，获得imu真实的bias，用来计算当前时刻之后的imu预积分。*/void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){// 当前帧激光里程计时间戳 就是获取时间的double currentCorrectionTime = ROS_TIME(odomMsg);// make sure we have imu data to integrate// 确保imu优化队列中有imu数据进行预积分// 在imuHander函数中，会将imu数据push到队列中，数据是imu原始数据经过旋转但没平移到雷达坐标系下的数据if (imuQueOpt.empty())return;// 当前帧激光位姿，来自scan-to-map匹配，因子图优化后的位姿float p_x = odomMsg->pose.pose.position.x;float p_y = odomMsg->pose.pose.position.y;float p_z = odomMsg->pose.pose.position.z;// 这个就是四元数的那四个值float r_x = odomMsg->pose.pose.orientation.x;float r_y = odomMsg->pose.pose.orientation.y;float r_z = odomMsg->pose.pose.orientation.z;float r_w = odomMsg->pose.pose.orientation.w;// 四舍五入 这个resetid一直没太看懂，后面是四舍五入了方差的第一个元素int currentResetId = round(odomMsg->pose.covariance[0]);// 用订阅的激光雷达里程计消息，初始化一个lidarpose 包括一个四元数和pointsgtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));// correction pose jumped, reset imu pre-integration//比较当前里程计id 和 imu预积分的id 来判断是否属于同一个时间段内，雷达的id 可以连续几帧都是一个idif (currentResetId != imuPreintegrationResetId){resetParams();imuPreintegrationResetId = currentResetId;return;}// 0. initialize system// 系统初始化，第一帧if (systemInitialized == false){resetOptimization(); // 具体可以转到这个函数的声明，没啥难度 先重新设置参数// pop old IMU message// 从imu优化队列中删除当前帧激光里程计时刻之前的imu数据while (!imuQueOpt.empty()){// 最终得到了离currentCorrectionTime - delta_t 最近的那个imu时间if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t){// 这里判断条件中 delta_t的值为0//将队首元素的时间戳记录下来，更新lastImuT_opt，再将元素弹出lastImuT_opt = ROS_TIME(&imuQueOpt.front());imuQueOpt.pop_front();}elsebreak;}// initial pose// 添加里程计位姿先验因子// 将雷达位置转换到Imu坐标系下prevPose_ = lidarPose.compose(lidar2Imu);gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);// 先验因子中的参数：位姿信息和噪声，在构造函数中初始化过，是一个常量// 上面priorPose里面的第一个参数X ，参见：using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 姿态graphFactors.add(priorPose); // 添加先验位姿 先验是计算得到的，后验是观测得到的// initial velocity// 添加里程计速度先验因子prevVel_ = gtsam::Vector3(0, 0, 0);gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise); // 这个没啥好说的 和上面是一个模式// 类似于位姿先验因子的添加graphFactors.add(priorVel);// initial bias// 添加imu偏差先验因子prevBias_ = gtsam::imuBias::ConstantBias();gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);graphFactors.add(priorBias);// add values// 变量节点赋初值，没啥好说的，这里赋值为0graphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);// optimize once// 假定起始为0速度，进行一次优化，用于刚开始的scan-matching// 对于速度小于10m/s 角速度小于180度/s 效果都非常好// 但是这总归是基于0起始速度估计的，起始估计并不是实际情况，因此消除图优化中内容optimizer.update(graphFactors, graphValues);// 下面清空操作的意义：但是这总归是基于0起始速度估计的，起始估计并不是实际情况，因此消除图优化中内容graphFactors.resize(0);graphValues.clear();//重置预积分器imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);//key = 1 表示有一帧//系统初始化成功key = 1;systemInitialized = true;return;}// reset graph for speedif (key == 100){// get updated noise before reset// 下面三行赋值操作gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));// reset graphresetOptimization();// 下面就是一些和上面初始化相同的操作，就是初始化个值为0的容器，然后加入到因子图中，然后因子图的值也赋为0。// add posegtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);graphFactors.add(priorPose);// add velocitygtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);graphFactors.add(priorVel);// add biasgtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);graphFactors.add(priorBias);// add valuesgraphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);// optimize once// 假定起始为0速度，进行一次优化，用于刚开始的scan-matching// 对于速度小于10m/s 角速度小于180度/s 效果都非常好// 但是这总归是基于0起始速度估计的，起始估计并不是实际情况，因此消除图优化中内容optimizer.update(graphFactors, graphValues);graphFactors.resize(0);graphValues.clear();key = 1;}// 1. integrate imu data and optimize// 初始化完成后就可以估计IMU偏差，机器人位姿，速度// 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计// 添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态while (!imuQueOpt.empty()){// pop and integrate imu data that is between two optimizations// 提取前一帧与当前帧之间的imu数据，计算预积分sensor_msgs::Imu *thisImu = &imuQueOpt.front();double imuTime = ROS_TIME(thisImu);// 判断这一帧是否已经超过了当前激光雷达帧的时间戳if (imuTime < currentCorrectionTime - delta_t){double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);// 此处积分只用到了线加速度和角速度// 加入的是用来因子图优化的预积分器imuIntegratorOpt_.注意加入了上一步计算出的dt// 作者要求的9轴imu数据中欧拉角在本程序中没有任何用到，全在地图优化里用到的imuIntegratorOpt_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);lastImuT_opt = imuTime;//从队列中删除已经处理的imu数据imuQueOpt.pop_front();}elsebreak;}// add imu factor to graph// 加imu预积分因子，利用两帧之间的IMU数据完成了预积分后增加imu因子到因子图中const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);//函数参数：前一帧位姿，前一帧速度，当前帧位姿，当前帧速度，前一帧偏差，预积分量gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);graphFactors.add(imu_factor);// add imu bias between factorgraphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));// add pose factorgtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, correctionNoise);//添加位姿因子graphFactors.add(pose_factor);// insert predicted values//用前一帧的状态、偏差、施加imu预积分量来得到当前帧的状态gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);graphValues.insert(X(key), propState_.pose());graphValues.insert(V(key), propState_.v());graphValues.insert(B(key), prevBias_);// optimize 这里的优化为什么也清零了？？ 可能每个优化都会清零，也有可能是这还在初始阶段，优化结果不准确optimizer.update(graphFactors, graphValues);optimizer.update();graphFactors.resize(0);graphValues.clear();// Overwrite the beginning of the preintegration for the next step.// 优化结果gtsam::Values result = optimizer.calculateEstimate();// 更新当前帧位姿、速度 --> 上一帧 这步就是当前帧的变成了上一帧的prevPose_  = result.at<gtsam::Pose3>(X(key));prevVel_   = result.at<gtsam::Vector3>(V(key));// 更新当前帧状态 --> 上一帧prevState_ = gtsam::NavState(prevPose_, prevVel_);// 更新当前帧imu偏差prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));// Reset the optimization preintegration object.// 重置预积分器，设置新的偏差，这样下一帧激光里程计进来的时候，预积分量就是两帧之间的增量imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);// check optimization// 检查是否失效 对速度 和 偏差进行检查// 这个判断里面的failureDetection很easyif (failureDetection(prevVel_, prevBias_)){resetParams();return;}// 2. after optiization, re-propagate imu odometry preintegration // re-propagate：重新传播prevStateOdom = prevState_;prevBiasOdom  = prevBias_;// first pop imu message older than current correction datadouble lastImuQT = -1;// 从imu队列中删除当前激光里程计时刻之前的imu数据while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t){// 弹出lastImuQT = ROS_TIME(&imuQueImu.front());imuQueImu.pop_front();}// repropogate// 理由：对剩余的imu数据计算预积分，因为bias改变了if (!imuQueImu.empty()){// reset bias use the newly optimized bias// 重置预积分器和最新的偏置，使用最新的偏差更新bias值imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);// integrate imu message from the beginning of this optimizationfor (int i = 0; i < (int)imuQueImu.size(); ++i){//重新进行预积分//与imuHandler中的预积分过程相同sensor_msgs::Imu *thisImu = &imuQueImu[i];double imuTime = ROS_TIME(thisImu);double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);lastImuQT = imuTime;}}//记录帧数++key;//优化器开启doneFirstOpt = true;}

里程计handler函数，可以说句柄函数，也可以说是回调函数。然后这里的话就先判断各种条件，然后初始化系统，然后判断帧是否到了100，如果到了100就重新设置图。个人认为到了100帧有可能累计误差太大的原因，会导致图优化不准确，因此需要重置一下。然后重置以后，添加图因子，然后插入graphValues的值，然后用optimizer进行计算更新，最后的话对graphFactors和graphValues的值进行清空和重新赋值空间，这步操作在多处出现，应该是计算优化的一种套路。然后系统初始化以后开始积分imu的数据和优化，优化过后，re-propagate imu odometry preintegration。最后记录帧数 and doneFirstOpt的值进行更新一下就可以。这里的话，图优化的部分不是理解很深刻，就知道了一些基本的添加因子，计算的操作。对于到底是怎么计算的，还不是很清楚明白。

这里再整理一下这步的思路：

0. initialize system

1. integrate imu data and optimize

2. after optiization, re-propagate imu odometry preintegration

就是以上三步，完成了odomHandler里面的主要内容。

    //速度大于30或者偏差大于0.1，则返回失效，否则没有失效bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur){Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());if (vel.norm() > 30){ROS_WARN("Large velocity, reset IMU-preintegration!");return true;}Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());if (ba.norm() > 0.1 || bg.norm() > 0.1){ROS_WARN("Large bias, reset IMU-preintegration!");return true;}return false;}

一个失败检测的小小函数，看看到底是怎么写的

    // header中包含了三个参数，方向协方差，角速度协方差，加速度协方差// 这个叫做回调函数// 接受从imu得到的原始数据进行处理，利用时间戳信息在imu与积分器中加入该帧// 然后利用上一帧中的激光里程计时刻对应的状态和偏差，加入当前帧的预测，得到当前时刻的状态。void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg){// 这里的imuConverter函数只有旋转，没有平移sensor_msgs::Imu thisImu = imuConverter(*imuMsg);//后续与thisImu有关的都是在差一个平移的雷达坐标系中// publish static tftfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, thisImu.header.stamp, "map", "odom"));imuQueOpt.push_back(thisImu);imuQueImu.push_back(thisImu);// 这里要求上一次imu因子图优化执行成功，确保更新了上一帧的状态、bias、预积分的信息// 如果没有执行过odom优化，或者上一次优化失败导致系统重置，则等待一次odom的优化再继续函数流程。if (doneFirstOpt == false)return;// 获取当前imu因子图的时间戳double imuTime = ROS_TIME(&thisImu);// double imuTime = ROS_TIME(&thisImu);// lastImuT_imu初始值为-1// 如果首次优化，则定义初始时间为1/500秒，否则与上一帧作差double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);// 更新上一帧imu因子图的时间戳lastImuT_imu = imuTime;// integrate this single imu message// imu预积分器添加一帧imu数据imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),gtsam::Vector3(thisImu.angular_velocity.x,    thisImu.angular_velocity.y,    thisImu.angular_velocity.z), dt);// predict odometry// 用上一帧激光里程计时刻对应的信息，加上imu预积分量，得到当前时刻的状态。gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);// publish odometry// 发布imu里程计（转到lidar系，与激光里程计同一个系）// 这里出现header的第二个内容frame_id 帧的名称nav_msgs::Odometry odometry;odometry.header.stamp = thisImu.header.stamp;odometry.header.frame_id = "odom";odometry.child_frame_id = "odom_imu";// transform imu pose to ldiar// 预测值curretnState获得imu位姿，再由imu到雷达变换，获得雷达位姿gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);// pose表示位置的point 和 quaternion，以及对应的协方差矩阵// twist表示速度的linear 和 angular，以及对应的协方差矩阵// 第一个pose:geometry_msgs/PoseWithCovariance// 第二个pose:geometry_msgs/Pose,包含一个point 和 一个quaternionodometry.pose.pose.position.x = lidarPose.translation().x();odometry.pose.pose.position.y = lidarPose.translation().y();odometry.pose.pose.position.z = lidarPose.translation().z();odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();odometry.twist.twist.linear.x = currentState.velocity().x();odometry.twist.twist.linear.y = currentState.velocity().y();odometry.twist.twist.linear.z = currentState.velocity().z();odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();// information for VINS initialization// 这8个是什么要清楚，但是第一个好像没看懂，有点套娃的感觉odometry.pose.covariance[0] = double(imuPreintegrationResetId); // 第一个元素是imuPreintegrationResetId 这个地方先存疑odometry.pose.covariance[1] = prevBiasOdom.accelerometer().x();odometry.pose.covariance[2] = prevBiasOdom.accelerometer().y();odometry.pose.covariance[3] = prevBiasOdom.accelerometer().z();odometry.pose.covariance[4] = prevBiasOdom.gyroscope().x();odometry.pose.covariance[5] = prevBiasOdom.gyroscope().y();odometry.pose.covariance[6] = prevBiasOdom.gyroscope().z();odometry.pose.covariance[7] = imuGravity;//发布里程计信息pubImuOdometry.publish(odometry);// publish imu pathstatic nav_msgs::Path imuPath;static double last_path_time = -1;//当前imu因子图的时间戳if (imuTime - last_path_time > 0.1){last_path_time = imuTime;//定义一个带有时间戳的pose的信息geometry_msgs::PoseStamped pose_stamped;/*把这一帧imu的时间戳、关联的坐标系id、位置相关信息赋值给pose_stampedthisImu是在原始的imu数据旋转到雷达坐标系后的数据*/pose_stamped.header.stamp = thisImu.header.stamp;pose_stamped.header.frame_id = "odom";pose_stamped.pose = odometry.pose.pose;//加入pathimuPath.poses.push_back(pose_stamped);while(!imuPath.poses.empty() && abs(imuPath.poses.front().header.stamp.toSec() - imuPath.poses.back().header.stamp.toSec()) > 3.0)imuPath.poses.erase(imuPath.poses.begin());if (pubImuPath.getNumSubscribers() != 0){//给imuPath添加时间戳stamp 和 坐标系id frame_idimuPath.header.stamp = thisImu.header.stamp;imuPath.header.frame_id = "odom";//发布消息pubImuPath.publish(imuPath);}}// publish transformation// 发布odom到base_link的变换，其实也就是到imu的变换tf::Transform tCur;// pose.pose 是imu里程计的pose信息（位置和方向）tf::poseMsgToTF(odometry.pose.pose, tCur);tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, thisImu.header.stamp, "odom", "base_link");tfOdom2BaseLink.sendTransform(odom_2_baselink);}
};

这里的话，就涉及到了坐标系的转换，一定要弄清楚这里面的关系。这一步的话，没看出来有啥特别的，就是再不同坐标系更新数据，然后进行发布。这里我没看来什么特别的地方。

int main(int argc, char** argv)
{ros::init(argc, argv, "lidar");IMUPreintegration ImuP;ROS_INFO("\033[1;32m----> Lidar IMU Preintegration Started.\033[0m");ros::spin();return 0;
}

然后是主函数，先ros::init，然后类的实体，然后spin（）在监控回调函数，这一部分基本ok。

这部分的话就是预积分的内容，虽然代码看过感觉没问题，但是里面的具体的计算还是没有进行，这部分还是要继续看。

问题：

odometry.pose.covariance[0] = double(imuPreintegrationResetId); // 第一个元素是imuPreintegrationResetId 这个地方先存疑

这个重新set的值到底来自哪里，并且它的数据记录都是啥样的没看懂，我看了一下上下文，一种套娃的感觉。

这部分的话，就到这里。然后现在知道了header里面不仅有stamp 还有 frame_id这个东西。这个header还是要多加关注，而且那个covariance[0]的那个内容也得多加关注！

LVI-SAM imuPreintegration代码解析相关推荐

matrix_multiply代码解析
matrix_multiply代码解析关于matrix_multiply 程序执行代码里两个矩阵的乘法,并将相乘结果打印在屏幕上. 示例的主要目的是展现怎么实现一个自定义CPU计算任务. 参考:ht ...
CornerNet代码解析——损失函数
CornerNet代码解析--损失函数文章目录 CornerNet代码解析--损失函数前言总体损失 1.Heatmap的损失 2.Embedding的损失 3.Offset的损失前言今天要解 ...
视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
java获取object属性值_java反射获取一个object属性值代码解析
有些时候你明明知道这个object里面是什么,但是因为种种原因,你不能将它转化成一个对象,只是想单纯地提取出这个object里的一些东西,这个时候就需要用反射了. 假如你这个类是这样的: privat ...
python中的doc_基于Python获取docx/doc文件内容代码解析
这篇文章主要介绍了基于Python获取docx/doc文件内容代码解析,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友可以参考下整体思路: 下载文件并修改后缀 ...
mongoose框架示例代码解析（一）
mongoose框架示例代码解析(一) 参考: Mongoose Networking Library Documentation(Server) Mongoose Networking Librar ...
ViBe算法原理和代码解析
ViBe - a powerful technique for background detection and subtraction in video sequences 算法官网:http:// ...
【Android 逆向】使用 Python 代码解析 ELF 文件 ( PyCharm 中进行断点调试 | ELFFile 实例对象分析 )
文章目录一.PyCharm 中进行断点调试二.ELFFile 实例对象分析一.PyCharm 中进行断点调试在上一篇博客 [Android 逆向]使用 Python 代码解析 ELF 文件 ( ...
密码算法中iv值是什么_?标检测中的?极?值抑制算法(nms)：python代码解析
⾮极⼤值抑制(Non-Maximum Suppression)原理⾮极⼤值抑制,顾名思义,找出极⼤值,抑制⾮极⼤值.这种思路和算法在各个领域中应⽤⼴泛,⽐如边缘检测算法canny算⼦中就使⽤了该⽅法 ...

LVI-SAM imuPreintegration代码解析

LVI-SAM imuPreintegration代码解析相关推荐

最新文章

热门文章