Robot Perception Skill

When to Use This Skill

• Setting up and configuring camera, LiDAR, or depth sensors
• Building RGB, depth, or point cloud processing pipelines
• Calibrating cameras (intrinsic, extrinsic, hand-eye)
• Implementing object detection, segmentation, or tracking for robots
• Fusing data from multiple sensor modalities
• Streaming sensor data with proper threading and buffering
• Synchronizing multi-sensor rigs
• Deploying perception models on robot hardware (GPU, edge)
• Debugging perception failures (latency, dropped frames, misalignment)

Sensor Landscape

Sensor Types and Characteristics

Sensor Type        Output              Range       Rate     Best For
─────────────────────────────────────────────────────────────────────────
RGB Camera         (H,W,3) uint8       ∞           30-120Hz Object detection, tracking, visual servoing
Stereo Camera      (H,W,3)+(H,W,3)    0.3-20m     30-90Hz  Dense depth from passive stereo
Structured Light   (H,W) float + RGB   0.2-10m     30Hz     Indoor manipulation, short range
ToF Depth          (H,W) float + RGB   0.1-10m     30Hz     Indoor, medium range
LiDAR (spinning)   (N,3) or (N,4)     0.5-200m    10-20Hz  Outdoor navigation, mapping
LiDAR (solid-st.)  (N,3)              0.5-200m    10-30Hz  Automotive, outdoor
IMU                (6,) or (9,)        N/A         200-1kHz Orientation, motion estimation
Force/Torque       (6,) float          N/A         1kHz+    Contact detection, force control
Tactile            (H,W) or (N,3)      Contact     30-100Hz Grasp quality, texture
Event Camera       Events (x,y,t,p)    ∞           μs       High-speed tracking, HDR scenes

Common Sensor Hardware

Device             Type               SDK/Driver           ROS2 Package
──────────────────────────────────────────────────────────────────────────
Intel RealSense    Structured Light   pyrealsense2         realsense2_camera
Stereolabs ZED     Stereo + IMU       pyzed                zed_wrapper
Luxonis OAK-D      Stereo + Neural    depthai              depthai_ros
FLIR/Basler        Industrial RGB     PySpin/pypylon       spinnaker_camera_driver
Velodyne           Spinning LiDAR     velodyne_driver      velodyne
Ouster             Spinning LiDAR     ouster-sdk           ros2_ouster
Livox              Solid-state LiDAR  livox_sdk            livox_ros2_driver
USB Webcam         RGB                OpenCV VideoCapture  usb_cam / v4l2_camera

Camera Models and Calibration

Pinhole Camera Model

                    3D World Point (X, Y, Z)
                           |
                    [R | t] — Extrinsic (world → camera)
                           |
                    Camera Point (Xc, Yc, Zc)
                           |
                    K — Intrinsic (camera → pixel)
                           |
                    Pixel (u, v)

K = [ fx   0   cx ]      fx, fy = focal lengths (pixels)
    [  0  fy   cy ]      cx, cy = principal point
    [  0   0    1 ]

Projection:  [u, v, 1]^T = K @ [R | t] @ [X, Y, Z, 1]^T

Intrinsic Calibration

import cv2
import numpy as np
from pathlib import Path

class IntrinsicCalibrator:
    """Camera intrinsic calibration using checkerboard pattern"""

    def __init__(self, board_size=(9, 6), square_size_m=0.025):
        self.board_size = board_size
        self.square_size = square_size_m

        # Prepare object points (3D coordinates of checkerboard corners)
        self.objp = np.zeros((board_size[0] * board_size[1], 3), np.float32)
        self.objp[:, :2] = np.mgrid[
            0:board_size[0], 0:board_size[1]
        ].T.reshape(-1, 2) * square_size_m

    def collect_calibration_images(self, camera, num_images=30,
                                    min_coverage=0.6):
        """Collect calibration images with good spatial coverage.

        IMPORTANT: Move the board to cover all regions of the image,
        including corners and edges. Tilt the board at various angles.
        Bad coverage = bad calibration, especially at image edges.
        """
        obj_points = []
        img_points = []
        coverage_map = np.zeros((4, 4), dtype=int)  # Track board positions

        while len(obj_points) < num_images:
            frame = camera.capture()
            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

            found, corners = cv2.findChessboardCorners(
                gray, self.board_size,
                cv2.CALIB_CB_ADAPTIVE_THRESH |
                cv2.CALIB_CB_NORMALIZE_IMAGE |
                cv2.CALIB_CB_FAST_CHECK
            )

            if found:
                # Sub-pixel refinement — critical for accuracy
                criteria = (cv2.TERM_CRITERIA_EPS +
                           cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
                corners = cv2.cornerSubPix(
                    gray, corners, (11, 11), (-1, -1), criteria)

                # Track coverage
                center = corners.mean(axis=0).flatten()
                grid_x = int(center[0] / gray.shape[1] * 4)
                grid_y = int(center[1] / gray.shape[0] * 4)
                grid_x = min(grid_x, 3)
                grid_y = min(grid_y, 3)
                coverage_map[grid_y, grid_x] += 1

                obj_points.append(self.objp)
                img_points.append(corners)

        coverage = (coverage_map > 0).sum() / coverage_map.size
        if coverage < min_coverage:
            print(f"WARNING: Only {coverage:.0%} coverage. "
                  f"Move board to uncovered regions.")

        return obj_points, img_points, gray.shape[::-1]

    def calibrate(self, obj_points, img_points, image_size):
        """Run calibration and return camera matrix + distortion coeffs"""
        ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
            obj_points, img_points, image_size, None, None)

        if ret > 1.0:
            print(f"WARNING: High reprojection error ({ret:.3f} px). "
                  f"Check image quality and board detection.")

        # Compute per-image reprojection errors
        errors = []
        for i in range(len(obj_points)):
            projected, _ = cv2.projectPoints(
                obj_points[i], rvecs[i], tvecs[i], K, dist)
            err = cv2.norm(img_points[i], projected, cv2.NORM_L2)
            err /= len(projected)
            errors.append(err)

        print(f"Calibration complete:")
        print(f"  RMS reprojection error: {ret:.4f} px")
        print(f"  Per-image errors: mean={np.mean(errors):.4f}, "
              f"max={np.max(errors):.4f}")
        print(f"  Focal length: fx={K[0,0]:.1f}, fy={K[1,1]:.1f}")
        print(f"  Principal point: cx={K[0,2]:.1f}, cy={K[1,2]:.1f}")

        return CalibrationResult(
            camera_matrix=K, dist_coeffs=dist,
            rms_error=ret, image_size=image_size)

    def save(self, result, path):
        """Save calibration to YAML (OpenCV-compatible format)"""
        fs = cv2.FileStorage(str(path), cv2.FILE_STORAGE_WRITE)
        fs.write("camera_matrix", result.camera_matrix)
        fs.write("dist_coeffs", result.dist_coeffs)
        fs.write("image_width", result.image_size[0])
        fs.write("image_height", result.image_size[1])
        fs.write("rms_error", result.rms_error)
        fs.release()

    @staticmethod
    def load(path):
        """Load calibration from YAML"""
        fs = cv2.FileStorage(str(path), cv2.FILE_STORAGE_READ)
        K = fs.getNode("camera_matrix").mat()
        dist = fs.getNode("dist_coeffs").mat()
        w = int(fs.getNode("image_width").real())
        h = int(fs.getNode("image_height").real())
        fs.release()
        return CalibrationResult(
            camera_matrix=K, dist_coeffs=dist,
            image_size=(w, h), rms_error=0.0)

Extrinsic Calibration (Camera-to-Camera, Camera-to-LiDAR)

class ExtrinsicCalibrator:
    """Compute transform between two sensors using shared targets"""

    def calibrate_stereo(self, calib_left, calib_right,
                          obj_points, img_points_left, img_points_right,
                          image_size):
        """Stereo calibration: find relative pose between two cameras"""
        ret, K1, d1, K2, d2, R, T, E, F = cv2.stereoCalibrate(
            obj_points, img_points_left, img_points_right,
            calib_left.camera_matrix, calib_left.dist_coeffs,
            calib_right.camera_matrix, calib_right.dist_coeffs,
            image_size,
            flags=cv2.CALIB_FIX_INTRINSIC  # Use pre-calibrated intrinsics
        )

        print(f"Stereo calibration RMS: {ret:.4f} px")
        print(f"Baseline: {np.linalg.norm(T):.4f} m")

        return StereoCalibration(R=R, T=T, E=E, F=F, rms_error=ret)

    def calibrate_camera_to_lidar(self, camera_points_2d,
                                    lidar_points_3d, K, dist):
        """Find camera-to-LiDAR transform using corresponding points.

        Use a calibration target visible to both sensors (e.g.,
        checkerboard with reflective tape corners).
        """
        # PnP: find pose of 3D points relative to camera
        success, rvec, tvec = cv2.solvePnP(
            lidar_points_3d, camera_points_2d, K, dist,
            flags=cv2.SOLVEPNP_ITERATIVE
        )

        if not success:
            raise CalibrationError("PnP failed — check point correspondences")

        R, _ = cv2.Rodrigues(rvec)
        T_camera_lidar = np.eye(4)
        T_camera_lidar[:3, :3] = R
        T_camera_lidar[:3, 3] = tvec.flatten()

        # Verify by reprojecting
        projected, _ = cv2.projectPoints(
            lidar_points_3d, rvec, tvec, K, dist)
        error = np.mean(np.linalg.norm(
            camera_points_2d - projected.reshape(-1, 2), axis=1))
        print(f"Camera-LiDAR reprojection error: {error:.2f} px")

        return T_camera_lidar

Hand-Eye Calibration (Camera-to-Robot)

class HandEyeCalibrator:
    """Solve AX = XB for camera mounted on robot end-effector (eye-in-hand)
    or camera mounted on a fixed base (eye-to-hand).

    Requires moving the robot to multiple poses while observing a
    fixed calibration target.
    """

    def __init__(self, K, dist, board_size=(9, 6), square_size=0.025):
        self.K = K
        self.dist = dist
        self.board_size = board_size
        self.square_size = square_size
        self.objp = np.zeros((board_size[0] * board_size[1], 3), np.float32)
        self.objp[:, :2] = np.mgrid[
            0:board_size[0], 0:board_size[1]
        ].T.reshape(-1, 2) * square_size

    def collect_poses(self, camera, robot, num_poses=20):
        """Collect camera-target and robot poses at multiple configurations.

        IMPORTANT: Move to diverse robot orientations. At least 3 different
        rotation axes. Pure translations are NOT sufficient.
        """
        R_gripper2base = []
        t_gripper2base = []
        R_target2cam = []
        t_target2cam = []

        for i in range(num_poses):
            input(f"Move robot to pose {i+1}/{num_poses}, press Enter...")

            # Get robot end-effector pose
            ee_pose = robot.get_ee_pose()  # 4x4 homogeneous matrix
            R_gripper2base.append(ee_pose[:3, :3])
            t_gripper2base.append(ee_pose[:3, 3])

            # Detect calibration target in camera
            frame = camera.capture()
            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            found, corners = cv2.findChessboardCorners(
                gray, self.board_size, None)

            if not found:
                print(f"  Board not detected at pose {i+1}, skip.")
                continue

            corners = cv2.cornerSubPix(
                gray, corners, (11, 11), (-1, -1),
                (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))

            ret, rvec, tvec = cv2.solvePnP(
                self.objp, corners, self.K, self.dist)
            R, _ = cv2.Rodrigues(rvec)
            R_target2cam.append(R)
            t_target2cam.append(tvec.flatten())

        return (R_gripper2base, t_gripper2base,
                R_target2cam, t_target2cam)

    def calibrate_eye_in_hand(self, R_g2b, t_g2b, R_t2c, t_t2c):
        """Eye-in-hand: camera mounted on end-effector.
        Solves for T_camera_to_gripper."""
        R, t = cv2.calibrateHandEye(
            R_g2b, t_g2b, R_t2c, t_t2c,
            method=cv2.CALIB_HAND_EYE_TSAI  # Also: PARK, HORAUD, DANIILIDIS
        )
        T_cam2gripper = np.eye(4)
        T_cam2gripper[:3, :3] = R
        T_cam2gripper[:3, 3] = t.flatten()
        return T_cam2gripper

    def calibrate_eye_to_hand(self, R_g2b, t_g2b, R_t2c, t_t2c):
        """Eye-to-hand: camera fixed in workspace.
        Solves for T_camera_to_base."""
        # Invert robot poses (base-to-gripper → gripper-to-base)
        R_b2g = [R.T for R in R_g2b]
        t_b2g = [-R.T @ t for R, t in zip(R_g2b, t_g2b)]

        R, t = cv2.calibrateHandEye(
            R_b2g, t_b2g, R_t2c, t_t2c,
            method=cv2.CALIB_HAND_EYE_TSAI
        )
        T_cam2base = np.eye(4)
        T_cam2base[:3, :3] = R
        T_cam2base[:3, 3] = t.flatten()
        return T_cam2base

    def verify_calibration(self, T_cam2ee, robot, camera, target_points_3d):
        """Verify by projecting a known 3D point through the full chain.
        Error should be < 5mm for manipulation tasks."""
        ee_pose = robot.get_ee_pose()
        T_cam2base = ee_pose @ T_cam2ee

        # Project world point to camera
        point_in_cam = np.linalg.inv(T_cam2base) @ np.append(
            target_points_3d[0], 1.0)
        projected, _ = cv2.projectPoints(
            point_in_cam[:3].reshape(1, 3),
            np.zeros(3), np.zeros(3), self.K, self.dist)

        print(f"Verification projection: {projected.flatten()}")
        return projected.flatten()

Calibration Quality Checklist

✅ Intrinsic calibration:
   - RMS reprojection error < 0.5 px (good), < 0.3 px (excellent)
   - ≥ 20 images with board covering full image area (corners + edges)
   - Board tilted at multiple angles (not just fronto-parallel)
   - Fixed focus / fixed zoom during calibration AND operation

✅ Extrinsic calibration (stereo / camera-LiDAR):
   - RMS < 1.0 px for stereo
   - Reprojection error < 3 px for camera-LiDAR
   - Verified with independent measurement (ruler / known distance)

✅ Hand-eye calibration:
   - ≥ 15 poses with diverse orientations (≥ 3 rotation axes)
   - Verification error < 5mm for manipulation, < 10mm for navigation
   - Robot repeatability accounted for (UR5 ≈ ±0.03mm, low-cost ≈ ±1mm)

⚠️  Recalibrate when:
   - Camera is physically bumped or remounted
   - Lens focus or zoom is changed
   - Temperature changes significantly (thermal expansion shifts intrinsics)
   - Robot is re-homed or joints are recalibrated

Sensor Streaming Best Practices

Camera Streaming Architecture

import threading
import time
from collections import deque
from dataclasses import dataclass, field
from typing import Optional
import numpy as np

@dataclass
class StampedFrame:
    """Frame with capture timestamp for synchronization"""
    data: np.ndarray
    timestamp: float          # time.monotonic() at capture
    sequence: int             # Frame counter
    sensor_id: str

class CameraStream:
    """Thread-safe camera streaming with bounded buffer.

    Design principles:
    1. Capture runs in a dedicated thread at sensor rate
    2. Processing never blocks capture
    3. Always use the LATEST frame (drop old ones for real-time)
    4. Timestamp at capture, not at processing time
    """

    def __init__(self, camera, buffer_size=2, name="camera"):
        self.camera = camera
        self.name = name
        self._buffer = deque(maxlen=buffer_size)
        self._lock = threading.Lock()
        self._new_frame = threading.Event()
        self._running = False
        self._sequence = 0
        self._thread = None

        # Diagnostics
        self._capture_times = deque(maxlen=100)
        self._drop_count = 0

    def start(self):
        """Start capture thread"""
        self._running = True
        self._thread = threading.Thread(
            target=self._capture_loop, daemon=True, name=f"{self.name}_capture")
        self._thread.start()

    def stop(self):
        """Stop capture thread"""
        self._running = False
        if self._thread:
            self._thread.join(timeout=2.0)

    def _capture_loop(self):
        while self._running:
            t_start = time.monotonic()

            try:
                raw = self.camera.capture()
                timestamp = time.monotonic()  # Timestamp AFTER capture

                frame = StampedFrame(
                    data=raw,
                    timestamp=timestamp,
                    sequence=self._sequence,
                    sensor_id=self.name
                )
                self._sequence += 1

                with self._lock:
                    if len(self._buffer) == self._buffer.maxlen:
                        self._drop_count += 1
                    self._buffer.append(frame)

                self._new_frame.set()
                self._capture_times.append(time.monotonic() - t_start)

            except Exception as e:
                print(f"[{self.name}] Capture error: {e}")
                time.sleep(0.01)  # Back off on error

    def get_latest(self) -> Optional[StampedFrame]:
        """Get most recent frame (non-blocking). Returns None if empty."""
        with self._lock:
            if self._buffer:
                return self._buffer[-1]
        return None

    def wait_for_frame(self, timeout=1.0) -> Optional[StampedFrame]:
        """Block until a new frame arrives"""
        self._new_frame.clear()
        if self._new_frame.wait(timeout=timeout):
            return self.get_latest()
        return None

    def get_diagnostics(self) -> dict:
        """Streaming health metrics"""
        if self._capture_times:
            times = list(self._capture_times)
            fps = 1.0 / np.mean(times) if np.mean(times) > 0 else 0
        else:
            fps = 0
        return {
            "sensor": self.name,
            "fps": round(fps, 1),
            "frames_captured": self._sequence,
            "frames_dropped": self._drop_count,
            "buffer_size": len(self._buffer),
            "avg_capture_ms": round(np.mean(times) * 1000, 1) if self._capture_times else 0,
        }

Multi-Sensor Synchronization

class SyncedMultiSensor:
    """Synchronize frames from multiple sensors by timestamp.

    Uses nearest-neighbor matching within a time tolerance.
    For hardware-synced sensors, use hardware trigger instead.
    """

    def __init__(self, sensors: dict, max_time_diff_ms=33):
        """
        Args:
            sensors: {"rgb": CameraStream, "depth": CameraStream, ...}
            max_time_diff_ms: Maximum allowed time difference between
                              synced frames. Default 33ms (1 frame at 30Hz).
        """
        self.sensors = sensors
        self.max_dt = max_time_diff_ms / 1000.0
        self._synced_callback = None

    def start(self):
        for s in self.sensors.values():
            s.start()

    def stop(self):
        for s in self.sensors.values():
            s.stop()

    def get_synced(self) -> Optional[dict]:
        """Get time-synchronized frames from all sensors.
        Returns None if any sensor is missing or too far out of sync."""
        frames = {}
        for name, stream in self.sensors.items():
            frame = stream.get_latest()
            if frame is None:
                return None
            frames[name] = frame

        # Check time alignment against the first sensor
        ref_time = list(frames.values())[0].timestamp
        for name, frame in frames.items():
            dt = abs(frame.timestamp - ref_time)
            if dt > self.max_dt:
                return None  # Out of sync

        return frames

    def get_synced_interpolated(self) -> Optional[dict]:
        """For sensors at different rates, interpolate to common timestamp.
        Useful for IMU + camera fusion."""
        # Get latest from each sensor
        frames = {}
        for name, stream in self.sensors.items():
            frame = stream.get_latest()
            if frame is None:
                return None
            frames[name] = frame

        # Use the SLOWEST sensor's timestamp as reference
        ref_time = min(f.timestamp for f in frames.values())

        result = {}
        for name, frame in frames.items():
            dt = frame.timestamp - ref_time
            if abs(dt) <= self.max_dt:
                result[name] = frame
            # For high-rate sensors (IMU), could interpolate here

        return result if len(result) == len(self.sensors) else None

Hardware-Triggered Synchronization

class HardwareSyncConfig:
    """Configure hardware trigger for multi-camera synchronization.

    ALWAYS prefer hardware sync over software sync for:
    - Stereo depth computation
    - Multi-camera 3D reconstruction
    - Fast-moving scenes

    Common trigger methods:
    - GPIO pulse from microcontroller (Arduino, ESP32)
    - Camera's own sync output → other cameras' trigger input
    - PTP (Precision Time Protocol) for GigE cameras
    """

    @staticmethod
    def setup_realsense_sync(master_serial, slave_serials):
        """Configure RealSense hardware sync (Inter-Cam sync mode)"""
        import pyrealsense2 as rs

        # Master camera: generates sync signal
        master = rs.pipeline()
        master_cfg = rs.config()
        master_cfg.enable_device(master_serial)
        master_sensor = master.start(master_cfg)

        # Set master to mode 1 (master)
        depth_sensor = master_sensor.get_device().first_depth_sensor()
        depth_sensor.set_option(rs.option.inter_cam_sync_mode, 1)

        # Slave cameras: receive sync signal
        slaves = []
        for serial in slave_serials:
            pipe = rs.pipeline()
            cfg = rs.config()
            cfg.enable_device(serial)
            profile = pipe.start(cfg)
            sensor = profile.get_device().first_depth_sensor()
            sensor.set_option(rs.option.inter_cam_sync_mode, 2)  # Slave
            slaves.append(pipe)

        return master, slaves

    @staticmethod
    def setup_ptp_sync(camera_ips):
        """Enable PTP synchronization for GigE Vision cameras.

        Requires PTP-capable network switch.
        Achieves < 1μs sync accuracy.
        """
        # Example with Basler/pylon cameras
        # import pypylon.pylon as py
        #
        # for ip in camera_ips:
        #     cam = py.InstantCamera(py.TlFactory.GetInstance()
        #         .CreateDevice(py.CDeviceInfo().SetIpAddress(ip)))
        #     cam.Open()
        #     cam.GevIEEE1588.Value = True  # Enable PTP
        #     cam.Close()
        pass

Streaming Anti-Patterns

# ❌ BAD: Capture and process in same thread — limits to slowest operation
def bad_pipeline():
    while True:
        frame = camera.capture()           # 5ms
        detections = model.detect(frame)   # 100ms
        # Effective rate: ~9 FPS regardless of camera's 30 FPS

# ✅ GOOD: Decouple capture from processing
def good_pipeline():
    stream = CameraStream(camera)
    stream.start()
    while True:
        frame = stream.get_latest()        # Always latest, non-blocking
        if frame:
            detections = model.detect(frame.data)  # 100ms
            # Camera still capturing at 30 FPS in background
            # Processing at ~10 FPS but always on freshest frame


# ❌ BAD: Unbounded buffer — memory grows until OOM
buffer = []  # Will grow forever if consumer is slower than producer!

# ✅ GOOD: Bounded buffer with drop policy
buffer = deque(maxlen=2)  # Keep latest 2, drop old automatically