1.1 概述
本文的书写参考了许多zzk大佬文章SLAM学习笔记(二十)LIO-SAM流程及代码详解(最全)以及知乎卢涛大佬的文章LIO-SAM源码解析(三):IMUPreintegration,在二者的基础上对源代码进行了梳理,关于GTSAM图优化部分的理解基本都是在学习两位大佬的解释,有读到这篇文章并在imu紧耦合方面有志于学的读者可以多多参考两位的文章。
1.2 整体梳理
整体梳理了一遍发现还是对图优化的部分云里雾里看不太明白,目前仅仅是对整体流程有了一点理解,这种main函数的书写方式也确实是令我耳目一新,主要起作用的部分是两个类TransformFusion类和ImuPreintegration类,前者的功能是利用高帧率的imu数据实现了对低帧率的雷达点云数据的修正,替代了一般意义上松耦合imu对雷达点云的线性外推与旋转补正功能,节约内存和计算时间,写的很好;后者的功能是整个LIO-SAM代码的精髓内容,这里引用卢涛大佬的说法,用激光里程计,两帧激光里程计之间的imu预计分量构建因子图,优化当前帧的状态(包括位姿、速度、偏置),以优化后的状态为基础,施加imu预计分量,得到每一时刻的imu里程计。这部分是mapOptmization的一个前馈修正,里面关于imu预积分和构建因子图的方式十分的精炼高效,对初学者的学习有很大的帮助。
1.3 代码阅读笔记
#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)
//这个类主要是实现利用高帧率的imu数据对低帧率的雷达里程计进行精度修正
class TransformFusion : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImuOdometry;
ros::Subscriber subLaserOdometry;
ros::Publisher pubImuOdometry;
ros::Publisher pubImuPath;
//激光里程计对应旋转变换矩阵
Eigen::Affine3f lidarOdomAffine;
//imu里程计队列中最早的一帧对应旋转变换矩阵
Eigen::Affine3f imuOdomAffineFront;
//imu里程计队列中最新的一帧对应旋转变换矩阵
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());
}
}
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
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)
{
//xyz信息直接从里程计中提取
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;
//rpy角信息先从里程计中获取四元数,利用tf库转换为3X3的矩阵,然后再转化为rpy三个角
tf::Quaternion orientation;
tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
//将给定的xyzrpy角创建平移旋转变换矩阵
return pcl::getTransformation(x, y, z, roll, pitch, yaw);
}
//雷达里程计回调函数,将位姿信息转化为旋转变换矩阵
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
lidarOdomAffine = odom2affine(*odomMsg);
lidarOdomTime = odomMsg->header.stamp.toSec();
}
//imu里程计回调函数,输入里程计信息 核心功能是保存imu队列,并计算imu里程计增量变换,用得到的增量旋转变换矩阵来修正雷达里程计,提高精度
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
//创建一个tf发布对象tfMap2Odom,创建一个tf对象map_to_odom,初始值全部是0 这块我没理清楚,感觉里面并没有赋值的过程,一直在发0,也就是两个坐标系重合在一块?
static tf::TransformBroadcaster tfMap2Odom;
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
//通过sendTransform发布tf变换
//tf::StampedTransform是tf的一种特殊情况:它需要frame_id和stamp以及child_frame_id
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
std::lock_guard<std::mutex> lock(mtx);
//imu里程计装入最新一帧
imuOdomQueue.push_back(*odomMsg);
//保证雷达里程计里面有数,没有的话直接返回了,只有初始化的时候触发一次
if (lidarOdomTime == -1)
return;
while (!imuOdomQueue.empty())
{
//把imu里程计中,时间戳小于当前雷达里程计时间戳的数据全都pop出去
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
//imu里程计队列中最早的一帧,也就是和当前雷达里程计时间最接近的那一帧,转化为旋转矩阵
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
//imu里程计队列中最新的一帧,转化为旋转矩阵
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
//矩阵运算front到back的旋转变换量,T_back = T_front * T_inc,那么T_inc = T_front逆 * T_back
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
//矩阵运算,将雷达里程计根据imu里程计的增量作修正 T_lidar_now = T_lidar * T_inc
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
float x, y, z, roll, pitch, yaw;
//把上面修正过的雷达里程计中的xyzrpy取出来,进行后面的运算
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
//把修正过的雷达里程计发布出来,初值用的是imuOdomQueue,这个我个人感觉无所谓,用空的nav_msgs::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);
//发布修正过后的雷达里程计到baselink的tf变换
static tf::TransformBroadcaster tfOdom2BaseLink;
//修正过后的雷达里程计装入tCur
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);
//发布imu里程计path,用来在rviz实时显示轨迹,不过根据while中的删选条件能够看到显示的是一秒内的imu轨迹
static nav_msgs::Path imuPath;
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
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);
}
}
}
};
//这个类的主要功能是构建因子图,把imu加入其中,实现紧耦合
class IMUPreintegration : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImu;
ros::Subscriber subOdometry;
ros::Publisher pubImuOdometry;
bool systemInitialized = false;
//gtsam构建的白噪声
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;
//imu预积分器,用来记录两帧激光里程计之间的imu数据预积分
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
//imu预积分器,根据最新接收到的激光雷达里程计,预测下一帧激光雷达里程计到达时的imu积分增量,用作因子图优化
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:雷达坐标系转化到imu坐标系
gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
// T_lb:imu坐标系转化到雷达坐标系
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
IMUPreintegration()
{
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
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
}
//重置ISAM优化器
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
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;
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
//初始化
if (systemInitialized == false)
{
resetOptimization();
//delta_t在类中声明为0,所以实质上是删除imu队列中早于激光雷达里程计时间的帧
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
//雷达坐标系转化到imu坐标系
prevPose_ = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
graphFactors.add(priorPose);
//速度因子
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
//噪声因子
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);
//graphValues赋值,把构建好的graphFactors赋进去
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
//进行一次图优化
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
//reset
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;
return;
}
//每当前面的优化进行一百次,将imu积分器内的数全部清0,减少累计误差
if (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 graph
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;
}
//引用知乎卢涛大佬的解释,计算前一帧与当前帧之间的imu预积分量,用前一帧状态施加预积分量得到当前帧初始状态估计,添加来自mapOptimization的当前帧位姿,进行因子图优化,更新当前帧状态
while (!imuQueOpt.empty())
{
//delta_t在类中声明为0,所以实质上是删除imu队列中早于激光雷达里程计时间的帧
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
if (imuTime < currentCorrectionTime - delta_t)
{
//将imu信息的线加速度与角速度加入预积分器中
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
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;
imuQueOpt.pop_front();
}
else
break;
}
//imufactor中存入前一帧位姿与速度,当前帧位姿与速度,以及之前形成的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
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
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
double lastImuQT = -1;
//引用zzk大佬的解释
//注意,这里是要“删除”当前帧“之前”的imu数据,是想根据当前帧“之后”的累积递推。
//而前面imuIntegratorOpt_做的事情是,“提取”当前帧“之前”的imu数据,用两帧之间的imu数据进行积分。处理过的就弹出来。
//因此,新到一帧激光帧里程计数据时,imuQueOpt队列变化如下:
//当前帧之前的数据被提出来做积分,用一个删一个(这样下一帧到达后,队列中就不会有现在这帧之前的数据了)
//那么在更新完以后,imuQueOpt队列不再变化,剩下的原始imu数据用作下一次优化时的数据。
//而imuQueImu队列则是把当前帧之前的imu数据都给直接剔除掉,仅保留当前帧之后的imu数据,
//用作两帧lidar里程计到达时刻之间发布的imu增量式里程计的预测。
//imuQueImu和imuQueOpt的区别要明确,imuIntegratorImu_和imuIntegratorOpt_的区别也要明确
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
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
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;
doneFirstOpt = true;
}
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;
}
//imu回调函数
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
//imuConverter函数定义在utility.h文件里,把imu的数据旋转到前左上坐标系下
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
//odometryHandler结束标志位
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// gtsam中的imu预积分器,将thisImu里的数据和dt存进去
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);
//发布imu里程计
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
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 ImuP;
TransformFusion TF;
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
ros::MultiThreadedSpinner spinner(4);
spinner.spin();
return 0;
}
1.4后记
LIO-SAM中的imu紧耦合激光雷达SLAM的方式是作者Tixiao Shan做这一套系统的核心,其他部分与LeGO-LOAM并无太大区别,作者后续又在这套系统的基础上加入了视觉系统即LVI-SAM,也做的十分优秀,值得学习。