| 627 | |
| 628 | self.K = None |
| 629 | self.baseline = None |
| 630 | self.odom = {} |
| 631 | self.pose_graph_used_pose = {} |
| 632 | self.relative_pose_constraint = [] |
| 633 | self.last_keyframe_timestamp = None |
| 634 | self.continuous_odom_recorder = OdomPoseRecorder(map_save_path, "mapping") |
| 635 | |
| 636 | os.makedirs(f"{map_save_path}", exist_ok=True) |
| 637 | self.db = TinyNavDB(map_save_path) |
| 638 | |
| 639 | self.marker_id = 0 |
| 640 | |
| 641 | self.loop_similarity_threshold = 0.90 |
| 642 | self.loop_top_k = 1 |
| 643 | |
| 644 | self.map_save_path = map_save_path |
| 645 | self._save_completed = False |
| 646 | self.tf_sub = Subscriber(self, TFMessage, "/tf") |
| 647 | self.tf_sub.registerCallback(self.tf_callback) |
| 648 | self.tf_static_sub = Subscriber(self, TFMessage, "/tf_static") |
| 649 | self.tf_static_sub.registerCallback(self.tf_callback) |
| 650 | self.T_rgb_to_infra1 = None |
| 651 | self.rgb_camera_info_sub = Subscriber(self, CameraInfo, "/camera/camera/color/camera_info") |
| 652 | self.rgb_camera_info_sub.registerCallback(self.rgb_camera_info_callback) |
| 653 | self.rgb_camera_K = None |
| 654 | |
| 655 | def tf_callback(self, msg:TFMessage): |
| 656 | T_infra1_to_link = None |
| 657 | T_infra1_optical_to_infra1 = None |
| 658 | T_rgb_to_link = None |
| 659 | T_rgb_optical_to_rgb = None |
| 660 | tf_messages: Dict[int, Dict[str, np.ndarray]] = {} |
| 661 | for t in msg.transforms: |
| 662 | frame_id, child_frame_id, T = tf2np(t) |
| 663 | timestamp_ns = int(t.header.stamp.sec * 1e9) + int(t.header.stamp.nanosec) |
| 664 | if timestamp_ns not in tf_messages: |
| 665 | tf_messages[timestamp_ns] = {} |
| 666 | tf_messages[timestamp_ns][f"{frame_id}->{child_frame_id}"] = T |
| 667 | if frame_id == "camera_link" and child_frame_id == "camera_infra1_frame": |