本文介绍了TransformFusion类和IMUPreintegration类在激光里程计与IMU数据融合中的应用。TransformFusion订阅并融合来自MapOptimization的激光里程计和IMU里程计，计算并发布当前时刻的IMU里程计轨迹。IMUPreintegration则通过预积分技术，结合激光里程计优化状态，并预测和发布高频率的IMU里程计信息。整个系统旨在提高机器人定位的精度和稳定性。
写在前面
TransformFusion类
功能简介：
主要功能是订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；
rviz展示IMU里程计轨迹（局部）。
订阅：
1、订阅激光里程计，来自MapOptimization；
2、订阅imu里程计，来自ImuPreintegration。
发布：
1、发布IMU里程计，用于rviz展示；
2、发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。
IMUPreintegration类
功能简介：
1、用激光里程计，两帧激光里程计之间的IMU预积分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）;
2、以优化后的状态为基础，施加IMU预积分量，得到每一时刻的IMU里程计。
订阅：
1、订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预积分量，预测每一时刻（IMU频率）的IMU里程计；
2、订阅激光里程计（来自MapOptimization）,用两帧之间的IMU预积分构建因子图，优化当前帧位姿（每个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。
发布：
1、发布IMU里程计。
定义参数：
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)
TransformFusion整体代码
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;
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()
{
// 如果lidar系与baselink系不同（激光系和载体系），需要外部提供二者之间的转换关系
if(lidarFrame != baselinkFrame)
{
try
{
// 等待3s
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
// lidar系到baselink系的变换
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
}
//订阅激光里程计，来自mapOptimization
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
//订阅imu里程计，来自IMUPreintegration
subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
//发布imu里程计，用于rviz展示
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
//发布imu里程计轨迹
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);
}
/**
* 订阅激光里程计，来自mapOptimization
*/
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
//激光里程计对应变换矩阵
lidarOdomAffine = odom2affine(*odomMsg);
//激光里程计时间戳
lidarOdomTime = odomMsg->header.stamp.toSec();
}
/**
* 订阅imu里程计，来自IMUPreintegration
* 1.以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿
* 2.发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段
*/
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
// static tf
// 发布tf，map与odom系设为同一个系
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);
// 添加imu里程计到队列
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());
// 当前时刻imu里程计位姿
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
// imu里程计增量位姿变换
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
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，当前时刻odom与baselink系变换关系
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if(l
最低0.47元/天 解锁文章
扫一扫
837
到【灌水乐园】发言
