在文章开始先介绍一下预积分环节使用的GTSAM库，GTSAM是基于因子图的c++库，可以用于slam优化环节，因子图由因子和变量组成，变量表示待估计的变量，因子表示变量之间的约束。在slam问题中，变量一般表示位姿，因子表示位姿之间的约束，可能是帧间约束，闭环约束。
对于它的使用，首先要构建因子图（包括构建因子图、添加先验位姿、里程计测量噪音、相邻位姿添加里程计因子、回环检测噪声、添加回环约束因子）、设置变量初值（变量初始化）、进行优化。
在进入程序之前，先看一下文件内函数的相互关系，虽然vs code可以跳转，但是跳来跳去非常容易把结构搞乱，在这里贴上一张大体的结构图：
在贴上程序注释前，把需要请提前注意的几个点放在前面，以便于读到这里时可以更有针对性的阅读。
1、 首先是IMU与Lidar坐标系之间的变换问题，在头文件中定义了一个imuConverter函数，这个函数内只包含了旋转变化，将imu坐标系变换到了lidar坐标系。在本文件中，还包含了imu2lidar和lidar2imu两个变换（这里把to记作2哈哈哈学到了），乍一看这两个变换似乎是两个坐标系之间的转换矩阵。但事实是这两个变换只包含平移。流程是这样的，作者将imu通过旋转转至雷达坐标系下，但是两个坐标系之间缺一个平移。通过lidar2imu变换将两个坐标系对齐。最后通过imu2lidar将两个坐标系最终转至lidar系下。
2、在储存imu数据的时候，有imuQueImu和imuQueOpt两个队列。这两个队列的作用是不同的，从结果来说imuQueImu是后面程序需要用到的数据，而imuQueOpt储存的是用于优化的缓存数据。从过程来说，每当lidar里程计新到一帧时，imuQueImu会删除此帧前的数据，只保留之后的数据用于两帧lidar里程计间发布imu增量式里程计；而imuQueOpt将此帧前的数据提取出来做积分，边用边删，数据用于后续优化过程。
建议从main函数按程序运行顺序来看下面的注释：
//imu预积分
//下文中涉及到雷达里程计或者激光里程计的地方
//其实两者指代同一内容（lidar）
//因为注释时间不同注释的有所不同（不太想改了）
#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>
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)
class TransformFusion : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImuOdometry;
ros::Subscriber subLaserOdometry;
ros::Publisher pubImuOdometry;
ros::Publisher pubImuPath;
Eigen::Affine3f lidarOdomAffine;
Eigen::Affine3f imuOdomAffineFront;
Eigen::Affine3f imuOdomAffineBack;
tf::TransformListener tfListener;
tf::StampedTransform lidar2Baselink;
double lidarOdomTime = -1;
deque<nav_msgs::Odometry> imuOdomQueue;
TransformFusion()
{
//如果雷达系与载体系并不相同，需要执行下面代码
if(lidarFrame != baselinkFrame)
{
//尝试获取雷达系到载体系的变换
try
{
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
}
//如果异常发生，捕获异常，输出报错信息
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
}
//订阅雷达里程计信息，来自mapOptmization.cpp
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
//订阅IMU里程计信息，来自本文件中imuHandler结尾
subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
//发布imu里程计信息以及imu轨迹
//注意此处订阅的imu是用于rviz的，上面订阅代码为odomTopic+"_incremental"是增量式
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1);
}
//里程计对应变换矩阵，在下面的雷达里程计回调函数中被调用
Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
{
double x, y, z, roll, pitch, yaw;
x = odom.pose.pose.position.x;
y = odom.pose.pose.position.y;
z = odom.pose.pose.position.z;
tf::Quaternion orientation;
tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
return pcl::getTransformation(x, y, z, roll, pitch, yaw);
}
//雷达里程计回调函数，订阅的lio_sam/mapping/odometry将被传入此函数
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
//锁定线程
std::lock_guard<std::mutex> lock(mtx);
//接收odom2affine函数传入的tf信息
lidarOdomAffine = odom2affine(*odomMsg);
//获取时间戳
lidarOdomTime = odomMsg->header.stamp.toSec();
}
//imu里程计回调函数，订阅的odomTopic+"_incremental"将被传入此函数处理
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
// static tf
//将tfmap转换到odom系下，发布tf
static tf::TransformBroadcaster tfMap2Odom;
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
//锁线程
std::lock_guard<std::mutex> lock(mtx);
//将里程计信息添加到队列
imuOdomQueue.push_back(*odomMsg);
// get latest odometry (at current IMU stamp)
//删除激光里程计前面的imu队列里的数据
if (lidarOdomTime == -1)
return;
while (!imuOdomQueue.empty())
{
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
//与激光里程计最近的imu
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
//当前最新
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
//上述两状态的矩阵变换（好像是增量位姿变换？）
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
//当前imu位姿是激光里程计位姿乘imu位姿增量变化雷达
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
// publish latest odometry（发布最新的里程计）
nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
laserOdometry.pose.pose.position.x = x;
laserOdometry.pose.pose.position.y = y;
laserOdometry.pose.pose.position.z = z;
laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
pubImuOdometry.publish(laserOdometry);
// publish tf发布tf，主要是lidarFrame与baselinkFrame变换关系
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if(lidarFrame != baselinkFrame)
tCur = tCur * lidar2Baselink;
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);
// publish IMU path
//主要是最近雷达里程计于当前时间的路径
static nav_msgs::Path imuPath;
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
//可以看出发布频率为0.1秒
if (imuTime - last_path_time > 0.1)
{
last_path_time = imuTime;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose = laserOdometry.pose.pose;
imuPath.poses.push_back(pose_stamped);
while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
imuPath.poses.erase(imuPath.poses.begin());
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
}
};
class IMUPreintegration : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImu;
ros::Subscriber subOdometry;
ros::Publisher pubImuOdometry;
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::noiseModel::Diagonal::shared_ptr correctionNoise2;
gtsam::Vector noiseModelBetweenBias;
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
std::deque<sensor_msgs::Imu> imuQueOpt;
std::deque<sensor_msgs::Imu> imuQueImu;
gtsam::Pose3 prevPose_;
gtsam::Vector3 prevVel_;
gtsam::NavState prevState_;
gtsam::imuBias::ConstantBias prevBias_;
gtsam::NavState prevStateOdom;
gtsam::imuBias::ConstantBias prevBiasOdom;
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;
//下面两处坐标变换只有平移部分
// T_bl: tramsform points from lidar frame to imu frame
gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
// T_lb: tramsform points from imu frame to lidar frame
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
IMUPreintegration()
{
//订阅imu话题发送的原始消息，
//队列长度2000，数据传入函数imuHandler处理，数据采取tcpNoDelay方式传递，该方式延迟较低
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
//订阅雷达里程计消息
//队列长度为5，数据传入odometryHandler，同样采取tcpNoDelay方式传递
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
//发布增量IMU里程计信息
pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
//关于噪声协方差部分
//噪声参数已在params.yaml文件中填入，最后一行假设了初始bias为零
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, 1e4); // m/s
priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
}
void resetOptimization()
{
gtsam::ISAM2Params optParameters;
optParameters.relinearizeThreshold = 0.1;
optParameters.relinearizeSkip = 1;
optimizer = gtsam::ISAM2(optParameters);
gtsam::NonlinearFactorGraph newGraphFactors;
graphFactors = newGraphFactors;
gtsam::Values NewGraphValues;
graphValues = NewGraphValues;
}
void resetParams()
{
lastImuT_imu = -1;
doneFirstOpt = false;
systemInitialized = false;
}
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
//锁定线程
std::lock_guard<std::mutex> lock(mtx);
//取出消息中的时间戳
double currentCorrectionTime = ROS_TIME(odomMsg);
// make sure we have imu data to integrate
//保证有imu数据可以用来积分
if (imuQueOpt.empty())
return;
//获取了位置和四元数信息
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;
//用布尔变量接收判断pose.covariance[0] == 1的结果，true时雷达里程计有退化风险
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
//将位姿转化为gtsam的格式
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
// 0. initialize system系统初始化，只有在标识为为false时执行，通常仅执行一次
if (systemInitialized == false)
{
//重置参数（isam2）
resetOptimization();
// pop old IMU message
//丢弃掉雷达里程计之前的imu数据，imu频率是高于雷达里程计数据的
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
//以下的结构类似，以第一组为例
//首先用gtsam下的函数compose将雷达位姿转移至imu下，注意这个变化只有平移，在雷达与imu变换这块一定多看看防止着道
//设置初始位姿与置信度
//将其添加到因子图中
// initial pose
prevPose_ = lidarPose.compose(lidar2Imu);
//gtsam::PriorFactor为先验因子，约束值不会离先验值太远
//下面的X、V、B是定义在头文件下面的，分别表示六自由度的位姿、三坐标速度以及六噪声
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
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
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
//将参数传入isam优化器进行优化
optimizer.update(graphFactors, graphValues);
//下面将因子和图都清除了
graphFactors.resize(0);
graphValues.clear();
//重置积分器
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;
return;
}
// reset graph for speed
//这里的key指的是激光里程计的帧，每100帧激光里程计数据就执行下面的函数
if (key == 100)
{
// get updated noise before reset
//储存一下上一帧的X、V、B噪声数据
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 graph
//重置参数（isam2）
//下面流程与初始化系统后半段内容差不多
resetOptimization();
// add pose
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
graphFactors.add(priorPose);
// add velocity
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
graphFactors.add(priorVel);
// add bias
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
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);
graphFactors.resize(0);
graphValues.clear();
key = 1;
}
// 1. integrate imu data and optimize（整合IMU数据并优化）
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);
//currentCorrectionTime是激光里程计时间数据，delta_t定义时赋值0，没找到修改此参数值的代码
if (imuTime < currentCorrectionTime - delta_t)
{
//int c=a>b?a:b; //判断a和b的大小，如果a大于b为真，则把a的值赋予c,否则把b赋予c
//判断lastImuT_opt < 0的是否成立，成立赋值dt为五百分之一，否则赋值imuTime - lastImuT_opt
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
//预积分的数据输入，包含线加速度与角速度，以及上式中dt
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);
//删除imu数据前记录下数据时间，然后将其删除
lastImuT_opt = imuTime;
imuQueOpt.pop_front();
}
else
break;
}
// add imu factor to graph
//imuIntegratorOpt_传入preint_imu。将前一帧的X、V、B与这一帧X、V与preint_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 factor
//添加了前一帧B、此帧B、观测偏差、协噪声方差
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
// add pose factor
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
// insert predicted values
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_);
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
// Reset the optimization preintegration object.
//重置预积分优化对象
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
//检查因子图优化结果，通过failureDetection函数返回if判断true或者false
if (failureDetection(prevVel_, prevBias_))
{
resetParams();
return;
}
// 2. after optiization, re-propagate imu odometry preintegration
//优化之后，重新传播imu里程计预积分
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// first pop imu message older than current correction data
//移除激光里程计帧当前时间之前的imu数据
double lastImuQT = -1;
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
// repropogate
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
//使用最新的优化后的偏置设置bias
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
//使用imuQueImu队列的数据进行预积分，bias采用的处理过的最新数据
for (int i = 0; i < (int)imuQueImu.size(); ++i)
{
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;
//使imuHandler函数可以运行通过if判断
doneFirstOpt = true;
}
//integrate imu data and optimize（整合IMU数据并优化）环节末尾判断优化结果函数
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() > 1.0 || bg.norm() > 1.0)
{
ROS_WARN("Large bias, reset IMU-preintegration!");
return true;
}
return false;
}
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
//锁定线程，防止后进入队列干扰
std::lock_guard<std::mutex> lock(mtx);
//使用在头文件中定义的函数imuConverter处理原始imu数据
//该函数将imu转移至雷达坐标系，注意只进行了旋转，没有进行平移
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
//将转换完的数据传入两个队列
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
//如果没有发生位姿变换的优化，return
//doneFirstOpt默认置false，odometryHandler函数完成置为true
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// integrate this single imu message（添加单帧）
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（预计从上一激光里程计到这一时刻的状态）
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
// publish odometry
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame;
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar（将imu里程计转移至雷达里程计，只有平移变换）
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
odometry.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();
pubImuOdometry.publish(odometry);
}
};
int main(int argc, char** argv)
{
//节点初始化
ros::init(argc, argv, "roboat_loam");
//类的实例：IMUPreintegration
IMUPreintegration ImuP;
//类的实例：TransformFusion
TransformFusion TF;
//打印消息，说明该节点已经开始工作
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
//开了四个线程
ros::MultiThreadedSpinner spinner(4);
spinner.spin();
return 0;
}
1644
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
