i am using these 3 script to create rgb slam. but its quite bad for now. i have an idea.…
- User
i am using these 3 script to create rgb slam. but its quite bad for now. i have an idea. with using airsim, i will create a dataset to train a model than use this model to pose estimation. i will take rgb video data and camera trajectory from airsim to train model. how can i accomplish this? give me detailed instructions. also remind that i will use this slam project on jetson nano. so it must be eligible for embedded process.
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 [] # 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.999 * n.distance: good_matches.append(m) return good_matches 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}") print(f"Estimated rotation R:\n{R}") print(f"Estimated translation t:\n{t.T}") return R, t except Exception as e: print(f"Exception during pose estimation: {e}") return None, Noneimport 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, 0], [0, 0, 0], [0, 0, 0] ]) # Transformation matrix if transformation is None: self.transformation = np.array([ [1, 0, 0, 0], [0, 1, 0, 0], # Flipping Y-axis [0, 0, 1, 0], # Flipping Z-axis (may cause issues) [0, 0, 0, 0] ]) 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 = 5.0 # Adjust based on your scene min_brightness = 10 # 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((valid_points, np.ones((valid_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 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
Creating an efficient RGB-based SLAM (Simultaneous Localization and Mapping) system using a deep learning-based pose estimator on a Jetson Nano involves several key steps:
- Data Collection with AirSim
- Data Preprocessing
- Model Selection and Training
- Model Optimization for Jetson Nano
- Integration into the SLAM System
- Testing and Iteration
Below are detailed instructions for each step:
1. Data Collection with AirSim
AirSim is a powerful simulator for collecting synthetic datasets, especially useful for tasks like pose estimation. Here's how to set up and collect data:
a. Install AirSim
-
Prerequisites:
- Windows/Linux: Depending on your OS, follow the corresponding build instructions.
- Unreal Engine: Install Unreal Engine 4.26 or compatible version.
-
Clone and Build AirSim:
git clone https://github.com/microsoft/AirSim.git cd AirSim ./setup.sh # For Linux/Mac ./build.sh # For Linux/Mac -
Launch AirSim:
- Navigate to the
Unreal/Environmentsdirectory. - Open your preferred environment in Unreal Engine.
- Press the play button to start the simulation.
- Navigate to the
b. Configure AirSim for Data Collection
-
Edit
settings.json: Create or modify thesettings.jsonfile in the AirSim root directory to enable data recording.{ "SettingsVersion": 1.2, "SimMode": "Multirotor", // or "Car" based on your preference "Vehicles": { "Drone1": { "VehicleType": "SimpleFlight", "X": 0, "Y": 0, "Z": 0, "EnableCollisionPassthrough": false } }, "CameraDefaults": { "CaptureSettings": [ { "ImageType": 0, // 0: Scene, 1: Depth, etc. "Width": 640, "Height": 480, "FOV_Degrees": 90 } ] } } -
Enable API Control: Ensure that AirSim allows API control for data collection. This can be done via scripts or using the built-in API.
c. Develop a Data Collection Script
Use Python to control AirSim and collect synchronized RGB images and camera poses.
-
Install AirSim Python API:
pip install airsim -
Sample Data Collection Script:
import airsim import numpy as np import os import cv2 import json def create_dir(path): if not os.path.exists(path): os.makedirs(path) def save_pose(pose, filename): with open(filename, 'w') as f: json.dump({ 'position': {'x': pose.position.x_val, 'y': pose.position.y_val, 'z': pose.position.z_val}, 'orientation': {'w': pose.orientation.w_val, 'x': pose.orientation.x_val, 'y': pose.orientation.y_val, 'z': pose.orientation.z_val} }, f) def main(): # Connect to AirSim client = airsim.MultirotorClient() client.confirmConnection() client.enableApiControl(True, "Drone1") client.armDisarm(True, "Drone1") client.takeoffAsync().join() # Define data storage directories data_dir = "airsim_data" rgb_dir = os.path.join(data_dir, "rgb") pose_dir = os.path.join(data_dir, "poses") create_dir(rgb_dir) create_dir(pose_dir) try: for i in range(1000): # Number of frames # Capture RGB image responses = client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) ]) img1d = np.frombuffer(responses[0].image_data_uint8, dtype=np.uint8) img_rgb = img1d.reshape(responses[0].height, responses[0].width, 3) # Save RGB image img_filename = os.path.join(rgb_dir, f"frame_{i:06d}.png") cv2.imwrite(img_filename, img_rgb) # Get camera pose state = client.getMultirotorState() pose_filename = os.path.join(pose_dir, f"frame_{i:06d}.json") save_pose(state.kinematics_estimated.pose, pose_filename) # Move the drone (example: move forward by 0.5 meters) client.moveByVelocityAsync(0.5, 0, 0, 0.5, vehicle_name="Drone1").join() print(f"Captured frame {i}") finally: client.armDisarm(False, "Drone1") client.reset() # Reset to initial state if __name__ == "__main__": main()
d. Run the Data Collection Script
Ensure AirSim is running, then execute the script:
python data_collection.pyThis script will:
- Capture RGB images.
- Save corresponding camera poses.
- Move the drone forward incrementally to simulate motion.
Note: Adjust movement commands to navigate through your environment effectively.
2. Data Preprocessing
After collecting data, preprocessing is essential to prepare it for training a pose estimation model.
a. Organize the Data
Ensure that each RGB image has a corresponding pose file:
airsim_data/ ├── rgb/ │ ├── frame_000000.png │ ├── frame_000001.png │ └── ... └── poses/ ├── frame_000000.json ├── frame_000001.json └── ...b. Parse Pose Data
Extract position and orientation from JSON files.
import json import numpy as np def load_pose(pose_filepath): with open(pose_filepath, 'r') as f: data = json.load(f) position = data['position'] orientation = data['orientation'] # Convert quaternion to rotation matrix q = [orientation['w'], orientation['x'], orientation['y'], orientation['z']] rot_matrix = quaternion_to_rotation_matrix(q) translation = np.array([position['x'], position['y'], position['z']]) return rot_matrix, translation def quaternion_to_rotation_matrix(q): w, x, y, z = q return np.array([ [1-2*(y**2 + z**2), 2*(x*y - z*w), 2*(x*z + y*w)], [2*(x*y + z*w), 1-2*(x**2 + z**2), 2*(y*z - x*w)], [2*(x*z - y*w), 2*(y*z + x*w), 1-2*(x**2 + y**2)] ])c. Create Dataset for Pose Estimation
Depending on the model architecture (e.g., regression-based), format data as pairs of images and pose vectors.
-
Define Output: Pose can be represented as a 6-DoF vector (3 for rotation, 3 for translation). You may choose to represent rotation as Euler angles or use other representations like quaternions.
-
Example Dataset Structure:
dataset/ ├── images/ │ ├── 000000.png │ ├── 000001.png │ └── ... └── poses/ ├── 000000.npy ├── 000001.npy └── ... -
Save Poses as Numpy Arrays:
import os def prepare_dataset(rgb_dir, poses_dir, output_image_dir, output_pose_dir): create_dir(output_image_dir) create_dir(output_pose_dir) file_names = sorted(os.listdir(rgb_dir)) for file_name in file_names: if file_name.endswith('.png'): frame_id = file_name.split('_')[1].split('.')[0] img_path = os.path.join(rgb_dir, file_name) pose_path = os.path.join(poses_dir, f"frame_{frame_id}.json") rot, trans = load_pose(pose_path) pose = np.hstack((rot.flatten(), trans)) # 12 elements np.save(os.path.join(output_pose_dir, f"{frame_id}.npy"), pose) # Optionally, copy images to a standardized directory # shutil.copy(img_path, os.path.join(output_image_dir, file_name)) prepare_dataset('airsim_data/rgb', 'airsim_data/poses', 'dataset/images', 'dataset/poses')Note: Depending on your model architecture, you may need to adjust the representation of the pose.
d. Split the Dataset
Divide the dataset into training, validation, and test sets.
import shutil from sklearn.model_selection import train_test_split def split_dataset(image_dir, pose_dir, train_ratio=0.8, val_ratio=0.1, test_ratio=0.1): images = sorted([f for f in os.listdir(image_dir) if f.endswith('.png')]) train_val, test = train_test_split(images, test_size=test_ratio, random_state=42) train, val = train_test_split(train_val, test_size=val_ratio / (train_ratio + val_ratio), random_state=42) for split, subset in zip(['train', 'val', 'test'], [train, val, test]): subset_img_dir = os.path.join('dataset', split, 'images') subset_pose_dir = os.path.join('dataset', split, 'poses') create_dir(subset_img_dir) create_dir(subset_pose_dir) for img in subset: shutil.copy(os.path.join(image_dir, img), os.path.join(subset_img_dir, img)) frame_id = img.split('_')[1].split('.')[0] shutil.copy(os.path.join(pose_dir, f"{frame_id}.npy"), os.path.join(subset_pose_dir, f"{frame_id}.npy")) split_dataset('dataset/images', 'dataset/poses')
3. Model Selection and Training
Choosing an appropriate pose estimation model is crucial, especially for deployment on Jetson Nano. Here, we'll use PoseNet, a popular regression-based model for camera pose estimation.
a. Choose a Suitable Architecture
For embedded systems like Jetson Nano:
- Lightweight Models: MobileNet-based architectures offer a good trade-off between accuracy and computational efficiency.
- Pretrained Models: Leverage models pretrained on large datasets and fine-tune them on your dataset.
b. Implement a Pose Estimation Model
Here’s a simplified example using PyTorch and a MobileNet backbone.
-
Define the Model:
import torch import torch.nn as nn import torchvision.models as models class PoseNet(nn.Module): def __init__(self, pretrained=True): super(PoseNet, self).__init__() self.backbone = models.mobilenet_v2(pretrained=pretrained).features self.avgpool = nn.AdaptiveAvgPool2d((1, 1)) self.fc = nn.Sequential( nn.Linear(1280, 512), nn.ReLU(), nn.Linear(512, 6) # 3 for rotation, 3 for translation ) def forward(self, x): x = self.backbone(x) x = self.avgpool(x) x = torch.flatten(x, 1) x = self.fc(x) return x -
Create a Custom Dataset:
from torch.utils.data import Dataset from PIL import Image class PoseDataset(Dataset): def __init__(self, image_dir, pose_dir, transform=None): self.image_dir = image_dir self.pose_dir = pose_dir self.transform = transform self.images = sorted([f for f in os.listdir(image_dir) if f.endswith('.png')]) def __len__(self): return len(self.images) def __getitem__(self, idx): img_name = self.images[idx] img_path = os.path.join(self.image_dir, img_name) image = Image.open(img_path).convert('RGB') if self.transform: image = self.transform(image) frame_id = img_name.split('_')[1].split('.')[0] pose_path = os.path.join(self.pose_dir, f"{frame_id}.npy") pose = np.load(pose_path).astype(np.float32) pose = torch.from_numpy(pose) return image, pose -
Data Transformations:
import torchvision.transforms as transforms data_transforms = transforms.Compose([ transforms.Resize((224, 224)), transforms.ToTensor(), transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) ]) -
Training Script:
import torch.optim as optim from torch.utils.data import DataLoader from tqdm import tqdm def train_model(model, dataloaders, criterion, optimizer, num_epochs=25, device='cuda'): model.to(device) best_model_wts = copy.deepcopy(model.state_dict()) best_loss = float('inf') for epoch in range(num_epochs): print(f'Epoch {epoch}/{num_epochs - 1}') print('-' * 10) # Each epoch has a training and validation phase for phase in ['train', 'val']: if phase == 'train': model.train() else: model.eval() running_loss = 0.0 # Iterate over data for inputs, poses in tqdm(dataloaders[phase]): inputs = inputs.to(device) poses = poses.to(device) optimizer.zero_grad() with torch.set_grad_enabled(phase == 'train'): outputs = model(inputs) loss = criterion(outputs, poses) if phase == 'train': loss.backward() optimizer.step() running_loss += loss.item() * inputs.size(0) epoch_loss = running_loss / len(dataloaders[phase].dataset) print(f'{phase} Loss: {epoch_loss:.4f}') # Deep copy the model if phase == 'val' and epoch_loss < best_loss: best_loss = epoch_loss best_model_wts = copy.deepcopy(model.state_dict()) print(f'Best val loss: {best_loss:.4f}') # Load best model weights model.load_state_dict(best_model_wts) return model def main_training(): # Create datasets train_dataset = PoseDataset('dataset/train/images', 'dataset/train/poses', transform=data_transforms) val_dataset = PoseDataset('dataset/val/images', 'dataset/val/poses', transform=data_transforms) # Create dataloaders dataloaders = { 'train': DataLoader(train_dataset, batch_size=32, shuffle=True, num_workers=4), 'val': DataLoader(val_dataset, batch_size=32, shuffle=False, num_workers=4) } # Initialize model, loss, optimizer model = PoseNet(pretrained=True) criterion = nn.MSELoss() optimizer = optim.Adam(model.parameters(), lr=1e-4) # Train the model trained_model = train_model(model, dataloaders, criterion, optimizer, num_epochs=50, device='cuda' if torch.cuda.is_available() else 'cpu') # Save the model torch.save(trained_model.state_dict(), 'pose_estimator.pth') print("Model training completed and saved.") if __name__ == "__main__": main_training()
c. Training Considerations
- Normalization: Ensure uniform image sizes and normalization for consistent training.
- Data Augmentation: Use augmentations like random rotations, flips, brightness adjustments to improve model robustness.
- Loss Function: MSELoss is suitable for regression. Alternatively, consider separate losses for rotation and translation.
- Learning Rate Scheduler: Implement schedulers like
ReduceLROnPlateauto adjust learning rates during training.
d. Validate Model Performance
After training, evaluate the model on the validation and test sets to ensure it generalizes well.
4. Model Optimization for Jetson Nano
Jetson Nano has limited computational resources, so optimizing the model for efficient inference is crucial.
a. Convert the Model to ONNX
ONNX (Open Neural Network Exchange) format allows for interoperability and optimization.
import torch def convert_to_onnx(model_path, onnx_path, device='cpu'): model = PoseNet() model.load_state_dict(torch.load(model_path, map_location=device)) model.eval() dummy_input = torch.randn(1, 3, 224, 224, device=device) torch.onnx.export(model, dummy_input, onnx_path, input_names=['input'], output_names=['output'], opset_version=11) print(f"Model converted to ONNX and saved at {onnx_path}") convert_to_onnx('pose_estimator.pth', 'pose_estimator.onnx')b. Optimize with TensorRT
TensorRT accelerates inference on NVIDIA hardware like Jetson Nano.
-
Install TensorRT: TensorRT is typically pre-installed on Jetson Nano. Verify installation:
dpkg -l | grep nvinfer -
Convert ONNX to TensorRT Engine:
import tensorrt as trt TRT_LOGGER = trt.Logger(trt.Logger.WARNING) def build_engine(onnx_path, engine_path, max_batch_size=1, fp16_mode=True): builder = trt.Builder(TRT_LOGGER) network = builder.create_network(1 << int(trt.NetworkDefinitionCreationFlag.EXPLICIT_BATCH)) parser = trt.OnnxParser(network, TRT_LOGGER) with open(onnx_path, 'rb') as model: if not parser.parse(model.read()): for error in range(parser.num_errors): print(parser.get_error(error)) return None builder.max_batch_size = max_batch_size builder.max_workspace_size = 1 << 30 # 1GB if fp16_mode and builder.platform_has_fast_fp16: builder.fp16_mode = True engine = builder.build_cuda_engine(network) with open(engine_path, "wb") as f: f.write(engine.serialize()) print(f"TensorRT engine built and saved at {engine_path}") build_engine('pose_estimator.onnx', 'pose_estimator.trt')Note: Adjust parameters like
max_workspace_sizeand enablefp16_modeif supported.
c. Test the Optimized Model
Perform a test inference to ensure the TensorRT engine works correctly.
import pycuda.driver as cuda import pycuda.autoinit # This is necessary for initializing CUDA driver def load_engine(trt_engine_path): runtime = trt.Runtime(TRT_LOGGER) with open(trt_engine_path, 'rb') as f: engine = runtime.deserialize_cuda_engine(f.read()) return engine def infer_pose(engine, input_image): import pycuda.driver as cuda import numpy as np # Allocate buffers inputs, outputs, bindings, stream = [], [], [], None for binding in engine: size = trt.volume(engine.get_binding_shape(binding)) * engine.max_batch_size dtype = trt.nptype(engine.get_binding_dtype(binding)) host_mem = cuda.pagelocked_empty(size, dtype) device_mem = cuda.mem_alloc(host_mem.nbytes) bindings.append(int(device_mem)) if engine.binding_is_input(binding): inputs.append({'host': host_mem, 'device': device_mem}) else: outputs.append({'host': host_mem, 'device': device_mem}) # Preprocess image input_image = cv2.resize(input_image, (224, 224)) input_image = input_image.astype(np.float32) / 255.0 mean = np.array([0.485, 0.456, 0.406]) std = np.array([0.229, 0.224, 0.225]) input_image = (input_image - mean) / std input_image = np.transpose(input_image, (2, 0, 1)) # HWC to CHW input_image = np.expand_dims(input_image, axis=0) input_image = input_image.flatten() # Transfer input data to device np.copyto(inputs[0]['host'], input_image) cuda.memcpy_htod(inputs[0]['device'], inputs[0]['host']) # Execute inference context = engine.create_execution_context() context.execute(batch_size=1, bindings=bindings) # Transfer predictions back for output in outputs: cuda.memcpy_dtoh(output['host'], output['device']) # Convert output to numpy array pose = outputs[0]['host'] return pose # Example usage # engine = load_engine('pose_estimator.trt') # input_img = cv2.imread('dataset/test/images/frame_000000.png') # pose = infer_pose(engine, input_img) # print("Predicted Pose:", pose)Note: Integration with TensorRT can be complex. Consider using higher-level APIs or leveraging existing examples from NVIDIA’s TensorRT repository.
5. Integration into the SLAM System
Replace the existing ORB-based
PoseEstimatorwith the trained deep learning-based pose estimator.a. Modify the PoseEstimator Class
Update the
PoseEstimatorclass to use the optimized model for pose estimation.-
New PoseEstimator Using TensorRT:
import pycuda.driver as cuda import pycuda.autoinit import tensorrt as trt import numpy as np import cv2 class DLPoseEstimator: def __init__(self, engine_path): self.engine = self.load_engine(engine_path) self.context = self.engine.create_execution_context() # Allocate buffers self.inputs, self.outputs, self.bindings, self.stream = self.allocate_buffers(self.engine) def load_engine(self, engine_path): TRT_LOGGER = trt.Logger(trt.Logger.WARNING) runtime = trt.Runtime(TRT_LOGGER) with open(engine_path, 'rb') as f: engine = runtime.deserialize_cuda_engine(f.read()) return engine def allocate_buffers(self, engine): inputs = [] outputs = [] bindings = [] stream = cuda.Stream() for binding in engine: size = trt.volume(engine.get_binding_shape(binding)) * engine.max_batch_size dtype = trt.nptype(engine.get_binding_dtype(binding)) host_mem = cuda.pagelocked_empty(size, dtype) device_mem = cuda.mem_alloc(host_mem.nbytes) bindings.append(int(device_mem)) if engine.binding_is_input(binding): inputs.append({'host': host_mem, 'device': device_mem}) else: outputs.append({'host': host_mem, 'device': device_mem}) return inputs, outputs, bindings, stream def infer_pose(self, image): # Preprocess image input_img = cv2.resize(image, (224, 224)) input_img = input_img.astype(np.float32) / 255.0 mean = np.array([0.485, 0.456, 0.406]) std = np.array([0.229, 0.224, 0.225]) input_img = (input_img - mean) / std input_img = np.transpose(input_img, (2, 0, 1)) # HWC to CHW input_img = np.expand_dims(input_img, axis=0) input_img = input_img.flatten() # Transfer input data to device np.copyto(self.inputs[0]['host'], input_img) cuda.memcpy_htod_async(self.inputs[0]['device'], self.inputs[0]['host'], self.stream) # Execute inference self.context.execute_async(batch_size=1, bindings=self.bindings, stream_handle=self.stream.handle) # Transfer predictions back cuda.memcpy_dtoh_async(self.outputs[0]['host'], self.outputs[0]['device'], self.stream) self.stream.synchronize() # Get pose pose = self.outputs[0]['host'] return pose # Array of 6 floats -
Update SLAMSystem to Use DLPoseEstimator:
class SLAMSystem: def __init__(self, video_path, calibration_file, real_time_vis=False, model='zoe', save_point_clouds=False, frame_interval=1, pose_model_path='pose_estimator.trt'): # Existing initialization code... # Replace PoseEstimator with DLPoseEstimator self.pose_estimator = DLPoseEstimator(pose_model_path) # Initialize other necessary components def run(self): # Modify SLAM loop to use deep learning-based pose estimation frame_count = 0 depth_prev = None 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}") # Process frame to get point cloud point_cloud_np, depth_map = self.generator.process_frame_with_depth(frame) if self.save_point_clouds: # Save point cloud as before pass 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) # Estimate pose using DL-based estimator pose = self.pose_estimator.infer_pose(frame) if pose is not None: # Convert pose array to transformation matrix R = rotation_vector_to_matrix(pose[:3]) t = pose[3:] T = np.eye(4) T[:3, :3] = R T[:3, 3] = t self.pose = self.pose @ T print(f"Updated pose:\n{self.pose}") self.camera_positions.append(self.pose[:3, 3]) else: print(f"Frame {frame_count}: Pose estimation failed.") # Transform and update global map 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: # Save outputs as before pass def rotation_vector_to_matrix(rot_vec): # Assuming rot_vec represents Euler angles, convert to rotation matrix # Alternatively, define based on model's rotation representation import torch import math rx, ry, rz = rot_vec Rx = np.array([ [1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)] ]) Ry = np.array([ [math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)] ]) Rz = np.array([ [math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1] ]) R = Rz @ Ry @ Rx return RNotes:
- Rotation Representation: Ensure the rotation output from the model matches the expected format. If the model predicts Euler angles, convert them to a rotation matrix. If using quaternions or other representations, adjust accordingly.
- Scaling: The original SLAM system applied a scale factor. With a deep learning-based approach, scaling might be implicitly handled, but verify its accuracy.
- Error Handling: Implement robustness for cases where pose estimation fails or produces invalid outputs.
b. Remove Feature-Based Pose Estimation
Since the pose is now estimated via the deep learning model, you can remove ORB feature detection and matching from the SLAM pipeline.
c. Update Dependencies
Ensure all necessary dependencies (e.g., TensorRT, PyCUDA) are installed and properly configured on the Jetson Nano.
6. Testing and Iteration
a. Test on Jetson Nano
-
Transfer Model and Code:
- Copy the optimized TensorRT engine (
pose_estimator.trt) and updated SLAM scripts to the Jetson Nano.
- Copy the optimized TensorRT engine (
-
Install Dependencies on Jetson Nano:
- PyTorch: Either install directly or convert to use TensorRT exclusively.
- OpenCV: Ensure it's installed with the required modules.
- Open3D and Other Libraries: Install necessary Python packages.
-
Run the SLAM System:
Execute the SLAM program on Jetson Nano and monitor performance.
python slam_system.py --video_path path_to_video.mp4 --calibration_file calibration.npz --real_time_vis --model metric3d -
Monitor Resource Usage:
Use tools like
htoportegrastatsto monitor CPU and GPU usage, ensuring the system runs within Jetson Nano’s capabilities.
b. Validate Pose Estimation Accuracy
Compare the estimated poses with ground truth from AirSim to assess accuracy.
- Visual Inspection: Overlay estimated camera trajectory with ground truth.
- Quantitative Metrics: Calculate errors in position and orientation.
c. Optimize Further if Needed
- Batch Size: Ensure the inference batch size is set to 1 for real-time performance.
- Model Pruning/Quantization: Further reduce model size and increase inference speed if necessary.
- Parallel Processing: Utilize multi-threading to handle pose estimation and point cloud processing concurrently.
Additional Tips and Considerations
-
Handling Rotation Representations:
- Euler angles can suffer from gimbal lock; consider using rotation matrices or quaternions.
- Ensure consistency in rotation representation between training and inference.
-
Scale Ambiguity in Pose Estimation:
- Deep learning-based pose estimators often predict relative pose rather than absolute scale.
- Incorporate absolute scale information if necessary, possibly leveraging depth data from your SLAM system.
-
Synchronization Between RGB and Pose Data:
- Ensure that each RGB image correctly corresponds to its pose data.
- Handle any discrepancies or dropped frames gracefully.
-
Data Augmentation for Robustness:
- Include various environmental conditions in training data (lighting, weather) to enhance model robustness.
-
Fallback Mechanisms:
- Implement fallback strategies if the deep learning-based pose estimation fails, such as relying on odometry or feature-based methods temporarily.
-
Logging and Debugging:
- Log estimated poses, errors, and system states to facilitate debugging and performance monitoring.
-
Explore Pretrained Models:
- Leverage existing pretrained pose estimation models and fine-tune them for your specific application to save training time and improve performance.
-
Leverage Jetson SDKs:
- NVIDIA provides JetPack SDK, including TensorRT and other optimizations tailored for Jetson devices. Ensure you’re utilizing these tools effectively.
Example Integration Code Snippet
Below is an example of how to integrate the
DLPoseEstimatorinto your existing SLAM system:import cv2 import numpy as np import open3d as o3d from dl_pose_estimator import DLPoseEstimator # Assuming DLPoseEstimator is in dl_pose_estimator.py class SLAMSystem: def __init__(self, video_path, calibration_file, real_time_vis=False, model='zoe', save_point_clouds=False, frame_interval=1, pose_model_path='pose_estimator.trt'): # Existing initialization code... self.generator = PointCloudGenerator(calibration_path=calibration_file) self.pose_estimator = DLPoseEstimator(pose_model_path) self.cap = cv2.VideoCapture(video_path) self.global_map = o3d.geometry.PointCloud() self.pose = np.eye(4) # Additional initialization... def run(self): frame_count = 0 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}") # Generate point cloud from depth point_cloud_np, depth_map = self.generator.process_frame_with_depth(frame) if self.save_point_clouds: # Save point cloud pass 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) # Estimate pose using DL-based estimator pose = self.pose_estimator.infer_pose(frame) if pose is not None: R = rotation_vector_to_matrix(pose[:3]) t = pose[3:] T = np.eye(4) T[:3, :3] = R T[:3, 3] = t self.pose = self.pose @ T print(f"Updated pose:\n{self.pose}") self.camera_positions.append(self.pose[:3, 3]) else: print(f"Frame {frame_count}: Pose estimation failed.") # Transform point cloud 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.update_visualization() 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.terminate_visualization() else: o3d.visualization.draw_geometries([self.global_map]) if len(self.camera_positions) > 0: camera_positions = np.array(self.camera_positions) # Plot and save trajectory pass def rotation_vector_to_matrix(rot_vec): # Implement based on rotation representation pass def main(): # Argument parsing as before pass if __name__ == "__main__": main()Note: Replace
rotation_vector_to_matrixwith an appropriate implementation based on how rotation is represented in your pose estimator.
Final Thoughts
Transitioning from a traditional feature-based SLAM to a deep learning-based pose estimation approach can significantly enhance performance, especially in environments where feature matching struggles. However, it introduces new challenges, such as ensuring real-time inference on embedded hardware and managing scale ambiguities inherent in learning-based methods.
By following the steps outlined above—collecting a robust dataset with AirSim, training an efficient model, optimizing it for Jetson Nano, and integrating it seamlessly into your SLAM pipeline—you can develop a more reliable and accurate RGB-based SLAM system. Consistent testing, iterative improvements, and leveraging community resources will further enhance your project's success.
Good luck with your SLAM project!