I have a SLAM system with three scripts: slam.py, pose_estimator.py, and point_cloud_generator.py. The input video shows steady one-way motion in a corridor, but the output point cloud has a flower-like shape.

Questions:

Could there be an issue in the pose estimation (pose_estimator.py) that misinterprets linear motion as rotational, causing the distorted map?
Could missing trajectory correction or loop closure be the reason for the flower-shaped point cloud?
Is the frame-to-frame transformation incorrect? How can I verify the accuracy between frames?
Could camera calibration (distortion coefficients, focal length) be causing the point cloud distortion?
Is there a synchronization issue between pose estimation and point cloud generation?
Could an error in point_cloud_generator.py lead to this flower-shaped distortion?
How can I log transformations and pose estimates to debug the issue?
I will supply the slam.py, pose_estimator.py, and point_cloud_generator.py scripts for further analysis.

import os
import sys
import cv2
import numpy as np
import open3d as o3d
import argparse
import threading
import time

# Add the current directory to the Python path
current_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.append(current_dir)

# Add the 'zoe' and 'Metric3D' directories to the Python path
zoe_dir = os.path.join(current_dir, 'zoe')
metric3d_dir = os.path.join(current_dir, 'metric3dd')
sys.path.append(zoe_dir)
sys.path.append(metric3d_dir)

# Import point cloud generators
from point_cloud_generator import PointCloudGenerator as MidasPointCloudGenerator
from zoe.point_cloud_generator import PointCloudGenerator as ZoePointCloudGenerator
from metric3dd.point_cloud_generator import PointCloudGenerator as Metric3DPointCloudGenerator
from pose_estimator import PoseEstimator

class SLAMSystem:
    def __init__(self, video_path, focal_length, principal_point, real_time_vis=False, model='zoe', save_point_clouds=False):
        if model == 'midas':
            self.generator = MidasPointCloudGenerator()
            self.pcd_folder = 'pcd_midas'
        elif model == 'metric3d':
            self.generator = Metric3DPointCloudGenerator()
            self.pcd_folder = 'pcd_metric'
        else:  # default to zoe
            self.generator = ZoePointCloudGenerator()
            self.pcd_folder = 'pcd_zoe'
        
        self.estimator = PoseEstimator(focal_length, principal_point)
        self.cap = cv2.VideoCapture(video_path)
        self.global_map = o3d.geometry.PointCloud()
        self.prev_frame = None
        self.pose = np.eye(4)
        self.real_time_vis = real_time_vis
        self.save_point_clouds = save_point_clouds
        self.vis = None
        self.is_running = True
        if self.real_time_vis:
            self.vis = o3d.visualization.Visualizer()
            self.vis.create_window()
            self.vis.add_geometry(self.global_map)
            self.vis_thread = threading.Thread(target=self.visualization_loop)
            self.vis_thread.start()

    def calculate_scale(self, depth_prev, depth_curr):
        depth_prev_nonzero = depth_prev[depth_prev > 0]
        depth_curr_nonzero = depth_curr[depth_curr > 0]

        if len(depth_prev_nonzero) == 0 or len(depth_curr_nonzero) == 0:
            print("Insufficient depth information for scale calculation. Using default scale.")
            return 1.0

        median_prev = np.median(depth_prev_nonzero)
        median_curr = np.median(depth_curr_nonzero)

        if median_curr == 0:
            print("Median current depth is zero, cannot calculate scale.")
            return 1.0

        scale = median_prev / median_curr
        print(f"Calculated scale factor: {scale}")
        return scale

    def update_visualization(self):
        if self.real_time_vis:
            self.vis.update_geometry(self.global_map)
            self.vis.poll_events()
            self.vis.update_renderer()

    def visualization_loop(self):
        while self.is_running:
            self.update_visualization()
            time.sleep(0.1)  # Update every 100ms

    def run(self):
        frame_count = 0
        depth_prev = None

        if self.save_point_clouds:
            os.makedirs(self.pcd_folder, exist_ok=True)

        while True:
            ret, frame = self.cap.read()
            if not ret:
                print("End of video stream.")
                break
            print(f"Processing frame {frame_count}")

            point_cloud_np, depth_map = self.generator.process_frame_with_depth(frame)

            if self.save_point_clouds:
                pcd_filename = os.path.join(self.pcd_folder, f"frame_{frame_count:04d}.txt")
                self.generator.save_point_cloud_txt(point_cloud_np, pcd_filename)
                print(f"Saved point cloud to {pcd_filename}")
            else:
                current_pcd = o3d.geometry.PointCloud()
                current_pcd.points = o3d.utility.Vector3dVector(point_cloud_np[:, :3])
                current_pcd.colors = o3d.utility.Vector3dVector(point_cloud_np[:, 3:] / 255.0)

                scale = None

                if self.prev_frame is not None and depth_prev is not None:
                    gray_prev = cv2.cvtColor(self.prev_frame, cv2.COLOR_BGR2GRAY)
                    gray_current = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                    kp1, desc1 = self.estimator.detect_and_compute(gray_prev)
                    kp2, desc2 = self.estimator.detect_and_compute(gray_current)

                    matches = self.estimator.match_features(desc1, desc2)
                    if len(matches) == 0:
                        print(f"Frame {frame_count}: No valid matches found.")
                    else:
                        scale = self.calculate_scale(depth_prev, depth_map)
                        R, t = self.estimator.estimate_pose(kp1, kp2, matches, scale=scale)
                        if R is not None and t is not None:
                            T = np.eye(4)
                            T[:3, :3] = R
                            T[:3, 3] = t.squeeze()
                            self.pose = self.pose @ T
                            print(f"Updated pose:\n{self.pose}")
                        else:
                            print(f"Frame {frame_count}: Pose estimation failed.")

                current_pcd.transform(self.pose)
                self.global_map += current_pcd
                self.global_map = self.global_map.voxel_down_sample(voxel_size=0.05)

                self.prev_frame = frame.copy()
                depth_prev = depth_map.copy()

            frame_count += 1

        self.cap.release()

        if not self.save_point_clouds:
            o3d.io.write_point_cloud("global_map.pcd", self.global_map)
            print("Saved global map to 'global_map.pcd'.")

            if self.real_time_vis:
                self.is_running = False
                self.vis_thread.join()
                self.vis.destroy_window()
            else:
                o3d.visualization.draw_geometries([self.global_map])

def main():
    parser = argparse.ArgumentParser(description="Run SLAM system with optional real-time visualization or save point clouds")
    parser.add_argument("--video_path", type=str, default="videos/kutupkisa.mp4", help="Path to input video")
    parser.add_argument("--focal_length", type=float, default=700, help="Focal length in pixels")
    parser.add_argument("--principal_point_x", type=float, default=320, help="X-coordinate of principal point")
    parser.add_argument("--principal_point_y", type=float, default=240, help="Y-coordinate of principal point")
    parser.add_argument("--real_time_vis", action="store_true", help="Enable real-time visualization")
    parser.add_argument("--model", type=str, choices=['midas', 'zoe', 'metric3d'], default='zoe', help="Choose the point cloud generation model")
    parser.add_argument("--save_point_clouds", action="store_true", help="Save point clouds as txt files without running SLAM")
    args = parser.parse_args()

    principal_point = (args.principal_point_x, args.principal_point_y)
    slam = SLAMSystem(args.video_path, args.focal_length, principal_point, args.real_time_vis, args.model, args.save_point_clouds)
    slam.run()

if __name__ == "__main__":
    main()

import cv2
import numpy as np

class PoseEstimator:
    def __init__(self, focal_length, principal_point):
        # Initialize ORB detector
        self.orb = cv2.ORB_create(5000)
        # Initialize BFMatcher with Hamming distance for ORB
        self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        # Camera intrinsic parameters
        self.K = np.array([
            [focal_length, 0, principal_point[0]],
            [0, focal_length, principal_point[1]],
            [0, 0, 1]
        ])

    def detect_and_compute(self, img):
        keypoints, descriptors = self.orb.detectAndCompute(img, None)
        return keypoints, descriptors

    def match_features(self, desc1, desc2):
        """
        Matches features between two sets of descriptors after ensuring type and size consistency.
        Returns a list of matches sorted by distance.
        """
        try:
            # Check if descriptors are valid
            if desc1 is None or desc2 is None:
                print("One of the descriptors is None.")
                return []

            # Ensure descriptors are of type CV_8U or CV_32F
            if desc1.dtype != desc2.dtype:
                print(f"Descriptor type mismatch: desc1 dtype = {desc1.dtype}, desc2 dtype = {desc2.dtype}. Converting desc2 to match desc1.")
                desc2 = desc2.astype(desc1.dtype)

            # Ensure descriptors have the same number of dimensions
            if desc1.shape[1] != desc2.shape[1]:
                print(f"Descriptor size mismatch: desc1 cols = {desc1.shape[1]}, desc2 cols = {desc2.shape[1]}. Adjusting descriptor sizes.")
                min_cols = min(desc1.shape[1], desc2.shape[1])
                desc1 = desc1[:, :min_cols]
                desc2 = desc2[:, :min_cols]

            # Ensure descriptors are of valid type
            if desc1.dtype not in [np.uint8, np.float32] or desc2.dtype not in [np.uint8, np.float32]:
                print(f"Invalid descriptor types: desc1 dtype = {desc1.dtype}, desc2 dtype = {desc2.dtype}. Skipping matching.")
                return []

            # Perform matching
            matches = self.matcher.match(desc1, desc2)
            matches = sorted(matches, key=lambda x: x.distance)
            return matches

        except Exception as e:
            print(f"Exception during feature matching: {e}")
            return []

    def estimate_pose(self, kp1, kp2, matches, scale=None):
        """
        Estimates the relative pose between two frames given matched keypoints.
        Returns rotation matrix R and translation vector t, optionally scaled.
        """
        if len(matches) < 8:
            print("Not enough matches to compute pose.")
            return None, None
        try:
            # Extract matched keypoints
            pts1 = np.float32([kp1[m.queryIdx].pt for m in matches])
            pts2 = np.float32([kp2[m.trainIdx].pt for m in matches])
            # Compute Essential matrix
            E, mask = cv2.findEssentialMat(pts1, pts2, self.K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
            if E is None:
                print("Essential matrix computation failed.")
                return None, None
            # Recover pose from Essential matrix
            _, R, t, mask_pose = cv2.recoverPose(E, pts1, pts2, self.K)

            if scale is not None:
                t = t * scale
                print(f"Applied scale factor: {scale}")
            else:
                print("No scale applied.")

            return R, t
        except Exception as e:
            print(f"Exception during pose estimation: {e}")
            return None, None

import cv2
import torch
import numpy as np
from PIL import Image
from torchvision import transforms

class PointCloudGenerator:
    def __init__(self, calibration_path='samsung_cam_cal.npz', transformation=None, voxel_size=0.02):
        # Metric3D setup
        self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
        self.metric3d_model = torch.hub.load('yvanyin/metric3d', 'metric3d_vit_small', pretrain=True)
        #self.metric3d_model = torch.hub.load('yvanyin/metric3d', 'metric3d_vit_', pretrained=True)
        self.metric3d_model.to(self.device)
        self.metric3d_model.eval()

        # Load camera parameters from NPZ file
        if calibration_path:
            self.load_camera_parameters(calibration_path)
        else:
            self.set_default_camera_parameters()

        # Compute Q matrix
        self.compute_Q_matrix()

        # Rotation matrix
        self.R = np.array([
            [0, 0, -1],
            [0, 1, 0],
            [1, 0, 0]
        ])

        # Transformation matrix
        if transformation is None:
            self.transformation = np.array([
                [1, 0, 0, 0],
                [0, -1, 0, 0],
                [0, 0, -1, 0],
                [0, 0, 0, 1]
            ])
        else:
            self.transformation = transformation

        self.voxel_size = voxel_size

    def set_default_camera_parameters(self):
        self.FX = 350.0  # Default focal length
        self.FY = 350.0
        self.CX = 160.0  # Default principal point
        self.CY = 120.0
        print("Using default camera parameters")

    def load_camera_parameters(self, calibration_path):
        try:
            calibration_data = np.load(calibration_path)
            print(f"Available keys in calibration file: {calibration_data.files}")
            
            if 'mtx' in calibration_data:
                camera_matrix = calibration_data['mtx']
                self.FX = camera_matrix[0, 0]
                self.FY = camera_matrix[1, 1]
                self.CX = camera_matrix[0, 2]
                self.CY = camera_matrix[1, 2]
                print(f"Camera parameters loaded from {calibration_path}")
                print(f"FX: {self.FX}, FY: {self.FY}, CX: {self.CX}, CY: {self.CY}")
            else:
                print(f"Camera matrix not found in {calibration_path}")
                self.set_default_camera_parameters()

            if 'dist' in calibration_data:
                self.dist_coeffs = calibration_data['dist'].ravel()
                print(f"Distortion coefficients loaded: {self.dist_coeffs}")
            else:
                print("Distortion coefficients not found, assuming no distortion")
                self.dist_coeffs = np.zeros(5)

        except Exception as e:
            print(f"Error loading camera parameters: {e}")
            self.set_default_camera_parameters()

    def compute_Q_matrix(self):
        self.Q = np.array([[1.0, 0.0, 0.0, -self.CX],
                           [0.0, 1.0, 0.0, -self.CY],
                           [0.0, 0.0, 0.0, self.FX],
                           [0.0, 0.0, 1.0/self.FY, 0.0]], dtype=np.float32)

    def process_frame_with_depth(self, frame):
        """
        Processes a frame and returns both the point cloud and the depth map.
        """
        # Undistort the frame if distortion coefficients are available
        if hasattr(self, 'dist_coeffs'):
            h, w = frame.shape[:2]
            new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(
                np.array([[self.FX, 0, self.CX], [0, self.FY, self.CY], [0, 0, 1]]), 
                self.dist_coeffs, (w, h), 1, (w, h)
            )
            frame = cv2.undistort(frame, 
                                  np.array([[self.FX, 0, self.CX], [0, self.FY, self.CY], [0, 0, 1]]), 
                                  self.dist_coeffs, 
                                  None, 
                                  new_camera_matrix)

        # RGB to Depth
        color_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        original_height, original_width = color_image.shape[:2]
        print(f"Original image shape: {color_image.shape}")

        # Convert to tensor for Metric3D
        image_tensor = transforms.ToTensor()(color_image).unsqueeze(0).to(self.device)

        with torch.no_grad():
            pred_depth, confidence, output_dict = self.metric3d_model.inference({'input': image_tensor})
            depth_map = pred_depth.squeeze().cpu().numpy()
            # Optional: get normal prediction if needed
            # pred_normal = output_dict['prediction_normal'][:, :3, :, :]
            # normal_confidence = output_dict['prediction_normal'][:, 3, :, :]

        print(f"Depth map shape before resize: {depth_map.shape}")
        
        # Resize depth map to match original image size
        depth_map = cv2.resize(depth_map, (original_width, original_height), interpolation=cv2.INTER_LINEAR)
        print(f"Depth map shape after resize: {depth_map.shape}")

        # Depth to Point Cloud
        points_3D = cv2.reprojectImageTo3D(depth_map, self.Q, handleMissingValues=False)
        mask_map = depth_map > 0.4  # Threshold to filter out invalid points
        print(f"Mask map shape: {mask_map.shape}")
        
        output_points = points_3D[mask_map]
        output_colors = color_image[mask_map]

        print(f"Output points shape: {output_points.shape}")
        print(f"Output colors shape: {output_colors.shape}")

        # Downsample
        downsample_rate = int(1 / self.voxel_size)
        if downsample_rate > 1:
            downsampled_indices = np.arange(0, output_points.shape[0], downsample_rate)
            output_points = output_points[downsampled_indices]
            output_colors = output_colors[downsampled_indices]

        # Apply transformations
        output_points = np.dot(output_points, self.R.T)
        homogeneous_points = np.hstack((output_points, np.ones((output_points.shape[0], 1))))
        transformed_points = np.dot(homogeneous_points, self.transformation.T)[:, :3]

        # Combine points and colors
        point_cloud = np.hstack((transformed_points, output_colors))

        return point_cloud, depth_map

    def save_point_cloud_txt(self, point_cloud, filename):
        np.savetxt(filename, point_cloud, fmt='%.6f %.6f %.6f %d %d %d')

def save_point_cloud_pcd(point_cloud, filename):
    with open(filename, 'w') as f:
        f.write("# .PCD v0.7 - Point Cloud Data file format\n")
        f.write("VERSION 0.7\n")
        f.write("FIELDS x y z rgb\n")
        f.write("SIZE 4 4 4 4\n")
        f.write("TYPE F F F U\n")
        f.write("COUNT 1 1 1 1\n")
        f.write(f"WIDTH {len(point_cloud)}\n")
        f.write("HEIGHT 1\n")
        f.write("VIEWPOINT 0 0 0 1 0 0 0 0\n")
        f.write(f"POINTS {len(point_cloud)}\n")
        f.write("DATA ascii\n")
        for point in point_cloud:
            x, y, z, r, g, b = point
            rgb = int(r) << 16 | int(g) << 8 | int(b)
            f.write(f"{x} {y} {z} {rgb}\n")
# Function to save point cloud to pcd file (if needed)