| 254 | } |
| 255 | |
| 256 | void InputManager::Odom2SlamTf() |
| 257 | { |
| 258 | if (keyPoses_.size() == 0) |
| 259 | return; |
| 260 | |
| 261 | auto slam_pose = keyPoses_[keyPoses_.size()-1]; |
| 262 | //vio_odom is odomTb, slam_pose which is slamTb |
| 263 | //get odom2slam transform slamTodom = slamTb * inv(odomTb) |
| 264 | |
| 265 | // compute the tf based on odom when the graph slam optimization is called |
| 266 | SE3 vio_odom = latestOdom.pose; |
| 267 | SE3 odom2slam = slam_pose * (vio_odom.inverse()); |
| 268 | |
| 269 | std::string parent_frame_id = map_frame_id_; |
| 270 | std::string child_frame_id = odom_frame_id_; |
| 271 | PublishOdomAsTf(sloam::toRosOdom_(odom2slam, map_frame_id_, latestOdom.stamp), parent_frame_id, child_frame_id); |
| 272 | // publish pose AFTER TF so that the visualization looks correct |
| 273 | pubPose_.publish(sloam::toRosOdom_(slam_pose, map_frame_id_, latestOdom.stamp)); |
| 274 | } |
| 275 | |
| 276 | void InputManager::PublishOdomAsTf(const nav_msgs::Odometry &odom_msg, |
| 277 | const std::string &parent_frame_id, |
nothing calls this directly
no test coverage detected