real time visualization is not working right now. at start i get this error and then the opened window is just black screen and frozen. i want to see real time visualization. you are free to change all of the aspects of current real time visualization but just give me the real time vis. we will examine the map from stationary camera view

[Open3D WARNING] The number of points is 0 when creating axis-aligned bounding box.

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


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.vis = None
        self.is_running = True
        self.camera_positions = []
        self.vis_updated = threading.Event()
        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()
        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:
            self.vis.update_geometry(self.global_map)
            self.vis.poll_events()
            self.vis.update_renderer()
            self.vis_updated.set()

    def visualization_loop(self):
        while self.is_running:
            self.vis_updated.wait()
            self.update_visualization()
            self.vis_updated.clear()
    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
                        # 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()
                    self.vis.destroy_window()
                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()