I am working on a SLAM method using RGB input along with depth estimation, pose estimation, and point cloud generation. The input video is footage of a straight corridor, but the output point cloud shows multiple side-by-side copies of the corridor (around 15-20) instead of a single, straight representation. My SLAM system consists of three scripts: slam.py, pose_estimator.py, and point_cloud_generator.py. Could you help identify why the point clouds are duplicated and how to correct the pose estimation or depth processing to ensure the output is a single, accurate point cloud of the corridor? I will provide the three scripts for review. make changes and think as much as you want and give me the code changes.

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

def register_point_clouds(source_pcd, target_pcd):
        icp_threshold = 0.02
        trans_init = np.eye(4)
        reg_p2p = o3d.pipelines.registration.registration_icp(
            source_pcd, target_pcd, icp_threshold, trans_init,
            o3d.pipelines.registration.TransformationEstimationPointToPoint())
        return reg_p2p.transformation

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

        # Use percentile-based scaling
        prev_depth = np.percentile(depth_prev_nonzero, 50)
        curr_depth = np.percentile(depth_curr_nonzero, 50)

        if curr_depth == 0:
            print("Current depth is zero, cannot calculate scale.")
            return 1.0

        scale = prev_depth / curr_depth

        # Limit scale factor to prevent extreme values
        max_scale = 1.5
        min_scale = 0.5
        scale = np.clip(scale, min_scale, max_scale)

        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)
                
                if frame_count > 0:
                    transformation = register_point_clouds(current_pcd, self.global_map)
                    current_pcd.transform(transformation)
                    self.pose = self.pose @ transformation

                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 torch
import numpy as np
from PIL import Image
from torchvision import transforms
import open3d as o3d

class PointCloudGenerator:
    def __init__(self, calibration_path='zoe/CalibrationMatrix_college_cpt.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_large', pretrain=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 (keeping this as it was in the original code)
        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
        self.dist_coeffs = np.zeros(5)
        print("Using default camera parameters")

    def load_camera_parameters(self, calibration_path):
        try:
            calibration_data = np.load(calibration_path)
            if 'Camera_matrix' in calibration_data:
                camera_matrix = calibration_data['Camera_matrix']
                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: FX: {self.FX}, FY: {self.FY}, CX: {self.CX}, CY: {self.CY}")
            else:
                raise KeyError("Camera matrix not found in calibration file")

            if 'distCoeff' in calibration_data:
                self.dist_coeffs = calibration_data['distCoeff'].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):
        # Undistort the frame
        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)
        )
        undistorted_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(undistorted_frame, cv2.COLOR_BGR2RGB)
        image_tensor = transforms.ToTensor()(color_image).unsqueeze(0).to(self.device)

        with torch.no_grad():
            pred_depth, confidence, _ = self.metric3d_model.inference({'input': image_tensor})
            depth_map = pred_depth.squeeze().cpu().numpy()

        # Ensure depth_map matches the original frame size
        depth_map = cv2.resize(depth_map, (w, h), interpolation=cv2.INTER_LINEAR)

        # Scale depth map to real-world units (e.g., meters)
        depth_scale = 1.0  # Adjust this value based on your depth model's output scale
        scaled_depth_map = depth_map * depth_scale

        # Create 3D points
        rows, cols = scaled_depth_map.shape
        c, r = np.meshgrid(np.arange(cols), np.arange(rows))
        points = np.zeros((rows * cols, 3), dtype=np.float32)
        points[:, 0] = (c.flatten() - self.CX) * scaled_depth_map.flatten() / self.FX
        points[:, 1] = (r.flatten() - self.CY) * scaled_depth_map.flatten() / self.FY
        points[:, 2] = scaled_depth_map.flatten()

        # Improved filtering
        min_depth = 0.1  # Adjust based on your scene
        max_depth = 10.0  # Adjust based on your scene
        min_brightness = 20  # Adjust to filter out very dark points

        mask = (scaled_depth_map.flatten() > min_depth) & \
               (scaled_depth_map.flatten() < max_depth) & \
               (np.mean(color_image.reshape(-1, 3), axis=1) > min_brightness)

        valid_points = points[mask]
        valid_colors = color_image.reshape(-1, 3)[mask]

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

        # Downsample
        if self.voxel_size > 0:
            pcd = o3d.geometry.PointCloud()
            pcd.points = o3d.utility.Vector3dVector(transformed_points)
            pcd.colors = o3d.utility.Vector3dVector(valid_colors.astype(np.float64) / 255.0)
            downsampled_pcd = pcd.voxel_down_sample(voxel_size=self.voxel_size)
            transformed_points = np.asarray(downsampled_pcd.points)
            valid_colors = np.asarray(downsampled_pcd.colors) * 255

        # Combine points and colors for final point cloud
        point_cloud = np.hstack((transformed_points, valid_colors.astype(np.uint8)))

        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")
            
            
 import cv2
import numpy as np

class PoseEstimator:
    def __init__(self, focal_length, principal_point):
        self.orb = cv2.ORB_create(5000)
        self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        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):
        if desc1 is None or desc2 is None:
            print("One of the descriptors is None.")
            return []

        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)

        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]

        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 []

        try:
            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=1.0):
        if len(matches) < 8:
            print("Not enough matches to compute pose.")
            return None, None

        try:
            pts1 = np.float32([kp1[m.queryIdx].pt for m in matches])
            pts2 = np.float32([kp2[m.trainIdx].pt for m in matches])
            
            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

            _, R, t, mask_pose = cv2.recoverPose(E, pts1, pts2, self.K)

            # Apply scale to the translation vector
            t = t * scale
            print(f"Applied scale factor: {scale}")

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