İ will provide you 2 code and one camera trajectory file. my codes are used for Rgb slam. But I have some camera trajectory problem. for exp, the provided camera trajectory data came from a video of straight corridor with just forward camera motion no up down left right motion present on the video. But when I look at the camera trajectory, I can see the camera is moving on the x=y line and linearly going up on z direction. I have look it up following definitions of point cloud data and I have already try different combinations of this data to get true result. But at the end, I have similar or worst results. What is the problem and how I can solve it? Do not suggest me already tried methods. Also give me explanation about the code changes. I already said that I try to change following definitions on the code. Here is the part that I am talking about:


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()


x = (u - self.camera_matrix[0, 2]) * d / self.camera_matrix[0, 0]
                                            y = (v - self.camera_matrix[1, 2]) * d / self.camera_matrix[1, 1]
                                            z = d
                                            pts1_3d.append([x, y, z])

Camera trajectory output of straight forward motion corridor:

X,Y,Z
0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00
-2.106866019708370300e-02,-1.233237029586714203e-02,1.737252275186531303e-01
6.883458426173452716e-03,-3.054231843243775113e-02,3.808254298075146038e-01
-5.133316609047243462e-02,-4.859879684944488909e-02,6.237961550115738163e-01
-5.119167321386901975e-02,-7.877574962953182158e-02,8.992191253800179140e-01
-8.078823998486120428e-02,-1.100876053918823649e-01,1.176712554749953510e+00
-7.509711472412294864e-02,-1.426913381153557259e-01,1.442482613183612816e+00
-7.654255075969726430e-02,-1.705903399183592850e-01,1.742348932455024091e+00
-1.316147972722124526e-01,-1.996772481796073861e-01,2.037094749481995759e+00
-1.276273585845077996e-01,-2.184926521932097909e-01,2.324575003887544788e+00
-1.807760550459199256e-01,-2.462585650691015982e-01,2.629088539981847994e+00
-2.171164883803595458e-01,-2.712031886917366519e-01,2.898067823724506553e+00
-2.911004052833894851e-01,-2.962285220530532226e-01,3.202829773122042756e+00
-3.486545506237072045e-01,-3.331660823720809517e-01,3.502327362306211445e+00
-3.951947810089714297e-01,-4.113970671210018804e-01,3.764368058909312698e+00
-4.971633380379245626e-01,-4.638402831432880680e-01,4.030135715208872149e+00
-5.541058493205149871e-01,-5.187230412909290544e-01,4.317516257299349292e+00
-6.798805826739782177e-01,-5.731123115014977731e-01,4.629502231498921816e+00
-7.072196131009170772e-01,-6.231280943644793568e-01,4.969602513277716405e+00
-8.588952460755496787e-01,-6.847977160532663365e-01,5.259502430743088652e+00
-1.024868763518981485e+00,-8.103785040777916615e-01,5.699660969306527214e+00
-1.150224086263879286e+00,-8.386896141767747892e-01,6.025618908308659982e+00
-1.216764860009744709e+00,-8.876521959924494887e-01,6.385021697742146785e+00
-1.247476729764108949e+00,-9.034884977206073531e-01,6.656252287231204434e+00
-1.317133995078187958e+00,-9.598323418948327967e-01,7.002153678816116944e+00
-1.486498242671893122e+00,-1.079558321453500946e+00,7.278452942825688332e+00
-1.600315722457464895e+00,-1.141148395297252538e+00,7.590733708147659797e+00
-1.512250887207540195e+00,-1.139902977669578155e+00,7.826627690326575681e+00
-1.458677037351645778e+00,-1.173456836555333949e+00,8.107916981782384624e+00
-1.370329419224681988e+00,-1.211507011799148215e+00,8.403368348986839820e+00
-1.259261702611637279e+00,-1.265747076622927514e+00,8.682946512528602057e+00
-1.170226033667295829e+00,-1.303297575530007579e+00,9.067637981726445418e+00
-9.726017267008273404e-01,-1.324778176984357847e+00,9.333121738923253474e+00
-7.816474993150085426e-01,-1.364738575021868794e+00,9.578961176364709118e+00
-5.587009439137169409e-01,-1.394306019625943138e+00,9.876666984042818953e+00
-2.825309381645368378e-01,-1.437855874386702038e+00,1.014949668909382297e+01
-3.077352005835254367e-04,-1.470759671162354287e+00,1.047587782579307714e+01
3.006872388953815700e-01,-1.503530262755513158e+00,1.080669619217027488e+01
5.661895267278354016e-01,-1.484911104108220359e+00,1.114669045088428234e+01
8.571982419762034233e-01,-1.511297587685040300e+00,1.141081367191367946e+01
1.016576054131162588e+00,-1.466735003043260654e+00,1.163161579370800602e+01
1.242574265662406585e+00,-1.568012810785285760e+00,1.204110426469472017e+01
1.460885977661744439e+00,-1.654219386434585326e+00,1.203293839545145971e+01
1.617754709287845705e+00,-1.669131785077293761e+00,1.228300591424877553e+01
1.810302303022492065e+00,-1.827008244286108418e+00,1.273736968020872062e+01
2.046771097047959120e+00,-1.893293245325989416e+00,1.293696303557884519e+01
2.355425742576942483e+00,-1.899897048861528548e+00,1.300900410593773771e+01
2.679916639152808422e+00,-1.897118306870251736e+00,1.298811362122612856e+01
2.949104374527607764e+00,-1.894988681276544407e+00,1.289058337174301805e+01
3.253669048268652553e+00,-1.911099090364773323e+00,1.283029145246196379e+01
3.564883501744642302e+00,-1.919596693345936744e+00,1.274962517459836242e+01
3.877607649684990054e+00,-1.994974422905789968e+00,1.275914845781588625e+01
4.225014893754975631e+00,-2.122841080291316906e+00,1.270876868250763714e+01
4.605931628555754109e+00,-2.127212742136513679e+00,1.266144266891287806e+01
5.005099406760360026e+00,-2.137514863555801892e+00,1.260213260319611273e+01
5.423024892978770772e+00,-2.032346836036432958e+00,1.262081662498913737e+01
5.853550004899219061e+00,-2.033882882026692762e+00,1.253422461916970398e+01
6.331720530918479639e+00,-1.989597364858153083e+00,1.250486916594199549e+01
6.846764044526421955e+00,-1.925501506030818932e+00,1.229418357392218830e+01
7.368066593172399870e+00,-1.898277888937924374e+00,1.216111366925088788e+01


slam.py code:

import os
import sys
import cv2
import numpy as np
import open3d as o3d
import argparse
import threading
import time
import matplotlib.pyplot as plt
from datetime import datetime


#direction definitions for scripts
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

#timestap function for folders

def create_timestamped_log_folder():
    timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
    base_folder = f"logs/log_{timestamp}"
    subfolders = ['log', 'point_cloud_file', 'camera_trajectory', 'frame_point_cloud_file']
    
    for subfolder in subfolders:
        os.makedirs(os.path.join(base_folder, subfolder), exist_ok=True)
    
    return base_folder


#real time matplot visualization class

class MatplotlibVisualizer:
    def __init__(self):
        plt.ion()  # Enable interactive mode before creating figure
        dpi = 100  # You can adjust this value if needed
        width_inches = 640 / dpi
        height_inches = 480 / dpi
        
        self.fig = plt.figure(figsize=(width_inches, height_inches), dpi=dpi)
        self.ax = self.fig.add_subplot(111, projection='3d')
        self.points = None
        self.colors = None
        self.camera_positions = []
        self.is_running = True
        
        # Set up the plot
        self.ax.set_xlabel('X')
        self.ax.set_ylabel('Y')
        self.ax.set_zlabel('Z')
        self.ax.set_title('SLAM Visualization')
        
        self.fig.show()
        plt.pause(0.1)  # Add small pause to ensure window appears
    

    def close(self):
        plt.close(self.fig)  # Close the figure window
        self.is_running = False


    def update(self, points, colors, camera_position):
        if not self.is_running:
            return
            
        try:
            self.ax.cla()  # Clear the current plot
            
            # Plot point cloud
            if len(points) > 0:
                # Downsample points for visualization if necessary
                max_points = 5000
                if len(points) > max_points:
                    idx = np.random.choice(len(points), max_points, replace=False)
                    vis_points = points[idx]
                    vis_colors = colors[idx]
                else:
                    vis_points = points
                    vis_colors = colors
                
                self.ax.scatter(vis_points[:, 0], vis_points[:, 1], vis_points[:, 2], 
                              c=vis_colors, s=1)
            
            # Update camera trajectory
            if camera_position is not None:
                self.camera_positions.append(camera_position)
                positions = np.array(self.camera_positions)
                self.ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], 'r-', linewidth=2)
                self.ax.plot([positions[-1, 0]], [positions[-1, 1]], [positions[-1, 2]], 'r*', markersize=10)
            
            # Adjust view based on points
            if len(points) > 0:
                max_range = np.array([points[:, 0].max() - points[:, 0].min(),
                                    points[:, 1].max() - points[:, 1].min(),
                                    points[:, 2].max() - points[:, 2].min()]).max() / 2.0
                mid_x = (points[:, 0].max() + points[:, 0].min()) * 0.5
                mid_y = (points[:, 1].max() + points[:, 1].min()) * 0.5
                mid_z = (points[:, 2].max() + points[:, 2].min()) * 0.5
                self.ax.set_xlim(mid_x - max_range, mid_x + max_range)
                self.ax.set_ylim(mid_y - max_range, mid_y + max_range)
                self.ax.set_zlim(mid_z - max_range, mid_z + max_range)
            
            self.ax.set_xlabel('X')
            self.ax.set_ylabel('Y')
            self.ax.set_zlabel('Z')
            self.ax.set_title('SLAM Visualization')
            
            plt.draw()
            plt.pause(0.01)  # Add small pause to allow plot to update
            
        except Exception as e:
            print(f"Error updating visualization: {str(e)}")
            




class SLAMSystem:
    def __init__(self, video_path, calibration_file, real_time_vis=False, end_vis = True, model='zoe', save_point_clouds=False, 
                 frame_interval=1, output_pcd_name='global_map.pcd', voxel_size=0.02, min_depth=2.0, max_depth=10.0,
                 min_brightness=10, orb_features=5000, feature_match_ratio=0.75, essential_matrix_threshold=1.0, output_format = 'pcd'):
        

        #model path definitions
        if model == 'midas':
            self.generator = MidasPointCloudGenerator(calibration_path=calibration_file, voxel_size=voxel_size, min_brightness = min_brightness, max_depth = max_depth, min_depth = min_depth)
            self.pcd_folder = 'pcd_midas'
        elif model == 'metric3d':
            self.generator = Metric3DPointCloudGenerator(calibration_path=calibration_file, voxel_size=voxel_size, min_brightness = min_brightness, max_depth = max_depth, min_depth = min_depth)
            self.pcd_folder = 'pcd_metric'
        else:  # default to zoe
            self.generator = ZoePointCloudGenerator(calibration_path=calibration_file, voxel_size=voxel_size, min_brightness = min_brightness, max_depth = max_depth, min_depth = min_depth)
            self.pcd_folder = 'pcd_zoe'

        self.save_point_clouds = save_point_clouds
        self.output_format = output_format
        #Folder definitions
        self.base_folder = create_timestamped_log_folder()
        self.log_folder = os.path.join(self.base_folder, 'log')
        if save_point_clouds:
            self.save_pcd_folder = os.path.join(self.base_folder, 'frame_point_cloud_file')
        else:
            self.save_pcd_folder = os.path.join(self.base_folder, 'point_cloud_file')
            self.trajectory_folder = os.path.join(self.base_folder, 'camera_trajectory')
        
        
        #local definitions of parameters
        self.model = model
        self.video_path = video_path
        self.calibration_file = calibration_file
        self.camera_matrix, self.dist_coeffs = self.load_camera_parameters()
        self.frame_interval = frame_interval
        self.output_pcd_name = output_pcd_name
        self.voxel_size = voxel_size
        self.min_depth = min_depth
        self.max_depth = max_depth
        self.min_brightness = min_brightness
        self.orb_features = orb_features
        self.feature_match_ratio = feature_match_ratio
        self.essential_matrix_threshold = essential_matrix_threshold
        
        
        #estimator definitions
        focal_length = (self.camera_matrix[0, 0] + self.camera_matrix[1, 1]) / 2
        principal_point = (self.camera_matrix[0, 2], self.camera_matrix[1, 2])
        self.estimator = PoseEstimator(focal_length, principal_point, orb_features, feature_match_ratio, essential_matrix_threshold)
        

        #video capture, camera, global map, frame definitions
        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.end_vis = end_vis
        self.camera_positions = []
        self.pcd_list = []

        #real time visualization definitions
        self.is_running = True
        self.vis = None
        self.vis_updated = threading.Event()  # Initialize the Event

        if self.real_time_vis:
            self.vis = MatplotlibVisualizer()
            #self.vis_thread = threading.Thread(target=self.visualization_loop)
            #self.vis_thread.start()


        # Initialize logging
        self.log_filename = os.path.join(self.log_folder, 'slam_log.txt')
        self.log_parameters()

    def log_parameters(self):
        with open(self.log_filename, 'w') as log_file:
            log_file.write(f"SLAM Parameters Log\n")
            log_file.write(f"Timestamp: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}\n\n")
            log_file.write(f"Model: {self.model}\n")
            log_file.write(f"Video Path: {self.video_path}\n")
            log_file.write(f"Calibration File: {self.calibration_file}\n")
            log_file.write(f"Output PCD Name: {self.output_pcd_name}\n")
            log_file.write(f"Frame Interval: {self.frame_interval}\n")
            log_file.write(f"Voxel Size: {self.voxel_size}\n")
            log_file.write(f"Minimum Depth: {self.min_depth}\n")
            log_file.write(f"Maximum Depth: {self.max_depth}\n")
            log_file.write(f"Minimum Brightness: {self.min_brightness}\n")
            log_file.write(f"ORB Features: {self.orb_features}\n")
            log_file.write(f"Feature Match Ratio: {self.feature_match_ratio}\n")
            log_file.write(f"Essential Matrix Threshold: {self.essential_matrix_threshold}\n")

    def log_message(self, message):
    # Format the message with timestamp
        formatted_message = f"{datetime.now().strftime('%Y-%m-%d %H:%M:%S')} - {message}"
        
        # Print the message to the terminal
        print(formatted_message)
        
        # Log the message to the file
        with open(self.log_filename, 'a') as log_file:
            log_file.write(formatted_message + '\n')

   
    def load_camera_parameters(self):
        try:
            data = np.load(self.calibration_file)
            camera_matrix = data['Camera_matrix']
            dist_coeffs = data['distCoeff'].ravel()
            print(f"Camera matrix:\n{camera_matrix}")
            print(f"Distortion coefficients: {dist_coeffs}")
            return camera_matrix, dist_coeffs
        except Exception as e:
            print(f"Error loading camera parameters: {str(e)}")
            print("Using default camera parameters")
            return np.array([[525.0, 0, 320.0], [0, 525.0, 240.0], [0, 0, 1]]), np.zeros(5)


    def save_point_cloud(self, pcd, index):
        pcd_o3d = o3d.geometry.PointCloud()
        pcd_o3d.points = o3d.utility.Vector3dVector(pcd[:, :3])
        pcd_o3d.colors = o3d.utility.Vector3dVector(pcd[:, 3:] / 255.0)

        filename = f"frame_{index:04d}.{self.output_format}"
        filepath = os.path.join(self.save_pcd_folder, filename)

        if self.output_format == 'pcd':
            o3d.io.write_point_cloud(filepath, pcd_o3d)
        elif self.output_format == 'txt':
            np.savetxt(filepath, pcd, fmt='%.6f', header='x y z r g b', comments='')

        print(f"Saved point cloud: {filepath}")
        

    def update_visualization(self):
        if self.real_time_vis and self.vis:
            try:
                # Convert Open3D point cloud to numpy arrays
                points = np.asarray(self.global_map.points)
                colors = np.asarray(self.global_map.colors)
                
                if len(points) > 0:  # Only update if there are points to display
                    if len(self.camera_positions) > 0:
                        camera_position = self.camera_positions[-1]
                    else:
                        camera_position = None
                        
                    self.vis.update(points, colors, camera_position)
                    
                self.vis_updated.set()
            except Exception as e:
                print(f"Visualization update error: {str(e)}")

    def visualization_loop(self):
        while self.is_running:
            self.vis_updated.wait()
            self.update_visualization()
            self.vis_updated.clear()
            time.sleep(0.1)  # Add small delay to prevent excessive updates

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

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

        try:
            while True:
                ret, frame = self.cap.read()
                if not ret:
                    self.log_message("End of video stream.")
                    break

                if frame_count % self.frame_interval == 0:
                    self.log_message(f"Processing frame {frame_count}")

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

                    if self.save_point_clouds:
                        self.save_point_cloud(point_cloud_np, frame_count)
                    else:
                        # Create current point cloud
                        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)

                        # Remove outliers
                        cl, ind = current_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
                        current_pcd = current_pcd.select_by_index(ind)


                        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) < 8:
                                self.log_message(f"Frame {frame_count}: Insufficient matches found ({len(matches)}). Skipping pose estimation.")
                            else:
                                # Get matched keypoints
                                pts1 = np.array([kp1[m.queryIdx].pt for m in matches])
                                pts2 = np.array([kp2[m.trainIdx].pt for m in matches])

                                # Initialize lists for 3D-2D correspondences
                                pts1_3d = []
                                pts2_2d = []

                                for i, (u, v) in enumerate(pts1):
                                    u_int, v_int = int(round(u)), int(round(v))
                                    if 0 <= u_int < depth_prev.shape[1] and 0 <= v_int < depth_prev.shape[0]:
                                        d = depth_prev[v_int, u_int]
                                        if d > 0:
                                            # Compute 3D point in camera coordinates
                                            x = (u - self.camera_matrix[0, 2]) * d / self.camera_matrix[0, 0]
                                            y = (v - self.camera_matrix[1, 2]) * d / self.camera_matrix[1, 1]
                                            z = d
                                            pts1_3d.append([x, y, z])
                                            pts2_2d.append(pts2[i])

                                pts1_3d = np.array(pts1_3d).reshape(-1, 3)
                                pts2_2d = np.array(pts2_2d).reshape(-1, 2)

                                if len(pts1_3d) < 6:
                                    self.log_message(f"Frame {frame_count}: Not enough 3D-2D correspondences ({len(pts1_3d)}). Skipping pose estimation.")
                                else:
                                    # Use PnP to estimate pose
                                    retval, rvec, tvec, inliers = cv2.solvePnPRansac(
                                        pts1_3d, pts2_2d, self.camera_matrix, self.dist_coeffs
                                    )

                                    if retval:
                                        # Convert rotation vector to rotation matrix
                                        R, _ = cv2.Rodrigues(rvec)
                                        t = tvec
                                        T = np.eye(4)
                                        T[:3, :3] = R
                                        T[:3, 3] = t.flatten()

                                        # Update the global pose
                                        self.pose = self.pose @ np.linalg.inv(T)
                                        #self.log_message(f"Updated pose with PnP:\n{self.pose}")

                                        # Store the camera position
                                        camera_position = self.pose[:3, 3]
                                        self.camera_positions.append(camera_position)
                                    else:
                                        self.log_message(f"Frame {frame_count}: PnP pose estimation failed.")
                        else:
                            # For the first frame, the pose is identity
                            self.pose = np.eye(4)
                            camera_position = self.pose[:3, 3]
                            self.camera_positions.append(camera_position)

                        # Transform current point cloud to global coordinate frame
                        current_pcd.transform(self.pose)
                        current_pcd.voxel_down_sample(voxel_size=self.voxel_size)
                        self.global_map += current_pcd
                        
                        if self.real_time_vis:
                            self.update_visualization()
                        # Store the current point cloud
                        self.pcd_list.append(current_pcd)

                        # Prepare for the next iteration
                        self.prev_frame = frame.copy()
                        depth_prev = depth_map.copy()

                        processed_frame_count += 1

                frame_count += 1

        except Exception as e:
            self.log_message(f"An error occurred during SLAM processing: {str(e)}")

        finally:
            self.cap.release()
            

            if not self.save_point_clouds:
                pcd_name = os.path.join(self.save_pcd_folder, self.output_pcd_name)
                o3d.io.write_point_cloud(pcd_name, self.global_map)
                self.log_message(f"Saved global map to '{pcd_name}'.")

                if self.real_time_vis:
                    self.is_running = False
                    #self.vis_thread.join()
                    if self.vis:
                        self.vis.close()
                
                if self.end_vis:

                    o3d.visualization.draw_geometries([self.global_map])
                

            if len(self.camera_positions) > 0:
                camera_positions = np.array(self.camera_positions)
                fig = plt.figure()
                ax = fig.add_subplot(111, projection='3d')
                ax.plot(camera_positions[:, 0], camera_positions[:, 1], camera_positions[:, 2], 'r*-')
                ax.set_xlabel('X')
                ax.set_ylabel('Y')
                ax.set_zlabel('Z')
                ax.set_title('Camera Trajectory')
                #plt.show()

                cam_traj_name = os.path.join(self.trajectory_folder, 'camera_trajectory.txt')
            
                # Save camera trajectory to file
                np.savetxt(cam_traj_name, camera_positions, delimiter=",", header="X,Y,Z", comments="")
                self.log_message(f"Saved camera trajectory to {cam_traj_name}.")

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("--calibration_file", type=str, help="Path to camera calibration NPZ file")
    parser.add_argument("--real_time_vis", action="store_true", help="Enable real-time visualization")
    parser.add_argument("--end_vis", action="store_false", help="Disable result visualization on Open3D")
    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")
    parser.add_argument("--frame_interval", type=int, default=1, help="Process every Nth frame (default: 1)")
    parser.add_argument("--output_pcd", type=str, default="global_pcd.pcd", help="Filename for the final global point cloud (default: global_pcd.pcd)")
    parser.add_argument("--voxel_size", type=float, default=0.02, help="Voxel size for point cloud downsampling")
    parser.add_argument("--min_depth", type=float, default=2.0, help="Minimum depth for point cloud filtering")
    parser.add_argument("--max_depth", type=float, default=10.0, help="Maximum depth for point cloud filtering")
    parser.add_argument("--min_brightness", type=int, default=10, help="Minimum brightness for point cloud filtering")
    parser.add_argument("--orb_features", type=int, default=5000, help="Number of ORB features to detect")
    parser.add_argument("--feature_match_ratio", type=float, default=0.75, help="Ratio test threshold for feature matching")
    parser.add_argument("--essential_matrix_threshold", type=float, default=1.0, help="RANSAC threshold for essential matrix estimation"),
    parser.add_argument("--output_format", type=str, default="pcd", choices=["pcd", "txt"], help="Output format: pcd or txt.")


    args = parser.parse_args()

    slam = SLAMSystem(args.video_path, args.calibration_file, args.real_time_vis, args.end_vis, args.model, args.save_point_clouds, 
                      args.frame_interval, args.output_pcd, args.voxel_size, args.min_depth, args.max_depth,
                      args.min_brightness,args.orb_features,args.feature_match_ratio,args.essential_matrix_threshold, args.output_format)
    slam.run()
if __name__ == "__main__":
    main()


point_cloud_generator.py code 

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, model_type="DPT_Large", calibration_path='zoe/CalibrationMatrix_college_cpt.npz', transformation=None, voxel_size=0.02, min_brightness = 10, max_depth= 10, min_depth = 2):
        # MiDaS setup
        self.model_type = model_type  # e.g., "DPT_Large", "MiDaS_small"
        self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
        self.midas = torch.hub.load("intel-isl/MiDaS", self.model_type)
        self.midas.to(self.device)
        self.midas.eval()
        midas_transforms = torch.hub.load("intel-isl/MiDaS", "transforms")
        # Choose the appropriate transform
        if self.model_type == "MiDaS_small":
            self.transform = midas_transforms.small_transform
        else:
            self.transform = midas_transforms.default_transform

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

        # Rotation matrix (keeping this as it was in the original code)
        self.R = np.eye(3)
        if transformation is None:
            self.transformation = np.eye(4)
        else:
            self.transformation = transformation

        self.voxel_size = voxel_size

    def set_default_camera_parameters(self):
        self.FX = 525.0  # Default focal length
        self.FY = 525.0
        self.CX = 320.0  # Default principal point
        self.CY = 220.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 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)
        input_batch = self.transform(color_image).to(self.device)

        with torch.no_grad():
            prediction = self.midas(input_batch)
            prediction = torch.nn.functional.interpolate(
                prediction.unsqueeze(1),
                size=(h, w),
                mode="bicubic",
                align_corners=False,
            ).squeeze()
            depth_map = prediction.cpu().numpy()

        # Normalize depth map
        depth_min = depth_map.min()
        depth_max = depth_map.max()
        print(f"Depth map min: {depth_min}, max: {depth_max}")
        depth_map = (depth_map - depth_min) / (depth_max - depth_min)

        # Invert depth map (optional, depending on MiDaS version)
        depth_map = 1 - depth_map

        # Scale depth map to approximate real-world units
        # You can adjust the scale_factor based on known distances in your scene
        scale_factor = 5.0  # Adjust this value based on your scene and desired depth range
        scaled_depth_map = depth_map * scale_factor
        print(f"Scaled depth map min: {scaled_depth_map.min()}, max: {scaled_depth_map.max()}")

        # 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 = self.min_depth           # Adjust based on your scene
        max_depth = self.max_depth           # Adjust based on your scene
        min_brightness = self.min_brightness # Adjust to filter out very dark points

        brightness = np.mean(color_image.reshape(-1, 3), axis=1)

        mask = (scaled_depth_map.flatten() > min_depth) & \
               (scaled_depth_map.flatten() < max_depth) & \
               (brightness > min_brightness)

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

        # Apply transformations
        

        # Downsample
        if self.voxel_size > 0:
            pcd = o3d.geometry.PointCloud()
            pcd.points = o3d.utility.Vector3dVector(valid_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, scaled_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")