196 lines
6.8 KiB
Markdown
196 lines
6.8 KiB
Markdown
# IMX219 Camera Calibration Workflow
|
||
|
||
Full step-by-step calibration procedure for the 4× IMX219 160° fisheye CSI cameras
|
||
on saltybot (cam0=front, cam1=right, cam2=rear, cam3=left).
|
||
|
||
## Hardware
|
||
|
||
| Camera | Position | Yaw | Topic |
|
||
|--------|----------|-------|------------------------------|
|
||
| cam0 | Front | 0° | /camera/front/image_raw |
|
||
| cam1 | Right | -90° | /camera/right/image_raw |
|
||
| cam2 | Rear | 180° | /camera/rear/image_raw |
|
||
| cam3 | Left | 90° | /camera/left/image_raw |
|
||
|
||
Mount geometry: radius=5cm from robot centre, height=30cm, 90° angular spacing.
|
||
|
||
## Prerequisites
|
||
|
||
- OpenCV with fisheye support: `pip3 install opencv-python numpy pyyaml`
|
||
- For ROS2 mode: ROS2 Humble, cv_bridge, image_transport
|
||
- Printed checkerboard: 8×6 inner corners, 25mm squares (A3 paper recommended)
|
||
- Generator: `python3 -c "import cv2; board=cv2.aruco.CharucoBoard((9,7),0.025,0.0175,cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)); cv2.imwrite('board.png', board.generateImage((700,500)))"`
|
||
- Or download from: https://calib.io/pages/camera-calibration-pattern-generator
|
||
|
||
## Step 1 — Print and Prepare the Checkerboard
|
||
|
||
- Print the 8×6 inner-corner checkerboard at exactly 25mm per square
|
||
- Mount flat on a rigid board (cardboard or foamcore) — no warping allowed
|
||
- Measure actual square size with calipers, update `--square-size` if different
|
||
|
||
## Step 2 — Start Cameras
|
||
|
||
```bash
|
||
# On the Jetson, in the saltybot docker container:
|
||
ros2 launch saltybot_cameras cameras.launch.py
|
||
|
||
# Verify all 4 topics are publishing:
|
||
ros2 topic list | grep camera
|
||
ros2 topic hz /camera/front/image_raw
|
||
```
|
||
|
||
## Step 3 — Capture Calibration Images (30 per camera)
|
||
|
||
Run for each camera in turn. Move the board to many different positions and
|
||
angles within the field of view — especially corners and edges.
|
||
|
||
```bash
|
||
# Option A: ROS2 mode (recommended on Jetson, requires running cameras)
|
||
ros2 run saltybot_calibration capture_calibration_images.py \
|
||
--ros --camera front --n-images 30 \
|
||
--output-dir /mnt/nvme/saltybot/calibration/raw
|
||
|
||
ros2 run saltybot_calibration capture_calibration_images.py \
|
||
--ros --camera right --n-images 30 \
|
||
--output-dir /mnt/nvme/saltybot/calibration/raw
|
||
|
||
ros2 run saltybot_calibration capture_calibration_images.py \
|
||
--ros --camera rear --n-images 30 \
|
||
--output-dir /mnt/nvme/saltybot/calibration/raw
|
||
|
||
ros2 run saltybot_calibration capture_calibration_images.py \
|
||
--ros --camera left --n-images 30 \
|
||
--output-dir /mnt/nvme/saltybot/calibration/raw
|
||
|
||
# Option B: All cameras sequentially (prompts between cameras)
|
||
ros2 run saltybot_calibration capture_calibration_images.py \
|
||
--ros --camera all --n-images 30 \
|
||
--output-dir /mnt/nvme/saltybot/calibration/raw
|
||
|
||
# Option C: Standalone (no ROS2, uses /dev/videoX directly)
|
||
python3 capture_calibration_images.py \
|
||
--camera front --n-images 30 \
|
||
--device /dev/video0 \
|
||
--output-dir /mnt/nvme/saltybot/calibration/raw
|
||
```
|
||
|
||
### Capture Tips for Fisheye Lenses
|
||
|
||
- Cover the entire FoV — include corners and edges (where distortion is strongest)
|
||
- Use 5+ different orientations of the board (tilt, rotate, skew)
|
||
- Keep the board fully visible (no partial views)
|
||
- Good lighting, no motion blur
|
||
- Aim for 20-30 images with varied positions
|
||
|
||
## Step 4 — Compute Intrinsics per Camera
|
||
|
||
```bash
|
||
for cam in front right rear left; do
|
||
ros2 run saltybot_calibration calibrate_camera.py \
|
||
--images-dir /mnt/nvme/saltybot/calibration/raw/cam_${cam} \
|
||
--output /mnt/nvme/saltybot/calibration/${cam}/camera_info.yaml \
|
||
--camera-name camera_${cam} \
|
||
--board-size 8x6 \
|
||
--square-size 0.025 \
|
||
--image-size 640x480 \
|
||
--show-reprojection
|
||
done
|
||
```
|
||
|
||
## Step 5 — Verify Reprojection Error
|
||
|
||
Target: RMS reprojection error < 1.5 px per camera.
|
||
|
||
If RMS > 1.5 px:
|
||
1. Remove outlier images (highest per-image error from table)
|
||
2. Recapture with more varied board positions
|
||
3. Check board is truly flat and square size is correct
|
||
4. Try `--image-size 1280x720` if capturing at higher resolution
|
||
|
||
```bash
|
||
# Quick check — view undistorted preview
|
||
ros2 run saltybot_calibration preview_undistorted.py \
|
||
--image /mnt/nvme/saltybot/calibration/raw/cam_front/frame_0000.png \
|
||
--calibration /mnt/nvme/saltybot/calibration/front/camera_info.yaml
|
||
```
|
||
|
||
## Step 6 — Compute Extrinsics (Mechanical Mode)
|
||
|
||
Uses known mount geometry (radius=5cm, height=30cm, 90° spacing) to compute
|
||
analytically exact camera-to-base_link transforms.
|
||
|
||
```bash
|
||
ros2 run saltybot_calibration calibrate_extrinsics.py \
|
||
--mode mechanical \
|
||
--mount-radius 0.05 \
|
||
--mount-height 0.30 \
|
||
--output /mnt/nvme/saltybot/calibration/extrinsics.yaml \
|
||
--generate-launch \
|
||
~/ros2_ws/src/saltybot_bringup/launch/static_transforms.launch.py
|
||
```
|
||
|
||
If mechanical dimensions differ from defaults, update `--mount-radius` and
|
||
`--mount-height` from your actual measurements.
|
||
|
||
## Step 7 — Generate Static Transform Launch File
|
||
|
||
(Already done by --generate-launch in Step 6, but can regenerate independently:)
|
||
|
||
```bash
|
||
ros2 run saltybot_calibration generate_static_transforms.py \
|
||
--extrinsics /mnt/nvme/saltybot/calibration/extrinsics.yaml \
|
||
--output ~/ros2_ws/src/saltybot_bringup/launch/static_transforms.launch.py
|
||
```
|
||
|
||
## Step 8 — Update surround_vision_params.yaml
|
||
|
||
Edit `jetson/config/surround_vision_params.yaml` (or the equivalent in your
|
||
saltybot_surround package) to point camera_info_url at the new calibration files:
|
||
|
||
```yaml
|
||
camera_info_urls:
|
||
front: 'file:///mnt/nvme/saltybot/calibration/front/camera_info.yaml'
|
||
right: 'file:///mnt/nvme/saltybot/calibration/right/camera_info.yaml'
|
||
rear: 'file:///mnt/nvme/saltybot/calibration/rear/camera_info.yaml'
|
||
left: 'file:///mnt/nvme/saltybot/calibration/left/camera_info.yaml'
|
||
```
|
||
|
||
## Step 9 — Restart Nodes
|
||
|
||
```bash
|
||
# Restart camera and surround nodes to pick up new calibration
|
||
ros2 lifecycle set /saltybot_cameras shutdown
|
||
ros2 launch saltybot_bringup cameras.launch.py
|
||
ros2 launch saltybot_surround surround.launch.py
|
||
```
|
||
|
||
## Output Files
|
||
|
||
After full calibration, you should have:
|
||
|
||
```
|
||
/mnt/nvme/saltybot/calibration/
|
||
front/camera_info.yaml # intrinsics: K, D (equidistant)
|
||
right/camera_info.yaml
|
||
rear/camera_info.yaml
|
||
left/camera_info.yaml
|
||
extrinsics.yaml # base_link -> camera_*_optical_frame
|
||
```
|
||
|
||
Template files are in `jetson/calibration/` (tracked in git) with placeholder
|
||
IMX219 values (fx=229px @ 640×480). Replace with calibrated values.
|
||
|
||
## Distortion Model Reference
|
||
|
||
The IMX219 160° fisheye uses the **equidistant** (Kannala-Brandt) model:
|
||
|
||
```
|
||
r(theta) = k1*theta + k2*theta^3 + k3*theta^5 + k4*theta^7
|
||
```
|
||
|
||
where theta is the angle of incidence. OpenCV's `cv2.fisheye` module implements
|
||
this model with 4 coefficients [k1, k2, k3, k4].
|
||
|
||
This is different from the `plumb_bob` (radial/tangential) model used for
|
||
standard lenses — do not mix them up.
|