TransformFusion类
准备工作
- 变量定义
/** * @brief 变换融合 * 1、接收预积分类发送的incremental消息，mapoptimization节点发送的lidar里程计消息 * 2、把最新的IMU预积分变换增量，叠加到lidar里程计上，发布/odometry/imu * 3、以10hz的频率发布上一激光里程计到当前激光里程计之间的IMU轨迹，lio_sam/imu/path */ class TransformFusion : public ParamServer { public: std::mutex mtx; // 订阅地图优化节点的全局位姿和预积分节点的增量位姿 ros::Subscriber subImuOdometry; // 预积分节点的增量位姿 "odometry/imu_incremental" ros::Subscriber subLaserOdometry; // 全局位姿 "lio_sam/mapping/odometry" // 发布IMU里程计与轨迹 ros::Publisher pubImuOdometry; ros::Publisher pubImuPath; // lidar里程计，IMU里程计 Eigen::Affine3f lidarOdomAffine; // 用于计算IMU数据的增量imuOdomAffineIncre Eigen::Affine3f imuOdomAffineFront; Eigen::Affine3f imuOdomAffineBack; // 接听tf消息 tf::TransformListener tfListener; tf::StampedTransform lidar2Baselink; // lidar里程计时间 double lidarOdomTime = -1; deque<nav_msgs::Odometry> imuOdomQueue;
- 构造函数
TransformFusion() { // 如果lidar帧和baselink帧不是同一个坐标系，则查询是否有tf消息发送两者之间的转换关系 // 通常baselink指车体系 参数文件中lidar与车体系相同，都是baselink if(lidarFrame != baselinkFrame) { try { // 查询一下lidar和baselink之间的tf变换 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()); } } // 订阅地图优化节点的全局位姿（lio_sam/mapping/odometry）， 和预积分节点的增量位姿（"odometry/imu_incremental"） 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()); // 发布IMU里程计"odometry/imu" (最新的预测的位姿,lidarOdomAffine * imuOdomAffineIncre;) pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000); // 发布IMU路径"lio_sam/imu/path" pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1); }
- 常用自定义函数
// nav_msgs::Odometry格式转换为Eigen::Affine3f格式数据 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); } /** * @brief 接听图优化节点的全局位姿 将全局位姿保存下来 * * @param odomMsg */ void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) { std::lock_guard<std::mutex> lock(mtx); // lidar里程计数据（Affine3f格式） lidarOdomAffine = odom2affine(*odomMsg); // 记录lidar里程计消息的时间 lidarOdomTime = odomMsg->header.stamp.toSec(); }
订阅LiDAR里程计数据并处理
/** * @brief 接听图优化节点的全局位姿 将全局位姿保存下来 * * @param odomMsg */ void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) { std::lock_guard<std::mutex> lock(mtx); // lidar里程计数据（Affine3f格式） lidarOdomAffine = odom2affine(*odomMsg); // 记录lidar里程计消息的时间 lidarOdomTime = odomMsg->header.stamp.toSec(); }
订阅IMU里程计数据并处理
// 接听预积分节点的增量位姿 odomTopic+"_incremental" /** * 订阅imu里程计，来自本文件中的IMUPreintegration类 * 1、以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻间imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿 * 2、发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径（只是最近一帧激光里程计时刻与当前时刻之间的一段） */ void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) { // static tf static tf::TransformBroadcaster tfMap2Odom; static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0)); // 发送静态tf，odom系和map系将他们重合（分别是”map“与”odom“，这里设置的变换为单位阵） 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) // 如果没有收到lidar位姿就return if (lidarOdomTime == -1) return; // 弹出时间戳小于最新lidar位姿时刻之前的imu里程记数据 while (!imuOdomQueue.empty()) { // 弹出比lidarOdomTime时间戳早的odomTopic+"_incremental"消息 if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime) imuOdomQueue.pop_front(); else break; } // 计算最新队列里imu里程记的增量 Front对应于当前lidar里程计时间，back是最新的IMU数据 Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front()); Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back()); // 以当前Lidar里程计时间为基础，IMU的增量 Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack; // 增量补偿到lidar的位姿上去，就得到了最新的预测的位姿 Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; float x, y, z, roll, pitch, yaw; // 分解成平移+欧拉角的形式 把预测的位姿提取出xyz，欧拉角 pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw); // publish latest odometry // 发送最新的IMU位姿 nav_msgs::Odometry laserOdometry = imuOdomQueue.back(); // 最新的预测的位姿，发布为odomTopic（odometry/imu） 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 laserOdometry转换为tcur， lidar2Baselink为接听的两个坐标系之间的变换 static tf::TransformBroadcaster tfOdom2BaseLink; tf::Transform tCur; // 把预测的位姿转换成tf tf::poseMsgToTF(laserOdometry.pose.pose, tCur); // 这里lidar与baselink的frame都是相同的 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; // 预测的位姿 laserOdometry = imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre 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); } } } };
Main函数
初始化一个IMUPreintegration类与TransformFusion 类，并执行：
int main(int argc, char** argv) { ros::init(argc, argv, "roboat_loam"); /** * @brief IMU预积分类 * 订阅IMU原始数据消息（imuTopic， /imu/data） * 订阅后端mapOptmization模块发出的"lio_sam/mapping/odometry_incremental"消息 * 发布预积分计算的增量位姿（"odometry/imu_incremental"）从最近的LiDAR帧时刻到当前IMU帧时刻的里程计增量 */ IMUPreintegration ImuP; /** * @brief 转换增量，与最新的LiDAR里程计结合，输出预测的位姿 * 订阅地图优化节点的全局位姿（lio_sam/mapping/odometry） * 订阅本节点的增量位姿（"odometry/imu"+"_incremental"） * 发布IMU里程计"odometry/imu" (最新的预测的位姿,lidarOdomAffine * imuOdomAffineIncre;) * 发布组成的IMU路径"lio_sam/imu/path" */ TransformFusion TF; ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m"); // TransformFusion类有两个subscriber // IMUPreintegration类有两个subscriber // 所以共设置线程为4 ros::MultiThreadedSpinner spinner(4); spinner.spin(); return 0; }
