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

in this following code, visualization method doesnt work. here how it must work, it will…

  1. User

    in this following code, visualization method doesnt work. here how it must work, it will generate frame point cloud than make the slam calculation and and them in proper way according to pose estimator. then it will show it on the online visualization. code is now i think keep the all values of slam on the ram and at the end write them on the global_map.pcd file .but i want to see the process and current method is not working. Give me the whole code with changes

    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

    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, calibration_file, real_time_vis=False, model='zoe', save_point_clouds=False, frame_interval=1): self.calibration_file = calibration_file self.camera_matrix, self.dist_coeffs = self.load_camera_parameters() self.frame_interval = frame_interval

        if model == 'midas':
            self.generator = MidasPointCloudGenerator(calibration_path=calibration_file)
            self.pcd_folder = 'pcd_midas'
        elif model == 'metric3d':
            self.generator = Metric3DPointCloudGenerator(calibration_path=calibration_file)
            self.pcd_folder = 'pcd_metric'
        else:  # default to zoe
            self.generator = ZoePointCloudGenerator(calibration_path=calibration_file)
            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)
        
        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()
    
    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 run(self):
        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:
                    print("End of video stream.")
                    break
    
                if frame_count % self.frame_interval == 0:
                    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}")
                                    # 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)
                        
                        self.global_map += current_pcd
                        self.global_map = self.global_map.voxel_down_sample(voxel_size=0.05)
    
                        if self.real_time_vis:
                            self.vis_updated.set()
    
                        self.prev_frame = frame.copy()
                        depth_prev = depth_map.copy()
    
                frame_count += 1
    
        except Exception as e:
            print(f"An error occurred during SLAM processing: {str(e)}")
    
        finally:
            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])
    
            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()
    
                # Save camera trajectory to file
                np.savetxt("camera_trajectory.txt", camera_positions, delimiter=",", header="X,Y,Z", comments="")
                print("Saved camera trajectory to 'camera_trajectory.txt'.")
    

    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)") 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)
    slam.run()
    

    if name == "main": main()

  2. Assistant

    Certainly! The issue you're experiencing is likely due to the use of threading with Open3D's visualization. Open3D's visualization functions are not thread-safe, and updating the visualization from a separate thread can lead to unexpected behavior or no visualization at all.

    To resolve this, we'll modify the code to update the visualization in the main thread. We'll use Open3D's Visualizer class in a way that allows us to process frames and update the point cloud within the main visualization loop.

    Here's the updated code with the necessary changes to enable real-time visualization:

    import os
    import sys
    import cv2
    import numpy as np
    import open3d as o3d
    import argparse
    import matplotlib.pyplot as plt
    
    # 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, calibration_file, real_time_vis=False, model='zoe', save_point_clouds=False, frame_interval=1):
            self.calibration_file = calibration_file
            self.camera_matrix, self.dist_coeffs = self.load_camera_parameters()
            self.frame_interval = frame_interval
    
            if model == 'midas':
                self.generator = MidasPointCloudGenerator(calibration_path=calibration_file)
                self.pcd_folder = 'pcd_midas'
            elif model == 'metric3d':
                self.generator = Metric3DPointCloudGenerator(calibration_path=calibration_file)
                self.pcd_folder = 'pcd_metric'
            else:  # default to zoe
                self.generator = ZoePointCloudGenerator(calibration_path=calibration_file)
                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)
    
            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.camera_positions = []
            self.frame_count = 0  # Move frame_count to instance variable
            self.depth_prev = None
    
        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 median-based scaling
            prev_depth = np.median(depth_prev_nonzero)
            curr_depth = np.median(depth_curr_nonzero)
    
            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 run(self):
            if self.save_point_clouds:
                os.makedirs(self.pcd_folder, exist_ok=True)
    
            if self.real_time_vis:
                self.vis = o3d.visualization.Visualizer()
                self.vis.create_window(window_name='Real-Time SLAM', width=1280, height=720)
                render_option = self.vis.get_render_option()
                render_option.background_color = np.array([0, 0, 0])
                render_option.point_size = 1.0
                self.vis.add_geometry(self.global_map)
    
            try:
                while True:
                    ret, frame = self.cap.read()
                    if not ret:
                        print("End of video stream.")
                        break
    
                    if self.frame_count % self.frame_interval == 0:
                        print(f"Processing frame {self.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_{self.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 self.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 {self.frame_count}: No valid matches found.")
                                else:
                                    scale = self.calculate_scale(self.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}")
                                        # Store the camera position
                                        camera_position = self.pose[:3, 3]
                                        self.camera_positions.append(camera_position)
                                    else:
                                        print(f"Frame {self.frame_count}: Pose estimation failed.")
    
                            current_pcd.transform(self.pose)
    
                            self.global_map += current_pcd
                            self.global_map = self.global_map.voxel_down_sample(voxel_size=0.05)
    
                            if self.real_time_vis:
                                # Update the visualization
                                self.vis.update_geometry(self.global_map)
                                self.vis.poll_events()
                                self.vis.update_renderer()
    
                            self.prev_frame = frame.copy()
                            self.depth_prev = depth_map.copy()
    
                    self.frame_count += 1
    
            except Exception as e:
                print(f"An error occurred during SLAM processing: {str(e)}")
    
            finally:
                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.vis.destroy_window()
                    else:
                        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()
    
                    # Save camera trajectory to file
                    np.savetxt("camera_trajectory.txt", camera_positions, delimiter=",", header="X,Y,Z", comments="")
                    print("Saved camera trajectory to 'camera_trajectory.txt'.")
    
    
    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)")
        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)
        slam.run()
    
    
    if __name__ == "__main__":
        main()
    

    Explanation of Changes:

    1. Removed Threading for Visualization:

      • Removed the threading and Event objects related to visualization updates.
      • Removed the visualization_loop and update_visualization methods.
      • Open3D visualizations should be updated in the main thread to avoid threading issues.
    2. Updated Visualization in Main Loop:

      • Initialized the Visualizer and added the global map geometry only once if real_time_vis is enabled.
      • Inside the main loop, after updating the global_map, called:
        self.vis.update_geometry(self.global_map)
        self.vis.poll_events()
        self.vis.update_renderer()
        
        This updates the visualization in real-time within the main thread.
    3. Visualization Settings:

      • Set visualization window name, size, and render options for better display control.
      • Ensured that the visualization window is properly destroyed at the end.
    4. Frame Count and Depth Tracking:

      • Moved frame_count and depth_prev to instance variables to maintain their state across method calls.
    5. Adjusted Scale Calculation:

      • Changed from percentile to median for depth scaling to improve robustness.
    6. Ensured Thread Safety:

      • By updating the visualization within the main thread, we avoid any thread safety issues associated with Open3D.

    Note: Make sure that all dependencies (like point_cloud_generator and pose_estimator) are properly imported and available in your environment. If any modifications are needed in those modules to accommodate these changes, ensure they are updated accordingly.

    Usage Example:

    To run the SLAM system with real-time visualization, you can use the following command:

    python your_script_name.py --video_path path_to_your_video.mp4 --calibration_file path_to_calibration.npz --real_time_vis
    

    Replace your_script_name.py, path_to_your_video.mp4, and path_to_calibration.npz with the actual paths.

    Additional Tips:

    • Performance Optimization:

      • If you experience performance issues with real-time visualization, consider adjusting the voxel_size in voxel_down_sample or processing every Nth frame by modifying frame_interval.
    • Error Handling:

      • Ensure that all frames are correctly read from the video, and handle any exceptions or errors that may occur during frame processing.
    • Camera Trajectory Visualization:

      • After the SLAM processing is complete, the camera trajectory is plotted and saved. Ensure that matplotlib is installed and working correctly.

    I hope this updated code meets your requirements and provides the real-time visualization functionality you're looking for. Let me know if you have any questions or need further assistance!