A perception sensor produces measurements in its own native coordinate system: pixels in a camera, ranges in a LiDAR, angular rates in an IMU. For a robot to act on these measurements, it needs a mapping from measurements to physical geometry. Calibration provides this mapping.

Conceptually, calibration estimates parameters \( \boldsymbol{\theta} \) of a measurement model \( \mathbf{z} = h(\mathbf{x};\boldsymbol{\theta}) + \boldsymbol{\eta} \), where \( \mathbf{x} \) is the physical quantity of interest (e.g., 3D point), \( \mathbf{z} \) is the sensor output (e.g., pixel), and \( \boldsymbol{\eta} \) is noise.

flowchart TD
  A["Collect data with known geometry"] --> B["Choose sensor model"]
  B --> C["Separate parameters"]
  C --> C1["Intrinsic (internal)"]
  C --> C2["Extrinsic (pose wrt robot/world)"]
  C1 --> D["Solve estimation problem"]
  C2 --> D
  D --> E["Validate / compute reprojection error"]
  E --> F["Deploy calibrated model"]
        

Intrinsic parameters describe how a sensor maps physical rays to measurement coordinates, independent of where the sensor sits on the robot. For a pinhole camera, intrinsics include focal lengths and principal point.

Extrinsic parameters describe the rigid pose (position and orientation) of the sensor relative to another reference (robot body or world). They are needed to relate multiple sensors or to fuse perception with motion.

flowchart TD
  P3["3D point in world"] --> |"Extrinsic: R,t"| Pc["3D point in camera coords"]
  Pc --> |"Intrinsic: K + distortion"| p2["Pixel measurement"]
        

In symbols, the overall mapping is decomposed as:

\[ \mathbf{p} \;=\; \Pi\!\left(\mathbf{K},\boldsymbol{\kappa}, \;\mathbf{R},\mathbf{t},\;\mathbf{P}\right), \]

where \( \mathbf{P} \in \mathbb{R}^3 \) is a 3D point, \( (\mathbf{R},\mathbf{t}) \) are extrinsics, \( \mathbf{K} \) is the intrinsic matrix, and \( \boldsymbol{\kappa} \) are distortion coefficients.

We model the camera as a pinhole with a 3D point mapped to the image plane by perspective projection. Let a point in camera coordinates be \( \mathbf{P}_c = [X_c, Y_c, Z_c]^T \), with \( Z_c > 0 \).

The normalized image coordinates are:

\[ x_n = \frac{X_c}{Z_c}, \qquad y_n = \frac{Y_c}{Z_c}. \]

The intrinsic matrix \( \mathbf{K} \in \mathbb{R}^{3\times 3} \) is

\[ \mathbf{K} = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}, \]

where \( f_x, f_y \) are focal lengths in pixel units, \( (c_x,c_y) \) is the principal point, and \( s \) is skew (usually near zero).

Pixel coordinates (undistorted) are:

\[ \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} x_n \\ y_n \\ 1 \end{bmatrix}. \]

Extrinsics relate the point’s coordinates in a reference system (e.g., robot/world) to the camera coordinate system via a rigid transform:

\[ \mathbf{P}_c = \mathbf{R}\mathbf{P} + \mathbf{t}, \]

where \( \mathbf{R}\in SO(3) \) is a rotation matrix and \( \mathbf{t}\in\mathbb{R}^3 \) is a translation vector. We only assume basic linear algebra properties here; full frame-tree concepts come in Chapter 9.

In a single compact equation, the ideal (undistorted) projection is:

\[ \lambda \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K}\, \left[\mathbf{R}\;|\;\mathbf{t}\right] \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}, \qquad \lambda = Z_c. \]

Intrinsics are shared across all images (for a fixed camera), while extrinsics differ per image or per sensor mounting.

Real lenses deviate from pinhole geometry. A standard model adds radial and tangential distortions. Let \( r^2 = x_n^2 + y_n^2 \).

Radial distortion:

\[ x_r = x_n\left(1 + k_1 r^2 + k_2 r^4 + k_3 r^6\right),\quad y_r = y_n\left(1 + k_1 r^2 + k_2 r^4 + k_3 r^6\right). \]

Tangential distortion:

\[ \begin{aligned} x_d &= x_r + 2p_1 x_n y_n + p_2\left(r^2 + 2x_n^2\right), \\ y_d &= y_r + p_1\left(r^2 + 2y_n^2\right) + 2p_2 x_n y_n. \end{aligned} \]

Finally, distorted pixels are: \( [u_d, v_d, 1]^T = \mathbf{K}[x_d, y_d, 1]^T \).

Suppose we observe \( N \) images of a known calibration target (e.g., a planar checkerboard). Let \( \mathbf{P}_{ij} \) be the \( j \)-th known 3D target point in the reference system of image \( i \), and \( \mathbf{p}_{ij}^{\text{obs}} \) be its measured pixel.

Define the reprojection function \( \hat{\mathbf{p}}_{ij}(\boldsymbol{\theta}) \) using Sections 3–5. Calibration estimates \( \boldsymbol{\theta} = (\mathbf{K},\boldsymbol{\kappa},\{\mathbf{R}_i,\mathbf{t}_i\}_{i=1}^N) \) by minimizing reprojection error:

\[ \min_{\boldsymbol{\theta}} \sum_{i=1}^N\sum_{j=1}^M \left\| \mathbf{p}_{ij}^{\text{obs}} - \hat{\mathbf{p}}_{ij}(\boldsymbol{\theta}) \right\|_2^2. \]

This is a nonlinear least-squares problem (because of division by \( Z_c \) and distortion). We solve it by iterative methods such as Gauss–Newton or Levenberg–Marquardt.

When the calibration target is planar (say \( Z=0 \)), the mapping becomes a homography between plane coordinates and the image. Let target coordinates be \( \mathbf{X}=[X,Y,1]^T \).

From Section 4, for \( Z=0 \):

\[ \lambda \mathbf{p} = \mathbf{K} \left[\mathbf{r}_1\;\mathbf{r}_2\;\mathbf{t}\right] \mathbf{X} = \mathbf{H}\mathbf{X}, \]

where \( \mathbf{H} \) is a 3×3 homography matrix.

Claim: Each image provides two independent linear constraints on \( \mathbf{K} \).

Sketch: Let \( \mathbf{h}_1,\mathbf{h}_2,\mathbf{h}_3 \) be columns of \( \mathbf{H} \). We have:

\[ \mathbf{h}_1 = \lambda\mathbf{K}\mathbf{r}_1,\quad \mathbf{h}_2 = \lambda\mathbf{K}\mathbf{r}_2. \]

Because \( \mathbf{r}_1 \) and \( \mathbf{r}_2 \) are orthonormal columns of a rotation: \( \mathbf{r}_1^T\mathbf{r}_2=0 \) and \( \|\mathbf{r}_1\|^2=\|\mathbf{r}_2\|^2 \).

Substitute \( \mathbf{r}_k = \lambda^{-1}\mathbf{K}^{-1}\mathbf{h}_k \):

\[ \mathbf{h}_1^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h}_2 = 0, \qquad \mathbf{h}_1^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h}_1 = \mathbf{h}_2^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h}_2. \]

Let \( \mathbf{B} = \mathbf{K}^{-T}\mathbf{K}^{-1} \). Then each image yields:

\[ \mathbf{h}_1^T\mathbf{B}\mathbf{h}_2 = 0, \qquad \mathbf{h}_1^T\mathbf{B}\mathbf{h}_1 - \mathbf{h}_2^T\mathbf{B}\mathbf{h}_2 = 0. \]

These are linear in the six independent entries of symmetric matrix \( \mathbf{B} \). With enough images (typically \( N \ge 3 \)), we solve for \( \mathbf{B} \), then recover \( \mathbf{K} \) by Cholesky factorization. Nonlinear refinement then adds distortion and improves accuracy.

A parameter is observable if different values of it would change the predicted measurements. Poor motion or poor target coverage may make parameters weakly observable.

Common accuracy metric is mean reprojection error:

\[ \epsilon_{\text{rms}} = \sqrt{ \frac{1}{NM} \sum_{i=1}^N\sum_{j=1}^M \left\| \mathbf{p}_{ij}^{\text{obs}} - \hat{\mathbf{p}}_{ij}(\boldsymbol{\theta}) \right\|_2^2 }. \]

If the noise is i.i.d. Gaussian with covariance \( \sigma^2\mathbf{I} \), least-squares is also a maximum-likelihood estimator.

We assume you captured multiple checkerboard images. The code estimates intrinsics \( \mathbf{K} \), distortion \( \boldsymbol{\kappa} \), and per-image extrinsics.

import cv2
import numpy as np
import glob

# Checkerboard settings
pattern_size = (9, 6)  # inner corners (cols, rows)
square_size = 0.025    # meters

# Prepare known 3D points on plane Z=0
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 = []  # 3D target points in reference coords
imgpoints = []  # detected 2D points

images = glob.glob("calib_images/*.png")

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ok, corners = cv2.findChessboardCorners(gray, pattern_size)

    if ok:
        # refine corners
        corners = cv2.cornerSubPix(
            gray, corners, (11,11), (-1,-1),
            (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)
        )
        objpoints.append(objp)
        imgpoints.append(corners)

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

print("RMS reprojection error:", ret)
print("Intrinsic matrix K:\n", K)
print("Distortion coefficients:\n", dist.ravel())

# Example: get extrinsic for first view
R0, _ = cv2.Rodrigues(rvecs[0])
t0 = tvecs[0]
print("Extrinsic (first image):")
print("R0=\n", R0)
print("t0=\n", t0)
      

Libraries used: opencv-python, numpy. OpenCV implements Zhang-style planar calibration plus nonlinear refinement.

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>

int main() {
    cv::Size patternSize(9, 6);
    float squareSize = 0.025f;

    std::vector<std::vector<cv::Point3f>> objpoints;
    std::vector<std::vector<cv::Point2f>> imgpoints;

    // Prepare planar object points
    std::vector<cv::Point3f> objp;
    for (int y = 0; y < patternSize.height; ++y)
        for (int x = 0; x < patternSize.width; ++x)
            objp.emplace_back(x*squareSize, y*squareSize, 0.0f);

    std::vector<cv::String> images;
    cv::glob("calib_images/*.png", images);

    cv::Size imageSize;

    for (const auto& fname : images) {
        cv::Mat img = cv::imread(fname, cv::IMREAD_COLOR);
        imageSize = img.size();
        cv::Mat gray;
        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);

        std::vector<cv::Point2f> corners;
        bool ok = cv::findChessboardCorners(gray, patternSize, corners);

        if (ok) {
            cv::cornerSubPix(
                gray, corners, cv::Size(11,11), cv::Size(-1,-1),
                cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 1e-6)
            );
            objpoints.push_back(objp);
            imgpoints.push_back(corners);
        }
    }

    cv::Mat K, dist;
    std::vector<cv::Mat> rvecs, tvecs;
    double rms = cv::calibrateCamera(objpoints, imgpoints, imageSize, K, dist, rvecs, tvecs);

    std::cout << "RMS error: " << rms << std::endl;
    std::cout << "K=\n" << K << std::endl;
    std::cout << "dist=\n" << dist << std::endl;

    return 0;
}
      

Libraries used: OpenCV C++ API. (Eigen is optional here for later pose algebra.)

import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;

import java.util.*;

public class CameraCalib {
    static { System.loadLibrary(Core.NATIVE_LIBRARY_NAME); }

    public static void main(String[] args) {
        Size patternSize = new Size(9,6);
        double squareSize = 0.025;

        List<Mat> objpoints = new ArrayList<>();
        List<Mat> imgpoints = new ArrayList<>();

        // Prepare planar object points
        MatOfPoint3f objp = new MatOfPoint3f();
        List<Point3> pts3 = new ArrayList<>();
        for (int y=0; y<patternSize.height; y++)
            for (int x=0; x<patternSize.width; x++)
                pts3.add(new Point3(x*squareSize, y*squareSize, 0.0));
        objp.fromList(pts3);

        List<String> images = Arrays.asList("calib_images/1.png","calib_images/2.png"); // extend
        Size imageSize = null;

        for (String fname : images) {
            Mat img = Imgcodecs.imread(fname);
            imageSize = img.size();
            Mat gray = new Mat();
            Imgproc.cvtColor(img, gray, Imgproc.COLOR_BGR2GRAY);

            MatOfPoint2f corners = new MatOfPoint2f();
            boolean ok = Calib3d.findChessboardCorners(gray, patternSize, corners);

            if (ok) {
                objpoints.add(objp);
                imgpoints.add(corners);
            }
        }

        Mat K = Mat.eye(3,3, CvType.CV_64F);
        Mat dist = Mat.zeros(8,1, CvType.CV_64F);
        List<Mat> rvecs = new ArrayList<>();
        List<Mat> tvecs = new ArrayList<>();

        double rms = Calib3d.calibrateCamera(objpoints, imgpoints, imageSize, K, dist, rvecs, tvecs);

        System.out.println("RMS error: " + rms);
        System.out.println("K:\n" + K.dump());
        System.out.println("dist:\n" + dist.dump());
    }
}
      

Libraries used: OpenCV Java bindings. In robotics Java stacks, this is often wrapped inside ROS-Java or custom perception nodes.

MATLAB Computer Vision Toolbox provides high-level calibration while still exposing the same math.

% Checkerboard settings
patternSize = [9, 6];
squareSize = 25; % mm

% Detect corners
images = imageDatastore('calib_images');
[imagePoints, boardSize] = detectCheckerboardPoints(images.Files);

% Generate world points on plane Z=0
worldPoints = generateCheckerboardPoints(boardSize, squareSize);

% Read one image to get size
I = readimage(images,1);
imageSize = [size(I,1), size(I,2)];

% Calibrate
params = estimateCameraParameters(imagePoints, worldPoints, 'ImageSize', imageSize);

% Results
K = params.IntrinsicMatrix';
dist = [params.RadialDistortion, params.TangentialDistortion];
rms = params.MeanReprojectionError;

disp(K);
disp(dist);
disp(rms);
      

In Simulink, the equivalent pipeline uses blocks: Image From File → Detect Checkerboard → Estimate Camera Parameters. The parameters feed into downstream perception blocks (e.g., undistort, pose estimation).

Problem 1 (Intrinsic Matrix Structure): Show that if pixels are square and axes are orthogonal, then the intrinsic matrix must satisfy \( f_x = f_y \) and \( s = 0 \).

Solution:

Square pixels mean the physical scaling in x and y is equal, so the same focal length in meters maps to equal pixel scalings: \( f_x = f_y = f \). Orthogonal pixel axes imply no shear between x and y directions, so skew \( s=0 \). Therefore:

\[ \mathbf{K} = \begin{bmatrix} f & 0 & c_x \\ 0 & f & c_y \\ 0 & 0 & 1 \end{bmatrix}. \]

Problem 2 (Projection Consistency): Suppose \( \mathbf{P}_c = [X_c,Y_c,Z_c]^T \) with \( Z_c > 0 \). Prove that all points on a ray \( \alpha \mathbf{P}_c \) for \( \alpha > 0 \) project to the same normalized coordinates \( (x_n,y_n) \).

Solution:

For scaled point \( \mathbf{P}'_c = \alpha\mathbf{P}_c \):

\[ x_n' = \frac{\alpha X_c}{\alpha Z_c} = \frac{X_c}{Z_c} = x_n,\qquad y_n' = \frac{\alpha Y_c}{\alpha Z_c} = \frac{Y_c}{Z_c} = y_n. \]

Hence perspective projection is invariant along rays through the pinhole.

Problem 3 (Least Squares Form): Let \( \mathbf{e}_{ij}(\boldsymbol{\theta}) = \mathbf{p}_{ij}^{\text{obs}} - \hat{\mathbf{p}}_{ij}(\boldsymbol{\theta}) \). Derive the normal equations for a Gauss–Newton step \( \Delta\boldsymbol{\theta} \).

Solution:

Linearize around current \( \boldsymbol{\theta}_0 \):

\[ \mathbf{e}_{ij}(\boldsymbol{\theta}_0 + \Delta\boldsymbol{\theta}) \approx \mathbf{e}_{ij}(\boldsymbol{\theta}_0) - \mathbf{J}_{ij}\Delta\boldsymbol{\theta}, \]

where \( \mathbf{J}_{ij} = \frac{\partial \hat{\mathbf{p}}_{ij}}{\partial \boldsymbol{\theta}} \big|_{\boldsymbol{\theta}_0} \). Stack all residuals into \( \mathbf{e} \) and Jacobians into \( \mathbf{J} \). Then minimize \( \|\mathbf{e} - \mathbf{J}\Delta\boldsymbol{\theta}\|_2^2 \). The normal equations are:

\[ \mathbf{J}^T\mathbf{J}\Delta\boldsymbol{\theta} = \mathbf{J}^T\mathbf{e}. \]

Problem 4 (Homography Constraints Count): For planar calibration (Section 7), how many images are minimally needed to solve for intrinsics in the noiseless case? Justify.

Solution:

The symmetric matrix \( \mathbf{B} \) has 6 unknowns. Each image yields 2 independent linear constraints: \( \mathbf{h}_1^T\mathbf{B}\mathbf{h}_2=0 \) and \( \mathbf{h}_1^T\mathbf{B}\mathbf{h}_1= \mathbf{h}_2^T\mathbf{B}\mathbf{h}_2 \). Therefore we need at least \( 3 \) images to get 6 equations. In practice we use many more to handle noise and observability.

Calibration separates a sensor’s internal geometry (intrinsics) from its placement on the robot (extrinsics). Using the pinhole model plus distortion, we formulated calibration as a nonlinear least-squares problem and showed why planar targets allow a useful linear initialization. The calibrated model is foundational for feature extraction, fusion, and perception pipelines in later lessons.