Camera matrix:
[[726.65937849   0.         419.37380341]
 [  0.         726.16188065 249.3021722 ]
 [  0.           0.           1.        ]]
Distortion coefficients: [ 1.69558413e-01 -6.27337858e-01 -1.37015088e-03  7.96379361e-04
  8.22763173e-01]
Using cache found in /home/samet/.cache/torch/hub/yvanyin_metric3d_main
xFormers not available
xFormers not available
xFormers not available
xFormers not available
Camera parameters loaded: FX: 726.6593784859651, FY: 726.1618806471192, CX: 419.37380340520747, CY: 249.30217219800434
Distortion coefficients loaded: [ 1.69558413e-01 -6.27337858e-01 -1.37015088e-03  7.96379361e-04
  8.22763173e-01]
2024-10-18 14:43:15 - Processing frame 0
2024-10-18 14:43:17 - Processing frame 15
Error updating visualization: RGBA values should be within 0-1 range
2024-10-18 14:43:18 - Processing frame 30
2024-10-18 14:43:19 - Processing frame 45
2024-10-18 14:43:20 - Processing frame 60
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:21 - Processing frame 75
/home/samet/Projects/SLAM/RGB-SLAM-MIDAS/slam.py:149: UserWarning: Starting a Matplotlib GUI outside of the main thread will likely fail.
  plt.pause(0.01)
2024-10-18 14:43:22 - Processing frame 90
2024-10-18 14:43:23 - Processing frame 105
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:24 - Processing frame 120
2024-10-18 14:43:24 - Processing frame 135
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:25 - Processing frame 150
2024-10-18 14:43:26 - Processing frame 165
2024-10-18 14:43:27 - Processing frame 180
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:28 - Processing frame 195
2024-10-18 14:43:29 - Processing frame 210
2024-10-18 14:43:30 - Processing frame 225
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:31 - Processing frame 240
2024-10-18 14:43:32 - Processing frame 255
2024-10-18 14:43:33 - Processing frame 270
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:34 - Processing frame 285
2024-10-18 14:43:35 - Processing frame 300
2024-10-18 14:43:36 - Processing frame 315
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:37 - Processing frame 330
2024-10-18 14:43:38 - Processing frame 345
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:39 - Processing frame 360
2024-10-18 14:43:39 - Processing frame 375
2024-10-18 14:43:40 - Processing frame 390
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:41 - Processing frame 405
2024-10-18 14:43:42 - Processing frame 420
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:43 - Processing frame 435
2024-10-18 14:43:44 - Processing frame 450
2024-10-18 14:43:45 - Processing frame 465
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:46 - Processing frame 480
2024-10-18 14:43:47 - Processing frame 495
2024-10-18 14:43:48 - Processing frame 510
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:49 - Processing frame 525
2024-10-18 14:43:50 - Processing frame 540
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:51 - Processing frame 555
2024-10-18 14:43:52 - Processing frame 570
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:52 - Processing frame 585
2024-10-18 14:43:53 - Processing frame 600
2024-10-18 14:43:54 - Processing frame 615
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:43:55 - Processing frame 630
2024-10-18 14:43:56 - Processing frame 645
Error updating visualization: main thread is not in main loop
2024-10-18 14:43:57 - Processing frame 660
2024-10-18 14:43:58 - Processing frame 675
2024-10-18 14:43:59 - Processing frame 690
Error updating visualization: main thread is not in main loop
2024-10-18 14:44:00 - Processing frame 705
2024-10-18 14:44:01 - Processing frame 720
2024-10-18 14:44:02 - Processing frame 735
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:44:03 - Processing frame 750
2024-10-18 14:44:04 - Processing frame 765
2024-10-18 14:44:05 - Processing frame 780
One of the descriptors is None.
2024-10-18 14:44:05 - Frame 780: Insufficient matches found (0). Skipping pose estimation.
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:44:06 - Processing frame 795
One of the descriptors is None.
2024-10-18 14:44:06 - Frame 795: Insufficient matches found (0). Skipping pose estimation.
2024-10-18 14:44:07 - Processing frame 810
2024-10-18 14:44:07 - Processing frame 825
Error updating visualization: Calling Tcl from different apartment
2024-10-18 14:44:08 - Processing frame 840
2024-10-18 14:44:09 - Processing frame 855
Error updating visualization: main thread is not in main loop
2024-10-18 14:44:10 - Processing frame 870
[1]    41886 segmentation fault (core dumped)  python slam.py --model metric3d --video_path "videos/kutup_depo.mp4"    15   


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
from queue import Queue

class MatplotlibVisualizer:
    def __init__(self):
        # Use TkAgg backend which is more widely available
        import matplotlib
        matplotlib.use('TkAgg')
        import matplotlib.pyplot as plt
        from matplotlib.widgets import Button
        import numpy as np
        
        plt.ion()  # Enable interactive mode
        self.fig = plt.figure(figsize=(12, 10))
        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')
        
        # Initialize view parameters
        self.view_distance = 10
        # Set initial view angle to 90 degrees to view from X-Z plane
        self.view_angle = 90
        self.view_elevation = 0
        
        # Add keyboard event handling
        self.fig.canvas.mpl_connect('key_press_event', self.on_key)
        
        # Add help text
        self.fig.text(0.02, 0.98, 
                     'Controls:\n' +
                     '+ : Zoom in\n' +
                     '- : Zoom out\n' +
                     'Left/Right : Rotate view\n' +
                     'Up/Down : Adjust elevation\n' +
                     'r : Reset view', 
                     fontsize=10, 
                     verticalalignment='top')
        
        # Initial view setup
        self.ax.view_init(elev=self.view_elevation, azim=self.view_angle)
        
        self.fig.show()
        plt.pause(0.1)

    def on_key(self, event):
        if event.key == '+':
            self.zoom(0.8)
        elif event.key == '-':
            self.zoom(1.2)
        elif event.key == 'left':
            self.view_angle = (self.view_angle - 5) % 360
            self.update_view()
        elif event.key == 'right':
            self.view_angle = (self.view_angle + 5) % 360
            self.update_view()
        elif event.key == 'up':
            self.view_elevation = min(self.view_elevation + 5, 89)
            self.update_view()
        elif event.key == 'down':
            self.view_elevation = max(self.view_elevation - 5, -89)
            self.update_view()
        elif event.key == 'r':
            self.reset_view()

    def zoom(self, factor):
        self.view_distance *= factor
        self.update_view()

    def reset_view(self):
        self.view_distance = 10
        # Reset to X-Z plane view
        self.view_angle = 90
        self.view_elevation = 0
        self.update_view()

    def update_view(self):
        if len(self.camera_positions) > 0:
            # Calculate center point of the scene
            center = np.mean(self.camera_positions, axis=0)
            
            # Update view parameters
            self.ax.view_init(elev=self.view_elevation, azim=self.view_angle)
            
            # Set axis limits based on view distance
            radius = self.view_distance
            self.ax.set_xlim(center[0] - radius, center[0] + radius)
            self.ax.set_ylim(center[1] - radius, center[1] + radius)
            self.ax.set_zlim(center[2] - radius, center[2] + radius)

    def update(self, points, colors, camera_position):
        if not self.is_running:
            return
            
        try:
            # Clear the current plot while preserving the view
            self.ax.cla()
            
            # Plot point cloud
            if len(points) > 0:
                # Downsample points for visualization
                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, label='Camera Path')
                self.ax.plot([positions[-1, 0]], [positions[-1, 1]], [positions[-1, 2]], 
                           'r*', markersize=10, label='Current Position')
            
            # Update view and labels
            self.update_view()
            self.ax.set_xlabel('X')
            self.ax.set_ylabel('Y')
            self.ax.set_zlabel('Z')
            self.ax.set_title('SLAM Visualization')
            self.ax.legend()
            
            # Update the plot
            self.fig.canvas.draw_idle()
            plt.pause(0.01)
            
        except Exception as e:
            print(f"Error updating visualization: {str(e)}")
    
    def close(self):
        plt.close(self.fig)
        self.is_running = False

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']
    
    for subfolder in subfolders:
        os.makedirs(os.path.join(base_folder, subfolder), exist_ok=True)
    
    return base_folder

# 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):
    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, calibration_file, real_time_vis=False, 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, min_keyframe_distance=10, min_translation=2, min_rotation=20, icp_threshold=0.02,
                 orb_features=5000, feature_match_ratio=0.75, essential_matrix_threshold=1.0):
        
        self.base_folder = create_timestamped_log_folder()
        self.log_folder = os.path.join(self.base_folder, 'log')
        self.save_pcd_folder = os.path.join(self.base_folder, 'point_cloud_file')
        self.trajectory_folder = os.path.join(self.base_folder, 'camera_trajectory')
        
        
        
        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.min_keyframe_distance = min_keyframe_distance
        self.min_translation = min_translation
        self.min_rotation = min_rotation
        self.icp_threshold = icp_threshold
        self.orb_features = orb_features
        self.feature_match_ratio = feature_match_ratio
        self.essential_matrix_threshold = essential_matrix_threshold

        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'
        
        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)
        
        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.camera_positions = []

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

        self.prev_pcd = None

        self.last_keyframe_index = -1
        self.keyframes = []
        self.pose_graph = o3d.pipelines.registration.PoseGraph()
        self.odometry = np.identity(4)
        self.pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(self.odometry))
        self.pcd_list = []

        # 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"Minimum Keyframe Distance: {self.min_keyframe_distance}\n")
            log_file.write(f"Minimum Translation: {self.min_translation}\n")
            log_file.write(f"Minimum Rotation: {self.min_rotation}\n")
            log_file.write(f"ICP Threshold: {self.icp_threshold}\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 is_keyframe(self, frame_index, current_pose):
        if not self.keyframes:
            return True  # Always select the first frame as a keyframe

        frames_since_last_keyframe = frame_index - self.last_keyframe_index
        if frames_since_last_keyframe < self.min_keyframe_distance:
            return False

        last_keyframe = self.keyframes[-1]
        last_pose = last_keyframe['pose']

        # Calculate translation and rotation between current pose and last keyframe
        translation = np.linalg.norm(current_pose[:3, 3] - last_pose[:3, 3])
        rotation = np.degrees(np.arccos((np.trace(current_pose[:3, :3] @ last_pose[:3, :3].T) - 1) / 2))

        return translation > self.min_translation or rotation > self.min_rotation

    def add_keyframe(self, frame, depth_map, pose, frame_index):
        self.keyframes.append({
            'frame': frame,
            'depth_map': depth_map,
            'pose': pose,
            'index': frame_index
        })
        self.last_keyframe_index = frame_index
        print(f"Added keyframe at frame {frame_index}")

    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([[700, 0, 320], [0, 700, 240], [0, 0, 1]]), np.zeros(5)

    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.1
        min_scale = 0.9
        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 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 save_point_cloud(self, pcd, index):
        pcd_o3d = o3d.geometry.PointCloud()
        pcd_o3d.points = o3d.utility.Vector3dVector(pcd[:, :3])  # XYZ
        pcd_o3d.colors = o3d.utility.Vector3dVector(pcd[:, 3:] / 255.0)  # RGB

        # Save the point cloud
        o3d.io.write_point_cloud(f"pcds/frame_{index:04d}.pcd", pcd_o3d)
        
    
    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()
                else:
                    #o3d.visualization.draw_geometries([self.global_map])
                    pass

            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, required=True, help="Path to camera calibration NPZ file")
    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")
    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("--min_keyframe_distance", type=int, default=10, help="Minimum number of frames between keyframes")
    parser.add_argument("--min_translation", type=float, default=2.0, help="Minimum translation to consider a new keyframe")
    parser.add_argument("--min_rotation", type=float, default=20.0, help="Minimum rotation to consider a new keyframe")
    parser.add_argument("--icp_threshold", type=float, default=0.02, help="ICP threshold for point cloud registration")
    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")

    args = parser.parse_args()

    slam = SLAMSystem(args.video_path, args.calibration_file, args.real_time_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.min_keyframe_distance, args.min_translation, args.min_rotation, args.icp_threshold,
                      args.orb_features,args.feature_match_ratio,args.essential_matrix_threshold)
    slam.run()
if __name__ == "__main__":
    main()