Rig-to-Pattern Hand–Eye Calibration
Robust estimation of $T_{RP}$ from synchronized motion capture and visual pattern tracking.
1. Problem Setup
A rigid body (rig) is tracked by an external motion-capture system, which reports the rig pose in a world frame at each time index $i$. Attached rigidly to the rig is a calibration pattern (e.g., checkerboard, ChArUco), observed by a static camera fixed in the world. Per-frame PnP on the pattern returns the pattern pose in the camera frame. The transform between the rig frame $R$ and the pattern frame $P$ is unknown but constant; recovering it is the calibration goal.
Coordinate frames.
- $W$ – mocap world frame
- $R$ – rig frame (mocap-tracked rigid body)
- $P$ – pattern frame (rigidly attached to $R$)
- $C$ – camera frame (fixed in $W$)
Per-frame observations. At time $i = 1, \ldots, N$:
\[A_i \;:=\; T_{WR}^{(i)} \;\in\; \mathrm{SE}(3) \quad \text{(rig in world, from mocap)} \tag{1}\]
\[B_i \;:=\; T_{CP}^{(i)} \;\in\; \mathrm{SE}(3) \quad \text{(pattern in camera, from PnP)} \tag{2}\]
Unknowns (constant in $i$).
\[X \;:=\; T_{RP} \;\in\; \mathrm{SE}(3) \quad \text{(pattern in rig -- the goal)} \tag{3}\]
\[Y \;:=\; T_{WC} \;\in\; \mathrm{SE}(3) \quad \text{(camera in world -- nuisance)} \tag{4}\]
Constraint. The pattern-in-world pose $T_{WP}$ at time $i$ may be expressed via either chain:
\[T_{WP}^{(i)} \;=\; T_{WR}^{(i)} \, T_{RP} \;=\; T_{WC}^{(i)} \, T_{CP}^{(i)}.\]
Since $T_{WC}$ is constant ($Y$) and $T_{RP}$ is constant ($X$), we obtain the classical robot–world / hand–eye constraint
\[\boxed{\; A_i\, X \;=\; Y\, B_i, \qquad i = 1, \ldots, N \;} \tag{5}\]
holding simultaneously for all $i$.
Parameterization. Write each pose as $E = (\mathbf{R}, \mathbf{t})$ with action $E(\boldsymbol{x}) = \mathbf{R} \boldsymbol{x} + \mathbf{t}$. Then (5) decouples into a rotation equation and a translation equation:
\[\mathbf{R}_{A_i} \mathbf{R}_X = \mathbf{R}_Y \mathbf{R}_{B_i}, \tag{6}\]
\[\mathbf{R}_{A_i} \mathbf{t}_X + \mathbf{t}_{A_i} = \mathbf{R}_Y \mathbf{t}_{B_i} + \mathbf{t}_Y. \tag{7}\]
2. Identifiability
A single motion pair $(A_i, B_i)$ is insufficient: with one observation, (5) constrains the composition $Y B_i X^{-1}$ but leaves a continuous family of $(X, Y)$ that satisfy it.
Differential form. For two motions $(A_i, B_i)$ and $(A_j, B_j)$, the relative motions $A_{ij} := A_i^{-1} A_j$ and $B_{ij} := B_i^{-1} B_j$ satisfy
\[X^{-1} A_{ij}\, X = Y^{-1} B_{ij}\, Y^{-1}\cdot Y = \cdots\]
The standard result (Tsai & Lenz 1989; Chen 1991) is:
The hand–eye problem is solvable iff there exist at least two relative motions whose rotation axes are not parallel.
With a smooth trajectory of $N \gg 2$ poses, this is satisfied generically, but a degenerate dataset (e.g., the rig only rotated about gravity) leaves the component about the common axis unobservable. The largest pairwise angle between rotation axes of $\{R_{A_i}\}$ serves as a diagnostic: a value below a threshold signals this degeneracy.
3. Closed-Form Initialization (Shah)
Shah's 2013 solver yields a closed-form seed for $(X, Y)$ by separating the rotation and translation blocks.
3.1 Rotation block
Vectorizing (6) using the identity $\mathrm{vec}\!\left(AXB\right) = (B^\top \otimes A)\,\mathrm{vec}\!\left(X\right)$:
\[\big(\mathbf{I}_3 \otimes \mathbf{R}_{A_i}\big)\, \mathrm{vec}\!\left(\mathbf{R}_X\right) \;-\; \big(\mathbf{R}_{B_i}^{\!\top} \otimes \mathbf{I}_3\big)\, \mathrm{vec}\!\left(\mathbf{R}_Y\right) \;=\; \boldsymbol{0}.\]
Stacking $N$ such $9 \times 18$ blocks gives a system
\[\mathbf{M}\,\boldsymbol{v} = \boldsymbol{0}, \qquad \boldsymbol{v} = \begin{bmatrix} \mathrm{vec}\!\left(\mathbf{R}_X\right) \\ \mathrm{vec}\!\left(\mathbf{R}_Y\right) \end{bmatrix} \in \mathbb{R}^{18}.\]
The right singular vector corresponding to the smallest singular value of $\mathbf{M}$ provides the least-squares solution. Reshape into two $3\times3$ matrices $\hat{\mathbf{R}}_X, \hat{\mathbf{R}}_Y$, then project each to the nearest rotation in $\mathrm{SO}(3)$ via SVD:
\[\mathbf{R}_X \;=\; U \,\mathrm{diag}(1, 1, \det(UV^\top))\, V^\top, \qquad \hat{\mathbf{R}}_X = U\Sigma V^\top.\]
(The diagonal correction enforces $\det \mathbf{R}_X = +1$, avoiding accidental reflections.)
3.2 Translation block
With $\mathbf{R}_X, \mathbf{R}_Y$ now fixed, (7) is linear in $(\mathbf{t}_X, \mathbf{t}_Y)$:
\[\begin{bmatrix} \mathbf{R}_{A_i} & -\mathbf{I}_3 \end{bmatrix} \begin{bmatrix} \mathbf{t}_X \\ \mathbf{t}_Y \end{bmatrix} \;=\; \mathbf{R}_Y \mathbf{t}_{B_i} - \mathbf{t}_{A_i}.\]
Stacking yields a $3N \times 6$ overdetermined system; solve in the least-squares sense (QR or normal equations).
4. Stage A: Decoupled SE(3) Refinement
The closed-form seed minimizes algebraic error, which mixes degrees of freedom unequally and is sensitive to outliers. Stage A refines $(X, Y)$ by minimizing a robust pose-pair residual.
4.1 Residual definition
Define
\[\mathbf{R}_{e,i} \;:=\; \mathbf{R}_Y\, \mathbf{R}_{B_i}\, \mathbf{R}_X^{\!\top}\, \mathbf{R}_{A_i}^{\!\top}, \tag{8}\]
\[\mathbf{t}_{e,i} \;:=\; \big(\mathbf{R}_Y \mathbf{t}_{B_i} + \mathbf{t}_Y\big) - \big(\mathbf{R}_{A_i} \mathbf{t}_X + \mathbf{t}_{A_i}\big). \tag{9}\]
At the optimum $\mathbf{R}_{e,i} = \mathbf{I}_3$ and $\mathbf{t}_{e,i} = \boldsymbol{0}$ for every $i$. Pack the rotation error into its axis–angle vector via the inverse exponential map of $\mathrm{SO}(3)$:
\[\boldsymbol{\omega}_i \;:=\; \operatorname{Log}\!\big(\mathbf{R}_{e,i}\big) \;\in\; \mathbb{R}^3\]
(implemented as Rotations.jl's Rotations.params(RotationVec(R))). The per-frame stacked residual is
\[\boldsymbol{r}_i(X, Y) \;=\; \begin{bmatrix} \boldsymbol{\omega}_i \\ \mathbf{t}_{e,i} \end{bmatrix} \;\in\; \mathbb{R}^6. \tag{10}\]
4.2 Cost
Let $\rho : \mathbb{R}_{\geq 0} \to \mathbb{R}_{\geq 0}$ be a robust loss (Huber, Cauchy, $\ldots$), $\hat\sigma$ a robust scale estimate (e.g., MAD over $\{\|\boldsymbol{r}_i\|\}$). The Stage A cost is
\[J_A(X, Y) \;=\; \sum_{i=1}^{N} \rho\!\bigg(\frac{\|\boldsymbol{r}_i(X, Y)\|}{\hat\sigma}\bigg). \tag{11}\]
4.3 Left-perturbation parameterization
To take Gauss–Newton steps on the manifold $\mathrm{SE}(3)\times\mathrm{SE}(3)$ we parameterize local updates by a $12$-vector
\[\boldsymbol{\delta} \;=\; (\boldsymbol{\delta}_{\!\omega X}, \boldsymbol{\delta}_{t X}, \boldsymbol{\delta}_{\!\omega Y}, \boldsymbol{\delta}_{t Y}) \;\in\; \mathbb{R}^{12}\]
with the left-perturbation retraction
\[X \;\leftarrow\; \Delta X \cdot X, \qquad \Delta X \;=\; \big(\mathbf{R}(\boldsymbol{\delta}_{\!\omega X}),\; \boldsymbol{\delta}_{t X}\big), \tag{12}\]
and analogously for $Y$. Here $\mathbf{R}(\boldsymbol{\omega}) := \operatorname{Exp}_{\mathrm{SO}(3)}[\boldsymbol{\omega}]_{\!\times}$ (implemented as Rotations.jl's RotMatrix(RotationVec(ω))). Concretely,
\[\mathbf{R}_X \mapsto \mathbf{R}(\boldsymbol{\delta}_{\!\omega X})\, \mathbf{R}_X, \qquad \mathbf{t}_X \mapsto \mathbf{R}(\boldsymbol{\delta}_{\!\omega X})\, \mathbf{t}_X + \boldsymbol{\delta}_{t X}.\]
4.4 Analytical Jacobians
Differentiating (8)–(9) at $\boldsymbol{\delta} = 0$.
Rotation residual. Using $\mathbf{R}(\boldsymbol{\delta})^{\!\top} \approx \mathbf{I} - [\boldsymbol{\delta}]_{\!\times}$ for small $\boldsymbol{\delta}$ and the identity $\mathbf{R}[v]_{\!\times}\mathbf{R}^{\!\top} = [\mathbf{R} v]_{\!\times}$ for any rotation $\mathbf{R}$:
\[\begin{aligned} \mathbf{R}_{e,i}\,(\boldsymbol{\delta}_{\!\omega X}) &= \mathbf{R}_Y \mathbf{R}_{B_i} \big(\mathbf{R}(\boldsymbol{\delta}_{\!\omega X})\mathbf{R}_X\big)^{\!\top}\mathbf{R}_{A_i}^{\!\top} \;=\; \mathbf{R}_Y \mathbf{R}_{B_i}\mathbf{R}_X^{\!\top}\big(\mathbf{I} - [\boldsymbol{\delta}_{\!\omega X}]_{\!\times}\big)\mathbf{R}_{A_i}^{\!\top}\\ &= \mathbf{R}_{e,i} - \mathbf{R}_{e,i}\, [\mathbf{R}_{A_i} \boldsymbol{\delta}_{\!\omega X}]_{\!\times} \;\approx\; \mathbf{R}_{e,i}\,\operatorname{Exp}\![-\mathbf{R}_{A_i}\boldsymbol{\delta}_{\!\omega X}]_{\!\times}. \end{aligned}\]
For $\boldsymbol{\delta}_{\!\omega Y}$:
\[\mathbf{R}_{e,i}\,(\boldsymbol{\delta}_{\!\omega Y}) = \mathbf{R}(\boldsymbol{\delta}_{\!\omega Y}) \mathbf{R}_Y \mathbf{R}_{B_i} \mathbf{R}_X^{\!\top}\mathbf{R}_{A_i}^{\!\top} = \operatorname{Exp}\![\boldsymbol{\delta}_{\!\omega Y}]_{\!\times} \, \mathbf{R}_{e,i}.\]
Taking $\operatorname{Log}$ and using the right (resp. left) Jacobians $J_r, J_l$ of $\mathrm{SO}(3)$:
\[\frac{\partial \boldsymbol{\omega}_i}{\partial \boldsymbol{\delta}_{\!\omega X}} = -\,J_r^{-1}(\boldsymbol{\omega}_i)\, \mathbf{R}_{A_i}, \quad \frac{\partial \boldsymbol{\omega}_i}{\partial \boldsymbol{\delta}_{\!\omega Y}} = +\,J_l^{-1}(\boldsymbol{\omega}_i), \quad \frac{\partial \boldsymbol{\omega}_i}{\partial \boldsymbol{\delta}_{t X}} = \mathbf{0}, \quad \frac{\partial \boldsymbol{\omega}_i}{\partial \boldsymbol{\delta}_{t Y}} = \mathbf{0}. \tag{13}\]
Near convergence $\mathbf{R}_{e,i} \to \mathbf{I}$, so $\boldsymbol{\omega}_i \to \boldsymbol{0}$ and $J_l^{-1}(\boldsymbol{\omega}_i), J_r^{-1}(\boldsymbol{\omega}_i) \to \mathbf{I}_3$. In practice we use the near-identity approximation throughout; the iteration converges quadratically once inside the basin.
Translation residual.$\mathbf{t}_{e,i}$ is independent of rotations only through $\mathbf{R}_Y$ acting on $\mathbf{t}_{B_i}$ and $\mathbf{R}_{A_i}$ acting on $\mathbf{t}_X$. Perturbing:
\[\begin{aligned} \mathbf{R}_{A_i}\mathbf{t}_X &\mapsto \mathbf{R}_{A_i}\big(\mathbf{R}(\boldsymbol{\delta}_{\!\omega X})\mathbf{t}_X + \boldsymbol{\delta}_{t X}\big) \approx \mathbf{R}_{A_i}\mathbf{t}_X - \mathbf{R}_{A_i}[\mathbf{t}_X]_{\!\times}\boldsymbol{\delta}_{\!\omega X} + \mathbf{R}_{A_i}\boldsymbol{\delta}_{t X},\\ \mathbf{R}_Y\mathbf{t}_{B_i} + \mathbf{t}_Y &\mapsto \mathbf{R}(\boldsymbol{\delta}_{\!\omega Y})\big(\mathbf{R}_Y\mathbf{t}_{B_i} + \mathbf{t}_Y\big) + \boldsymbol{\delta}_{t Y}\\ &\approx \big(\mathbf{R}_Y\mathbf{t}_{B_i}+\mathbf{t}_Y\big) - [\mathbf{R}_Y\mathbf{t}_{B_i}+\mathbf{t}_Y]_{\!\times}\,\boldsymbol{\delta}_{\!\omega Y} + \boldsymbol{\delta}_{t Y}. \end{aligned}\]
Letting $\boldsymbol{p}_i := \mathbf{R}_Y \mathbf{t}_{B_i} + \mathbf{t}_Y$ (pattern in world):
\[\frac{\partial \mathbf{t}_{e,i}}{\partial \boldsymbol{\delta}_{\!\omega X}} = +\mathbf{R}_{A_i}[\mathbf{t}_X]_{\!\times}, \quad \frac{\partial \mathbf{t}_{e,i}}{\partial \boldsymbol{\delta}_{t X}} = -\mathbf{R}_{A_i}, \quad \frac{\partial \mathbf{t}_{e,i}}{\partial \boldsymbol{\delta}_{\!\omega Y}} = -[\boldsymbol{p}_i]_{\!\times}, \quad \frac{\partial \mathbf{t}_{e,i}}{\partial \boldsymbol{\delta}_{t Y}} = +\mathbf{I}_3. \tag{14}\]
Per-frame Jacobian. Assembling (13)–(14) into the $6 \times 12$ block (rows: $[\boldsymbol{\omega}_i\,;\,\mathbf{t}_{e,i}]$, columns: $[\boldsymbol{\delta}_{\!\omega X}, \boldsymbol{\delta}_{t X}, \boldsymbol{\delta}_{\!\omega Y}, \boldsymbol{\delta}_{t Y}]$):
\[\mathbf{J}_i \;=\; \begin{bmatrix} -\mathbf{R}_{A_i} & \mathbf{0}_3 & +\mathbf{I}_3 & \mathbf{0}_3 \\[2pt] +\mathbf{R}_{A_i}[\mathbf{t}_X]_{\!\times} & -\mathbf{R}_{A_i} & -[\boldsymbol{p}_i]_{\!\times} & +\mathbf{I}_3 \end{bmatrix} \in \mathbb{R}^{6 \times 12} \tag{15}\]
(omitting the near-identity $J_l^{-1}, J_r^{-1}$ factors in the rotation block; they may be retained for principled steps far from the optimum).
4.5 IRLS update
At iterate $(X^{(k)}, Y^{(k)})$:
Compute residuals $\boldsymbol{r}_i$ via (10) and per-residual magnitudes $\eta_i = \|\boldsymbol{r}_i\|$.
Update scale: $\hat\sigma \leftarrow \mathrm{MAD}(\eta_i) / 0.6745$.
Per-residual weight: $w_i = \psi(\eta_i / \hat\sigma) / (\eta_i / \hat\sigma)$, where $\psi = \rho'$ is the influence function of the robust loss.
Solve the normal equations
\[\Big(\sum_i w_i\, \mathbf{J}_i^{\!\top} \mathbf{J}_i\Big)\, \boldsymbol{\delta} \;=\; -\sum_i w_i\, \mathbf{J}_i^{\!\top} \boldsymbol{r}_i.\]
Retract via (12): $X^{(k+1)} = (\mathbf{R}(\boldsymbol{\delta}_{\!\omega X})\mathbf{R}_X,\; \mathbf{R}(\boldsymbol{\delta}_{\!\omega X})\mathbf{t}_X + \boldsymbol{\delta}_{t X})$, and analogously for $Y$.
Terminate on $\|\boldsymbol{\delta}\| < \tau$ or after a maximum number of iterations.
5. Stage B: Reprojection Refinement (Optional)
If the caller additionally supplies the 3D pattern points $\{\boldsymbol{P}_j\}_{j=1}^M \subset \mathbb{R}^3$ (in $P$-coordinates), the per-frame pixel observations $\{\boldsymbol{u}_{ij}\}$, and the camera intrinsics $K$, the Stage A solution is refined against pixel reprojection.
5.1 Geometry
The pattern point $\boldsymbol{P}_j$ expressed in the camera frame at time $i$ is
\[\boldsymbol{X}_{ij}^{C} \;=\; Y^{-1}\big(A_i(X(\boldsymbol{P}_j))\big) \;=\; \mathbf{R}_Y^{\!\top}\bigl(\mathbf{R}_{A_i}(\mathbf{R}_X \boldsymbol{P}_j + \mathbf{t}_X) + \mathbf{t}_{A_i} - \mathbf{t}_Y\bigr).\]
Its predicted projection is $\pi(\boldsymbol{X}_{ij}^{C})$ where $\pi([x,y,z]^{\!\top}) = [x/z,\, y/z]^{\!\top}$ is perspective division; the calibrated residual is
\[\boldsymbol{g}_{ij}(X, Y) \;=\; \pi\!\big(\boldsymbol{X}_{ij}^{C}\big) \;-\; \pi\!\big(\boldsymbol{ray}_{ij}\big), \tag{16}\]
where $\boldsymbol{ray}_{ij}$ is the back-projected pixel observation (calibration handled inside the ray).
5.2 Jacobian via composition
VisualGeometryToolkitCore exposes the calibrated reprojection Jacobian of an $\mathrm{SE}(3)$ pose applied to a 3D point, against a left-perturbation parameterization (constraint_with_jacobians(pose::EuclideanMap{3}, X, ray)):
\[G_\theta \;=\; J_\pi\,\big[\,-[\boldsymbol{X}_{ij}^{C}]_{\!\times} \;\; \mathbf{I}_3\,\big] \;\in\; \mathbb{R}^{2\times6}.\]
For our composite pose $T^{(i)} = Y^{-1} A_i X$, define $\boldsymbol{Z}_{ij} := A_i(X(\boldsymbol{P}_j))$ (pattern in world, fixed wrt $X$ only).
Sensitivity to $X$. A left perturbation $X \mapsto \Delta X \cdot X$ pushes through $A_i$ unchanged in form: $A_i \cdot \Delta X \cdot X$. The effect on the camera-frame point is
\[\boldsymbol{X}_{ij}^{C} \;\mapsto\; Y^{-1} A_i \Delta X (X \boldsymbol{P}_j) \;=\; \mathbf{R}_Y^{\!\top}\mathbf{R}_{A_i}\Big(\mathbf{R}(\boldsymbol{\delta}_{\!\omega X})(\mathbf{R}_X \boldsymbol{P}_j + \mathbf{t}_X) + \boldsymbol{\delta}_{t X}\Big) + \cdots\]
To first order:
\[\frac{\partial \boldsymbol{X}_{ij}^{C}}{\partial \boldsymbol{\delta}_{\!\omega X}} = -\mathbf{R}_Y^{\!\top}\mathbf{R}_{A_i}\,[\mathbf{R}_X \boldsymbol{P}_j + \mathbf{t}_X]_{\!\times}, \qquad \frac{\partial \boldsymbol{X}_{ij}^{C}}{\partial \boldsymbol{\delta}_{t X}} = +\mathbf{R}_Y^{\!\top}\mathbf{R}_{A_i}. \tag{17}\]
Sensitivity to $Y$. A left perturbation $Y \mapsto \Delta Y \cdot Y$ implies $Y^{-1} \mapsto Y^{-1} \Delta Y^{-1}$, so
\[\boldsymbol{X}_{ij}^{C} \;\mapsto\; \mathbf{R}_Y^{\!\top}\mathbf{R}(-\boldsymbol{\delta}_{\!\omega Y}) \big(\boldsymbol{Z}_{ij} - \mathbf{t}_Y - \boldsymbol{\delta}_{tY}\big) + O(\boldsymbol{\delta}^2).\]
At $\boldsymbol{\delta} = 0$,
\[\frac{\partial \boldsymbol{X}_{ij}^{C}}{\partial \boldsymbol{\delta}_{\!\omega Y}} = +\mathbf{R}_Y^{\!\top}\,[\boldsymbol{Z}_{ij} - \mathbf{t}_Y]_{\!\times}, \qquad \frac{\partial \boldsymbol{X}_{ij}^{C}}{\partial \boldsymbol{\delta}_{t Y}} = -\mathbf{R}_Y^{\!\top}. \tag{18}\]
Chain rule through $\pi$ gives the per-observation $2\times12$ Jacobian
\[\mathbf{H}_{ij} \;=\; J_\pi(\boldsymbol{X}_{ij}^{C})\; \Big[\, -\mathbf{R}_Y^{\!\top}\mathbf{R}_{A_i}[\mathbf{R}_X\boldsymbol{P}_j+\mathbf{t}_X]_{\!\times} \;\; +\mathbf{R}_Y^{\!\top}\mathbf{R}_{A_i} \;\; +\mathbf{R}_Y^{\!\top}[\boldsymbol{Z}_{ij}-\mathbf{t}_Y]_{\!\times} \;\; -\mathbf{R}_Y^{\!\top}\, \Big]. \tag{19}\]
5.3 Stage B cost and IRLS step
\[J_B(X, Y) \;=\; \sum_i \sum_j \rho\!\left(\frac{\|\boldsymbol{g}_{ij}(X,Y)\|}{\hat\sigma_{\!px}}\right), \tag{20}\]
with $\hat\sigma_{\!px}$ a robust scale estimate over the pixel residuals. The IRLS update is identical in structure to Stage A but assembles
\[\mathbf{H} = \sum_{ij} w_{ij}\, \mathbf{H}_{ij}^{\!\top}\mathbf{H}_{ij}, \qquad \boldsymbol{b} = -\sum_{ij} w_{ij}\, \mathbf{H}_{ij}^{\!\top}\boldsymbol{g}_{ij}, \tag{21}\]
solves $\mathbf{H}\boldsymbol{\delta} = \boldsymbol{b}$, and retracts via (12).
6. Algorithm Summary
Require: pose pairs $\{(A_i, B_i)\}_{i=1}^N$; optional pattern points $\{\boldsymbol{P}_j\}$, pixel observations $\{\boldsymbol{u}_{ij}\}$, intrinsics $K$; loss $\rho$, scale estimator, tolerance $\tau$. Ensure: rigid transforms $X = T_{RP}$, $Y = T_{WC}$.
- (0) Validate inputs; warn if the rotation-axis spread is below a threshold (Section 2).
- (1)Closed-form seed (Section 3):
- solve $\mathbf{M}\boldsymbol{v}=\boldsymbol{0}$ via SVD, project the halves to $\mathrm{SO}(3)$$\Rightarrow (\mathbf{R}_X^{(0)}, \mathbf{R}_Y^{(0)})$;
- solve the $3N\times6$ linear least squares $\Rightarrow (\mathbf{t}_X^{(0)}, \mathbf{t}_Y^{(0)})$.
- (2)Stage A: SE(3) M-estimator IRLS (Section 4): repeat residuals (10) $\rightarrow$ scale $\rightarrow$ weights $\rightarrow$ normal equations $\rightarrow$ retract, until $\|\boldsymbol{\delta}\| < \tau$.
- If pattern points, pixels and intrinsics are supplied: (3)Stage B: reprojection IRLS (Section 5): residuals (16) $\rightarrow$ scale $\rightarrow$ weights $\rightarrow$ assemble (19) $\rightarrow$ retract, until $\|\boldsymbol{\delta}\| < \tau_{px}$.
- Return$(X, Y)$.
7. Implemented Pipeline (AX = XB) — Loss Variants
The per-pair residual is the decoupled 6-vector
\[\boldsymbol{g}_i(X) \;=\; \begin{bmatrix} \operatorname{Log}\!\bigl(\mathbf{R}_{A_i}\mathbf{R}_X \mathbf{R}_{B_i}^{\!\top} \mathbf{R}_X^{\!\top}\bigr) \\[2pt] \mathbf{R}_{A_i}\mathbf{t}_X + \mathbf{t}_{A_i} - \mathbf{R}_X \mathbf{t}_{B_i} - \mathbf{t}_X \end{bmatrix} \;=\; \begin{bmatrix} \theta_i \, \hat{\boldsymbol{n}}_i \\[2pt] \boldsymbol{t}_{e,i} \end{bmatrix} \;\in\; \mathbb{R}^6, \tag{22}\]
where $\theta_i$ is the bi-invariant $\mathrm{SO}(3)$ geodesic angle between $\mathbf{R}_{A_i}\mathbf{R}_X$ and $\mathbf{R}_X\mathbf{R}_{B_i}$, and $\hat{\boldsymbol{n}}_i$ is the corresponding rotation axis.
All four variants below share the same residual $\boldsymbol{g}_i(X)$ and the same RANSAC + IRLS framework (the weighted refine! of any IRLS-family local optimization); they differ only in the per-pair weighting matrix that converts $\boldsymbol{g}_i$ into a scalar contribution to the cost. The IRLS weights $w_i^{(k)} \in [0,1]$ act on top of any of these losses by multiplying the per-pair $\ell_i$.
7.1 Variant 0 (implemented): single global L
Decoupled $\mathrm{SO}(3) \times \mathbb{R}^3$ split metric with one global length constant $L$ converting radians to length units:
\[\boxed{\;\ell_i^{\text{(0)}}(X) \;=\; L^2\,\theta_i^2 \;+\; \|\boldsymbol{t}_{e,i}\|^2\;} \tag{23}\]
Equivalently $\ell_i = \boldsymbol{g}_i^{\!\top} W \boldsymbol{g}_i$ with the constant $6\times6$
\[W \;=\; \mathrm{diag}(L^2, L^2, L^2,\, 1, 1, 1)\]
applied identically to every pair. Default $L = \mathrm{median}_i\!\|\boldsymbol{t}_{A_i}\|$ (the typical rig translation per motion); override via the char_length keyword.
Strengths. Zero extra inputs; one scalar to set. Weaknesses.$L$ is a heuristic that depends on the rig's typical motion magnitude; treats all frames as equally informative.
7.2 Variant A: two-scale data-driven Mahalanobis
Replace the single $L$ with two independent scale estimates, $\hat\sigma_R$ over the per-pair rotation magnitudes $\theta_i$ and $\hat\sigma_t$ over the translation magnitudes $\|\boldsymbol{t}_{e,i}\|$:
\[\boxed{\;\ell_i^{\text{(A)}}(X) \;=\; \frac{\theta_i^2}{\hat\sigma_R^2} \;+\; \frac{\|\boldsymbol{t}_{e,i}\|^2}{\hat\sigma_t^2}\;} \tag{24}\]
Equivalently $\ell_i = \boldsymbol{g}_i^{\!\top} W \boldsymbol{g}_i$ with the constant
\[W \;=\; \mathrm{diag}\bigl(\hat\sigma_R^{-2}, \hat\sigma_R^{-2}, \hat\sigma_R^{-2}, \hat\sigma_t^{-2}, \hat\sigma_t^{-2}, \hat\sigma_t^{-2}\bigr).\]
EM-like scale update (from block coordinate descent). Rather than using a MAD estimator on the residual magnitudes (a one-shot robust statistic, blind to the framework's own posterior weights), we reuse the weighted scale M-step of the Student-t / uniform mixture EM for scalar residuals (its scale block is update_scale!), of the form $\hat{s}^2 = \sum_i w_i r_i^2 / \sum_i w_i$. Applied componentwise:
\[\boxed{\;\hat\sigma_R^2 \;=\; \frac{\sum_i w_i^{(k)}\,\theta_i^2}{\sum_i w_i^{(k)}}, \qquad \hat\sigma_t^2 \;=\; \frac{\sum_i w_i^{(k)}\,\|\boldsymbol{t}_{e,i}\|^2}{\sum_i w_i^{(k)}}\;} \tag{25}\]
where $w_i^{(k)}$ are the current Student-t/uniform posterior weights from the E-step (soft_posterior_weights! / update_weights!). Each LO iteration runs:
- M-step (scale): $(\hat\sigma_R, \hat\sigma_t)$ via (25).
- Weighted GN step on $X$ using $\ell_i^{\text{(A)}}$ as the per-pair contribution.
- E-step (weights): refresh $w_i^{(k+1)}$ via the mixture posterior.
This is properly EM-like: the M-step is closed-form, monotonically improves the expected complete-data log-likelihood, and naturally absorbs the posterior-IRLS outlier rejection (because $w_i \to 0$ for outliers, they drop out of both scale estimates).
Strengths. Unit-invariant by construction; self-consistent with the framework's EM weights (no separate MAD estimate); robust by construction (outliers drop out via $w_i \to 0$); adapts to the actual residual distribution. Weaknesses. Still treats all pairs as equally informative within each component (constant $W$ at each iteration); $\hat\sigma_t \to 0$ in pure-rotation calibration — clamp required.
7.3 Variant B: per-pair heteroscedastic Mahalanobis
Pair-specific noise covariance derived from explicit observation-noise models for $A_{ij}$ (mocap, depending on inter-frame motion) and $B_{ij}$ (PnP, depending on pattern distance, foreshortening, image-area, etc.). With $\boldsymbol{x}_i = (A_i, B_i)$ jointly the noisy observation and $\Sigma_{x_i} = \mathrm{block\text{-}diag}(\Sigma_{A_i}, \Sigma_{B_i}) \in \mathbb{R}^{12\times12}$:
\[\boxed{\;\ell_i^{\text{(B)}}(X) \;=\; \boldsymbol{g}_i^{\!\top}\, \tilde\Sigma_{g,i}^{-1}\, \boldsymbol{g}_i, \qquad \tilde\Sigma_{g,i} \;=\; \partial_{\boldsymbol{x}}\boldsymbol{g}_i \, \Sigma_{x_i}\, \partial_{\boldsymbol{x}}\boldsymbol{g}_i^{\!\top}\;} \tag{26}\]
The framework supports this directly: return $\Sigma_{x_i}$ from the problem's observation_covariance(problem, i) beside its constraint_with_jacobians, and the default mahalanobis_distances! computes $\sqrt{\boldsymbol{g}_i^{\!\top}\,\tilde\Sigma_{g,i}^{-1}\,\boldsymbol{g}_i}$.
Strengths. Textbook-correct heteroscedastic weighting; a far-away barely-visible pattern frame is automatically down-weighted relative to a close-up well-lit one. Weaknesses. Requires per-frame noise models for both mocap and PnP; only pays off when uncertainty actually varies across frames.
7.4 Variant C: profile L per IRLS iteration
Single global $L$ as in Variant 0, but re-estimated at every IRLS iteration so that rotation and translation contribute equal Fisher information to the Hessian:
\[\boxed{\;\ell_i^{\text{(C)}}(X) \;=\; L_k^2\,\theta_i^2 \;+\; \|\boldsymbol{t}_{e,i}\|^2, \qquad L_k \;=\; \frac{\mathrm{median}_i\|\boldsymbol{t}_{e,i}^{(k)}\|} {\mathrm{median}_i\,\theta_i^{(k)}}\;} \tag{27}\]
where $\theta_i^{(k)}$ and $\boldsymbol{t}_{e,i}^{(k)}$ are evaluated at the current iterate $X^{(k)}$. At the optimum, $L_k$ stabilizes to the ratio of typical translation to typical rotation residual magnitudes.
Strengths. One degree of freedom (vs. two for Variant A); self-tuning; cheaper than Variant A (one division per iteration vs. two MAD computations). Weaknesses. Couples rotation and translation through a single scalar (Variant A is strictly more flexible); marginally more sensitive to outliers in early iterations than the MAD-based Variant A.
7.5 Summary table
| Variant | Per-pair weight $W_i$ | DOF | Needs |
|---|---|---|---|
| 0 (implemented) | $\mathrm{diag}(L^2 I_3,\, I_3)$, constant | 1 ($L$) | nothing (auto-default) |
| A | $\mathrm{diag}(\hat\sigma_R^{-2}I_3,\, \hat\sigma_t^{-2}I_3)$, EM M-step | 2 | residuals + framework weights |
| B | $\tilde\Sigma_{g,i}^{-1}$, per-pair | many | per-frame noise models |
| C | $\mathrm{diag}(L_k^2 I_3,\, I_3)$, per-iteration | 1 ($L_k$, auto) | nothing |
The variants form a natural progression of expressive power. The implementation (HandEyeProblem) uses Variant 0.
8. Building Blocks
From VisualGeometryToolkitCore.
EuclideanMap— composition∘,inv, applicationp(x).skew(v)for $[\cdot]_{\!\times}$;translation_anglefor diagnostics;log_so3/exp_so3for the $\mathrm{SO}(3)$ logarithm and exponential.constraint_with_jacobians(pose::EuclideanMap{3}, X, ray)returns the calibrated $2\times6$ pose-perturbation Jacobian used in Stage B's $\mathbf{H}_{ij}$ assembly (19).HuberShape,CauchyShape, … for the robust loss $\rho$.
From Rotations.jl.
- $\operatorname{Log}_{\mathrm{SO}(3)}(\mathbf{R})$ =
params(RotationVec(R))— the axis–angle 3-vector;rotation_anglefor diagnostics. - $\operatorname{Exp}_{\mathfrak{so}(3)}(\boldsymbol{\omega})$ =
RotMatrix(RotationVec(ω₁, ω₂, ω₃))— retraction.
References
- Chen, H. H. (1991). A Screw Motion Approach to Uniqueness Analysis of Head-Eye Geometry. In: IEEE Conference on Computer Vision and Pattern Recognition (CVPR); pp. 145–151.
- Shah, M. (2013). Solving the Robot-World/Hand-Eye Calibration Problem Using the Kronecker Product. ASME Journal of Mechanisms and Robotics 5, 031007.
- Tsai, R. Y. and Lenz, R. K. (1989). A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration. IEEE Transactions on Robotics and Automation 5, 345–358.