Skip to content
TheChatArchiveThe Chat Archive
o1-preview-2024-09-12· OpenAIAi2 WildChat-4.8M (AI2)

I am working on a SLAM method using RGB input along with depth estimation, pose…

  1. User

    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
    
  2. Assistant

    Analysis of the Problem:

    The duplication of the corridor in your point cloud output is likely due to a combination of issues in pose estimation and point cloud registration. Specifically:

    1. Incorrect Scale Estimation: The scale factor calculated between frames may not be accurate, leading to incorrect translations being applied to the pose.

    2. Overlapping Transformations: Applying both the pose estimation from feature matching and then an ICP (Iterative Closest Point) alignment can introduce additional transformations that accumulate errors over time.

    3. Misalignment in Pose Updates: The order and manner in which transformations (rotations and translations) are applied can significantly affect the final outcome.

    Proposed Solutions:

    1. Remove the ICP Registration Step:

      • Since you already estimate the pose between frames using feature matching and the Essential matrix, applying ICP might be redundant and could introduce errors.
      • Removing the ICP step can prevent misalignments due to inconsistent transformations.
    2. Improve Scale Estimation:

      • Review the calculate_scale function to ensure it provides an accurate scale factor.
      • Consider using a constant scale factor or integrating additional information (e.g., known object sizes) if scale cannot be reliably estimated from depth maps.
    3. Verify Transformation Updates:

      • Ensure that the cumulative pose is updated correctly.
      • Double-check the order of matrix multiplication when updating poses, as the order matters in transformations.
    4. Adjust Pose Estimation Parameters:

      • Increase the number of features detected to improve pose estimation reliability.
      • Ensure that feature matching and keypoint detection are robust to variations in the video frames.

    Code Changes:

    Here are the specific changes you can make to your code to address these issues.


    1. Remove the ICP Registration Step in slam.py:

    Comment out or remove the block of code that performs ICP registration. This is found in the run method of the SLAMSystem class.

    Original Code Block:

    if frame_count > 0:
        transformation = register_point_clouds(current_pcd, self.global_map)
        current_pcd.transform(transformation)
        self.pose = self.pose @ transformation
    

    Modified Code (Commented Out):

    # if frame_count > 0:
    #     transformation = register_point_clouds(current_pcd, self.global_map)
    #     current_pcd.transform(transformation)
    #     self.pose = self.pose @ transformation
    

    By commenting out this block, you prevent the additional transformation from potentially misaligning your point clouds.


    2. Verify and Adjust the Scale Estimation in slam.py:

    Modify the calculate_scale method to ensure that it provides a reasonable scale factor.

    Original calculate_scale Method:

    def calculate_scale(self, depth_prev, depth_curr):
        ...
    

    Adjusted calculate_scale Method (Optional - Use Constant Scale):

    If the scale estimation is unreliable, you might temporarily use a constant scale factor of 1.0.

    def calculate_scale(self, depth_prev, depth_curr):
        return 1.0
    

    Alternatively, adjust the existing method to use a more robust statistic or handle depth maps more appropriately.


    3. Ensure Correct Pose Transformation Multiplication Order in slam.py:

    Confirm that the pose updates are correctly applied. The order of matrix multiplication should be such that the most recent transformation is applied first.

    Original Pose Update:

    self.pose = self.pose @ T
    

    Modified Pose Update (Swap Multiplication Order):

    self.pose = T @ self.pose
    

    Changing the multiplication order ensures that the new transformation T is applied to the current pose self.pose correctly.


    4. Verify the Application of Pose to the Point Cloud in slam.py:

    Ensure that when you apply the pose to the current point cloud, you use the correct transformation.

    Original Point Cloud Transformation:

    current_pcd.transform(self.pose)
    

    No Change Needed Here Unless There's an Issue with Coordinate Frames

    However, if after making the changes above, you still encounter issues, you may need to check whether the coordinate frames between your pose estimation and point cloud are consistent.


    5. Increase the Number of Features in pose_estimator.py:

    Increase the number of features detected by adjusting the parameters of the ORB detector.

    Original ORB Initialization:

    self.orb = cv2.ORB_create(5000)
    

    Modified ORB Initialization:

    self.orb = cv2.ORB_create(nfeatures=10000)
    

    Increasing nfeatures can help improve the robustness of feature matching between frames.


    6. Improve Feature Matching Robustness in pose_estimator.py:

    Modify the match_features method to use K-Nearest Neighbors and ratio test for better matching.

    Replace the match_features Method with the Following:

    def match_features(self, desc1, desc2):
        if desc1 is None or desc2 is None:
            print("One of the descriptors is None.")
            return []
    
        # Use KNN matcher with k=2
        matcher = cv2.BFMatcher(cv2.NORM_HAMMING)
        knn_matches = matcher.knnMatch(desc1, desc2, k=2)
        
        # Apply ratio test
        good_matches = []
        for m, n in knn_matches:
            if m.distance < 0.75 * n.distance:
                good_matches.append(m)
                
        return good_matches
    

    Using KNN matching and the ratio test can reduce false matches and improve pose estimation.


    7. Adjust Parameters in Pose Estimation in pose_estimator.py:

    Ensure that the Essential matrix computation and pose recovery are robust by adjusting parameters.

    Modify the estimate_pose Method:

    Adjust the RANSAC parameters to be more permissive or restrictive based on your data.

    E, mask = cv2.findEssentialMat(pts1, pts2, self.K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
    

    You might try changing threshold to 0.5 or adjusting prob to see if it improves pose estimation.


    8. Test and Visualize the Camera Trajectory:

    Add code to record and visualize the estimated camera trajectory to help debug the pose estimation.

    In slam.py, Add the Following in the run Method:

    At the top of the class, initialize a list to store camera positions:

    self.camera_positions = []
    

    After updating the pose, extract and store the camera position:

    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 = T @ self.pose
        print(f"Updated pose:\n{self.pose}")
    
        # Store the camera position
        camera_position = self.pose[:3, 3]
        self.camera_positions.append(camera_position)
    else:
        print(f"Frame {frame_count}: Pose estimation failed.")
    

    After processing all frames, you can plot the trajectory using matplotlib:

    import matplotlib.pyplot as plt
    
    # Plot camera trajectory
    camera_positions = np.array(self.camera_positions)
    plt.plot(camera_positions[:, 0], camera_positions[:, 2], 'r*-')
    plt.xlabel('X')
    plt.ylabel('Z')
    plt.title('Camera Trajectory')
    plt.show()
    

    Visualizing the trajectory can help you see if the camera movement makes sense.


    Summary of Changes:

    1. Removed ICP Registration Step: Prevents misalignment due to redundant transformations.

    2. Adjusted Scale Estimation: Ensures translations are applied correctly.

    3. Verified Pose Multiplication Order: Corrects the application of new poses.

    4. Improved Feature Detection and Matching: Enhances pose estimation reliability.

    5. Visualized Camera Trajectory: Aids in debugging and understanding pose estimation results.


    Final Notes:

    After implementing these changes, run your SLAM system again on the corridor video. The point cloud output should now represent a single, continuous corridor without duplication. Additionally, pay attention to any printed debug statements or visualizations to further validate the correctness of the pose estimations.

    Updated slam.py:

    Here is the updated slam.py with the changes applied:

    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
            self.camera_positions = []  # For trajectory visualization
            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):
            # Optionally, you can use a constant scale factor if scale estimation is unreliable
            return 1.0
    
        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 = T @ self.pose
                                print(f"Updated pose:\n{self.pose}")
                                # Store the camera position
                                camera_position = self.pose[:3, 3]
                                self.camera_positions.append(camera_position)
                            else:
                                print(f"Frame {frame_count}: Pose estimation failed.")
    
                    current_pcd.transform(self.pose)
                    
                    # Removed ICP Registration step
                    # 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])
    
            # Plot camera trajectory
            if len(self.camera_positions) > 0:
                import matplotlib.pyplot as plt
                camera_positions = np.array(self.camera_positions)
                plt.plot(camera_positions[:, 0], camera_positions[:, 2], 'r*-')
                plt.xlabel('X')
                plt.ylabel('Z')
                plt.title('Camera Trajectory')
                plt.show()
    
    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()
    

    Note: Remember to apply similar adjustments to pose_estimator.py and point_cloud_generator.py as outlined in the steps above.


    By carefully addressing each potential issue and thoroughly testing your SLAM system after making these changes, you should be able to resolve the duplication problem and achieve a more accurate reconstruction of the corridor.