(self, keyframe_image_msg:Image, keyframe_odom_msg:Odometry, depth_msg:Image, rgb_image_msg:Image)
| 706 | self.continuous_odom_recorder.record_odometry_msg(odom_msg) |
| 707 | |
| 708 | def mapping_stop_callback(self, msg: Bool): |
| 709 | if msg.data: |
| 710 | self.get_logger().info("Received benchmark stop signal, starting save process...") |
| 711 | try: |
| 712 | self.save_mapping() |
| 713 | self.get_logger().info("Mapping save completed successfully") |
| 714 | |
| 715 | # Publish save finished signal |
| 716 | save_finished_msg = Bool() |
| 717 | save_finished_msg.data = True |
| 718 | self.mapping_save_finished_pub.publish(save_finished_msg) |
| 719 | self.get_logger().info("Published data save finished signal") |
| 720 | |
| 721 | except Exception as e: |
| 722 | self.get_logger().error(f"Error during mapping save: {e}") |
| 723 | # Still publish completion signal even if there was an error |
| 724 | save_finished_msg = Bool() |
| 725 | save_finished_msg.data = False |
| 726 | self.mapping_save_finished_pub.publish(save_finished_msg) |
| 727 | |
| 728 | def keyframe_callback(self, keyframe_image_msg:Image, keyframe_odom_msg:Odometry, depth_msg:Image, rgb_image_msg:Image): |
| 729 | with self.stage_timer.timed("mapping_loop"): |
| 730 | if self.K is None: |
| 731 | return |
| 732 | self.process(keyframe_image_msg, keyframe_odom_msg, depth_msg, rgb_image_msg) |
| 733 | |
| 734 | def process(self, keyframe_image_msg:Image, keyframe_odom_msg:Odometry, depth_msg:Image, rgb_image_msg:Image): |
| 735 | with self.stage_timer.timed("msg_decode"): |
| 736 | keyframe_image_timestamp = int(keyframe_image_msg.header.stamp.sec * 1e9) + int(keyframe_image_msg.header.stamp.nanosec) |
| 737 | keyframe_odom_timestamp = int(keyframe_odom_msg.header.stamp.sec * 1e9) + int(keyframe_odom_msg.header.stamp.nanosec) |
| 738 | keyframe_depth_timestamp = int(depth_msg.header.stamp.sec * 1e9) + int(depth_msg.header.stamp.nanosec) |
| 739 | if keyframe_image_timestamp != keyframe_odom_timestamp or keyframe_image_timestamp != keyframe_depth_timestamp: |
| 740 | self.get_logger().error(f"Keyframe timestamp mismatch: {keyframe_image_timestamp} != {keyframe_odom_timestamp} != {keyframe_depth_timestamp}") |
| 741 | |
| 742 | depth = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding="32FC1") |
| 743 | odom, _ = msg2np(keyframe_odom_msg) |
| 744 | infra1_image = self.bridge.imgmsg_to_cv2(keyframe_image_msg, desired_encoding="mono8") |
| 745 | rgb_image = self.bridge.imgmsg_to_cv2(rgb_image_msg, desired_encoding="bgr8") |
| 746 | |
| 747 | with self.stage_timer.timed("save_image_and_depth"): |
| 748 | self.db.set_entry(keyframe_image_timestamp, depth = depth, infra1_image = infra1_image, rgb_image = rgb_image) |
| 749 | |
| 750 | with self.stage_timer.timed("get_dinov2_descriptors"): |
| 751 | embedding, patch_tokens = asyncio.run(self.dinov2_model.infer_global_and_patch_tokens(infra1_image)) |
| 752 | embedding = embedding / np.linalg.norm(embedding) |
| 753 | self.db.set_entry(keyframe_image_timestamp, embedding = embedding, patch_tokens = patch_tokens) |
| 754 | with self.stage_timer.timed("get_semantic_embedding"): |
no test coverage detected