in this following code, visualization method doesnt work. here how it must work, it will…
- 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()
- 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
Visualizerclass 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:
-
Removed Threading for Visualization:
- Removed the
threadingandEventobjects related to visualization updates. - Removed the
visualization_loopandupdate_visualizationmethods. - Open3D visualizations should be updated in the main thread to avoid threading issues.
- Removed the
-
Updated Visualization in Main Loop:
- Initialized the
Visualizerand added the global map geometry only once ifreal_time_visis enabled. - Inside the main loop, after updating the
global_map, called:
This updates the visualization in real-time within the main thread.self.vis.update_geometry(self.global_map) self.vis.poll_events() self.vis.update_renderer()
- Initialized the
-
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.
-
Frame Count and Depth Tracking:
- Moved
frame_countanddepth_prevto instance variables to maintain their state across method calls.
- Moved
-
Adjusted Scale Calculation:
- Changed from percentile to median for depth scaling to improve robustness.
-
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_generatorandpose_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_visReplace
your_script_name.py,path_to_your_video.mp4, andpath_to_calibration.npzwith the actual paths.Additional Tips:
-
Performance Optimization:
- If you experience performance issues with real-time visualization, consider adjusting the
voxel_sizeinvoxel_down_sampleor processing every Nth frame by modifyingframe_interval.
- If you experience performance issues with real-time visualization, consider adjusting the
-
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
matplotlibis installed and working correctly.
- After the SLAM processing is complete, the camera trajectory is plotted and saved. Ensure that
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!
-