Run pose-graph optimization and full TF publish when the map has grown enough. Loop detection still runs every keyframe; this batches the exp

Merge a local grid into a global grid.

Triangulate 3D point from multiple views using DLT method. Args: keypoints_list: List of 2D keypoints (N, 2) for each view

Publish the next message from the bag. Returns False when there are no more messages.

Publish robot footprint rectangle as a PointCloud for RViz.

Query poses at the specified timestamps using keyframe/relocalization poses + odom deltas. Args: timestamps: Array of timestamps to

Convert radians to degrees.

Unregister a frame callback; destroys the ROS subscription when the last caller leaves.

Estimate PnP for each candidate and return the pose with the most inliers. Args: pnp_candidates: list of (points_3d, points_2d) pair

Get rotation matrix that rotates vector a to vector b.

Convert a rotation vector to a rotation matrix using Rodrigues' formula.

Run benchmark using timestamp-based sampling instead of keyframe-based. Args: bag_a_path: Path to bag A (for creating reference map)

Generate evenly spaced timestamps between start and end. Args: start_ns: Start timestamp in nanoseconds end_

Score trajectories by minimum ESDF clearance across the robot footprint (center + 4 corners).

Select a bag from rosbags/ by name for map building.

Run StereoEngineTRT on the Looper stereo pair stored under tests/data/looper. Verifies that disparity/depth are produced with the expected sh

Run StereoEngineTRT on a RealSense stereo pair stored under tests/data/realsense. Verifies that disparity/depth are produced with the expecte

Run SuperPoint + LightGlue matching on the Looper stereo pair.

Run SuperPoint + LightGlue matching on the RealSense stereo pair.

Triangulate the landmarks.

Wait for mapping save completion or timeout. Returns: True if save completed successfully, False if timed out