Extrinsics Math

Notation

Symbol	Meaning
$p_{\text{lidar}}$	3D point in the LiDAR / vehicle frame (metres): $x$ forward, $y$ left, $z$ up
$p_{\text{cam}}$	Same point in the OpenCV camera frame (metres): $x$ right, $y$ down, $z$ forward
$R, t$	Rigid LiDAR→camera map: $p_{\text{cam}} = R\,p_{\text{lidar}} + t$ . $R\in\mathrm{SO}(3)$ , $t\in\mathbb{R}^3$
$K_{\text{runtime}}$	$3\times3$ camera matrix for the pixels in the pose images (undistorted; right cam already $180^\circ$ )
$P$	$3\times4$ projection matrix written to `params.yaml`: $P = K_{\text{runtime}}[R \mid t]$
$X_i$	$i$ -th checkerboard corner in the board frame ( $z=0$ ), spaced by `square_m`
$x_i^{\text{cam}}$	That corner expressed in the camera frame (metres)
$x_i^{\text{lidar}}$	The same corner recovered from the LiDAR board rectangle (metres)
$u^{\text{img}}$	Detected corner in image pixels (runtime frame)
$n, d$	Plane $n\cdot x + d = 0$ with unit normal $n$
$n_c, d_c$	Board plane in the camera frame
$u, v$ (axes)	Orthonormal in-plane axes of the fitted board rectangle (metres); not image pixels
Pose $k$	One synced capture: runtime image + `cloud.npy`

Homogeneous projection into pixels is written $\pi(P\tilde{x}) = (p_x/p_z,\, p_y/p_z)$ with $\tilde{x}=(x,y,z,1)^\top$ . More on $P$ and $K_{\text{runtime}}$ : projection math.

Overview

Estimate $(R,t)$ from multi-pose board observations, then write $P = K_{\text{runtime}}[R \mid t]$ .

Correspondences are not clicked. OpenCV recovers ordered chessboard corners on the camera; LiDAR recovers the same corners from a board-sized plane fit. Multi-pose consensus resolves planar labeling ambiguity that a single pose cannot.

Camera side (per pose)

Images are already in SeeCam runtime space. Detection:

findChessboardCorners + cornerSubPix on a $(9,6)$ internal-corner grid (cols $\times$ rows).
Object points $X_i$ lie in the board frame ( $z=0$ ), spaced by square_m.
solvePnP with $K_{\text{runtime}}$ (distortion coeffs unused; image already undistorted) yields the board pose in the camera frame: rotation $R_b$ and translation $t_b$ , so a board point maps as $x = R_b X + t_b$ .

Camera-frame pattern corners and the board plane:

x_i^{\text{cam}} = R_b X_i + t_b, \qquad n_c = R_b\begin{bmatrix}0\\0\\1\end{bmatrix}, \quad d_c = -n_c^\top t_b.

So $n_c$ is the board normal in the camera frame (third column of $R_b$ ), and the plane through the board is $n_c\cdot x + d_c = 0$ . pose_to_plane is that conversion. If the board is not found, the pose is skipped.

LiDAR side (per pose)

Input is a dense cloud (cloud.npy from /lidar_points), not /cones.

Soft radial gate (script defaults about $0.5$ to $12\,\text{m}$ from the LiDAR origin) and voxel downsample for discovery.
Iterative RANSAC peels dominant planes $n\cdot x + d = 0$ (inlier distance about $3\,\text{cm}$ ), then refines each plane with SVD on its inliers.
For each plane, project inliers into an orthonormal basis on that plane and fit a rectangle of known outer size (board_width_m $\times$ board_height_m).

Rectangle fit sweeps the in-plane angle (about $2^\circ$ steps). For each angle it measures percentile extents of the projected points, checks size within tolerance (about $25\%$ ), and scores fill fraction: fraction of inliers inside the known-size box centered on the observed extent. Candidates need enough inliers (about ≥40) and fill ≥ about $0.35$ . Rank by fill; later consensus uses only the top-ranked footprint.

From the fitted rectangle center and in-plane axes $u,v$ (unit vectors in metres, lying in the LiDAR plane), rebuild ordered pattern corners $x_i^{\text{lidar}}$ (pattern_corners_from_board_rectangle). The printed checker is assumed centered on the board (plus optional offsets in BoardSpec), with the same row-major OpenCV ordering as board_object_points / the $X_i$ above.

Orientation ambiguity

A rectangle has eight discrete corner labelings: 4 cyclic rotations × reflection (rectangle_corner_permutations). Each labeling produces a different ordered set $\{x_i^{\text{lidar}}\}$ .

For a single pose, Kabsch aligning $\{x_i^{\text{lidar}}\}$ to $\{x_i^{\text{cam}}\}$ often has near-zero RMSE even for a wrong $180^\circ$ flip of the board in its plane. Local 3D error cannot break that symmetry. The code therefore keeps all size-consistent orientations with low local RMSE (about $<0.15\,\text{m}$ mean error on pattern corners), not only the single best.

initial_rt is a nominal seed: fixed LiDAR→OpenCV axis map, about $14^\circ$ pitch, and a hand-set camera position in the LiDAR frame. The multi-pose solve does not depend on its accuracy.

Consensus across poses

Need ≥3 valid poses. Build a pool of candidate global transforms $(R_h, t_h)$ from the per-pose orientation hypotheses. For each candidate, and for each pose $k$ , pick the orientation of that pose’s LiDAR pattern that best matches under the same global map:

e_k(R_h,t_h) = \min_{\text{orientation}} \frac{1}{N}\sum_{i=1}^{N} \big\| R_h\,x_{k,i}^{\text{lidar}} + t_h - x_{k,i}^{\text{cam}} \big\|,

where $N$ is the number of internal corners (54 for a $9\times6$ grid), $x_{k,i}^{\text{lidar}}$ are that orientation’s LiDAR corners for pose $k$ , and $x_{k,i}^{\text{cam}}$ are the camera-frame corners from solvePnP on pose $k$ .

Accept a hypothesis only if every pose’s best $e_k$ stays below a few centimetres (script headroom about $0.12\,\text{m}$ ). Among survivors, pick the lowest $\sum_k e_k$ . That locks one oriented LiDAR pattern per pose.

Scoring applies the candidate global $(R_h,t_h)$ to each pose’s corners. It does not ask whether that pose’s own one-shot Kabsch rotation equals $R_h$ . A wrong flip can look great locally and still fail this global check.

Stacked Kabsch refine

With consensus labels fixed, stack every pose’s LiDAR corners into one source cloud and every matching camera corner into one destination cloud. Re-estimate a single rigid map (orthogonal Procrustes / Kabsch). Below, the sums run over all stacked correspondences (all poses, all corners):

\begin{aligned} \mu_s &= \text{mean of all } x^{\text{lidar}}, & \mu_d &= \text{mean of all } x^{\text{cam}}, \\ H &= \sum (x^{\text{lidar}}-\mu_s)(x^{\text{cam}}-\mu_d)^\top, \\ H &= U\Sigma V^\top \quad\text{(SVD)}, & R &= V U^\top \quad(\text{if }\det R < 0,\text{ flip the last row of }V^\top), \\ t &= \mu_d - R\mu_s. \end{aligned}

Here $H$ is the $3\times3$ cross-covariance used by Kabsch, not a homogeneous transform. The resulting $(R,t)$ is what enters $P = K_{\text{runtime}}[R \mid t]$ .

Observability

Even a clean Kabsch fit is under-constrained if every board pose is nearly the same orientation and position. assess_observability looks at the LiDAR planes $(n_\ell, d_\ell)$ across poses:

Max pairwise angle between normals $n_\ell$ (using $|\cos\theta|$ so sign flips do not matter) ≥ about $15^\circ$ .
Spread of plane points $-d_\ell n_\ell$ (a point on each plane along its normal): Euclidean std of those 3D points ≥ about $0.15\,\text{m}$ .

Poses with parallel normals at similar range fail observability.

Acceptance (pixel)

After $(R,t)$ is fixed, project every consensus LiDAR pattern corner and compare 1:1 to the detected image corner (structured_board_reprojection_error):

e_i = \big\| \pi\!\big(K_{\text{runtime}}[R \mid t]\,\tilde{x}_i^{\text{lidar}}\big) - u_i^{\text{img}} \big\|_2,

with $\tilde{x}_i^{\text{lidar}} = (x,y,z,1)^\top$ and $u_i^{\text{img}}$ the corresponding OpenCV corner in pixels.

Report mean and p95 of $e_i$ over all corners and poses. Script write gate: mean ≤ 2 px, p95 ≤ 4 px.

This is stricter than nearest-neighbor matching between projected cloud points and chessboard edges. Wrong orientation consensus can pass a soft nearest-neighbor check and still fail the structured gate.

LiDAR input

Plane peel and rectangle fill need many returns on the board face. The live cone stack publishes sparse centroids (interfaces/msg/Points on /cones); that representation cannot support this solve.

Procedure and sync gates: calibration procedure. Runtime $P$ and right-cam $180^\circ$ : projection math. Prod click path: legacy DLT.