Vision-Based Pose Estimation with AprilTags
Recovering 6-DOF pose of a nano-quadrotor from a single monocular camera using AprilTag fiducials. The CV part is short; the calibration and filtering are where the real work lives.
The goal: estimate the position and orientation of a small quadrotor flying over a workspace, from a single overhead camera, in real time. The setup uses a grid of AprilTag fiducial markers laid out in known world coordinates — the tags are how the system bootstraps from "pixels in an image" to "where the robot is in 3D."
Why AprilTags
Indoor environments don't have GPS. Motion capture rigs work beautifully but cost more than the robot. AprilTags hit the sweet spot — they're a square printed pattern with a unique ID encoded in the inner bits, designed to be detected robustly under lighting variation, perspective distortion, and partial occlusion. You print them, glue them to a surface, and any camera that sees one knows where it is relative to the tag.
For pose estimation, AprilTags have a key property: the four corners of the square are precisely localisable to sub-pixel accuracy. That gives you four well-conditioned point correspondences per detected tag, which is enough to solve for the camera's pose relative to the tag.
The pipeline
1 — Detect
Each frame goes through adaptive thresholding, quad detection, and decoding. The output is a list of detected tags, each with a unique ID and four sub-pixel corner coordinates in the image. MATLAB's Computer Vision Toolbox wraps this in a clean interface; the implementation under the hood is the standard AprilTag detector.
2 — Look up world coordinates
Each tag ID maps to a known position on the workspace, established once at
setup time. So a detection of tag id=7 tells you the four image
points and the four corresponding world points — a 2D ↔ 3D correspondence
ready to solve.
3 — Recover pose (PnP)
With four correspondences per tag and a calibrated camera (intrinsics
K, distortion coefficients), the camera-to-tag transform is a
perspective-n-point problem. Solving it gives you the rotation matrix
R and translation t from camera to tag:
x_image = K · [ R | t ] · x_world
Invert that transform and you have the camera pose in the tag's frame. Chain through the (known) tag-to-world transform and you have the camera in the world frame — and since the camera is rigidly mounted on the quadrotor, the quadrotor pose too.
4 — Fuse multiple tags
When multiple tags are in view simultaneously, each one gives an independent pose estimate. Fusing them — weighted by the tag's distance, detection confidence, and image area — produces a single estimate that's noticeably more stable than any single-tag estimate. Tags far from image center are downweighted (lens distortion errors dominate there).
Calibration
Camera intrinsics aren't optional. The PnP solution depends linearly on
K, and a 1% error in focal length translates almost directly
into a 1% error in metric pose. The standard checkerboard calibration
procedure took care of intrinsics; distortion coefficients (5-parameter
Brown-Conrady) corrected the lens curvature.
World coordinates of the tag corners come from measuring the tag layout once at setup. The accuracy of pose estimation is bounded by how accurately those world points are known — a millimetre of error in the tag layout shows up directly in the pose estimate.
Filtering
Frame-to-frame pose jitters by a few millimetres / fractions of a degree even with perfect detection — corner noise, image discretisation, nonlinearities in PnP. A simple low-pass filter on position (and SLERP on orientation quaternion) cleans this up at the cost of a small lag.
For real flight use, a full Kalman or extended Kalman filter integrating the visual pose with IMU would be the next step. For this project, the low-pass was enough to demonstrate the visual estimate worked.
What worked
Single-tag detection gives a useful pose at ~30 Hz with the camera I used. Multi-tag fusion brings the noise down by roughly a factor of two and is more robust to occlusions and bad detections at frame edges. The bottleneck in metric accuracy ends up being the world-coordinate calibration of the tag layout, not the detection or PnP solution itself.