How to test TUM dataset on MAGiC-SLAM with single agent mode?

[lemonci]

Can I directly use the dataloader from Gaussian-SLAM?

https://github.com/VladimirYugay/Gaussian-SLAM/blob/eaec10d73ce7511563882b8856896e06d1f804e3/src/entities/datasets.py#L75C1-L168C64

[VladimirYugay]

Yes. For single-agent model please read the readme

[lemonci]

Sorry, after I checked the dataloader of TUM in Gaussian-SLAM, I think it returns index, color_data, depth_data, self.poses[index] while the dataloader of Replica in MAGiC-SLAM returns self.frame_ids[index], color_data, depth_data, self.poses[index]. May I ask what's the difference between index in Gaussian-SLAM dataloader and self.frame_ids[index] in MAGiC dataloader?

[VladimirYugay]

Hey there, there's no difference. For simplicity, you can just not return index from tum dataset when using in the multiagent pipeline!

[lemonci]

Hi, I tried to move the dataloader from Gaussian-SLAM here adding a new class in datasets.py:

class TUM_RGBD(torch.utils.data.Dataset):
    def __init__(self, dataset_config: dict):
        self.dataset_path = Path(dataset_config["input_path"])
        self.frame_limit = dataset_config.get("frame_limit", -1)
        self.dataset_config = dataset_config
        self.height = dataset_config["H"]
        self.width = dataset_config["W"]
        self.fx = dataset_config["fx"]
        self.fy = dataset_config["fy"]
        self.cx = dataset_config["cx"]
        self.cy = dataset_config["cy"]

        self.depth_scale = dataset_config["depth_scale"]
        self.distortion = np.array(
            dataset_config['distortion']) if 'distortion' in dataset_config else None
        self.crop_edge = dataset_config['crop_edge'] if 'crop_edge' in dataset_config else 0
        if self.crop_edge:
            self.height -= 2 * self.crop_edge
            self.width -= 2 * self.crop_edge
            self.cx -= self.crop_edge
            self.cy -= self.crop_edge

        self.fovx = 2 * math.atan(self.width / (2 * self.fx))
        self.fovy = 2 * math.atan(self.height / (2 * self.fy))
        self.intrinsics = np.array(
            [[self.fx, 0, self.cx], [0, self.fy, self.cy], [0, 0, 1]])

        self.color_paths = []
        self.depth_paths = []
        self.poses = []
        self.color_transform = torchvision.transforms.ToTensor()
        self.color_paths, self.depth_paths, self.poses = self.loadtum(
            self.dataset_path, frame_rate=32)
        
    def __len__(self):
        return len(self.color_paths) if self.frame_limit < 0 else int(self.frame_limit)

    def parse_list(self, filepath, skiprows=0):
        """ read list data """
        return np.loadtxt(filepath, delimiter=' ', dtype=np.unicode_, skiprows=skiprows)

    def associate_frames(self, tstamp_image, tstamp_depth, tstamp_pose, max_dt=0.08):
        """ pair images, depths, and poses """
        associations = []
        for i, t in enumerate(tstamp_image):
            if tstamp_pose is None:
                j = np.argmin(np.abs(tstamp_depth - t))
                if (np.abs(tstamp_depth[j] - t) < max_dt):
                    associations.append((i, j))
            else:
                j = np.argmin(np.abs(tstamp_depth - t))
                k = np.argmin(np.abs(tstamp_pose - t))
                if (np.abs(tstamp_depth[j] - t) < max_dt) and (np.abs(tstamp_pose[k] - t) < max_dt):
                    associations.append((i, j, k))
        return associations

    def loadtum(self, datapath, frame_rate=-1):
        """ read video data in tum-rgbd format """
        if os.path.isfile(os.path.join(datapath, 'groundtruth.txt')):
            pose_list = os.path.join(datapath, 'groundtruth.txt')
        elif os.path.isfile(os.path.join(datapath, 'pose.txt')):
            pose_list = os.path.join(datapath, 'pose.txt')

        image_list = os.path.join(datapath, 'rgb.txt')
        depth_list = os.path.join(datapath, 'depth.txt')

        image_data = self.parse_list(image_list)
        depth_data = self.parse_list(depth_list)
        pose_data = self.parse_list(pose_list, skiprows=1)
        pose_vecs = pose_data[:, 1:].astype(np.float64)

        tstamp_image = image_data[:, 0].astype(np.float64)
        tstamp_depth = depth_data[:, 0].astype(np.float64)
        tstamp_pose = pose_data[:, 0].astype(np.float64)
        associations = self.associate_frames(
            tstamp_image, tstamp_depth, tstamp_pose)

        indicies = [0]
        for i in range(1, len(associations)):
            t0 = tstamp_image[associations[indicies[-1]][0]]
            t1 = tstamp_image[associations[i][0]]
            if t1 - t0 > 1.0 / frame_rate:
                indicies += [i]

        images, poses, depths = [], [], []
        inv_pose = None
        for ix in indicies:
            (i, j, k) = associations[ix]
            images += [os.path.join(datapath, image_data[i, 1])]
            depths += [os.path.join(datapath, depth_data[j, 1])]
            c2w = self.pose_matrix_from_quaternion(pose_vecs[k])
            if inv_pose is None:
                inv_pose = np.linalg.inv(c2w)
                c2w = np.eye(4)
            else:
                c2w = inv_pose@c2w
            poses += [c2w.astype(np.float32)]

        return images, depths, poses

    def pose_matrix_from_quaternion(self, pvec):
        """ convert 4x4 pose matrix to (t, q) """
        from scipy.spatial.transform import Rotation

        pose = np.eye(4)
        pose[:3, :3] = Rotation.from_quat(pvec[3:]).as_matrix()
        pose[:3, 3] = pvec[:3]
        return pose
    
    def get_point_cloud(self, frame_id: int, pose=None) -> o3d.geometry.PointCloud:
        """ Get the point cloud of a frame.
        Args:
            frame_id: The frame id.
            pose: The camera-to-world pose of shape (4, 4).
        Returns:
            Posed open3d point cloud
        """
        _, gt_color, gt_depth, gt_pose = self[frame_id]
        if pose is None:
            pose = gt_pose.copy()
        return rgbd2ptcloud(gt_color, gt_depth, self.intrinsics, np.linalg.inv(pose))

    def get_render_frame(self, frame_id: int, pose=None) -> dict:
        """ Get the render frame dictionary suitable for GS renderer
        Args:
            frame_id: The frame id.
            pose: The camera-to-world pose of shape (4, 4).
        Returns:
            The render frame dictionary
        """
        _, gt_color, gt_depth, gt_pose = self[frame_id]
        if pose is None:
            pose = np.linalg.inv(gt_pose)
        return {
            "gt_color": self.color_transform(gt_color).cuda(),
            "gt_depth": np2torch(gt_depth).cuda(),
            "render_settings": get_render_settings(self.width, self.height, self.intrinsics, pose)
        }

    def __getitem__(self, index):
        color_data = cv2.imread(str(self.color_paths[index]))
        if self.distortion is not None:
            color_data = cv2.undistort(
                color_data, self.intrinsics, self.distortion)
        color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB)

        depth_data = cv2.imread(
            str(self.depth_paths[index]), cv2.IMREAD_UNCHANGED)
        depth_data = depth_data.astype(np.float32) / self.depth_scale
        edge = self.crop_edge
        if edge > 0:
            color_data = color_data[edge:-edge, edge:-edge]
            depth_data = depth_data[edge:-edge, edge:-edge]
        # Interpolate depth values for splatting
        return index, color_data, depth_data, self.poses[index]

And write the config file for TUM:

project_name: "MAGiC-SLAM"
dataset_name: "tum_rgbd"
multi_gpu: False  # Put every agent on a separate GPU
use_wandb: False
wandb_offline: False
experiment_name: "tum_rgbd"
group_name: "tum_rgbd"
seed: 0
checkpoint_path: null
use_wandb: False
frame_limit: -1 # for debugging, set to -1 to disable
seed: 0
mapping:
  new_submap_every: 50
  map_every: 5
  iterations: 100
  new_submap_iterations: 100
  new_submap_points_num: 100000 
  new_submap_gradient_points_num: 50000 
#  new_frame_sample_size: 30000 
#  new_points_radius: 0.0001
#  current_view_opt_iterations: 0.4 # What portion of iterations to spend on the current view
  alpha_thre: 0.6
  pruning_thre: 0.5
  visualize: False
#  submap_using_motion_heuristic: True
tracking:
#  gt_camera: False
  w_color_loss: 0.6
  iterations: 200
  cam_rot_lr: 0.002
  cam_trans_lr: 0.01
  odometry_type: "const_speed"  # gt, const_speed, odometer
  help_camera_initialization: False # temp option to help const_init
  init_err_ratio: 5
  odometer_method: "hybrid" # hybrid or point_to_plane
  filter_alpha: False
  filter_outlier_depth: False
  alpha_thre: 0.98
  soft_alpha: True
  mask_invalid_depth: True
loop_detection:
  # feature_extractor_name: "netvlad"
  # weights_path: "<path_to>/TokyoTM_struct.mat"
  # embed_size: 4096
  # feature_dist_threshold: .8
  feature_extractor_name: "dino"  # netvlad
  weights_path: "./dinov2-small"
  embed_size: 384
  feature_dist_threshold: .35
  device: cpu
  time_threshold: 0
  max_loops_per_frame: 1
  fitness_threshold: 0.5
  inlier_rmse_threshold: 0.1
cam:
  crop_edge: 16
  depth_scale: 5000.0
data:
  agent_ids: [0]

The code could run if map_every: 5 . However, the PNSR is only 17.34 so I tried to map_every: 1, Then I got the following error:

ceback (most recent call last):
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/run_slam.py", line 52, in <module>
    coga_slam.run()
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/src/entities/magic_slam.py", line 137, in run
    agents_opt_kf_c2ws = self.optimize_poses(self.agents_submaps)
                         ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/src/entities/magic_slam.py", line 114, in optimize_poses
    agents_optimized_kf_poses = apply_pose_correction(graph_wrapper.get_poses(), agents_submaps)
                                ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/src/utils/magic_slam_utils.py", line 142, in apply_pose_correction
    for pose in submap["submap_c2ws"][submap_kf_ids]:
                ~~~~~~~~~~~~~~~~~~~~~^^^^^^^^^^^^^^^
IndexError: index 41 is out of bounds for axis 0 with size 41

As every submap contains 50 keyframes, the length for submap["submap_c2ws"] should be 50 instead of 41. Is there anything hard coded causing this error?

[lemonci]

After printing out submap_keyframe_ids and the length of submap_c2ws in

https://github.com/VladimirYugay/MAGiC-SLAM/blob/4ac5aa92ab70139546b42927b62e4f2fd6940222/src/utils/magic_slam_utils.py#L137C1-L144C1

I found that the bug was triggered in the last iteration if using map_every: 1:

submap:  0 submap_keyframe_ids: [ 0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
 48 49] submap_start_frame_id: 0
submap_c2ws: 50

...

submap:  10 submap_keyframe_ids: [500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517
 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535
 536 537 538 539 540 541 542 543 544 545 546 547 548 549] submap_start_frame_id: 500
submap_c2ws: 50
submap:  11 submap_keyframe_ids: [550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567
 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585
 586 587 588 589 590 591] submap_start_frame_id: 550
submap_c2ws: 41

In the last iteration, there are 42 keyframes but there are only 41 c2ws in the submap. This error doesn't occur if the last frame was not sampled as keyframe. However, I would like to know how to fix it because we cannot guarantee the last frame won't be selected as keyframe.

Thanks.

P.S. As I also tried map_every: 2 and got PNSR 17.830 (significantly lower than Gaussian-SLAM whose PNSR is 24.44 for map_every: 2) so I suppose MAGiC-SLAM traded off the rendering performance for efficiency?
P.P.S. I was using scene rgbd_dataset_freiburg1_desk.

[lemonci]

After checking the code, I think we need to fix this by changing

MAGiC-SLAM/src/entities/agent.py

Line 209 in 4ac5aa9

submap_data = self.get_submap_data(frame_id, gaussian_model)

to

submap_data = self.get_submap_data(frame_id + 1, gaussian_model)

[VladimirYugay]

Hey there.

1. You mentioned that the error happens only when the last keyframe is not selected as the start of a submap. Can you try start_new_submap = self.should_start_new_submap(frame_id) or frame_id == len(self.dataset) - 1 in this line and see whether submap every 1 works? This change should preserve the older results that were obtained with the current implementation.

2. As for rendering, we indeed optimised the speed over rendering performance. To boost it up, can you set up more iterations in the refinement stage and add densification in the same way as in the original Gaussian splatting repo?

[lemonci]

Hey there.

1. You mentioned that the error happens only when the last keyframe is not selected as the start of a submap. Can you try `start_new_submap = self.should_start_new_submap(frame_id) or frame_id == len(self.dataset) - 1` in this [line](https://github.com/VladimirYugay/MAGiC-SLAM/blob/4ac5aa92ab70139546b42927b62e4f2fd6940222/src/entities/agent.py#L185) and see whether submap every 1 works? This change should preserve the older results that were obtained with the current implementation.

2. As for rendering, we indeed optimised the speed over rendering performance. To boost it up, can you set up more iterations in the refinement stage and add densification in the same way as in the original Gaussian splatting repo?

Regarding 1. No, this won't solve the problem. The problem was caused by the last frame selected as keyframe but it was not included in the list of camera poses. Specifically, after such change, I have the following error:

Traceback (most recent call last):
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/run_slam.py", line 52, in <module>
    coga_slam.run()
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/src/entities/magic_slam.py", line 137, in run
    agents_opt_kf_c2ws = self.optimize_poses(self.agents_submaps)
                         ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/src/entities/magic_slam.py", line 97, in optimize_poses
    intra_loops, inter_loops = loop_detector.detect_loops(agents_submaps)
                               ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/lustre06/project/6003584/lemonci/MAGiC-SLAM/src/entities/loop_detection/loop_detector.py", line 161, in detect_loops
    rel_pose = np.linalg.inv(target_sub["submap_c2ws"][0]) @ source_sub["submap_c2ws"][0]
                                                             ~~~~~~~~~~^^^^^^^^^^^^^^^
TypeError: 'NoneType' object is not subscriptable

I still think changing

MAGiC-SLAM/src/entities/agent.py

Line 209 in 4ac5aa9

submap_data = self.get_submap_data(frame_id, gaussian_model)

to

submap_data = self.get_submap_data(frame_id + 1, gaussian_model)

is the right way to fix that.