I am working on a SLAM method using RGB input along with depth estimation, pose…
- User
I am working on a SLAM method using RGB input along with depth estimation, pose estimation, and point cloud generation. The input video is footage of a straight corridor, but the output point cloud shows multiple side-by-side copies of the corridor (around 15-20) instead of a single, straight representation. My SLAM system consists of three scripts: slam.py, pose_estimator.py, and point_cloud_generator.py. Could you help identify why the point clouds are duplicated and how to correct the pose estimation or depth processing to ensure the output is a single, accurate point cloud of the corridor? I will provide the three scripts for review. make changes and think as much as you want and give me the code changes.
import os import sys import cv2 import numpy as np import open3d as o3d import argparse import threading import time
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, focal_length, principal_point, real_time_vis=False, model='zoe', save_point_clouds=False): if model == 'midas': self.generator = MidasPointCloudGenerator() self.pcd_folder = 'pcd_midas' elif model == 'metric3d': self.generator = Metric3DPointCloudGenerator() self.pcd_folder = 'pcd_metric' else: # default to zoe self.generator = ZoePointCloudGenerator() self.pcd_folder = 'pcd_zoe'
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 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 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.5 min_scale = 0.5 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() def visualization_loop(self): while self.is_running: self.update_visualization() time.sleep(0.1) # Update every 100ms def run(self): frame_count = 0 depth_prev = None if self.save_point_clouds: os.makedirs(self.pcd_folder, exist_ok=True) while True: ret, frame = self.cap.read() if not ret: print("End of video stream.") break 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}") else: print(f"Frame {frame_count}: Pose estimation failed.") current_pcd.transform(self.pose) if frame_count > 0: transformation = register_point_clouds(current_pcd, self.global_map) current_pcd.transform(transformation) self.pose = self.pose @ transformation self.global_map += current_pcd self.global_map = self.global_map.voxel_down_sample(voxel_size=0.05) self.prev_frame = frame.copy() depth_prev = depth_map.copy() frame_count += 1 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])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("--focal_length", type=float, default=700, help="Focal length in pixels") parser.add_argument("--principal_point_x", type=float, default=320, help="X-coordinate of principal point") parser.add_argument("--principal_point_y", type=float, default=240, help="Y-coordinate of principal point") 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") args = parser.parse_args()
principal_point = (args.principal_point_x, args.principal_point_y) slam = SLAMSystem(args.video_path, args.focal_length, principal_point, args.real_time_vis, args.model, args.save_point_clouds) slam.run()if name == "main": main()
import cv2 import torch import numpy as np from PIL import Image from torchvision import transforms import open3d as o3d
class PointCloudGenerator: def init(self, calibration_path='zoe/CalibrationMatrix_college_cpt.npz', transformation=None, voxel_size=0.02): # Metric3D setup self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") self.metric3d_model = torch.hub.load('yvanyin/metric3d', 'metric3d_vit_large', pretrain=True) self.metric3d_model.to(self.device) self.metric3d_model.eval()
# Load camera parameters from NPZ file if calibration_path: self.load_camera_parameters(calibration_path) else: self.set_default_camera_parameters() # Compute Q matrix self.compute_Q_matrix() # Rotation matrix (keeping this as it was in the original code) self.R = np.array([ [0, 0, -1], [0, 1, 0], [1, 0, 0] ]) # Transformation matrix if transformation is None: self.transformation = np.array([ [1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1] ]) else: self.transformation = transformation self.voxel_size = voxel_size def set_default_camera_parameters(self): self.FX = 350.0 # Default focal length self.FY = 350.0 self.CX = 160.0 # Default principal point self.CY = 120.0 self.dist_coeffs = np.zeros(5) print("Using default camera parameters") def load_camera_parameters(self, calibration_path): try: calibration_data = np.load(calibration_path) if 'Camera_matrix' in calibration_data: camera_matrix = calibration_data['Camera_matrix'] self.FX = camera_matrix[0, 0] self.FY = camera_matrix[1, 1] self.CX = camera_matrix[0, 2] self.CY = camera_matrix[1, 2] print(f"Camera parameters loaded: FX: {self.FX}, FY: {self.FY}, CX: {self.CX}, CY: {self.CY}") else: raise KeyError("Camera matrix not found in calibration file") if 'distCoeff' in calibration_data: self.dist_coeffs = calibration_data['distCoeff'].ravel() print(f"Distortion coefficients loaded: {self.dist_coeffs}") else: print("Distortion coefficients not found, assuming no distortion") self.dist_coeffs = np.zeros(5) except Exception as e: print(f"Error loading camera parameters: {e}") self.set_default_camera_parameters() def compute_Q_matrix(self): self.Q = np.array([[1.0, 0.0, 0.0, -self.CX], [0.0, 1.0, 0.0, -self.CY], [0.0, 0.0, 0.0, self.FX], [0.0, 0.0, 1.0/self.FY, 0.0]], dtype=np.float32) def process_frame_with_depth(self, frame): # Undistort the frame h, w = frame.shape[:2] new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix( np.array([[self.FX, 0, self.CX], [0, self.FY, self.CY], [0, 0, 1]]), self.dist_coeffs, (w, h), 1, (w, h) ) undistorted_frame = cv2.undistort(frame, np.array([[self.FX, 0, self.CX], [0, self.FY, self.CY], [0, 0, 1]]), self.dist_coeffs, None, new_camera_matrix) # RGB to Depth color_image = cv2.cvtColor(undistorted_frame, cv2.COLOR_BGR2RGB) image_tensor = transforms.ToTensor()(color_image).unsqueeze(0).to(self.device) with torch.no_grad(): pred_depth, confidence, _ = self.metric3d_model.inference({'input': image_tensor}) depth_map = pred_depth.squeeze().cpu().numpy() # Ensure depth_map matches the original frame size depth_map = cv2.resize(depth_map, (w, h), interpolation=cv2.INTER_LINEAR) # Scale depth map to real-world units (e.g., meters) depth_scale = 1.0 # Adjust this value based on your depth model's output scale scaled_depth_map = depth_map * depth_scale # Create 3D points rows, cols = scaled_depth_map.shape c, r = np.meshgrid(np.arange(cols), np.arange(rows)) points = np.zeros((rows * cols, 3), dtype=np.float32) points[:, 0] = (c.flatten() - self.CX) * scaled_depth_map.flatten() / self.FX points[:, 1] = (r.flatten() - self.CY) * scaled_depth_map.flatten() / self.FY points[:, 2] = scaled_depth_map.flatten() # Improved filtering min_depth = 0.1 # Adjust based on your scene max_depth = 10.0 # Adjust based on your scene min_brightness = 20 # Adjust to filter out very dark points mask = (scaled_depth_map.flatten() > min_depth) & \ (scaled_depth_map.flatten() < max_depth) & \ (np.mean(color_image.reshape(-1, 3), axis=1) > min_brightness) valid_points = points[mask] valid_colors = color_image.reshape(-1, 3)[mask] # Apply transformations transformed_points = np.dot(valid_points, self.R.T) homogeneous_points = np.hstack((transformed_points, np.ones((transformed_points.shape[0], 1)))) transformed_points = np.dot(homogeneous_points, self.transformation.T)[:, :3] # Downsample if self.voxel_size > 0: pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(transformed_points) pcd.colors = o3d.utility.Vector3dVector(valid_colors.astype(np.float64) / 255.0) downsampled_pcd = pcd.voxel_down_sample(voxel_size=self.voxel_size) transformed_points = np.asarray(downsampled_pcd.points) valid_colors = np.asarray(downsampled_pcd.colors) * 255 # Combine points and colors for final point cloud point_cloud = np.hstack((transformed_points, valid_colors.astype(np.uint8))) return point_cloud, depth_map def save_point_cloud_txt(self, point_cloud, filename): np.savetxt(filename, point_cloud, fmt='%.6f %.6f %.6f %d %d %d')def save_point_cloud_pcd(point_cloud, filename): with open(filename, 'w') as f: f.write("# .PCD v0.7 - Point Cloud Data file format\n") f.write("VERSION 0.7\n") f.write("FIELDS x y z rgb\n") f.write("SIZE 4 4 4 4\n") f.write("TYPE F F F U\n") f.write("COUNT 1 1 1 1\n") f.write(f"WIDTH {len(point_cloud)}\n") f.write("HEIGHT 1\n") f.write("VIEWPOINT 0 0 0 1 0 0 0 0\n") f.write(f"POINTS {len(point_cloud)}\n") f.write("DATA ascii\n") for point in point_cloud: x, y, z, r, g, b = point rgb = int(r) << 16 | int(g) << 8 | int(b) f.write(f"{x} {y} {z} {rgb}\n")
import cv2 import numpy as np
class PoseEstimator: def init(self, focal_length, principal_point): self.orb = cv2.ORB_create(5000) self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) self.K = np.array([ [focal_length, 0, principal_point[0]], [0, focal_length, principal_point[1]], [0, 0, 1] ])
def detect_and_compute(self, img): keypoints, descriptors = self.orb.detectAndCompute(img, None) return keypoints, descriptors def match_features(self, desc1, desc2): if desc1 is None or desc2 is None: print("One of the descriptors is None.") return [] if desc1.dtype != desc2.dtype: print(f"Descriptor type mismatch: desc1 dtype = {desc1.dtype}, desc2 dtype = {desc2.dtype}. Converting desc2 to match desc1.") desc2 = desc2.astype(desc1.dtype) if desc1.shape[1] != desc2.shape[1]: print(f"Descriptor size mismatch: desc1 cols = {desc1.shape[1]}, desc2 cols = {desc2.shape[1]}. Adjusting descriptor sizes.") min_cols = min(desc1.shape[1], desc2.shape[1]) desc1 = desc1[:, :min_cols] desc2 = desc2[:, :min_cols] if desc1.dtype not in [np.uint8, np.float32] or desc2.dtype not in [np.uint8, np.float32]: print(f"Invalid descriptor types: desc1 dtype = {desc1.dtype}, desc2 dtype = {desc2.dtype}. Skipping matching.") return [] try: matches = self.matcher.match(desc1, desc2) matches = sorted(matches, key=lambda x: x.distance) return matches except Exception as e: print(f"Exception during feature matching: {e}") return [] def estimate_pose(self, kp1, kp2, matches, scale=1.0): if len(matches) < 8: print("Not enough matches to compute pose.") return None, None try: pts1 = np.float32([kp1[m.queryIdx].pt for m in matches]) pts2 = np.float32([kp2[m.trainIdx].pt for m in matches]) E, mask = cv2.findEssentialMat(pts1, pts2, self.K, method=cv2.RANSAC, prob=0.999, threshold=1.0) if E is None: print("Essential matrix computation failed.") return None, None _, R, t, mask_pose = cv2.recoverPose(E, pts1, pts2, self.K) # Apply scale to the translation vector t = t * scale print(f"Applied scale factor: {scale}") return R, t except Exception as e: print(f"Exception during pose estimation: {e}") return None, None - Assistant
Analysis of the Problem:
The duplication of the corridor in your point cloud output is likely due to a combination of issues in pose estimation and point cloud registration. Specifically:
-
Incorrect Scale Estimation: The scale factor calculated between frames may not be accurate, leading to incorrect translations being applied to the pose.
-
Overlapping Transformations: Applying both the pose estimation from feature matching and then an ICP (Iterative Closest Point) alignment can introduce additional transformations that accumulate errors over time.
-
Misalignment in Pose Updates: The order and manner in which transformations (rotations and translations) are applied can significantly affect the final outcome.
Proposed Solutions:
-
Remove the ICP Registration Step:
- Since you already estimate the pose between frames using feature matching and the Essential matrix, applying ICP might be redundant and could introduce errors.
- Removing the ICP step can prevent misalignments due to inconsistent transformations.
-
Improve Scale Estimation:
- Review the
calculate_scalefunction to ensure it provides an accurate scale factor. - Consider using a constant scale factor or integrating additional information (e.g., known object sizes) if scale cannot be reliably estimated from depth maps.
- Review the
-
Verify Transformation Updates:
- Ensure that the cumulative pose is updated correctly.
- Double-check the order of matrix multiplication when updating poses, as the order matters in transformations.
-
Adjust Pose Estimation Parameters:
- Increase the number of features detected to improve pose estimation reliability.
- Ensure that feature matching and keypoint detection are robust to variations in the video frames.
Code Changes:
Here are the specific changes you can make to your code to address these issues.
1. Remove the ICP Registration Step in
slam.py:Comment out or remove the block of code that performs ICP registration. This is found in the
runmethod of theSLAMSystemclass.Original Code Block:
if frame_count > 0: transformation = register_point_clouds(current_pcd, self.global_map) current_pcd.transform(transformation) self.pose = self.pose @ transformationModified Code (Commented Out):
# if frame_count > 0: # transformation = register_point_clouds(current_pcd, self.global_map) # current_pcd.transform(transformation) # self.pose = self.pose @ transformationBy commenting out this block, you prevent the additional transformation from potentially misaligning your point clouds.
2. Verify and Adjust the Scale Estimation in
slam.py:Modify the
calculate_scalemethod to ensure that it provides a reasonable scale factor.Original
calculate_scaleMethod:def calculate_scale(self, depth_prev, depth_curr): ...Adjusted
calculate_scaleMethod (Optional - Use Constant Scale):If the scale estimation is unreliable, you might temporarily use a constant scale factor of 1.0.
def calculate_scale(self, depth_prev, depth_curr): return 1.0Alternatively, adjust the existing method to use a more robust statistic or handle depth maps more appropriately.
3. Ensure Correct Pose Transformation Multiplication Order in
slam.py:Confirm that the pose updates are correctly applied. The order of matrix multiplication should be such that the most recent transformation is applied first.
Original Pose Update:
self.pose = self.pose @ TModified Pose Update (Swap Multiplication Order):
self.pose = T @ self.poseChanging the multiplication order ensures that the new transformation
Tis applied to the current poseself.posecorrectly.
4. Verify the Application of Pose to the Point Cloud in
slam.py:Ensure that when you apply the pose to the current point cloud, you use the correct transformation.
Original Point Cloud Transformation:
current_pcd.transform(self.pose)No Change Needed Here Unless There's an Issue with Coordinate Frames
However, if after making the changes above, you still encounter issues, you may need to check whether the coordinate frames between your pose estimation and point cloud are consistent.
5. Increase the Number of Features in
pose_estimator.py:Increase the number of features detected by adjusting the parameters of the ORB detector.
Original ORB Initialization:
self.orb = cv2.ORB_create(5000)Modified ORB Initialization:
self.orb = cv2.ORB_create(nfeatures=10000)Increasing
nfeaturescan help improve the robustness of feature matching between frames.
6. Improve Feature Matching Robustness in
pose_estimator.py:Modify the
match_featuresmethod to use K-Nearest Neighbors and ratio test for better matching.Replace the
match_featuresMethod with the Following:def match_features(self, desc1, desc2): if desc1 is None or desc2 is None: print("One of the descriptors is None.") return [] # Use KNN matcher with k=2 matcher = cv2.BFMatcher(cv2.NORM_HAMMING) knn_matches = matcher.knnMatch(desc1, desc2, k=2) # Apply ratio test good_matches = [] for m, n in knn_matches: if m.distance < 0.75 * n.distance: good_matches.append(m) return good_matchesUsing KNN matching and the ratio test can reduce false matches and improve pose estimation.
7. Adjust Parameters in Pose Estimation in
pose_estimator.py:Ensure that the Essential matrix computation and pose recovery are robust by adjusting parameters.
Modify the
estimate_poseMethod:Adjust the RANSAC parameters to be more permissive or restrictive based on your data.
E, mask = cv2.findEssentialMat(pts1, pts2, self.K, method=cv2.RANSAC, prob=0.999, threshold=1.0)You might try changing
thresholdto0.5or adjustingprobto see if it improves pose estimation.
8. Test and Visualize the Camera Trajectory:
Add code to record and visualize the estimated camera trajectory to help debug the pose estimation.
In
slam.py, Add the Following in therunMethod:At the top of the class, initialize a list to store camera positions:
self.camera_positions = []After updating the pose, extract and store the camera position:
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 = T @ self.pose 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.")After processing all frames, you can plot the trajectory using matplotlib:
import matplotlib.pyplot as plt # Plot camera trajectory camera_positions = np.array(self.camera_positions) plt.plot(camera_positions[:, 0], camera_positions[:, 2], 'r*-') plt.xlabel('X') plt.ylabel('Z') plt.title('Camera Trajectory') plt.show()Visualizing the trajectory can help you see if the camera movement makes sense.
Summary of Changes:
-
Removed ICP Registration Step: Prevents misalignment due to redundant transformations.
-
Adjusted Scale Estimation: Ensures translations are applied correctly.
-
Verified Pose Multiplication Order: Corrects the application of new poses.
-
Improved Feature Detection and Matching: Enhances pose estimation reliability.
-
Visualized Camera Trajectory: Aids in debugging and understanding pose estimation results.
Final Notes:
After implementing these changes, run your SLAM system again on the corridor video. The point cloud output should now represent a single, continuous corridor without duplication. Additionally, pay attention to any printed debug statements or visualizations to further validate the correctness of the pose estimations.
Updated
slam.py:Here is the updated
slam.pywith the changes applied:import os import sys import cv2 import numpy as np import open3d as o3d import argparse import threading import time # 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 class SLAMSystem: def __init__(self, video_path, focal_length, principal_point, real_time_vis=False, model='zoe', save_point_clouds=False): if model == 'midas': self.generator = MidasPointCloudGenerator() self.pcd_folder = 'pcd_midas' elif model == 'metric3d': self.generator = Metric3DPointCloudGenerator() self.pcd_folder = 'pcd_metric' else: # default to zoe self.generator = ZoePointCloudGenerator() self.pcd_folder = 'pcd_zoe' 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 = [] # For trajectory visualization 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 calculate_scale(self, depth_prev, depth_curr): # Optionally, you can use a constant scale factor if scale estimation is unreliable return 1.0 def update_visualization(self): if self.real_time_vis: self.vis.update_geometry(self.global_map) self.vis.poll_events() self.vis.update_renderer() def visualization_loop(self): while self.is_running: self.update_visualization() time.sleep(0.1) # Update every 100ms def run(self): frame_count = 0 depth_prev = None if self.save_point_clouds: os.makedirs(self.pcd_folder, exist_ok=True) while True: ret, frame = self.cap.read() if not ret: print("End of video stream.") break 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 = T @ self.pose 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) # Removed ICP Registration step # if frame_count > 0: # transformation = register_point_clouds(current_pcd, self.global_map) # current_pcd.transform(transformation) # self.pose = self.pose @ transformation self.global_map += current_pcd self.global_map = self.global_map.voxel_down_sample(voxel_size=0.05) self.prev_frame = frame.copy() depth_prev = depth_map.copy() frame_count += 1 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]) # Plot camera trajectory if len(self.camera_positions) > 0: import matplotlib.pyplot as plt camera_positions = np.array(self.camera_positions) plt.plot(camera_positions[:, 0], camera_positions[:, 2], 'r*-') plt.xlabel('X') plt.ylabel('Z') plt.title('Camera Trajectory') plt.show() 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("--focal_length", type=float, default=700, help="Focal length in pixels") parser.add_argument("--principal_point_x", type=float, default=320, help="X-coordinate of principal point") parser.add_argument("--principal_point_y", type=float, default=240, help="Y-coordinate of principal point") 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") args = parser.parse_args() principal_point = (args.principal_point_x, args.principal_point_y) slam = SLAMSystem(args.video_path, args.focal_length, principal_point, args.real_time_vis, args.model, args.save_point_clouds) slam.run() if __name__ == "__main__": main()Note: Remember to apply similar adjustments to
pose_estimator.pyandpoint_cloud_generator.pyas outlined in the steps above.
By carefully addressing each potential issue and thoroughly testing your SLAM system after making these changes, you should be able to resolve the duplication problem and achieve a more accurate reconstruction of the corridor.
-