Camera and IMU calibration

Example 1 Measuring distance on a landing pad from an oblique image

The four corners of a 10 × 20 m pad appear at these pixel positions. We want the ground distance between two points seen in the image (simulated data).

import numpy as np

img_pts = np.array([[412, 830], [1508, 812], [1320, 402], [590, 410.]])   # pixels
ground = np.array([[0, 0], [10, 0], [10, 20], [0, 20.]])                 # metres

def dlt(src, dst):
    rows = []
    for (x, y), (u, v) in zip(src, dst):
        rows.append([-x, -y, -1, 0, 0, 0, u * x, u * y, u])
        rows.append([0, 0, 0, -x, -y, -1, v * x, v * y, v])
    _, _, vt = np.linalg.svd(np.array(rows))
    h = vt[-1].reshape(3, 3)
    return h / h[2, 2]

def to_ground(H, p):
    q = H @ np.r_[p, 1.0]
    return q[:2] / q[2]

H = dlt(img_pts, ground)
a, b = to_ground(H, [960, 700]), to_ground(H, [960, 450])
print("point A on ground (m):", np.round(a, 2))
print("point B on ground (m):", np.round(b, 2))
print(f"distance A-B: {np.linalg.norm(a - b):.2f} m, while in the image they are {700 - 450} px apart")
print("check corner:", np.round(to_ground(H, img_pts[2]), 3))

point A on ground (m): [4.98 4.3 ]
point B on ground (m): [ 5.03 16.97]
distance A-B: 12.67 m, while in the image they are 250 px apart
check corner: [10. 20.]

The two points are 250 pixels apart in the image but more than 12 m apart on the ground, because the top of the image is farther from the camera, so one pixel there covers more ground. A homography applies only to points on the same plane; objects with height, such as poles, project to the wrong place.

Example 2 Finding the time offset with cross-correlation

The gyro rotation rate and the rotation rate estimated from images (sampled at 200 Hz), with the image signal 35 ms late (simulated data).

import numpy as np

fs = 200
t = np.arange(0, 10, 1 / fs)
gyro = np.sin(2 * np.pi * 0.7 * t) + 0.5 * np.sin(2 * np.pi * 1.9 * t + 1)
TRUE_OFFSET = 0.035                              # s
cam = np.interp(t - TRUE_OFFSET, t, gyro) + np.random.default_rng(1).normal(0, 0.05, t.size)

lags = np.arange(-40, 41)                        # samples = ±200 ms
score = [np.dot(gyro[50:-50], np.roll(cam, -lag)[50:-50]) for lag in lags]
est = lags[int(np.argmax(score))] / fs
print(f"estimated offset {est * 1000:.0f} ms (true {TRUE_OFFSET * 1000:.0f} ms, resolution {1000 / fs:.0f} ms)")
rate = 60                                        # deg/s while yawing
print(f"at {rate} deg/s an uncorrected offset of {TRUE_OFFSET * 1000:.0f} ms means {rate * TRUE_OFFSET:.1f} deg of heading error")

estimated offset 40 ms (true 35 ms, resolution 5 ms)
at 60 deg/s an uncorrected offset of 35 ms means 2.1 deg of heading error

The estimate is within one sample of the true value; the resolution depends on the sampling rate and can be refined by interpolating between samples. The offset looks small, but during fast yaw the computed heading is wrong by degrees, enough to degrade visual navigation.