only publish keyframes viz if the keyframe has changed

proroklab · Apr 13, 2024 · aaab322 · aaab322
1 parent 74f1d19
commit aaab322
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 0 deletions.
diff --git a/src/orb_slam3_ros/include/publish_ros_viz_topics.h b/src/orb_slam3_ros/include/publish_ros_viz_topics.h
@@ -7,6 +7,7 @@
 #include "sophus/se3.hpp"
 #include "sophus/sim3.hpp"
 #include <Eigen/src/Core/Matrix.h>
+#include <boost/uuid/uuid.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <interfaces/msg/sim3_transform_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -51,6 +52,8 @@ class PublishRosVizTopics {
   shared_ptr<rclcpp::Node> node;
   ReferenceFrameManager* referenceFrameManager;
 
+  map<boost::uuids::uuid, Sophus::SE3f> last_sent_keyframe_poses;
+
   // ROS publishers
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pose_marker_pub;

diff --git a/src/orb_slam3_ros/src/publish_ros_viz_topics.cpp b/src/orb_slam3_ros/src/publish_ros_viz_topics.cpp
@@ -188,6 +188,15 @@ void PublishRosVizTopics::publish_keyframes(std::vector<ORB_SLAM3::KeyFrame*> ke
     if (keyFrame->isBad())
       continue;
 
+    // Skip if the keyframe has already been sent and hasn't moved
+    if (last_sent_keyframe_poses.find(keyFrame->uuid) != last_sent_keyframe_poses.end()
+      && last_sent_keyframe_poses[keyFrame->uuid].translation().isApprox(keyFrame->GetPoseInverse().translation())
+      && last_sent_keyframe_poses[keyFrame->uuid].unit_quaternion().isApprox(
+        keyFrame->GetPoseInverse().unit_quaternion()))
+      continue;
+
+    last_sent_keyframe_poses[keyFrame->uuid] = keyFrame->GetPoseInverse();
+
     visualization_msgs::msg::Marker keyFrameWireframe;
     keyFrameWireframe.header.frame_id = referenceFrameManager->slam_system_frame_id;
     keyFrameWireframe.ns = "keyFrameWireframes";