前言
LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。
LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。
本篇博客重点解读LIO-SAM框架下IMU预积分功能数据初始化代码部分
LIO-SAM 的代码主要在其主目录内的src文件夹下的四个cpp文件，分别是：
- featureExtraction.cpp
- imageProjection.cpp
- imuPreintegration.cpp
- mapOptmization.cpp
每个cpp文件是一个独立的ROS节点，对应着下图的四个模块
lio-sam的所有文件即对应功能在下面做了如下总结：
lio-sam:.
│ CMakeLists.txt #项目工程配置文件，可以知道作者用了哪些第三方库及cpp生成了什么执行文件
│ LICENSE #软件版权
│ package.xml #ROS包配置文件
│ README.md #项目工程说明文件：文件构成、依赖、运行等
│
├─config
│ │ params.yaml #参数文件
│ │
│ └─doc #存储效果图、流程图、论文等
│ │
│ └─kitti2bag #将kitti数据集转换成bag格式
│ kitti2bag.py
│ README.md
│
├─include
│ utility.h #参数服务器类，初始化参数；各类公用函数
│
├─launch
│ │ run.launch #总运行launch文件
│ │
│ └─include #分模块运行文件
│ │ module_loam.launch
│ │ module_navsat.launch
│ │ module_robot_state_publisher.launch
│ │ module_rviz.launch
│ │
│ ├─config #存储rviz参数文件和机器人坐标系参数
│ │ rviz.rviz
│ │ robot.urdf.xacro
│
├─msg
│ cloud_info.msg #自定义ROS数据格式
│
├─src #源文件
│ featureExtraction.cpp #提取雷达线面特征，发布雷达点云
│ imageProjection.cpp #订阅提取的雷达点云、IMU数据和IMU里程计数据，对雷达做畸变矫正，进行雷达前端里程计位姿粗估计的发布（以IMU频率）
│ imuPreintegration.cpp #IMU预积分，订阅雷达里程计和IMU数据，估计IMU偏置，进行雷达里程计、IMU预积分因子的图优化，输出IMU里程计。
│ mapOptmization.cpp #订阅雷达前端信息、GPS信息，进行点云配准，进行雷达里程计、全局GPS、回环检测因子的图优化。
│
└─srv
save_map.srv
本篇主要解读 IMU预积分部分代码，也就是
imuPreintegration.cpp #IMU预积分，订阅雷达里程计和IMU数据，估计IMU偏置，进行雷达里程计、IMU预积分因子的图优化，输出IMU里程计。
代码解读
int main(int argc, char** argv)
{
ros::init(argc, argv, "roboat_loam");
IMUPreintegration ImuP;//IMUPreintegration 类的实例
TransformFusion TF;//TransformFusion 类的实例
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");//打印消息
ros::MultiThreadedSpinner spinner(4);//开四个线程 通过并发的方式使得速度得到提升
spinner.spin();//程序执行到这个地方 则等待 topic 回调函数执行
return 0;
}
main函数部分很简洁，功能主要完成部分都在定义的两个类中进行。
在main函数中进行
- 节点初始化
- IMUPreintegration 类的实例
- TransformFusion 类的实例
- 打印消息
- 开四个线程 通过并发的方式使得速度得到提升
- 等待 topic 回调函数执行
之后则看 IMUPreintegration 这个类，先看构造函数部分
在里面首先进行了 订阅imu信息
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic,2000, &IMUPreintegration::imuHandler,this, ros::TransportHints().tcpNoDelay());
imuTopic 为 imu_correct, imu原始数据，这个imuTopic是从参数服务器读取的，具体的配置在prams.yaml中
如果你的imu的topic和默认的不一致则需要修改
然后可以看其具体的回调函数 imuHandler
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
//首先把imu的状态做一个简单的转换
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
// 注意这里有两个imu队列，作用不相同，一个用来执行预积分和位姿变换的优化，一个用来更新最新imu状态
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
// 如果没有发生过优化 则 return
if (doneFirstOpt == false)
return;
- 回调函数先把imu的状态做一个简单的转换，转到lidar坐标系 下
- 将转换后的imu数据存入两个队列中，注意这里有两个imu队列，作用不相同，一个用来执行预积分和位姿变换的优化，一个用来更新最新imu状态
- 如果没有发生过优化 则 retur
doneFirstOpt这个标志位，在接受到帧间里程计信息后，则至为true
imuConverter函数在utility.h文件中
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{
sensor_msgs::Imu imu_out = imu_in;
// rotate acceleration //进行加速度坐标旋转
Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
acc = extRot * acc;
imu_out.linear_acceleration.x = acc.x();
imu_out.linear_acceleration.y = acc.y();
imu_out.linear_acceleration.z = acc.z();
// rotate gyroscope // 进行陀螺仪坐标旋转
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
gyr = extRot * gyr;
imu_out.angular_velocity.x = gyr.x();
imu_out.angular_velocity.y = gyr.y();
imu_out.angular_velocity.z = gyr.z();
// rotate roll pitch yaw // 进行姿态角坐标旋转
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
Eigen::Quaterniond q_final = q_from * extQRPY;
imu_out.orientation.x = q_final.x();
imu_out.orientation.y = q_final.y();
imu_out.orientation.z = q_final.z();
imu_out.orientation.w = q_final.w();
//检测姿态数据是否正常
if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
{
ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
ros::shutdown();
}
return imu_out;//返回变换后的imu数据
}
};
- 进行加速度坐标旋转
- 进行陀螺仪坐标旋转
- 进行姿态角坐标旋转
- 检测姿态数据是否正常
- 返回变换后的imu数据
在进行加速度和陀螺仪变换的时候，使用的是extRot，该参数的根源来源于prams.yaml中
进行姿态角坐标旋转，使用的是extQRPY，该参数的根源来源于prams.yaml中
所有终于明白为什么在配置文件中有两个外参了！
imuHandler这个回调函数，先看到这部分，后面的之后再看，需要回到上面的IMUPreintegration的构造函数，看订阅到帧间里程计信息做了哪些事情。
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
订阅雷达里程计信息
lio_sam/mapping/odometry_incremental 是mapOptmization发出的
odometryHandler回调函数，走起
double currentCorrectionTime = ROS_TIME(odomMsg);
通过ROS_TIME函数把消息中的时间戳取了出来
if (imuQueOpt.empty())
return;
保证imu队列中有数据
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;
该位姿是否出现退化 pose.covariance[0] 为1 则 雷达里程计有退化风险，该帧位姿精度有一定程序下降
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
把位姿转成 gtsam的格式
进入系统的初始化，下面部分仅执行一次
resetOptimization();
在函数内部 初始化gtsam的一些量
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
将这个雷达里程计之前的imu信息全部扔掉
整个LIO-SAM中作者对时间同步这块的思想都是这样的
保证imu与odometry消息时间同步 因为imu是高频数据所以这是必要的
prevPose_ = lidarPose.compose(lidar2Imu);
将lidar的位姿移到imu坐标系下
lidar2Imu 是lidar到imu的外参
compose是gtsam的一个功能函数
VIO和LIO的框架都在在IMU坐标系下进行的
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
graphFactors.add(priorPose);
设置其初始位姿和置信度
约束加入到因子中
gtsam::PriorFactor 模块涉及到的变量结点
- gtsam::Pose3 表示六自由度位姿
- gtsam::Vector3 表示三自由度速度
- gtsam::imuBias::ConstantBias 表示IMU零偏
以上也是预积分模型中涉及到的三种状态变量
gtsam::PriorFactor
其中的X(0)的，初始定义如下。 事先的符号
priorPoseNoise 是先验位姿的噪声
该值为
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
初始 位姿 置信度 设置 比较高 后面构成协方差矩阵 值越小 表示 置信度越高
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
和上面位姿基本一样
初始化速度，这里直接赋 0 了
将速度约束加到因子图中
其中priorVelNoise 速度的噪声是
priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
初始化速度 置信度 设置 差些 因为速度一开始设置的是0，不知道是多少
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);
初始化IMU 零偏 ，将零偏约束加到因子图中
gtsam::imuBias::ConstantBias()是gtsam做好的一个imu零偏，其中都是0，
所以对应bias的噪声置信度也要设置的高些
priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
以上把约束加入完毕，下面就开始添加状态量
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
给各个状态量赋成初始值
optimizer.update(graphFactors, graphValues);
约束和状态量更新 进isam优化器
graphFactors.resize(0);
graphValues.clear();
进优化器之后 保存约束和状态量的变量就清零
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
预积分的接口，使用初始零偏进行初始化 之前imu有两个队列，每个队列对应预积分处理器
key = 1;
systemInitialized = true;//系统初始化完成
return;
系统初始化完成
总结
评论（0）
您还未登录，请登录后发表或查看评论