在imu预积分的节点中,在main函数里面 还有一个类的实例对象,那就是TransformFusion TF。
其主要功能是做位姿融合输出,最终输出imu的预测结果,与上节中的imu预测结果的区别就是:
该对象的融合输出是基于全局位姿的基础上再进行imu的预测输出。全局位姿就是 经过回环检测后的lidar位姿。
建图优化会输出两种激光雷达的位姿:
- lidar 增量位姿
- lidar 全局位姿
其中lidar 增量位姿就是 通过 lidar的匹配功能,优化出的帧间的相对位姿,通过相对位姿的累积,形成世界坐标系下的位姿
lidar全局位姿 则是在 帧间位姿的基础上,通过 回环检测,再次进行优化的 世界坐标系下的位姿,所以对于增量位姿,全局位姿更加精准
在前面提到的发布的imu的预测位姿是在lidar的增量位姿上基础上预测的,那么为了更加准确,本部分功能就预测结果,计算到基于全局位姿的基础上面。首先看构造函数
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()); } }
判断lidar帧和baselink帧是不是同一个坐标系,通常baselink指车体系,如果不是,查询 一下 lidar 和baselink 之间的 tf变换 ros::Time(0) 表示最新的,等待两个坐标系有了变换,更新两个的变换 lidar2Baselink
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);
发布两个信息 odomTopic ImuPath,然后看第一个回调函数 lidarOdometryHandler
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) { std::lock_guard< std::mutex > lock(mtx); lidarOdomAffine = odom2affine(*odomMsg); lidarOdomTime = odomMsg- >header.stamp.toSec(); }
将全局位姿保存下来,将ros的odom格式转换成 Eigen::Affine3f 的形式,将最新帧的时间保存下来,第二个回调函数是 imuOdometryHandler,imu预积分之后所发布的imu频率的预测位姿
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) {
static tf::TransformBroadcaster tfMap2Odom; static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
建图的话 可以认为 map坐标系和odom坐标系 是重合的(初始化时刻)
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg- >header.stamp, mapFrame, odometryFrame));
发布静态tf,odom系和map系 他们是重合的
imuOdomQueue.push_back(*odomMsg);
imu得到的里程计结果送入到这个队列中
if (lidarOdomTime == -1) return;
如果没有收到lidar位姿 就returen
while (!imuOdomQueue.empty()) { if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime) imuOdomQueue.pop_front(); else break; }
弹出时间戳 小于 最新 lidar位姿时刻之前的imu里程计数据
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front()); Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back()); Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
计算最新队列里imu里程计的增量
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
增量补偿到lidar的位姿上去,就得到了最新的预测的位姿
float x, y, z, roll, pitch, yaw; pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
分解成平移 + 欧拉角的形式
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);
发送全局一致位姿的 最新位姿
static tf::TransformBroadcaster tfOdom2BaseLink; tf::Transform tCur; tf::poseMsgToTF(laserOdometry.pose.pose, tCur); if(lidarFrame != baselinkFrame) tCur = tCur * lidar2Baselink;
更新tf
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg- >header.stamp, odometryFrame, baselinkFrame); tfOdom2BaseLink.sendTransform(odom_2_baselink);
更新odom到baselink的tf
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); } } }
发布imu里程计的轨迹,控制一下更新频率,不超过10hz,将最新的位姿送入轨迹中,把lidar时间戳之前的轨迹全部擦除,发布轨迹,这个轨迹实践上是可视化imu预积分节点输出的预测值。
-
框架
+关注
关注
0文章
398浏览量
17400 -
检测
+关注
关注
5文章
4412浏览量
91301 -
SAM
+关注
关注
0文章
111浏览量
33478 -
激光雷达
+关注
关注
967文章
3919浏览量
189409
发布评论请先 登录
相关推荐
评论