📷 Camera Calibration and AprilTag Detection

Purpose of Camera Calibration

Camera calibration is necessary to obtain accurate intrinsic parameters of the camera and correct lens distortion. These parameters are required to:

• Remove image distortion caused by the lens
• Provide accurate pose estimation of AprilTags
• Ensure reliable spatial perception

Without calibration, pose estimation results (translation and rotation) could be incorrect due to inaccurate camera modeling.

What You Need

• The camera mounted on the BlueROV2 and streaming video
• A computer or Raspberry Pi connected to the camera (e.g., via IP or USB)
• ROS 2 workspace with a working camera publisher (e.g., bluerov2_camera_interface)
• A printed checkerboard calibration target

Our checkerboard has 7×6 inner corners and 25 mm squares.

Setup and Dependencies

Make sure the following Python packages are installed:

pip install opencv-python opencv-contrib-python numpy

If you're using ROS 2 (Foxy or newer), make sure:

• Your image topic is publishing (check with ros2 topic echo /camera or /bluerov2/camera)
• Calibration images are saved in a folder (e.g., calib_images/)

Step 1: Capture Calibration Images

Start your ROS 2 camera node and run the following image saver node to automatically save one image every 3 seconds:

image_saver_node.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import os

class ImageSaver(Node):
    def __init__(self):
        super().__init__('image_saver_node')
        self.subscription = self.create_subscription(
            Image,
            'camera',  # Change this if your topic is different
            self.listener_callback,
            10
        )
        self.bridge = CvBridge()
        self.frame_id = 0
        self.output_dir = 'calib_images'
        os.makedirs(self.output_dir, exist_ok=True)
        self.save_image_flag = False
        self.timer = self.create_timer(3.0, self.enable_image_saving)
        self.get_logger().info('ImageSaver node started. Saving an image every 3 seconds.')

    def enable_image_saving(self):
        self.save_image_flag = True

    def listener_callback(self, msg):
        if not self.save_image_flag:
            return
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        except Exception as e:
            self.get_logger().error(f'Image conversion failed: {e}')
            return
        filename = os.path.join(self.output_dir, f'image_{self.frame_id:05d}.png')
        cv2.imwrite(filename, cv_image)
        self.get_logger().info(f'Saved: {filename}')
        self.frame_id += 1
        self.save_image_flag = False

def main(args=None):
    rclpy.init(args=args)
    node = ImageSaver()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()

if __name__ == '__main__':
    main()

note

This node does not detect AprilTags, it only saves raw images for calibration.

Step 2: Calibrate the Camera

Once you've saved about 15-20 images from different angles, run the following script to compute the camera matrix and distortion coefficients.

calibrate_camera.py

import cv2
import numpy as np
import glob

# 1. Prepare object points for calibration pattern
pattern_size = (7, 6)  # (width, height) = (# inner corners)
square_size = 0.025  # in meters

objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2)
objp *= square_size

objpoints = []
imgpoints = []

# 2. Load images and find corners
images = glob.glob('calib_images/*.png')
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if not ret:
        continue
    corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
                                criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001))
    objpoints.append(objp)
    imgpoints.append(corners2)

# 3. Calibrate
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

# 4. Extract intrinsics
fx = mtx[0, 0]
fy = mtx[1, 1]
cx = mtx[0, 2]
cy = mtx[1, 2]

print("RMS re-projection error:", ret)
print(f"fx={fx:.2f}, fy={fy:.2f}, cx={cx:.2f}, cy={cy:.2f}")
print("Camera matrix:\n", mtx)
print("Distortion coefficients:\n", dist)

Step 3: Undistort Images

Use the calibration results to undistort your images before detection:

import cv2

# Load the image previously used
img = cv2.imread("test_image.png")

# Get optimal new camera matrix
h, w = img.shape[:2]
new_K, _ = cv2.getOptimalNewCameraMatrix(K, D, (w, h), 1, (w, h))

# Undistort the image
undistorted = cv2.undistort(img, K, D, None, new_K)

Step 4: Run AprilTag Detection with Intrinsics

Make sure you have installed the AprilTag Python package:

pip install dt-apriltags

Then run detection:

from dt_apriltags import Detector
import cv2

# Initialize detector
detector = Detector(families='tag36h11')

# Convert to grayscale
gray = cv2.cvtColor(undistorted, cv2.COLOR_BGR2GRAY)

# Camera intrinsics for pose estimation
fx = K[0, 0]
fy = K[1, 1]
cx = K[0, 2]
cy = K[1, 2]

# Run detection
tags = detector.detect(
   gray,
   estimate_tag_pose=True,
   camera_params=(fx, fy, cx, cy),
   tag_size=0.05  # in meters
)

tip

Adjust the tag_size parameter to match the actual size of your printed AprilTag (the black square), in meters.

Print or Use the Detection Results

for tag in tags:
    print(f"Detected tag ID: {tag.tag_id}")
    print("Translation (t):", tag.pose_t.flatten())
    print("Rotation matrix (R):\n", tag.pose_R)

This gives you the full 6DOF pose (6 Degrees of Freedom), including:

• 3D Position: X (left/right), Y (up/down), Z (forward/back)
• 3D Orientation: Roll, Pitch, Yaw (rotation around each axis)

Conclusion

The pose returned by the AprilTag detector (pose_t and pose_R) gives the position and orientation of the tag relative to the camera. This information can be used for:

• Real-time localization of the ROV
• Station-keeping near a tag
• Navigation relative to visual landmarks
• Sensor fusion with IMU or depth sensors

info

The camera calibration step is essential for accurate pose estimation. The values fx, fy, cx, and cy from the calibration script must be passed to the AprilTag detector as camera_params. Without them, the detector can still find tags but cannot estimate pose reliably.