LIO-SAM前端中位姿融合输出

这里同样是imuPreintegration.cpp文件中的一个类 TransformFusion TF

订阅的消息话题

1. 地图优化节点的全局位姿 “lio_sam/mapping/odometry”
2. 预积分节点的增量位姿 odomTopic+“_incremental”

订阅地图优化节点的全局位姿的回调函数

将位姿解算乘欧拉角保存起来并返回到 变量lidarOdomAffine 中存储

    // 将全局位姿保存下来
    void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard<std::mutex> lock(mtx);

        lidarOdomAffine = odom2affine(*odomMsg);

        lidarOdomTime = odomMsg->header.stamp.toSec();
    }
    // 将位姿解算乘欧拉角保存起来并返回到lidarOdomAffine 中
    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);
    }

订阅预积分节点增量位姿的回调函数

这里才是位姿融合的关键函数
实际操作简述： 说白了就是拿到优化后的全局位姿和imu的局部增量位姿，然后加起来就是当前时刻的位姿了，具体操作在下面第4点，这样就能高频输出比较准确的位姿了，全局位姿在后端优化比较久

1. 发送静态tf，odom系和map系将他们重合

tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));

2. 将imu得到的里程计结果存入队列中

imuOdomQueue.push_back(*odomMsg);

3. 弹出时间戳小于最新lidar位姿时刻之前的imu里程记数据

while (!imuOdomQueue.empty())
        {
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }

4. 计算最新队列里imu里程记的增量

Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
// 增量补偿到lidar的位姿上去，就得到了最新的预测的位姿
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; //全局位姿+局部增量
float x, y, z, roll, pitch, yaw;
// 分解成平移+欧拉角的形式
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);

5. 发送全局一致位姿的最新位姿

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);

6. 更新TF和发布imu里程计的轨迹

// publish tf
// 更新tf
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if(lidarFrame != baselinkFrame)
    tCur = tCur * lidar2Baselink;
// 更新odom到baselink的tf
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);

// publish IMU path
// 发送imu里程记的轨迹
static nav_msgs::Path imuPath;
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
// 控制一下更新频率，不超过10hz
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);
    // 把lidar时间戳之前的轨迹全部擦除
    while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
        imuPath.poses.erase(imuPath.poses.begin());
    // 发布轨迹，这个轨迹实际上是可视化imu预积分节点输出的预测值
    if (pubImuPath.getNumSubscribers() != 0)
    {
        imuPath.header.stamp = imuOdomQueue.back().header.stamp;
        imuPath.header.frame_id = odometryFrame;
        pubImuPath.publish(imuPath);
    }
}