Custom SLAM

#!/usr/bin/env python3
from neuronav import RealSenseSensor, SensorConfig, run_slam, RTABMapSLAM, SlamConfig
import threading
import subprocess

def run_camera(device_id, name):
    """Run SLAM for a single camera"""
    print(f"Starting {name} with device {device_id}")

    config = SensorConfig(device_id=device_id)
    sensor = RealSenseSensor(config)

    slam_config = SlamConfig(ros_domain_id=int(device_id[-1]))
    slam = RTABMapSLAM(slam_config)

    run_slam(sensor, slam)

# Find connected cameras
result = subprocess.run(
    ["rs-enumerate-devices", "-s"],
    capture_output=True,
    text=True
)

serials = [line.strip() for line in result.stdout.splitlines() if line]
print(f"Found {len(serials)} cameras: {serials}")

# Run each camera in a thread
threads = []
for i, serial in enumerate(serials):
    thread = threading.Thread(
        target=run_camera,
        args=(serial, f"Camera_{i}")
    )
    thread.start()
    threads.append(thread)

for thread in threads:
    thread.join()

#!/usr/bin/env python3
from neuronav import RealSenseSensor, RTABMapSLAM, run_slam
import numpy as np

class PoseMonitor:
    def __init__(self):
        self.poses = []
        self.last_pose = None

    def on_new_pose(self, position, quaternion):
        """Called when new pose is available"""
        self.poses.append({
            'position': position.copy(),
            'quaternion': quaternion.copy()
        })

        if self.last_pose is not None:
            distance = np.linalg.norm(position - self.last_pose)
            if distance > 0.01:  # Moved at least 1cm
                print(f"Position: {position}, Distance: {distance:.3f}m")

        self.last_pose = position.copy()

    def save_trajectory(self, filename="trajectory.npy"):
        """Save trajectory to file"""
        np.save(filename, self.poses)
        print(f"Saved {len(self.poses)} poses to {filename}")

sensor = RealSenseSensor()
slam = RTABMapSLAM()
monitor = PoseMonitor()

slam.register_pose_callback(monitor.on_new_pose)

try:
    run_slam(sensor, slam, duration=30)
finally:
    monitor.save_trajectory()

#!/usr/bin/env python3
from neuronav import RealSenseSensor, OAKDSensor, run_slam, RTABMapSLAM
import sys

# Get sensor type from command line
sensor_type = sys.argv[1] if len(sys.argv) > 1 else "realsense"

# Create appropriate sensor
if sensor_type == "realsense":
    sensor = RealSenseSensor()
    print("Using Intel RealSense")
elif sensor_type == "oakd":
    sensor = OAKDSensor()
    print("Using OAK-D Pro")
else:
    print(f"Unknown sensor: {sensor_type}")
    sys.exit(1)

slam = RTABMapSLAM()
run_slam(sensor, slam, visualize=True)

python sensor_switch.py realsense
python sensor_switch.py oakd

#!/usr/bin/env python3
from neuronav import RealSenseSensor, SensorConfig, RTABMapSLAM, SlamConfig, run_slam

# Fast sensor config
sensor = RealSenseSensor(SensorConfig(
    rgb_width=640,
    rgb_height=480,
    fps=60,
    enable_imu=True
))

# Fast SLAM config
slam = RTABMapSLAM(SlamConfig(
    custom_params={
        "Rtabmap/DetectionRate": "2.0",
        "Vis/MaxFeatures": "500",
        "RGBD/LinearUpdate": "0.2",
        "RGBD/AngularUpdate": "0.2"
    }
))

run_slam(sensor, slam)

#!/usr/bin/env python3
from neuronav import RealSenseSensor, SensorConfig, RTABMapSLAM, SlamConfig, run_slam

# High-res sensor
sensor = RealSenseSensor(SensorConfig(
    rgb_width=1920,
    rgb_height=1080,
    depth_width=1280,
    depth_height=720,
    fps=30
))

# Quality SLAM
slam = RTABMapSLAM(SlamConfig(
    custom_params={
        "Rtabmap/DetectionRate": "0",
        "Vis/MaxFeatures": "2000",
        "Grid/3D": "true",
        "Grid/CellSize": "0.01"
    }
))

run_slam(sensor, slam, visualize=True)

#!/usr/bin/env python3
from neuronav import BaseSLAM, RealSenseSensor, run_slam
import numpy as np

class CustomSLAM(BaseSLAM):
    """Custom SLAM implementation"""

    def __init__(self):
        super().__init__()
        self.trajectory = []
        self.map_points = []

    def process_frame(self, rgb_image, depth_image, timestamp):
        """Process incoming sensor data"""
        # Your custom SLAM logic here
        pose = self.estimate_pose(rgb_image, depth_image)
        self.trajectory.append(pose)

        # Extract and store map points
        points = self.extract_features(rgb_image, depth_image)
        self.map_points.extend(points)

        return pose

    def estimate_pose(self, rgb, depth):
        """Estimate camera pose"""
        # Implement your pose estimation
        return np.eye(4)

    def extract_features(self, rgb, depth):
        """Extract 3D features"""
        # Implement your feature extraction
        return []

    def get_pose(self):
        """Return current pose"""
        if self.trajectory:
            return self.trajectory[-1]
        return np.eye(4)

    def save_map(self, filename):
        """Save map to file"""
        np.savez(filename,
                 trajectory=self.trajectory,
                 map_points=self.map_points)

# Use custom SLAM
sensor = RealSenseSensor()
slam = CustomSLAM()
run_slam(sensor, slam, visualize=True)

#!/usr/bin/env python3
from neuronav import RealSenseSensor, RTABMapSLAM, SlamConfig, run_slam

slam_config = SlamConfig(
    enable_loop_closing=True,
    custom_params={
        # Loop closure detection
        "Rtabmap/LoopRatio": "0.8",
        "Rtabmap/LoopThr": "0.11",

        # Memory management
        "Rtabmap/TimeThr": "0",
        "Rtabmap/MemoryThr": "0",

        # Feature matching
        "Vis/MaxFeatures": "1500",
        "Vis/MinInliers": "15",

        # Global optimization
        "RGBD/OptimizeFromGraphEnd": "true",
        "Optimizer/Robust": "true"
    }
)

sensor = RealSenseSensor()
slam = RTABMapSLAM(slam_config)
run_slam(sensor, slam, visualize=True)