A certifiably correct algorithm for generalized robot-world and hand-eye calibration

Abstract

Automatic extrinsic sensor calibration is a fundamental problem for multi-sensor platforms. Reliable and general-purpose solutions should be computationally efficient, require few assumptions about the structure of the sensing environment, and demand little effort from human operators. In this work, we introduce a fast and certifiably globally optimal algorithm for solving a generalized formulation of the robot-world and hand-eye calibration (RWHEC) problem. The formulation of RWHEC presented is “generalized” in that it supports the simultaneous estimation of multiple sensor and target poses, and permits the use of monocular cameras that, alone, are unable to measure the scale of their environments. In addition to demonstrating our method’s superior performance over existing solutions through extensive simulated and real experiments, we derive novel identifiability criteria and establish a priori guarantees of global optimality for problem instances with bounded measurement errors. As part of our analysis, we propose a new constraint qualification for nonlinear programs with redundant constraints; this constraint qualification is of independent interest for establishing the exactness of SDP relaxations of QCQPs that have been tightened through the addition of redundant constraints. Finally, we provide a free and open-source implementation of our algorithms and experiments.

Introduction

Calibration is an essential but often painful process when working with common sensors for robot perception. In particular, extrinsic calibration refers to the problem of finding the spatial transformations between multiple sensors rigidly mounted to a fixed or mobile sensing platform. Existing approaches vary in terms of the type and number of sensors involved, assumptions about the robot’s motion or the geometry of the environment, and operator involvement or expertise. Critically, extrinsic calibration errors can have catastrophic consequences for downstream perception tasks that rely on fusion of data from multiple sensors. For example, an autonomous vehicle using multiple cameras for simultaneous localization and mapping (SLAM) must accurately fuse images from different cameras into a coherent model of the world.

In this work, we focus on the very general and widespread robot-world and hand-eye calibration (RWHEC) formulation of extrinsic calibration (Zhuang et al., 1994). The RWHEC problem can be adapted to a wide variety of sensor configurations, and conveniently distills extrinsic calibration down to estimation of two rigid transformations represented as elements X and Y of the special Euclidean group SE(3), that define the kinematic relationship depicted in Figure 1. The matrix equations

${\mathbf{A}}_i\mathbf{X}=\mathbf{Y}{\mathbf{B}}_i,i=1,\dots ,N,$

(1)

are formed from noisy measurements A _i , B _i ∈ SE(3). RWHEC is named after its original application to a robot manipulator (“hand”) holding a camera (“eye”), but it can be applied to any configuration of sensor and target forming the kinematic loop shown in Figure 1. We consider a calibration target to be any specialized structure or fiducial marker whose pose, shape, or appearance is known to a robot or its operator. In this work, we use the term generalized RWHEC to refer to a variant of RWHEC involving multiple (possibly monocular) sensors and/or targets.

Figure 1.

A diagram of the conventional application of RWHEC. In this application, the objective is to estimate X ∈ SE(3), the transformation from the wrist of a robotic manipulator to a camera, and Y ∈ SE(3), the transformation from the manipulator base to a known target. In this diagram, the base, joint 1, hand, camera, and target reference frames are labeled $\underrightarrow{{\mathcal{F}}_b}$ , $\underrightarrow{{\mathcal{F}}_1}$ , $\underrightarrow{{\mathcal{F}}_h}$ , $\underrightarrow{{\mathcal{F}}_c}$ , and $\underrightarrow{{\mathcal{F}}_t}$ , respectively. The red arrows indicate the axes of joint rotation. At time i, we use the forward kinematics of the manipulator to compute A _i , the transformation from the manipulator base to the wrist, and measure B _i , a noisy estimate of the transformation from the target to the camera.

Existing calibration procedures are error prone, especially when used by operators without sufficient expertise. These procedures often require that the operator excite a sensor platform with particular motions, or carefully select initial parameters close enough to the true solution. Without awareness of these idiosyncrasies, the optimizer for a calibration procedure may fail to converge to a critical point, or return a locally optimal solution that is inferior to the global minimizer. Often, the only indicator of inaccurate calibration parameters is the failure of downstream algorithms, which can place nearby people in danger and damage the robot or other infrastructure. To avoid these potentially catastrophic perception failures, end-users without significant expertise need calibration algorithms that automatically certify the global optimality of their solution. To this end, we present the following major contributions:

(1) the first certifiably globally optimal solver for a multi-sensor extrinsic calibration problem;

(2) the first theoretical analysis of parameter identifiability for the generalized robot-world and hand-eye calibration problem; and

(3) a free and open-source implementation of our method and experiments. ¹

We begin by surveying the extensive literature on robot-world and hand-eye calibration (RWHEC) in Related work, followed by a summary of our mathematical notation. We develop a detailed description of our problem in Generalized robot-world and hand-eye calibration. Certifiably globally optimal extrinsic calibration describes a convex relaxation of RWHEC that leads to a semidefinite program (SDP), while Uniqueness of solutions presents identifiability criteria in the form of geometric constraints on sensor measurements and motion. Global optimality guarantees proves that our SDP relaxations of identifiable problems with sufficiently low noise are guaranteed to be tight, enabling the extraction of the global minimum to the original nonconvex problem. In Simulation experiments and Real-world experiment, we demonstrate the superior properties of our RWHEC method as compared with existing solvers on synthetic and real-world data, respectively. Finally, our conclusion discusses remaining challenges and promising avenues for future work.

Related work

We begin our survey of related work by reviewing RWHEC methods in Robot-world and hand-eye calibration. This involves defining three common categories of algorithms: separate, joint, and probabilistic. ² The characteristics of all methods discussed are summarized in Table 1, which explicitly highlights the novelty of our solution. We also survey relevant applications of certifiably correct convex optimization methods to estimation problems in robotics and computer vision in Certifiably correct estimation.

Table 1.

A non-exhaustive summary of relevant calibration methods for RWHEC algorithms. The columns track the following algorithmic properties: whether the method jointly estimates rotation and position, whether a probabilistic problem formulation is employed, whether a post-hoc certificate of global optimality is produced by the algorithm, the algorithm’s ability to model multi-sensor or -target problems, support for scale-free (i.e., monocular) sensors, and the 3D pose representation used.

Method	Joint	Probabilistic	Certifiable	Multiple	Monocular	Representation
Dornaika and Horaud (1998)	✓	✓	✗	✗	✗	Multiple
Strobl and Hirzinger (2006)	✓	✓	✗	✗	✗	SE(3)
Li et al. (2010)	✗	✗	✗	✗	✗	DQ
Shah (2013)	✗	✗	✗	✗	✗	SE(3)
Heller et al. (2014)	✓	✗	✓	✗	✗	Multiple
Tabb and Yousef (2017)	✓	✗	✗	✗	✗	Multiple
Wise et al. (2020)	✓	✗	✓	✗	✓	SE(3)
Wang et al. (2022)	✗	✗	✗	Xs or Ys	✗	SE(3)
Evangelista et al. (2023)	✓	✗	✗	Xs	✗	SE(3)
Horn et al. (2023)	✓	✗	✓	Xs or Ys	✗	DQ
Ours	✓	✓	✓	Xs and Ys	✓	SE(3)

Robot-world and hand-eye calibration

A subset of RWHEC methods solve for the rotation and translation components of X and Y separately. One simple method is to ignore nonlinear constraints on the rotation components of X and Y and solve the resulting overdetermined linear system in equation (1) with a least squares approach. After projecting the estimated rotation matrices onto the rotation manifold SO(3), the translation components of X and Y are trivial to compute. This approach is used by Li et al. (2010) and extended to multiple X or Y variables in the generalized RWHEC formulation of Wang et al. (2022). The identifiability requirements of RWHEC are explored by Shah (2013), but to our knowledge have not been extended to the generalized case. Since these two-stage closed-form solvers ignore the coupling between the rotation and translation cost functions, they can corrupt translation estimates with error from noisy rotation measurements. Furthermore, since they all solve a linear relaxation of the true nonlinear RWHEC problem, these methods are best described as approximation schemes.

To decrease this sensitivity of RWHEC to measurement noise, some methods jointly estimate the translation and rotation components of X and Y. Tabb and Yousef (2017) introduce a variety of joint RWHEC cost functions and parameterizations that are less sensitive to measurement noise than two-stage closed-form RWHEC schemes. Horn et al. (2023) use a dual quaternion (DQ) representation for poses to cast the generalized RWHEC problem as a quadratically constrained quadratic program (QCQP) and use a methodology similar to ours to solve a convex relaxation of their problem formulation. However, we demonstrate in Simulation experiments that the unit DQ representation leads to poorer performance than the homogeneous transformation matrices we use. This is most likely because the set of unit quaternions $\mathbb{H}$ is a double cover of SO(3) in that the quadratic function $f:\mathbb{H}\to \mathrm{S}\mathrm{O}(3)$ which converts quaternions to rotation matrices maps $\mathbf{q}\in \mathbb{H}$ to the same rotation matrix as −q. As a result, for each measurement A _i or B _i appearing in equation (1), there are two distinct dual quaternions that represent the same element of SE(3). Therefore, there are 2^2N DQ-based formulations of any RWHEC problem with N pairs of measurements, and their solutions may vary significantly in quality. Most existing work, including Horn et al. (2023), ignores this unfortunate consequence of representing rotations as unit quaternions and does not describe a principled or heuristic approach to lifting elements of SO(3) to one of two valid unit quaternions.

Evangelista et al. (2023) use a local nonlinear method to solve a variant of generalized RWHEC with an objective function formed from camera reprojection error. This is in contrast to our approach and the classical RWHEC framework, which abstracts out direct sensor measurements to work with poses. One advantage of using reprojection error is that it additionally allows the estimation of intrinsic camera parameters, which is outside the scope of this work.

Although joint estimation methods are more robust to measurement noise than two-stage closed-form solvers, the methods surveyed thus far do not use a rigorous probabilistic error model. As a result, these methods cannot model the effect of measurements with varying accuracy. Probabilistic RWHEC algorithms are introduced by Dornaika and Horaud (1998) and Strobl and Hirzinger (2006). Dornaika and Horaud (1998) solve a nonlinear probabilistic formulation of RWHEC, but do not use an on-manifold method like the local solver presented in this work. Strobl and Hirzinger (2006) treat the RWHEC problem as an iteratively re-weighted nonlinear optimization problem, where the translation and rotation errors are corrupted by zero-mean Gaussian noise. While both methods account for the probabilistic nature of the problem, neither can provide a certificate of optimality. The probabilistic framework of Ha (2023) considers anisotropic noise models for measurements and estimates the uncertainty of a solution obtained by a local iterative method. Additionally, uncertainty-aware methods leveraging the Gauss-Helmert model have been applied to the hand-eye calibration problem with unknown scale (Ulrich and Hillemann, 2024; Čolaković-Bencerić et al., 2025), but this approach has not yet been applied to RWHEC.

The origins of our certifiably optimal approach to calibration lie in the formulation of RWHEC and hand-eye calibration as global polynomial optimization problems by Heller et al. (2014). They solve semidefinite programming (SDP) relaxations of their polynomial programs and explore multiple representations of SE(3), but they do not use a probabilistic framework and only solve a standard variant of RWHEC (see Table 1 for details).

Certifiably correct estimation

Convex SDP relaxations of QCQPs are a powerful tool for finding globally optimal solutions to geometric estimation problems in robotics and computer vision (Carlone et al., 2015; Cifuentes et al., 2022; Rosen et al., 2021). In addition to the pioneering work of Heller et al. (2014), our approach is heavily influenced by the SE-Sync algorithm introduced in Rosen et al. (2019). SE-Sync was the first efficient and certifiably optimal algorithm for simultaneous localization and mapping (SLAM), which, like generalized RWHEC, is formulated over many unknown pose variables. Rosen et al. (2019) reveal and exploit the smooth manifold structure of an SDP relaxation of a QCQP formulation of pose-graph SLAM with an MLE objective function. This approach spawned a host of solutions to spatial perception problems including point cloud registration (Yang et al., 2021), multiple variants of localization and mapping (Dumbgen et al., 2023; Fan et al., 2020; Holmes and Barfoot, 2023; Papalia et al., 2024; Tian et al., 2021; Yu and Yang, 2024), hand-eye calibration (Giamou et al., 2018; Wise et al., 2020; Wodtko et al., 2021), camera pose estimation (Garcia-Salguero et al., 2021; Zhao, 2020), as well as research into tools and optimization methods for user-specified problems (Dümbgen et al., 2024; Rosen, 2021; Yang and Carlone, 2020).

An alternative approach for obtaining global optima for noisy geometric estimation problems is outlined by Wu et al. (2022). In contrast to using convex SDP relaxations, Wu et al. (2022) use a Gröbner basis method to solve polynomial optimization problems over a single pose variable. This symbolic method was also extended to problems with unknown scale, including hand-eye calibration with a monocular camera (Xue et al., 2025). While promising, this approach has not been demonstrated to scale efficiently to problems with many unknown pose variables. Similarly, a branch-and-bound approach is applied to find global optima of hand-eye calibration problems in Heller et al. (2012, 2016), but these methods do not scale efficiently to problems with many sensors.

In this work, we extend the multi-frame (i.e., supporting multiple sensors and/or targets) RWHEC formulation introduced by Wang et al. (2022) to the scale-free case involving monocular sensors, and we form a joint objective function within a maximum likelihood estimation (MLE) framework. This leads to an estimation problem over a graph similar in nature to SE-Sync (Rosen et al., 2019), but that cannot be expressed in the algebraic (block-matrix) form necessary to employ the fast low-dimensional Riemannian optimization algorithms used in this prior work. ³ Alternatively, the algorithm presented in this work can be viewed as an extension of the method in Wise et al. (2020) to a probabilistic and multi-sensor formulation of RWHEC. Finally, we prove fundamental identifiability and global optimality theorems for the multi-frame RWHEC problem first introduced by Wang et al. (2022) and extended in Generalizing RWHEC to multiple sensors and targets.

Notation

In this paper, lowercase Latin and Greek characters represent scalar variables. We reserve lowercase and uppercase boldface characters for vectors and matrices, respectively. For an integer N > 0, [N] denotes the index set {1, …, N}. The space of n × n symmetric and symmetric positive semidefinite (PSD) matrices are written as ${\mathbb{S}}^n$ and ${\mathbb{S}}_{+}^n$ , respectively, and we also use A ⪰ B (A ≻ B) to indicate that A − B is PSD (positive definite). For a general matrix A, we use A^† to indicate the Moore-Penrose pseudoinverse. The (right) nullspace or kernel of A is written as ker(A), and its range is written as $\mathcal{R}(\mathbf{A})$ .

For a directed graph $\vec{\mathcal{G}}=(\mathcal{V},\vec{\mathcal{E}})$ , we write each directed edge as e = (i, j) and say that e leaves vertex $i\in \mathcal{V}$ and enters vertex $j\in \mathcal{V}$ . Additionally, δ⁻(v) denotes the set of incident edges leaving vertex $v\in \mathcal{V}$ , and δ⁺(v) denotes the set of incident edges entering v.

A right-handed reference frame is written as $\underrightarrow{\mathcal{F}}$ (Barfoot, 2024). The translation from $\underrightarrow{{\mathcal{F}}_a}$ to $\underrightarrow{{\mathcal{F}}_b}$ described in $\underrightarrow{{\mathcal{F}}_a}$ is written as ${\mathbf{p}}_a^{ba}\in {\mathbb{R}}^3$ . The matrix R _ab ∈ SO(3) represents the rotation taking coordinates expressed in $\underrightarrow{{\mathcal{F}}_b}$ to $\underrightarrow{{\mathcal{F}}_a}$ . Likewise, the rigid special Euclidean transformation from $\underrightarrow{{\mathcal{F}}_b}$ to $\underrightarrow{{\mathcal{F}}_a}$ is

${\mathbf{T}}_{ab}=\left[\begin{array}{cc}\hfill {\mathbf{R}}_{ab}\hfill & \hfill {\mathbf{p}}_a^{ba}\hfill \\ \hfill {\mathbf{0}}^{\top }\hfill & \hfill 1\hfill \end{array}\right]\in \mathrm{S}\mathrm{E}(3).$

(2)

The skew-symmetric operator (⋅)^∧ acts on the vector $\mathbf{p}\in {\mathbb{R}}^3$ such that

${\mathbf{p}}^{\wedge }=\left[\begin{array}{ccc}\hfill 0\hfill & \hfill -p_3\hfill & \hfill p_2\hfill \\ \hfill p_3\hfill & \hfill 0\hfill & \hfill -p_1\hfill \\ \hfill -p_2\hfill & \hfill p_1\hfill & \hfill 0\hfill \end{array}\right],$

(3)

and the operator (⋅)^∨ denotes its inverse. The standard Kronecker product of A and B is written as ⊗, and the Kronecker sum ⊕ is

$\mathbf{A}\oplus \mathbf{B}=\mathbf{A}\otimes \mathbf{I}+\mathbf{I}\otimes \mathbf{B}.$

(4)

The function $\mathrm{D}\mathrm{i}\mathrm{a}\mathrm{g}\left({\mathbf{A}}_1,\dots ,{\mathbf{A}}_N\right)$ creates a block-diagonal matrix with its ordered matrix arguments on the diagonal. A vector $\mathbf{x}\sim \mathcal{N}\left(\mathit{\mu },\mathbf{\Sigma }\right)$ is Gaussian distributed with a mean of $\mathit{\mu }\in {\mathbb{R}}^n$ and covariance of $\mathbf{\Sigma }\in {\mathbb{S}}_{+}^n$ . Finally, Lang(M, κ) denotes a Langevin distribution over SO(3) with mode M ∈ SO(3) and concentration κ ≥ 0.

Generalized robot-world and hand-eye calibration

In Geometric constraints, we review the geometric constraints of the robot-world and hand-eye calibration problem. In Maximum likelihood estimation, we formulate RWHEC as maximum likelihood estimation. In Monocular cameras, we extend our problem formulation to support monocular cameras observing targets of unknown size. In Quadratically constrained quadratic programming, we convert our calibration problems into QCQPs in standard form. In Generalizing RWHEC to multiple sensors and targets, we extend our probabilistic formulation to calibrate an arbitrary number of decision variables (i.e., multiple Xs and/or Ys). Finally, in Reducing the dimension of the QCQP, we present a marginalization strategy for our formulation.

Geometric constraints

While the robot-world and hand-eye geometric constraints apply to a large set of calibration problems (e.g., multiple cameras on a mobile manipulator or fixed cameras tracking a known target (Wang et al., 2022)), for convenience we will begin, without loss of generality, with terminology appropriate for calibrating a camera mounted on the “hand” of a robotic manipulator as shown in Figure 1. Fix reference frames $\underrightarrow{{\mathcal{F}}_b},\underrightarrow{{\mathcal{F}}_h},\underrightarrow{{\mathcal{F}}_t},\underrightarrow{{\mathcal{F}}_c}$ to the manipulator base, manipulator hand, target, and camera, respectively. Using joint encoder data and kinematic parameters of the robot arm, we are able to estimate the transform T _bh . Additionally, a camera observing a target of known scale enables estimation of the camera pose relative to the target, T _tc . At each discrete point in time indexed by i ∈ [N], these two measurements are related by

${\mathbf{T}}_{bh}(i){\mathbf{T}}_{hc}={\mathbf{T}}_{bt}{\mathbf{T}}_{tc}(i).$

(5)

In the RWHEC problem, we wish to estimate T _hc and T _bt , which we assume are static extrinsic transformation parameters. We introduce simpler symbols

$\begin{array}{cc}\hfill {\mathbf{A}}_i& \triangleq {\mathbf{T}}_{bh}(i)\hfill \\ \hfill \mathbf{X}& \triangleq {\mathbf{T}}_{hc}\hfill \\ \hfill \mathbf{Y}& \triangleq {\mathbf{T}}_{bt}\hfill \\ \hfill {\mathbf{B}}_i& \triangleq {\mathbf{T}}_{tc}(i),\hfill \end{array}$

(6)

for our matrices, resulting in the following familiar RWHEC constraint:

${\mathbf{A}}_i\mathbf{X}=\mathbf{Y}{\mathbf{B}}_i.$

(7)

We may separate the rotational and translational components of equation (7) according to:

$\begin{array}{ll}\hfill {\mathbf{R}}_{{\mathbf{A}}_i}{\mathbf{R}}_{\mathbf{X}}& ={\mathbf{R}}_{\mathbf{Y}}{\mathbf{R}}_{{\mathbf{B}}_i},\hfill \end{array}$

(8a)

$\begin{array}{ll}\hfill {\mathbf{R}}_{{\mathbf{A}}_i}{\mathbf{t}}_{\mathbf{X}}+{\mathbf{t}}_{{\mathbf{A}}_i}& ={\mathbf{R}}_{\mathbf{Y}}{\mathbf{t}}_{{\mathbf{B}}_i}+{\mathbf{t}}_{\mathbf{Y}},\hfill \end{array}$

(8b)

where ${\mathbf{R}}_{{\mathbf{A}}_i},{\mathbf{R}}_{\mathbf{X}},{\mathbf{R}}_{\mathbf{Y}},{\mathbf{R}}_{{\mathbf{B}}_i}$ and ${\mathbf{t}}_{{\mathbf{A}}_i},{\mathbf{t}}_{\mathbf{X}},{\mathbf{t}}_{\mathbf{Y}},{\mathbf{t}}_{{\mathbf{B}}_i}$ are the rotation and translation components of A _i , X, Y, B _i , respectively.

Note that alternative interpretations of the AX = YB equations are possible. For example, consider defining

$\begin{array}{cc}\hfill {\mathbf{A}}_i^{\prime }& \triangleq {\mathbf{T}}_{hb}(i)={\mathbf{T}}_{bh}^{-1}(i),\hfill \\ \hfill {\mathbf{B}}_i^{\prime }& \triangleq {\mathbf{T}}_{ct}(i)={\mathbf{T}}_{tc}^{-1}(i).\hfill \end{array}$

(9)

The kinematic chain in equation (5) becomes

${\mathbf{A}}_i^{\prime }{\mathbf{T}}_{bt}={\mathbf{T}}_{hc}{\mathbf{B}}_i^{\prime },$

(10)

which swaps the roles of X and Y. Equation (10) corresponds to the setup used for our experiments in Real-world experiment, which is illustrated in Figure 4. This flexibility affords practitioners some freedom in defining variables for their RWHEC problem, but care must be taken to conform to the noise models discussed in the next section. ⁴

Maximum likelihood estimation

Using equations (8a) and (8b), we can formulate an MLE problem for the unknown states X and Y. We use $\tilde{\mathbf{x}}$ to indicate a noisy measurement of the true value $\underline{\mathbf{x}}$ of a quantity of interest x. Our probabilistic formulation of RWHEC requires the following assumptions:

1. ${\tilde{\mathbf{R}}}_{{\mathbf{A}}_i}={\underline{\mathbf{R}}}_{{\mathbf{A}}_i}$ and ${\tilde{\mathbf{t}}}_{{\mathbf{A}}_i}={\underline{\mathbf{t}}}_{{\mathbf{A}}_i}\forall i=1,\dots ,N$ (i.e., they are noiseless measurements);

2. ${\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}={\underline{\mathbf{t}}}_{{\mathbf{B}}_i}+{\mathit{ϵ}}_i$ , where ${\mathit{ϵ}}_i\sim \mathcal{N}\left(0,{{\sigma }_i}^2\mathbf{I}\right)$ and σ _i is the standard deviation of the translation component of measurements B _i ∀ i = 1, …, N; and

3. ${\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}={\underline{\mathbf{R}}}_{{\mathbf{B}}_i}{\mathbf{R}}_{{ϵ}_i}$ , where ${\mathbf{R}}_{{ϵ}_i}\sim \text{Lang}(\mathbf{I},{\kappa }_i)$ , where κ _i is the concentration of the rotations of the measurement B _i ∀ i = 1, …, N.

Note that each individual rotation and translation measurement is distributed independently of all the others. These assumptions are consistent with convention and accurately approximate many practical scenarios (Ha, 2023, Section IV.C), including the classic “eye-in-hand” setup shown in Figure 1, where measurement A _i is computed with a calibrated manipulator’s forward kinematics and B _i is a function of a noisy camera measurement of a target. In order to maintain consistency with the majority of robot-world and hand-eye calibration literature, we slightly abuse our notation and use undecorated matrices A _i and B _i : this is in contrast to our treatment in Geometric constraints, which used these symbols for exact or noiseless measurements. More precisely, we overload the idealized expressions in equation (6) and henceforth write

$\begin{array}{cc}\hfill {\mathbf{A}}_i& \triangleq {\underline{\mathbf{T}}}_{bh}(i)=\left[\begin{array}{cc}\hfill {\underline{\mathbf{R}}}_{{\mathbf{A}}_i}\hfill & \hfill {\underline{\mathbf{t}}}_{{\mathbf{A}}_i}\hfill \\ \hfill {\mathbf{0}}^{\top }\hfill & \hfill 1\hfill \end{array}\right],\hfill \\ \hfill {\mathbf{B}}_i& \triangleq {\tilde{\mathbf{T}}}_{tc}(i)=\left[\begin{array}{cc}\hfill {\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}\hfill & \hfill {\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}\hfill \\ \hfill {\mathbf{0}}^{\top }\hfill & \hfill 1\hfill \end{array}\right].\hfill \end{array}$

(11)

With the introduction of noise into B _i in equation (11), the constraints in equation (7) no longer hold exactly for the true values of X and Y. Indeed, the best estimate for X and Y is the solution to the following maximum likelihood problem:

$\underset{\mathbf{X},\mathbf{Y}\in \mathrm{S}\mathrm{E}(3)}{\max }p(\{{\mathbf{B}}_1,\dots ,{\mathbf{B}}_N\}|\mathbf{X},\mathbf{Y}).$

(12)

The random variables ${\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}$ and ${\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}$ are distributed independently of one another for all i ∈ [N] by assumption. The rotations and translations in random variables B _i and B _j at different measurement indices i ≠ j are also independently distributed by assumption. Thus, taking the negative log-likelihood of the objective in equation (12) gives us the following equivalent minimization problem:

$\underset{\mathbf{X},\mathbf{Y}\in \mathrm{S}\mathrm{E}(3)}{\min }-\sum _{i=1}^N(\log (p({\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}|\mathbf{X},\mathbf{Y}))+\log (p({\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}|\mathbf{X},\mathbf{Y}))).$

(13)

The conditional log-likelihood of ${\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}$ is attained by applying our Gaussian translation noise assumption to equation (8b):

$\begin{array}{cc}\hfill \log & p({\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}|\mathbf{X},\mathbf{Y})=\hfill \\ \hfill & -\frac{1}{2{\sigma }_i^2}{‖{\mathbf{R}}_{\mathbf{Y}}^{\top }\left({\underline{\mathbf{R}}}_{{\mathbf{A}}_i}{\mathbf{t}}_{\mathbf{X}}\right.\left.+{\underline{\mathbf{t}}}_{{\mathbf{A}}_i}-{\mathbf{t}}_{\mathbf{Y}}\right)-{\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}‖}_2^2.\hfill \end{array}$

(14)

Left-multiplying the Euclidean norm’s argument with R _Y makes the log-likelihood quadratic in decision variables X and Y:

$\begin{array}{cc}\hfill \log p({\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}|\mathbf{X},\mathbf{Y})& =\hfill \\ \hfill -\frac{1}{2{\sigma }_i^2}& {‖{\underline{\mathbf{R}}}_{{\mathbf{A}}_i}{\mathbf{t}}_{\mathbf{X}}+{\underline{\mathbf{t}}}_{{\mathbf{A}}_i}-{\mathbf{t}}_{\mathbf{Y}}-{\mathbf{R}}_{\mathbf{Y}}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}‖}_2^2,\hfill \end{array}$

(15)

which we recognize as a weighted error function for the translation constraint in equation (8b). Similarly, the log-likelihood of $p({\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}|\mathbf{X},\mathbf{Y})$ is attained by applying the Langevin noise assumption to equation (8a) (Rosen et al., 2019):

$\begin{array}{c}\hfill \log p({\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}|\mathbf{X},\mathbf{Y})=\\ \hfill -c({\kappa }_i)-& \frac{{\kappa }_i}{2}{‖{\underline{\mathbf{R}}}_{{\mathbf{A}}_i}{\mathbf{R}}_{\mathbf{X}}-{\mathbf{R}}_{\mathbf{Y}}{\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}‖}_{\mathrm{F}}^2+3,\hfill \end{array}$

(16)

which is a weighted error function for the rotation constraint in equation (8a). Our MLE problem is the following QCQP:

Problem 1

Maximum Likelihood Estimation for RWHEC

$\underset{\begin{array}{c}{\mathbf{R}}_{\mathbf{X}},{\mathbf{R}}_{\mathbf{Y}}\in \mathrm{S}\mathrm{O}(3)\\ {\mathbf{t}}_{\mathbf{X}},{\mathbf{t}}_{\mathbf{Y}}\in {\mathbb{R}}^3,s^2=1\end{array}}{\min }{J}_{\mathbf{t}}+J_{\mathbf{R}},$

(17)

where

$\begin{array}{ll}\hfill J_{\mathbf{t}}& \triangleq \frac{1}{2}\sum _{i=1}^N\frac{1}{{\sigma }_i^2}{‖{\underline{\mathbf{R}}}_{{\mathbf{A}}_i}{\mathbf{t}}_{\mathbf{X}}+s{\underline{\mathbf{t}}}_{{\mathbf{A}}_i}-{\mathbf{t}}_{\mathbf{Y}}-{\mathbf{R}}_{\mathbf{Y}}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}‖}_2^2\hfill \end{array}$

(18a)

$\begin{array}{ll}\hfill J_{\mathbf{R}}& \triangleq \frac{1}{2}\sum _{i=1}^N{\kappa }_i{‖{\underline{\mathbf{R}}}_{{\mathbf{A}}_i}{\mathbf{R}}_{\mathbf{X}}-{\mathbf{R}}_{\mathbf{Y}}{\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}‖}_{\mathrm{F}}^2.\hfill \end{array}$

(18b)

Note that to ensure that the terms of the objective are quadratic or constant, we have homogenized equation (18a) with the quadratically constrained variable s² = 1. Homogenization simplifies the SDP relaxation employed in Certifiably globally optimal extrinsic calibration and the analysis in Global optimality guarantees. ⁵ Additionally, we ensure that Problem 1 is a QCQP by using the following constraints for each SO(3) variable (Tron et al., 2015):

$\begin{array}{ll}\hfill \mathbf{R}{\mathbf{R}}^{\top }& =\mathbf{I},\hfill \end{array}$

(19a)

$\begin{array}{ll}\hfill {\mathbf{R}}^{\top }\mathbf{R}& =\mathbf{I},\hfill \end{array}$

(19b)

$\begin{array}{ll}\hfill {\mathbf{R}}_{{\sigma }_i(1)}\times {\mathbf{R}}_{{\sigma }_i(2)}& ={\mathbf{R}}_{{\sigma }_i(3)},\forall i\in \{\mathrm{1,2,3}\},\hfill \end{array}$

(19c)

where R _i denotes the ith column of R, and G ≜{σ _i } is the group of cyclic permutations of {1, 2, 3}. ⁶ For generic R ∈ SO(d), the constraints would include det(R) = 1. However, the determinant is a polynomial of degree d, which for d = 3 is cubic. For SO(3) in particular, we can replace det(R) = 1 with the quadratic equation (19c), which ensures the columns of R obey the right-hand-rule.

Monocular cameras

If the scale of a target observed by a monocular camera is unknown, we can use a scaled pose sensor abstraction to model measurements. Repeating the MLE derivation from Maximum likelihood estimation, we replace the camera translation measurement model with

${\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}=\alpha {\mathbf{R}}_{\mathbf{Y}}^{\top }({\underline{\mathbf{R}}}_{{\mathbf{A}}_i}{\mathbf{t}}_{\mathbf{X}}+{\underline{\mathbf{t}}}_{{\mathbf{A}}_i}-{\mathbf{t}}_{\mathbf{Y}})+{\mathit{ϵ}}_i,$

(20)

where $\alpha \in \mathbb{R}$ is the unknown scale, ${\mathit{ϵ}}_i\sim \mathcal{N}\left(0,{\sigma }_i^2\mathbf{I}\right)$ , and σ _i is the standard deviation of the unscaled translation estimate. Consequently, the monocular MLE problem is the following QCQP:

Problem 2

Maximum Likelihood Estimation for Monocular RWHEC

$\underset{\begin{array}{c}{\mathbf{R}}_{\mathbf{X}},{\mathbf{R}}_{\mathbf{Y}}\in \mathrm{S}\mathrm{O}(3)\\ {\mathbf{t}}_{\mathbf{X},\alpha },{\mathbf{t}}_{\mathbf{Y},\alpha }\in {\mathbb{R}}^3,\alpha \in \mathbb{R}\end{array}}{\min }{J}_{\mathbf{t},\alpha }+J_{\mathbf{R}},$

(21)

where

$J_{\mathbf{t},\alpha }\triangleq \frac{1}{2}\sum _{i=1}^N\frac{1}{{\sigma }_i^2}{‖{\underline{\mathbf{R}}}_{{\mathbf{A}}_i}{\mathbf{t}}_{\mathbf{X},\alpha }+\alpha {\underline{\mathbf{t}}}_{{\mathbf{A}}_i}-{\mathbf{t}}_{\mathbf{Y},\alpha }-{\mathbf{R}}_{\mathbf{Y}}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}‖}_2^2.$

(22)

The variables t_X,α ≜ αt _X and t_Y,α ≜ αt _Y have “absorbed” the scale parameter α and must be divided by α after solving Problem 2. To maintain our assumption from Maximum likelihood estimation that measurements A _i are noiseless, the measurements B _i are assumed to come from the monocular camera as in Figure 1. Note that while a negative scale factor does not make physical sense, we have not included a positivity constraint on α in Problem 2. This is because we are focused on cases where measurement noise is low or moderate, and previous work on calibration of a single monocular camera suggests that α is positive at the global optimum in this noise regime (Wise et al., 2020). Therefore, we assume that α ≥ 0 holds but is not active (i.e., α > 0) for global optima of Problem 2. This assumption greatly simplifies our analysis and is borne out by our Simulation experiments and Real-world experiment. The analysis and solution method to follow can incorporate α ≥ 0, but analyzing the effect of its inclusion in extremely noisy problem instances is left for future work.

Our introduction of the unknown scale parameter α has produced a naturally homogeneous QCQP, obviating the need for the homogenizing variable s used in Problem 1, which can now be interpreted as the special case of Problem 2 for known scale α = 1. Therefore, in Quadratically constrained quadratic programming, we will deal solely with the monocular case in Problem 2.

Quadratically constrained quadratic programming

Herein we convert Problem 2 to a standard QCQP form with a vectorized decision variable and constraints defined by real symmetric matrices. The state vector is

$\mathbf{x}\triangleq {\left[\begin{array}{ccccc}\hfill {\mathbf{t}}_{\mathbf{X},\alpha }^{\top }\hfill & \hfill {\mathbf{t}}_{\mathbf{Y},\alpha }^{\top }\hfill & \hfill {\mathbf{r}}_{\mathbf{X}}^{\top }\hfill & \hfill {\mathbf{r}}_{\mathbf{Y}}^{\top }\hfill & \hfill \alpha \hfill \end{array}\right]}^{\top },$

(23)

where ${\mathbf{r}}_{\mathbf{X}}=\mathrm{v}\mathrm{e}\mathrm{c}\left({\mathbf{R}}_{\mathbf{X}}\right)$ and ${\mathbf{r}}_{\mathbf{Y}}=\mathrm{v}\mathrm{e}\mathrm{c}\left({\mathbf{R}}_{\mathbf{Y}}\right)$ . Equation (23) allows us to write the rotation part of the objectives of monocular RWHEC problems as

$J_{\mathbf{R}}=\frac{1}{2}\sum _{i=1}^N{\kappa }_i{\mathbf{x}}^{\top }{\mathbf{M}}_{{\mathbf{R}}_i}^{\top }{\mathbf{M}}_{{\mathbf{R}}_i}\mathbf{x},$

(24)

where

${\mathbf{M}}_{{\mathbf{R}}_i}\triangleq \left[\begin{array}{cccc}\hfill {\mathbf{0}}_{9\times 6}\hfill & \hfill \mathbf{I}\otimes {\underline{\mathbf{R}}}_{{\mathbf{A}}_i}\hfill & \hfill -{\tilde{\mathbf{R}}}_{{\mathbf{B}}_i}^{\top }\otimes \mathbf{I}\hfill & \hfill {\mathbf{0}}_{9\times 1}\hfill \end{array}\right],$

(25)

Equation (25) and many expressions to follow are obtained through a straightforward application of the column-major vectorization identity (Henderson and Searle, 1981)

$\mathrm{v}\mathrm{e}\mathrm{c}\left(\mathbf{A}\mathbf{X}\mathbf{B}\right)=({\mathbf{B}}^{\top }\otimes \mathbf{A})\mathrm{v}\mathrm{e}\mathrm{c}\left(\mathbf{X}\right),$

(26)

where A, X, and B are any compatible matrices. The translation components of the monocular RWHEC problem’s objective can now be written as

$J_{\mathbf{t}}=\frac{1}{2}\sum _{i=1}^N\frac{1}{{\sigma }_i^2}{\mathbf{x}}^{\top }{\mathbf{M}}_{{\mathbf{t}}_i}^{\top }{\mathbf{M}}_{{\mathbf{t}}_i}\mathbf{x},$

(27)

where

${\mathbf{M}}_{{\mathbf{t}}_i}\triangleq \left[\begin{array}{ccccc}\hfill {\underline{\mathbf{R}}}_{{\mathbf{A}}_i}\hfill & \hfill -\mathbf{I}\hfill & \hfill {\mathbf{0}}_{3\times 9}\hfill & \hfill -{\tilde{\mathbf{t}}}_{{\mathbf{B}}_i}^{\top }\otimes \mathbf{I}\hfill & \hfill {\underline{\mathbf{t}}}_{{\mathbf{A}}_i}\hfill \end{array}\right].$

(28)

The objective function of Problem 2 is now completely described by a quadratic forms with associated symmetric matrix

$\mathbf{Q}\triangleq \frac{1}{2}\sum _{i=1}^N{\kappa }_i{\mathbf{M}}_{{\mathbf{R}}_i}^{\top }{\mathbf{M}}_{{\mathbf{R}}_i}+\frac{1}{2}\sum _{i=1}^N\frac{1}{{\sigma }_i^2}{\mathbf{M}}_{{\mathbf{t}}_i}^{\top }{\mathbf{M}}_{{\mathbf{t}}_i}.$

(29)

Consequently, we can rewrite Problem 2 as

$\begin{array}{cc}\hfill \underset{\mathbf{x}}{\min }& {\mathbf{x}}^{\top }\mathbf{Q}\mathbf{x},\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{R}}_{\mathbf{X}},{\mathbf{R}}_{\mathbf{Y}}\in \mathrm{S}\mathrm{O}(3).\hfill \end{array}$

(30)

Generalizing RWHEC to multiple sensors and targets

We can extend robot-world and hand-eye calibration to robots employing more than one sensor, and calibration procedures involving more than one target (Wang et al., 2022). In this generalized form, we jointly estimate a collection of M hand-eye transformations X₁, …, X _M and P base-target transformations Y₁, …, Y _P . As shown in Figure 2, we can model the set of available measurements as a bipartite directed graph $\vec{\mathcal{G}}=(\mathcal{V},\vec{\mathcal{E}})$ , where

$\mathcal{V}\triangleq {\mathcal{V}}_{\mathbf{X}}\cup {\mathcal{V}}_{\mathbf{Y}}=[M+P],$

(31)

and each edge $e=(j,k)\in \vec{\mathcal{E}}\subseteq {\mathcal{V}}_{\mathbf{X}}\times {\mathcal{V}}_{\mathbf{Y}}$ represents a set of observations of target k by camera j. This multi-frame approach was introduced by Wang et al. (2022) for problems like “multiple eye-in-hand” calibration where either M = 1 or P = 1.

Figure 2.

An example of the bipartite graph structure of a simple generalized RWHEC problem with multiple Xs and Ys. Each edge corresponds to a set of measurements ${\mathcal{D}}_e$ in equation (32) forming an instance of Problem 1 or Problem 2 involving X _j and Y _k for e = (j, k).

The notation we use to describe the generalized RWHEC is inspired by the elegant graph-theoretic treatment of pose SLAM in Rosen et al. (2019). One notable feature of our formulation is that all observations involving variables X _j and Y _k are associated with a single directed edge (j, k). Therefore, each edge $e=(j,k)\in \vec{\mathcal{E}}$ is labeled with a set ${\mathcal{D}}_e$ of all N _e ≥ 1 noisy observations involving unknown variables X _j and Y _k :

${\mathcal{D}}_e\triangleq \{({\mathbf{A}}_{e,i},{\mathbf{B}}_{e,i},{\sigma }_{e,i},{\kappa }_{e,i})\in \mathrm{S}\mathrm{E}{(3)}^2\times {\mathbb{R}}_{+}^2|i\in [N_e]\}.$

(32)

Our approach ensures that $\vec{\mathcal{G}}$ is simple, whereas an alternative formulation could use a directed multigraph whose edges correspond to individual observations between X _j and Y _k . The problem graph $\vec{\mathcal{G}}$ summarizes the connection between $|\vec{\mathcal{E}}|$ coupled RWHEC subproblems of the form in Problem 1 or Problem 2. This graphical structure enables us to describe a joint RWHEC problem involving all unknown variables X _j and Y _k indexed by $\mathcal{V}$ :

Problem 3

Generalized Monocular RWHEC

$\underset{\begin{array}{c}{\mathbf{X}}_j,{\mathbf{Y}}_k\in \mathrm{S}\mathrm{E}(3),\\ \alpha \in \mathbb{R}\end{array}}{\min }\frac{1}{2}\sum _{(j,k)\in \vec{\mathcal{E}}}\sum _{i=1}^{N_{(j,k)}}{J}_{\mathit{ijk}}({\mathbf{X}}_j,{\mathbf{Y}}_k,\alpha ),$

where

$\begin{array}{cc}\hfill & J_{\mathit{ijk}}(\mathbf{X},\mathbf{Y},\alpha )\triangleq \hfill \\ \hfill & \frac{1}{{\sigma }_{(j,k),i}^2}{‖{\underline{\mathbf{R}}}_{{\mathbf{A}}_{(j,k),i}}{\mathbf{t}}_{\mathbf{X}}+\alpha {\underline{\mathbf{t}}}_{{\mathbf{A}}_{(j,k),i}}-{\mathbf{t}}_{\mathbf{Y}}-{\mathbf{R}}_{\mathbf{Y}}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_{(j,k),i}}‖}_2^2\hfill \\ \hfill & +{\kappa }_{(j,k),i}{‖{\underline{\mathbf{R}}}_{{\mathbf{A}}_{(j,k),i}}{\mathbf{R}}_{\mathbf{X}}-{\mathbf{R}}_{\mathbf{Y}}{\tilde{\mathbf{R}}}_{{\mathbf{B}}_{(j,k),i}}‖}_{\mathrm{F}}^2.\hfill \end{array}$

(33)

It is worth noting that Problem 2 is a special case of Problem 3 for a graph $\vec{\mathcal{G}}$ with only two vertices (M = 1 = P). Additionally, we will refer to the special case of Problem 3 with known scale (α = 1) as the standard RWHEC problem:

Problem 4

Generalized Standard RWHEC

$\underset{{\mathbf{X}}_j,{\mathbf{Y}}_k\in \mathrm{S}\mathrm{E}(3)}{\min }\frac{1}{2}\sum _{(j,k)\in \vec{\mathcal{E}}}\sum _{i=1}^{N_{(j,k)}}{J}_{\mathit{ijk}}({\mathbf{X}}_j,{\mathbf{Y}}_k,1).$

For the remainder of this section, we will concern ourselves solely with generalized monocular RWHEC formulation in Problem 3. Our Simulation experiments and Real-world experiment deal with both the standard and monocular cases, but our identifiability analysis in Uniqueness of solutions only applies to Problem 4.

While our problem formulation admits the use of heterogeneous measurement precisions, to ease notation in the sequel we will assume that all measurements associated with a single edge $e\in \vec{\mathcal{E}}$ have common rotational and translational precisions: ⁷

${\sigma }_{e,i}={\sigma }_e,{\kappa }_{e,i}={\kappa }_e\forall i\in [N_e].$

(34)

For the monocular case, the parameterization of unknown scale used in equation (22) limits our multi-sensor extension to cases using either a single target, or targets with the same unknown scale α (e.g., the fiducial markers of equal size deployed in our Real-world experiment). Our approach can be easily extended to support multiple unknown target scales by introducing target-specific scales α _i or using the conformal special orthogonal group CSO(3) employed by Yu and Yang (2024). ⁸

We can simplify our notation by introducing several matrices structured by blocks mirroring the graph Laplacian (Rosen et al., 2019, Section 4.1). Let $L({\vec{\mathcal{G}}}^{\tau })\in {\mathbb{S}}^{3(M+P)}$ be the symmetric (3 × 3)-block-structured matrix of the form

$L{({\vec{\mathcal{G}}}^{\tau })}_{jk}=\left\{\begin{array}{cc}\sum _{e\in {\delta }^{-}(j)}\frac{1}{{\sigma }_e^2}{N}_e\mathbf{I}\hfill & j=k\hfill \\ -\frac{1}{{\sigma }_{(j,k)}^2}\sum _{i=1}^{N_{(j,k)}}{\underline{\mathbf{R}}}_{{\mathbf{A}}_{(j,k),i}}^{\top }\hfill & (j,k)\in \vec{\mathcal{E}}\hfill \\ -\frac{1}{{\sigma }_{(k,j)}^2}\sum _{i=1}^{N_{(k,j)}}{\underline{\mathbf{R}}}_{{\mathbf{A}}_{(k,j),i}}\hfill & (k,j)\in \vec{\mathcal{E}}\hfill \\ {\mathbf{0}}_{3\times 3}\hfill & \text{\hspace{0.17em}otherwise}.\hfill \end{array}\right.$

(35)

Similarly, let $L({\vec{\mathcal{G}}}^{\rho })\in {\mathbb{S}}^{9(M+P)}$ be the symmetric (9 × 9)-block-structured matrix such that

$\begin{array}{cc}\hfill L& {({\vec{\mathcal{G}}}^{\rho })}_{jk}=\hfill \\ \hfill & \left\{\begin{array}{cc}\sum _{e\in {\delta }^{-}(j)}{\kappa }_e{N}_e\mathbf{I}\otimes \mathbf{I}\hfill & j=k\hfill \\ -{\kappa }_{(j,k)}\sum _{i=1}^{N_{(j,k)}}{\tilde{\mathbf{R}}}_{{\mathbf{B}}_{(j,k),i}}\otimes {\underline{\mathbf{R}}}_{{\mathbf{A}}_{(j,k),i}}^{\top }\hfill & (j,k)\in \vec{\mathcal{E}}\hfill \\ -{\kappa }_{(k,j)}\sum _{i=1}^{N_{(k,j)}}{\tilde{\mathbf{R}}}_{{\mathbf{B}}_{(k,j),i}}^{\top }\otimes {\underline{\mathbf{R}}}_{{\mathbf{A}}_{(k,j),i}}\hfill & (k,j)\in \vec{\mathcal{E}}\hfill \\ {\mathbf{0}}_{9\times 9}\hfill & \text{\hspace{0.17em}otherwise}.\hfill \end{array}\right.\hfill \end{array}$

(36)

Let $\mathbf{v}\in {\mathbb{R}}^{3(M+P)\times 1}$ be the (3 × 1)-block-structured vector with

${\mathbf{v}}_l=\left\{\begin{array}{cc}\sum _{e\in {\delta }^{-}(l)}\frac{1}{{\sigma }_e^2}\sum _{i=1}^{N_e}{\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}}^{\top }{\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,i}}\hfill & l\le M\hfill \\ -\sum _{e\in {\delta }^{+}(l)}\frac{1}{{\sigma }_e^2}\sum _{i=1}^{N_e}{\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,i}}\hfill & \text{\hspace{0.17em}otherwise},\hfill \end{array}\right.$

(37)

and

$v=\sum _{e\in \vec{\mathcal{E}}}\frac{1}{{\sigma }_e^2}\sum _{i=1}^{N_e}{\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,i}}^{\top }{\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,i}}.$

(38)

The matrix Σ is the symmetric (9 × 9)-block-structured matrix determined by

${\mathbf{\Sigma }}_{jk}=\left\{\begin{array}{cc}\sum _{e\in {\delta }^{+}(j)}\frac{1}{{\sigma }_e^2}\sum _{i=1}^{N_e}({\tilde{\mathbf{t}}}_{{\mathbf{B}}_{e,i}}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_{e,i}}^{\top })\otimes \mathbf{I}\hfill & j=k\hfill \\ {\mathbf{0}}_{9\times 9}\hfill & \text{\hspace{0.17em}otherwise}.\hfill \end{array}\right.$

(39)

Let $\mathbf{U}\in {\mathbb{R}}^{3(M+P)\times 9(M+P)}$ be the (3 × 9)-block-structured matrix

${\mathbf{U}}_{jk}=\left\{\begin{array}{cc}\sum _{e\in {\delta }^{+}(j)}\frac{1}{{\sigma }_e^2}\sum _{i=1}^{N_e}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_{e,i}}^{\top }\otimes \mathbf{I}\hfill & j=k\hfill \\ -\frac{1}{{\sigma }_e^2}\sum _{i=1}^{N_{(j,k)}}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_{(j,k),i}}^{\top }\otimes {\underline{\mathbf{R}}}_{{\mathbf{A}}_{(j,k),i}}^{\top }\hfill & (j,k)\in \vec{\mathcal{E}},\hfill \\ {\mathbf{0}}_{3\times 9}\hfill & \text{otherwise}.\hfill \end{array}\right.$

(40)

Let $\mathbf{u}\in {\mathbb{R}}^{9(M+P)\times 1}$ be the (9 × 1)-block-structured vector

${\mathbf{u}}_l=\left\{\begin{array}{cc}\mathbf{0}\hfill & l\le M\hfill \\ -\sum _{e\in {\delta }^{+}(l)}\sum _{i=1}^{N_e}{\tilde{\mathbf{t}}}_{{\mathbf{B}}_{e,i}}\otimes {\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,i}}\hfill & \text{\hspace{0.17em}otherwise}.\hfill \end{array}\right.$

(41)

Using the matrices in equations (35) to (41), we can define the monocular RWHEC objective function matrix as follows:

$\mathbf{Q}=\frac{1}{2}\left[\begin{array}{ccc}\hfill L({\vec{\mathcal{G}}}^{\tau })\hfill & \hfill \mathbf{v}\hfill & \hfill \mathbf{U}\hfill \\ \hfill {\mathbf{v}}^{\top }\hfill & \hfill v\hfill & \hfill {\mathbf{u}}^{\top }\hfill \\ \hfill {\mathbf{U}}^{\top }\hfill & \hfill \mathbf{u}\hfill & \hfill \mathbf{\Sigma }+L({\vec{\mathcal{G}}}^{\rho })\hfill \end{array}\right].$

(42)

With this new notation, the standard optimization problem for multiple Xs and Ys can be stated as a QCQP whose objective function is a quadratic form:

Problem 5

Homogeneous QCQP Formulation of Monocular Generalized RWHEC

$\begin{array}{cc}\hfill \underset{\mathbf{x}}{\min }& {\mathbf{x}}^{\top }\mathbf{Q}\mathbf{x}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{R}}_{{\mathbf{X}}_j}\in \mathrm{S}\mathrm{O}(3)\forall j\in {\mathcal{V}}_{\mathbf{X}}\hfill \\ \hfill & {\mathbf{R}}_{{\mathbf{Y}}_k}\in \mathrm{S}\mathrm{O}(3)\forall k\in {\mathcal{V}}_{\mathbf{Y}}.\hfill \end{array}$

(43)

The state vector used in Problem 5 is

$\mathbf{x}\triangleq {\left[\begin{array}{ccc}\hfill {\mathbf{t}}^{\top }\hfill & \hfill \alpha \hfill & \hfill {\mathbf{r}}^{\top }\hfill \end{array}\right]}^{\top },$

(44)

where

$\begin{array}{c}\mathbf{t}\triangleq {\left[\begin{array}{cc}\hfill {\mathbf{t}}_x^{\top }\hfill & \hfill {\mathbf{t}}_y^{\top }\hfill \end{array}\right]}^{\top }\hfill \\ {\mathbf{t}}_x\triangleq {\left[\begin{array}{ccc}\hfill {\mathbf{t}}_{{\mathbf{X}}_1}^{\top }\hfill & \hfill \cdots \hfill & \hfill {\mathbf{t}}_{{\mathbf{X}}_M}^{\top }\hfill \end{array}\right]}^{\top }\hfill \\ {\mathbf{t}}_y\triangleq {\left[\begin{array}{ccc}\hfill {\mathbf{t}}_{{\mathbf{Y}}_1}^{\top }\hfill & \hfill \cdots \hfill & \hfill {\mathbf{t}}_{{\mathbf{Y}}_P}^{\top }\hfill \end{array}\right]}^{\top },\hfill \end{array}$

(45)

and

$\begin{array}{cc}\hfill \mathbf{r}& \triangleq {\left[\begin{array}{cc}\hfill {\mathbf{r}}_x^{\top }\hfill & \hfill {\mathbf{r}}_y^{\top }\hfill \end{array}\right]}^{\top }\hfill \\ \hfill {\mathbf{r}}_x& \triangleq {\left[\begin{array}{ccc}\hfill \mathrm{v}\mathrm{e}\mathrm{c}{\left({\mathbf{R}}_{{\mathbf{X}}_1}\right)}^{\top }\hfill & \hfill \cdots \hfill & \hfill {\mathrm{v}\mathrm{e}\mathrm{c}\left({\mathbf{R}}_{{\mathbf{X}}_M}\right)}^{\top }\hfill \end{array}\right]}^{\top }\hfill \\ \hfill {\mathbf{r}}_y& \triangleq {\left[\begin{array}{ccc}\hfill \mathrm{v}\mathrm{e}\mathrm{c}{\left({\mathbf{R}}_{{\mathbf{Y}}_1}\right)}^{\top }\hfill & \hfill \cdots \hfill & \hfill \mathrm{v}\mathrm{e}\mathrm{c}{\left({\mathbf{R}}_{{\mathbf{Y}}_P}\right)}^{\top }\hfill \end{array}\right]}^{\top }.\hfill \end{array}$

(46)

Note that the order of translation, scale, and rotation variables in equation (44) is not the same as in equation (23). This change simplifies the presentation of the reduction used in Reducing the dimension of the QCQP.

Reducing the dimension of the QCQP

If we know the optimal rotation matrices ${\mathbf{R}}_{{\mathbf{X}}_j}^{\star },{\mathbf{R}}_{{\mathbf{Y}}_k}^{\star }$ for $j\in {\mathcal{V}}_{\mathbf{X}}$ and $k\in {\mathcal{V}}_{\mathbf{Y}}$ , then the unconstrained optimal X translation vectors ${\mathbf{t}}_{{\mathbf{X}}_j}^{\star }$ for $j\in {\mathcal{V}}_{\mathbf{X}}$ , Y translation vectors ${\mathbf{t}}_{{\mathbf{Y}}_k}^{\star }$ for $k\in {\mathcal{V}}_{\mathbf{Y}}$ , and scale α^⋆ can be recovered by solving the following linear system:

${\left[\begin{array}{cc}\hfill {{\mathbf{t}}_{\alpha }^{\star }}^{\top }\hfill & \hfill {\alpha }^{\star }\hfill \end{array}\right]}^{\top }=-{\left[\begin{array}{cc}\hfill L({\vec{\mathcal{G}}}^{\tau })\hfill & \hfill \mathbf{v}\hfill \\ \hfill {\mathbf{v}}^{\top }\hfill & \hfill v\hfill \end{array}\right]}^{†}\left[\begin{array}{c}\hfill \mathbf{U}\hfill \\ \hfill {\mathbf{u}}^{\top }\hfill \end{array}\right]{\mathbf{r}}^{\star }.$

(47)

Using the generalized Schur complement (Gallier, 2010), we reduce Q to

${\mathbf{Q}}^{\prime }=\frac{1}{2}\left(\mathbf{\Sigma }+L({\vec{\mathcal{G}}}^{\rho })-\left[\begin{array}{cc}\hfill {\mathbf{U}}^{\top }\hfill & \hfill \mathbf{u}\hfill \end{array}\right]{\left[\begin{array}{cc}\hfill L({\vec{\mathcal{G}}}^{\tau })\hfill & \hfill \mathbf{v}\hfill \\ \hfill {\mathbf{v}}^{\top }\hfill & \hfill v\hfill \end{array}\right]}^{†}\left[\begin{array}{c}\hfill \mathbf{U}\hfill \\ \hfill {\mathbf{u}}^{\top }\hfill \end{array}\right]\right).$

(48)

The result is a reduced form of Problem 5 that only depends on the rotation variables:

Problem 6

Reduced QCQP Formulation of Generalized Monocular RWHEC

$\begin{array}{cc}\hfill \underset{\mathbf{r}\in {\mathbb{R}}^{9(M+P)}}{\min }& {\mathbf{r}}^{\top }{\mathbf{Q}}^{\prime }\mathbf{r},\hfill \\ \hfill \text{s.t.}& {\mathbf{R}}_{{\mathbf{X}}_j}\in \mathrm{S}\mathrm{O}(3)\forall j\in {\mathcal{V}}_{\mathbf{X}},\hfill \\ \hfill & {\mathbf{R}}_{{\mathbf{Y}}_k}\in \mathrm{S}\mathrm{O}(3)\forall k\in {\mathcal{V}}_{\mathbf{Y}}.\hfill \end{array}$

(49)

Certifiably globally optimal extrinsic calibration

In this section, we present a convex SDP relaxation of our calibration problem. Deriving the standard Shor relaxation of Problem 6 and its dual requires the homogenization of the quadratic SO(3) constraints in equation (19a)–(19c) (Cifuentes et al., 2022, Section 1). Specifically, we introduce quadratically constrained variable s² = 1 and homogenize the linear and constant parts of equations (19a)–(19c):

$\begin{array}{ll}\hfill \mathbf{R}{\mathbf{R}}^{\top }& =s^2\mathbf{I},\hfill \end{array}$

(50a)

$\begin{array}{ll}\hfill {\mathbf{R}}^{\top }\mathbf{R}& =s^2\mathbf{I},\hfill \end{array}$

(50b)

$\begin{array}{ll}\hfill {\mathbf{R}}_{{\sigma }_i(1)}\times {\mathbf{R}}_{{\sigma }_i(2)}& =s{\mathbf{R}}_{{\sigma }_i(3)}\forall i\in \{\mathrm{1,2,3}\}.\hfill \end{array}$

(50c)

Treating the special case with known scale (α = 1) in Problem 4 also requires that we replace α in Problem 6 with s, which ensures that the objective function is homogeneous.

Applying the theory of duality for generalized inequalities to the homogenized constraints in Problem 6 yields the following expression for its Lagrangian (Boyd and Vandenberghe, 2004, Section 5.9):

$\mathcal{L}\left(\mathbf{x},\mathit{\lambda }\right)={\lambda }_s+{\mathbf{x}}^{\top }\mathbf{Z}(\mathit{\lambda })\mathbf{x},$

(51)

where λ _s and λ are dual variables and the expression for Z(λ) can be found in Appendix A. The minimum of the Lagrangian function is only defined if Z(λ)⪰0. As a result, the Lagrangian dual problem is the following SDP:

Problem 7

Dual of Monocular RWHEC

$\begin{array}{cc}\hfill \underset{\mathit{\lambda }}{\max }& {\lambda }_s,\hfill \\ \hfill \text{s.t.}& \mathbf{Z}(\mathit{\lambda })⪰0,\hfill \end{array}$

(52)

where $\mathit{\lambda }\in {\mathbb{R}}^{22\left(M+P\right)+1}$ and λ _s is the component of λ corresponding to the homogenizing constraint s² = 1 in the primal problem.

Our complete certifiable RWHEC algorithm is presented in Algorithm 1. Given a solution λ^⋆ to dual Problem 7, the KKT conditions for SDPs tell us that solution r^⋆ to the corresponding primal problem is a null vector of Z(λ^⋆) (Cifuentes et al., 2022). Given a null vector of Z(λ^⋆) with terminal element s′ corresponding to the homogenizing variable s, we can scale it by 1/s′ in order to recover r^⋆. Finally, we can determine the optimal α^⋆ and scaled translation t^⋆ with equation (47) and correct t^⋆ by a factor of 1/α^⋆.

In addition to providing the computational benefits of solving a convex problem, our SDP relaxation allows us to numerically certify the global optimality of a primal solution x^⋆: by weak Lagrangian duality (Boyd and Vandenberghe, 2004), ${\lambda }_s^{\star }$ is a lower bound on the optimal objective value of the primal Problem 6. Therefore, when this duality gap is small with respect to machine precision, we obtain a post hoc certificate of the optimality of our primal solution. We provide an example of this certification procedure in our Real-world experiment.

Uniqueness of solutions

In this section, we derive conditions on measurement data which ensure that the standard generalized RWHEC formulation of Problem 4 has a unique solution in the absence of noise (i.e., sufficient conditions for a problem instance to be an identifiable model). In addition to precisely characterizing which robot and sensor motions lead to a well-posed calibration problem with identifiable extrinsic parameters, the results in this section are used in Global optimality guarantees to prove that SDP relaxations of our QCQP are tight, even when noisy measurements are used. A similar analysis is conducted for hand-eye calibration of a single monocular camera in Andreff et al. (2001) and the standard RWHEC problem in Shah (2013), but to our knowledge the multi-sensor and multi-target case has not been addressed until now. We leave the derivation of similar results for the more complex monocular case of Problem 3 for future work.

The rotation-only case

We begin by characterizing the uniqueness of solutions to the RWHEC problem with SO(3)-valued data:

Problem 8

Rotation-Only Generalized RWHEC

$\begin{array}{cc}\hfill \underset{\mathbf{x}}{\min }& \frac{1}{2}\sum _{(j,k)\in \vec{\mathcal{E}}}\sum _{i=1}^{N_{(j,k)}}{‖{\mathbf{R}}_{{\mathbf{A}}_{(j,k),i}}{\mathbf{R}}_{{\mathbf{X}}_j}-{\mathbf{R}}_{{\mathbf{Y}}_k}{\mathbf{R}}_{{\mathbf{B}}_{(j,k),i}}‖}_{\mathrm{F}}^2\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{R}}_{{\mathbf{X}}_j}\in \mathrm{S}\mathrm{O}(3)\forall j\in [M]\hfill \\ \hfill & {\mathbf{R}}_{{\mathbf{Y}}_k}\in \mathrm{S}\mathrm{O}(3)\forall k\in [P].\hfill \end{array}$

(53)

Problem 8 with a single pair of variables R _X and R _Y is sometimes called conjugate rotation averaging in the computer vision literature (Hartley et al., 2013). It is essentially Problem 4 without translation variables, and with ${\kappa }_{e,i}=1\forall e\in \vec{\mathcal{E}}$ for notational convenience.

Theorem 1

Consider an instance of Problem 8 induced by a weakly connected bipartite directed graph $\vec{\mathcal{G}}=(\mathcal{V},\vec{\mathcal{E}})$ with exact rotation measurements ${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}},{\underline{\mathbf{R}}}_{{\mathbf{B}}_{e,i}}\in \mathrm{S}\mathrm{O}(3)$ for i ∈ [N _e ] associated with each edge $e\in \vec{\mathcal{E}}$ . If a single rotation ${\mathbf{R}}_{{\mathbf{X}}_v}$ or ${\mathbf{R}}_{{\mathbf{Y}}_v}$ for some $v\in \mathcal{V}$ can be uniquely determined with the problem data, then this instance of Problem 8 has a unique solution.

Proof. Exact measurements imply that each residual in Problem 8 is zero:

${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}}{\mathbf{R}}_{{\mathbf{X}}_j}{\underline{\mathbf{R}}}_{{\mathbf{B}}_{e,i}}^{\top }={\mathbf{R}}_{{\mathbf{Y}}_k}\forall e\in \vec{\mathcal{E}},i\in [N_e].$

(54)

Equation (54) gives us an explicit expression for ${\mathbf{R}}_{{\mathbf{Y}}_k}$ in terms of ${\mathbf{R}}_{{\mathbf{X}}_j}$ for any $(j,k)\in \vec{\mathcal{E}}$ . Since ${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}}$ and ${\underline{\mathbf{R}}}_{{\mathbf{B}}_{e,i}}$ are rotation matrices and therefore invertible, we can also solve equation (54) for ${\mathbf{R}}_{{\mathbf{X}}_j}$ in terms of ${\mathbf{R}}_{{\mathbf{Y}}_k}$ :

${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}}^{\top }{\mathbf{R}}_{{\mathbf{Y}}_k}{\underline{\mathbf{R}}}_{{\mathbf{B}}_{e,i}}={\mathbf{R}}_{{\mathbf{X}}_j}\forall e\in \vec{\mathcal{E}},i\in [N_e].$

(55)

Now suppose (by our hypothesis) that there is some $v\in \mathcal{V}$ such that we can uniquely identify the associated rotation ${\mathbf{R}}_{{\mathbf{X}}_v}$ or ${\mathbf{R}}_{{\mathbf{Y}}_k}$ . Since $\vec{\mathcal{G}}$ is weakly connected, there is a semi-path connecting v and any other $w\in \mathcal{V}$ . By repeatedly applying the identities in equations (54) and (55), we can uniquely identify the rotations associated with each vertex along the semi-path joining v to w. Since every vertex in $\vec{\mathcal{G}}$ can be joined to v by such a path, this shows that the rotations associated with all vertices in $\vec{\mathcal{G}}$ are uniquely identifiable.

Theorem 1 tells us that in the idealized noise-free version of Problem 8, a single rotation with a unique solution implies that the entire problem has a unique solution. The following corollary is a direct consequence of our result and Theorem 2.3 in Shah (2013):

Corollary 2

Consider an instance of Problem 8 with weakly connected graph $\vec{\mathcal{G}}=(\mathcal{V},\vec{\mathcal{E}})$ and exact measurements. If there exists an edge $e=(j,k)\in \vec{\mathcal{E}}$ with i₁, i₂, i₃ ∈ [N _e ] such that ${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_2}}^{\top }{\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_1}}$ and ${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_3}}^{\top }{\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_1}}$ have distinct principal axes, then there is a unique solution. Furthermore, this solution is an element of a 1-dimensional vector space containing all unconstrained minimizers of the homogeneous objective function of Problem 8.

Proof. Theorem 2.3 in Shah (2013) tells us that there are unique ${\mathbf{R}}_{{\mathbf{X}}_j}$ and ${\mathbf{R}}_{{\mathbf{Y}}_k}$ minimizing Problem 8, satisfying the hypothesis of Theorem 1 and guaranteeing the existence of a unique solution. If we use equation (26) to vectorize the inner sum over i in the homogeneous objective of Problem 8 for the edge e = (j, k) satisfying our hypotheses we get

$\begin{array}{cc}\hfill \sum _{i=1}^{N_{(j,k)}}& {‖{\mathbf{R}}_{{\mathbf{A}}_{(j,k),i}}{\mathbf{R}}_{{\mathbf{X}}_j}-{\mathbf{R}}_{{\mathbf{Y}}_k}{\mathbf{R}}_{{\mathbf{B}}_{(j,k),i}}‖}_{\mathrm{F}}^2=\hfill \\ \hfill & {‖{\mathbf{Q}}_{(j,k)}{\mathbf{r}}_{(j,k)}‖}_2^2,\hfill \end{array}$

(56)

where

${\mathbf{r}}_{(j,k)}\triangleq \left[\begin{array}{c}\hfill \mathrm{v}\mathrm{e}\mathrm{c}\left({\mathbf{R}}_{{\mathbf{X}}_j}\right)\hfill \\ \hfill \mathrm{v}\mathrm{e}\mathrm{c}\left({\mathbf{R}}_{{\mathbf{Y}}_k}\right)\hfill \end{array}\right].$

(57)

Furthermore, Q_(j,k) has a 1-dimensional kernel in our case of exact measurements (Andreff et al., 2001, Lemma 1). Since the objective of Problem 8 is nonnegative and attains a value of zero for our unique solution, any unconstrained minimizer must also attain a value of zero and therefore satisfy equation (54). Each element ηr_(j,k) ∈ ker(Q_(j,k)) is identified with $\eta {\mathbf{R}}_{{\mathbf{X}}_v}$ and $\eta {\mathbf{R}}_{{\mathbf{Y}}_v}$ for all $v\in \mathcal{V}$ by the same argument from weak connectedness used in the proof of Theorem 1. Therefore, we have established the existence of a 1-dimensional vector space of unconstrained minimizers of Problem 8 induced by the informative edge e and parameterized by $\eta \in \mathbb{R}$ .

Corollary 2 provides us with a geometrically interpretable sufficient condition for uniqueness: if there is a single sensor-target pair in the problem graph that gathered measurements from orientations related by rotations about two distinct axes, then the entire multi-frame rotational calibration problem has a unique solution. However, Theorem 1 in its full generality suggests that there exist cases where the data from more than one edge is required to verify that there is a unique solution. ⁹ Finally, the fact that the unique solution guaranteed by Corollary 2 is also a unique (up to scale) unconstrained minimizer ¹⁰ of the objective function of Problem 8 is used in Global optimality guarantees.

The full SE(3) case

We can use Theorem 1 to prove a similar result for the inhomogeneous formulation of standard RWHEC in Problem 4 with exact measurements:

Theorem 3

Consider an instance of Problem 4 induced by a weakly connected bipartite directed graph $\vec{\mathcal{G}}=(\mathcal{V},\vec{\mathcal{E}})$ with exact pose measurements A_e,i, B_e,i ∈ SE(3) for i ∈ [N _e ] associated with each edge $e\in \vec{\mathcal{E}}$ . If a single transformation X _v or Y _v for some $v\in \mathcal{V}$ can be uniquely determined with the problem data, then this instance of Problem 4 has a unique solution.

Proof. Since the rotation residual terms in the objective of Problem 4 are independent of translations, Theorem 1 gives us a unique solution for the rotation component of each X _j and Y _k . Exact measurements imply that the translational residuals are also zero:

${\mathbf{t}}_{{\mathbf{X}}_j}={\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}}^{\top }({\mathbf{R}}_{{\mathbf{Y}}_k}{\underline{\mathbf{t}}}_{{\mathbf{B}}_{e,i}}+{\mathbf{t}}_{{\mathbf{Y}}_k}-{\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,i}})\forall e\in \vec{\mathcal{E}}.$

(58)

Equation (58) relates each pair of translation components via an affine equation. Since each rotation matrix ${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}}^{\top }$ is full rank, any translation ${\mathbf{t}}_{{\mathbf{X}}_j}$ or ${\mathbf{t}}_{{\mathbf{Y}}_k}$ adjacent to a uniquely determined translation is itself uniquely determined via equation (58). Therefore, the fact that $\vec{\mathcal{G}}$ is weakly connected allows us to uniquely specify each translation through a procedure analogous to the one used to prove Theorem 1.

Once again, the results of Shah (2013) provide us with a geometrically interpretable sufficient condition that is analogous to the rotation-only case in Corollary 2:

Corollary 4

Consider an instance of Problem 4 with weakly connected graph $\vec{\mathcal{G}}=(\mathcal{V},\vec{\mathcal{E}})$ and exact measurements. If there exists an edge $e=(j,k)\in \vec{\mathcal{E}}$ with i₁, i₂, i₃ ∈ [N _e ] such that ${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_2}}^{\top }{\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_1}}$ and ${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_3}}^{\top }{\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i_1}}$ have distinct principal axes, then the problem has a unique solution. Furthermore, if

${\mathbf{t}}_e\triangleq \left[\begin{array}{c}\hfill {\mathbf{R}}_{{\mathbf{Y}}_k}{\underline{\mathbf{t}}}_{{\mathbf{B}}_{e,1}}\hfill \\ \hfill ⋮\hfill \\ \hfill {\mathbf{R}}_{{\mathbf{Y}}_k}{\underline{\mathbf{t}}}_{{\mathbf{B}}_{e,N_e}}\hfill \end{array}\right]\notin \mathcal{R}({\mathbf{M}}_e)$

(59)

where

${\mathbf{M}}_e\triangleq \left[\begin{array}{cc}\hfill -{\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,1}}\hfill & \hfill \mathbf{I}\hfill \\ \hfill ⋮\hfill & \hfill \hfill \\ \hfill -{\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,N_e}}\hfill & \hfill \mathbf{I}\hfill \end{array}\right]\in {\mathbb{R}}^{3N_e\times 6},$

(60)

then this solution is also the unique unconstrained minimizer of the inhomogeneous objective function of Problem 4.

Proof. To prove Corollary 4, begin by noting that Corollary 2 gives us the unique rotation solution (up to scale) to equation (54) for all edges of $\vec{\mathcal{G}}$ . Theorem 3.2 in Shah (2013) tells us that there are unique ${\mathbf{R}}_{{\mathbf{X}}_j}$ and ${\mathbf{R}}_{{\mathbf{Y}}_k}$ minimizing Problem 4, satisfying the hypotheses of Theorem 3.

To establish that this solution is also the unique unconstrained minimizer of the objective function, we can parameterize the 1-dimensional vector space of unconstrained minimizers of the rotational cost from Corollary 2 as $\eta {\mathbf{R}}_{{\mathbf{X}}_v}$ or $\eta {\mathbf{R}}_{{\mathbf{Y}}_v}$ for all $v\in \mathcal{V}$ and a single $\eta \in \mathbb{R}$ . Since we have determined with Theorem 3 that there is a constrained minimizer corresponding to η = 1 that attains a value of zero, it suffices to show that this is the only unconstrained minimizer that sets the inhomogeneous translation residual associated with our informative edge e to zero:

${\underline{\mathbf{R}}}_{{\mathbf{A}}_{e,i}}{\mathbf{t}}_{{\mathbf{X}}_j}+{\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,i}}-{\mathbf{t}}_{{\mathbf{Y}}_k}-\eta {\mathbf{R}}_{{\mathbf{Y}}_k}{\underline{\mathbf{t}}}_{{\mathbf{B}}_{e,i}}=\mathbf{0},i\in [N_e].$

(61)

This system of equations can be written as the matrix equation

${\overline{\mathbf{M}}}_e\left[\begin{array}{c}\hfill {\mathbf{t}}_{{\mathbf{X}}_j}\hfill \\ \hfill {\mathbf{t}}_{{\mathbf{Y}}_k}\hfill \\ \hfill \eta \hfill \end{array}\right]=\left[\begin{array}{c}\hfill {\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,1}}\hfill \\ \hfill ⋮\hfill \\ \hfill {\underline{\mathbf{t}}}_{{\mathbf{A}}_{e,N_e}}\hfill \end{array}\right],$

(62)

where

${\overline{\mathbf{M}}}_e\triangleq \left[\begin{array}{cc}\hfill {\mathbf{M}}_e\hfill & \hfill {\mathbf{t}}_e\hfill \end{array}\right]\in {\mathbb{R}}^{3N_e\times 7}.$

(63)

Since M _e is full rank (Shah, 2013, Theorem 3.2) and ${\mathbf{t}}_e\notin \mathcal{R}({\mathbf{M}}_e)$ , the augmented ${\overline{\mathbf{M}}}_e$ is also full rank. Furthermore, since ${\overline{\mathbf{M}}}_e$ has more rows than columns (N _e ≥ 3 by hypothesis), it is injective (Axler, 2024, Theorems 3.15 and 3.21) and any solution to equation (62) is therefore unique. The first part of this proof has furnished us with a solution for η = 1, and we have demonstrated that this is the unique unconstrained minimizer of the objective in Problem 4 as desired.

The requirement that measurements are made from at least three poses that differ by rotations about two distinct axes was first derived with unit quaternions by Zhuang et al. (1994). Theorem 3 reveals that satisfying this condition for a single X _j -Y _k pair is sufficient to ensure generalized RWHEC has a unique solution in the idealized case without noise. Additionally, note that since M _e is full rank and N _e ≥ 3 by hypothesis, $\mathcal{R}({\mathbf{M}}_e)$ is a 6-dimensional proper subspace of ${\mathbb{R}}^{3N_e}$ . Consequently, equation (59) holds for almost all values of ${\mathbf{t}}_e\in {\mathbb{R}}^{3N_e}$ , ensuring that in general a unique solution is also an unconstrained minimizer, which is pertinent to our results in Global optimality guarantees. These results indicate that a practitioner calibrating a multi-sensor rig can ensure all parameters are identifiable by exciting their platform about two axes when taking measurements of a single target-sensor pair, so long as the directed graph of measurements $\vec{\mathcal{G}}$ is weakly connected.

It is noteworthy that our identifiability analysis is limited to the noise-free case. This decision ensures that our results focus on the desired motion of a sensor platform or target, which can be designed by practitioners, rather than stochastic measurements. Defining identifiability criteria for a noise-free model of RWHEC also dovetails neatly with the theory developed by Cifuentes et al. (2022), which we apply in the proof of our main result in Global optimality guarantees.

Global optimality Guarantees

A solution to the convex SDP relaxation described in Certifiably globally optimal extrinsic calibration can provide a post hoc upper bound on global suboptimality via the duality gap. In this section, we demonstrate that the SDP relaxation also has a priori global optimality guarantees when measurement noise is below a problem-dependent threshold. This is achieved by applying the following theorem (Cifuentes et al., 2022, Theorem 3.9):

Theorem 5

Consider the family of parametric QCQPs of the form

$\begin{array}{cc}\hfill \underset{\mathbf{x}\in {\mathbb{R}}^n}{\min }& {\mathbf{x}}^{\top }\mathbf{F}(\mathit{\theta })\mathbf{x}+\mathbf{f}{(\mathit{\theta })}^{\top }\mathbf{x}+c(\mathit{\theta })\hfill \\ \hfill \text{s.t.}& g(\mathbf{x})=\mathbf{0},\hfill \end{array}$

(64)

where $\mathbf{F}:\mathrm{\Theta }\to {\mathbb{S}}^n$ , $\mathbf{f}:\mathrm{\Theta }\to {\mathbb{R}}^n$ , and $c:\mathrm{\Theta }\to \mathbb{R}$ are continuous functions of parameter $\mathit{\theta }\in \mathrm{\Theta }\subseteq {\mathbb{R}}^d$ , and the multivariate constraint function $g:{\mathbb{R}}^n\to {\mathbb{R}}^m$ is quadratic. Let $\underline{\mathit{\theta }}$ be such that the objective function of equation (64) is strictly convex, and its unique unconstrained minimizer $\underline{\mathbf{x}}$ is also the minimizer of the constrained problem. If the Abadie constraint qualification (ACQ) holds at the solution $\underline{\mathbf{x}}$ , then there is a neighborhood of $\underline{\mathit{\theta }}$ in which the primal and dual SDP relaxations of equation (64) are tight.

The ACQ is a weak regularity condition precisely stated in Definition 3.1 of Cifuentes et al. (2022), and it guarantees the existence of Lagrange multipliers at $\underline{\mathbf{x}}$ . Informally, if the feasible set of equation (64) describes a smooth manifold, then the ACQ holds at x if the rank of the Jacobian ∇g(x) of the system of constraint equations at x describing the feasible set is equal to the codimension of the smooth manifold.

By examining equation (42), Problem 4 can be written in the form of the objective of equation (64):

$\begin{array}{cc}\hfill \mathbf{F}(\mathit{\theta })& =\frac{1}{2}\left[\begin{array}{cc}\hfill L({\vec{\mathcal{G}}}^{\tau })\hfill & \hfill \mathbf{U},\hfill \\ \hfill {\mathbf{U}}^{\top }\hfill & \hfill \mathbf{\Sigma }+L({\vec{\mathcal{G}}}^{\rho })\hfill \end{array}\right],\hfill \\ \hfill \mathbf{f}(\mathit{\theta })& =\left[\begin{array}{c}\hfill \mathbf{v}\hfill \\ \hfill \mathbf{u}\hfill \end{array}\right],\hfill \\ \hfill c(\mathit{\theta })& =\frac{v}{2},\hfill \end{array}$

(65)

where $\mathit{\theta }\triangleq \mathrm{v}\mathrm{e}\mathrm{c}\left({\left\{{\mathbf{A}}_{e,i},{\mathbf{B}}_{e,i}\right\}}_{e\in \vec{\mathcal{E}},i\in [N_e]}\right)$ is a vectorization of the measurement data from which the elements on the right hand side of equation (65) are formed. However, in order to apply Theorem 5 to generalized RWHEC, we need to demonstrate that the ACQ holds for our redundant implementation of SO(3) constraints in equations (19a)–(19c):

Theorem 6

Let $g:{\mathbb{R}}^9\to {\mathbb{R}}^{21}$ be a vectorization of the quadratic constraints encoding SO(3) in equations (19a)–(19c). Then the ACQ holds for all x satisfying g(x) = 0, and by extension for a “stacked” vectorization of constraints encoding SO(3) ⁿ .

Proof. We will apply Theorem 8 from Appendix D to show that the ACQ holds for g on SO(3). We begin by defining the decomposition g = (g₁, g₂) as in equation (79). Let $g_1:{\mathbb{R}}^9\to {\mathbb{R}}^6$ encode a vectorization of the six scalar equations in the symmetric row orthogonality constraint in equation (19a), and let $g_2:{\mathbb{R}}^9\to {\mathbb{R}}^{15}$ encode equations (19b) and (19c). The constraint set described by g₁ is a symmetric vectorization of the function h in Section 7.3 of Boumal (2023) for dimension p = 3. Boumal (2023) shows that the differential of h has rank 6, proving that the Jacobian of g₁ has full rank everywhere and therefore satisfies the linear independence constraint qualification (LICQ) at all vectorized inputs in $\mathrm{O}(3)\subset {\mathbb{R}}^{3\times 3}$ . This satisfies the hypothesis of part (a) in Theorem 8. It remains to demonstrate that the hypothesis of part (b) holds. The first block of g₂ (corresponding to equation (19b)) is an algebraic rearrangement of g₁ and therefore locally constant on SO(3). The second block (equation (19c)) is equivalent to $\mathrm{d}\mathrm{e}\mathrm{t}\left(\mathbf{R}\right)=1$ when combined with g₁ (Tron et al., 2015). Since O(3) \SO(3) is disconnected from SO(3), this block is also locally constant on SO(3). We have satisfied the hypotheses of Theorem 8, demonstrating that ACQ holds at $\mathbf{x}=\mathrm{v}\mathrm{e}\mathrm{c}\left(\mathbf{R}\right)$ for all R ∈ SO(3). Since the constraint set for each SO(3)-valued variable is independent of the others, the ACQ holds for any point in SO(3) ^N as well, as the rank of a block diagonal matrix is the sum of the ranks of its blocks.

Theorem 6 establishes that redundant polynomial parameterizations of SO(3) constraints do not seriously interfere with the regularity properties of optimization problems. This result is important because the addition of redundant rotation constraints in a primal QCQP can improve the tightness of its SDP relaxations (Dümbgen et al., 2024; Tron et al., 2015). Additionally, it is worth noting that Theorem 6 is not limited to the various forms of generalized RWHEC studied in this work: it applies to any QCQP whose only constrained variables are in SO(3) or SE(3). We are now prepared to prove our main result, which is a straightforward application of Theorem 5:

Proposition 7

Let $\underline{\mathit{\theta }}\triangleq \mathrm{v}\mathrm{e}\mathrm{c}\left({\left\{{\mathbf{A}}_{e,i},{\mathbf{B}}_{e,i}\right\}}_{e\in \vec{\mathcal{E}},i\in [N_e]}\right)$ consist of exact measurements parameterizing an instance of Problem 4 as per equations (64) and (65). If there is a unique solution $\underline{\mathbf{x}}$ that is also an unconstrained minimizer of the objective function, then there is a neighborhood of $\underline{\mathit{\theta }}$ in which the SDP relaxation of Problem 4 is tight.

Proof. We must show that $\underline{\mathit{\theta }}$ defines a QCQP satisfying the hypotheses of Theorem 5. We proved that the ACQ holds in Theorem 6, and the objective’s dependence on θ is polynomial and therefore continuous. It remains to demonstrate that the objective is strictly convex, which is equivalent to demonstrating that $\mathbf{F}(\underline{\mathit{\theta }})$ is positive definite (Boyd and Vandenberghe, 2004, Example 3.2). Since $\underline{\mathbf{x}}$ is the unique unconstrained minimizer by hypothesis and our objective is convex, $\mathbf{F}(\underline{\mathit{\theta }})\succ \mathbf{0}$ (Boyd and Vandenberghe, 2004, Example 4.5) and our claim follows.

Proposition 7 tells us that a noisy instance of the RWHEC problem has a tight SDP relaxation so long as its measurements are sufficiently close to noise-free measurements describing a problem with a unique solution. The uniqueness result in Theorem 3 therefore gives us simple geometric criteria by which to determine whether an instance of RWHEC is well-posed and globally solvable via SDP relaxation under moderate noise. Note that the linear independence hypothesis ${\mathbf{t}}_e\notin \mathcal{R}({\mathbf{M}}_e)$ in Corollary 4 which ensures that the unique minimizer is also the unconstrained minimizer plays a key role in our proof of Proposition 7. Finally, Proposition 3.5 of Cifuentes et al. (2022) is used in the proof of Theorem 5 and additionally ensures that the problem instances in the neighborhood of $\underline{\mathit{\theta }}$ have a unique solution.

From a practical standpoint, Proposition 7 implies that any instance of Problem 4 whose ground-truth geometric configuration $\underline{\mathbf{x}}$ is identifiable (given noiseless measurements) can be certifiably optimally solved using Algorithm 1 for moderate measurement noise. In other words: instances of the generalized RWHEC problem are generically efficiently optimally solvable in practice whenever they are statistically well-posed. To the best of our knowledge, this result is the first a priori global optimality guarantee of its kind for a multi-sensor calibration problem. Although approximating the size of the region of SDP-tightness around noise-free problem instances is computationally impractical with presently available tools (Cifuentes et al., 2022, Thm. 3.11), we refer to this guarantee as a priori because it proceeds solely from the geometry of the problem’s ground-truth configuration $\underline{\mathbf{x}}$ . This distinguishes Proposition 7 from the standard post hoc numerical optimality certification procedure provided by the duality gap in Certifiably globally optimal extrinsic calibration, which requires us to observe noisy measurements and solve an SDP.

Simulation experiments

In this section, we use synthetic data from two simulated robotic systems to compare the accuracy and robustness of our algorithm with a variety of other RWHEC methods. The first system consists of a robotic manipulator with a hand-mounted camera observing a visual fiducial target. Using the simulated manipulator hand poses and camera-target measurements, this system forms a RWHEC problem with one X and one Y variable. The second system generates data for a RWHEC problem with four Xs and one Y by simulating a robotic manipulator with a hand-mounted target observed by four stationary cameras. For each system, we generate data to study both standard and monocular RWHEC.

To generate measurements for the robotic manipulator with a hand-mounted camera, we simulate a camera trajectory relative to the target and fix groundtruth values for X and Y. At each point in time indexed by i, we use the camera pose relative to the target to determine the camera-target transformation B _i . By combining the ground truth values of X, Y, and B _i , we calculate the ground truth values for each A _i . Similarly, for the cameras observing a robotic manipulator with a hand-mounted target, the pose of camera j and the target pose at time i are used to calculate ${\mathbf{B}}_{ij}\triangleq {\mathbf{B}}_{e_j,i}$ , where e _j is the unique edge in the problem graph associated with camera j. Once again, the ground truth values for X _j , Y, and B _ij , are used to compute the ground truth measurements for each ${\mathbf{A}}_{ij}\triangleq {\mathbf{A}}_{e_j,i}$ . We add samples from a Gaussian distribution $\mathcal{N}\left(\mathbf{0},{\sigma }^2\mathbf{I}\right)$ to the ground truth translation measurements ${\overline{\mathbf{t}}}_{{\mathbf{B}}_i}$ and ${\overline{\mathbf{t}}}_{{\mathbf{B}}_{ij}}$ . Furthermore, we right-perturb the noiseless rotation measurements ${\overline{\mathbf{R}}}_{{\mathbf{B}}_i}$ and ${\overline{\mathbf{R}}}_{{\mathbf{B}}_{ij}}$ with samples from an isotropic Langevin distribution ${\mathbf{R}}_n\sim \mathrm{L}\mathrm{a}\mathrm{n}\mathrm{g}\left(\mathbf{I},\kappa \right)$ . In each individual experimental trial, the noise parameters are fixed for all measurements: we consider translation noise with standard deviations of σ = 1 cm or σ = 5 cm, and rotation noise concentrations of κ = 125 or κ = 12. In the monocular RWHEC studies, we scale the translation component of the noisy B _i measurements by α = 0.5. Finally, we generate 100 random runs for each experiment.

Using the data from our simulation studies, we compare the estimated parameter accuracy of our standard and monocular RWHEC methods to those of five standard RWHEC methods and one monocular RWHEC method. The five benchmark RWHEC solvers are the two-stage closed-form methods in Shah (2013) and Wang et al. (2022), the certifiable method in Horn et al. (2023), the probabilistic method in Dornaika and Horaud (1998), and a local on-manifold method (LOM) of our own design (see Appendix C for details of our implementation). For the monocular RWHEC scenarios, we compare the accuracy of our algorithm to LOM. When a scenario has more than one X to solve for, we compare our methods with Wang et al. (2022), Horn et al. (2023), and LOM.

For the solvers that require initialization, we provide random initial values, the solution from Wang et al. (2022), or parameters close to the ground truth. For each experiment, our method and the method in Horn et al. (2023) are randomly initialized. Consequently, we also initialize the solver in Dornaika and Horaud (1998) and LOM using the solution from the method in Wang et al. (2022). However, the method in Wang et al. (2022) cannot solve monocular RWHEC problems. In the monocular RWHEC studies, we initialize LOM with either random calibration parameters that are within 10 cm and 10° of the ground truth values (close), or rotations sampled uniformly from SO(3) and translations drawn from [−1,1]³ (random). Additionally, we set the initial scale estimate to 1.

Our globally optimal solver was implemented in Julia with the JuMP modeling language (Dunning et al., 2017), which provides convenient access to several general-purpose optimization methods. For all experiments in this section and our Real-world experiment, our algorithm used the conic operator splitting method (COSMO) solver with default parameters except for residual norm convergence tolerances of ϵ_abs = ϵ_rel = 5 × 10⁻¹¹, initial step size ρ = 10⁻⁴, and a maximum of one million iterations to ensure convergence (Garstka et al., 2021). LOM was implemented in Ceres (Agarwal et al., 2023), and we used default settings with the exception of a maximum of 1000 iterations, a function tolerance of 10⁻¹⁵, and a gradient tolerance of 10⁻¹⁹ for all experiments.

Finally, it is important to note that the problem formulation solved by LOM uses a standard multiplicative exponentiated Gaussian noise model (Barfoot, 2024), and therefore does not minimize the same MLE objective function derived in Maximum likelihood estimation. Therefore, we cannot expect the global minima of LOM’s objective to exactly match those of the problem formulation solved by our convex method. We partially mitigate the experimental effects of this discrepancy by roughly matching the intensity of the isotropic variance ${\sigma }_R^2$ described in Appendix C to the Langevin concentration parameter κ using the asymptotic Gaussian approximation of the von Mises distribution described in Appendix A of Rosen et al. (2019).

Robot arm poses on a sphere

In this pair of standard and monocular RWHEC experiments, we consider the problem of extrinsically calibrating a hand-mounted camera and a target, relative to a robotic manipulator. A diagram of the type of system we simulate is shown in Figure 1. In particular, we estimate X = T _hc and Y = T _bt , where $\underrightarrow{{\mathcal{F}}_b}$ , $\underrightarrow{{\mathcal{F}}_t}$ , $\underrightarrow{{\mathcal{F}}_h}$ , $\underrightarrow{{\mathcal{F}}_c}$ , are the manipulator base, target, hand, and camera reference frames, respectively. For each problem instance, the camera collects measurements from 100 distinct poses along a trajectory on the surface of a sphere while its optical axis $\widehat{\mathbf{z}}$ is pointed toward the target, $\widehat{\mathbf{y}}$ points toward the south pole of the sphere, and $\widehat{\mathbf{x}}$ completes the orthonormal frame. Although they contain rotations about two distinct axes and therefore satisfy the identifiability conditions of Corollary 4, trajectories generated in this manner were empirically observed to not have a unique solution for the monocular RWHEC problem. We found that identifiable monocular RWHEC problem instances could be created by collecting measurements from the surface of two spheres: one with a unit radius, and another with a radius of 0.3 m.

The mean and standard deviation of the estimated translation (t_x,err, t_y,err) and rotation (r_x,err, r_y,err) errors are shown in Table 2. Generally, the least accurate methods are the two-stage closed-form solvers from Shah (2013) and Wang et al. (2022), while our method and LOM are the most accurate. In spite of the differences in their formulation, our method and LOM return extremely similar solutions, which outperform the other methods by up to 50 mm and 3°. However, the accuracy of LOM degrades significantly when it is initialized randomly, whereas our method returns the same global minimizer for any initialization. Finally, all algorithms have roughly the same accuracy when κ = 125 and σ = 5 cm. In this case, the rotation measurement noise is low, so the two-stage closed-form solvers are more likely to return accurate rotation estimates.

Table 2.

Calibration results for the experiments in Robot arm poses on a sphere with known scale. The values in each row are estimated by a different algorithm. The mean error magnitude and standard deviation are given in each cell. Dornaika and LOM are initialized with the method in Wang et al. (2022). Additionally, these results include tests where LOM is initialized randomly and often converges to local minima.

Noise level	Method (Init.)	tx,err [mm]	rx,err [deg]	ty,err [mm]	ry,err [deg]
κ = 125, σ = 1 cm	Shah	20.6 ± 10.5	1.37 ± 0.55	9.9 ± 4.8	1.34 ± 0.57
	Wang	20.6 ± 10.5	1.37 ± 0.55	9.9 ± 4.8	1.34 ± 0.57
	Dornaika (Wang)	19.9 ± 10.2	1.25 ± 0.53	8.8 ± 4.3	1.20 ± 0.54
	Horn	18.5 ± 9.4	1.16 ± 0.51	8.4 ± 4.2	1.10 ± 0.51
	LOM (random)	17.4 ± 8.98	2.35 ± 1.05	3.72 ± 2.11	2.22 ± 1.05
	LOM (Wang)	11.1 ± 5.82	0.78 ± 0.37	3.69 ± 2.12	0.63 ± 0.38
	Ours	10.9 ± 5.6	0.77 ± 0.36	3.71 ± 2.13	0.62 ± 0.36
κ = 125, σ = 5 cm	Shah	31.2 ± 15.0	1.61 ± 0.64	21.2 ± 10.0	1.57 ± 0.66
	Wang	31.2 ± 15.0	1.61 ± 0.64	21.2 ± 10.0	1.57 ± 0.66
	Dornaika (Wang)	30.6 ± 14.5	1.49 ± 0.62	20.4 ± 9.7	1.44 ± 0.63
	Horn	29.1 ± 13.6	1.42 ± 0.60	19.9 ± 9.2	1.36 ± 0.59
	LOM (random)	28.5 ± 13.3	2.88 ± 1.24	18.5 ± 8.72	2.77 ± 1.23
	LOM (Wang)	28.5 ± 13.4	1.45 ± 0.62	18.5 ± 8.8	1.39 ± 0.62
	Ours	28.4 ± 13.0	1.42 ± 0.63	18.5 ± 8.7	1.36 ± 0.61
κ = 12, σ = 1 cm	Shah	65.5 ± 38.3	4.34 ± 1.95	31.8 ± 17.2	4.41 ± 1.98
	Wang	65.5 ± 38.3	4.34 ± 1.95	31.8 ± 17.2	4.41 ± 1.98
	Dornaika (Wang)	63.4 ± 37.3	3.92 ± 1.87	27.4 ± 16.2	3.92 ± 1.90
	Horn	45.0 ± 24.8	3.30 ± 1.69	25.8 ± 14.1	3.19 ± 1.55
	LOM (random)	56.7 ± 33.5	7.45 ± 3.70	4.17 ± 1.99	7.35 ± 3.79
	LOM (Wang)	15.2 ± 10.0	1.84 ± 0.84	3.4 ± 1.8	0.88 ± 0.59
	Ours	15.1 ± 10.0	1.81 ± 0.83	3.4 ± 1.8	0.87 ± 0.59
κ = 12, σ = 5 cm	Shah	71.9 ± 44.7	4.74 ± 2.13	37.8 ± 19.3	4.63 ± 2.21
	Wang	71.9 ± 44.7	4.74 ± 2.13	37.8 ± 19.3	4.63 ± 2.21
	Dornaika (Wang)	69.9 ± 43.1	4.33 ± 2.08	33.9 ± 17.9	4.13 ± 2.14
	Horn	50.6 ± 28.4	3.69 ± 1.72	31.0 ± 16.2	3.40 ± 1.72
	LOM (random)	63.1 ± 38.4	8.35 ± 4.15	19.2 ± 8.93	7.85 ± 4.22
	LOM (Wang)	48.3 ± 27.3	3.18 ± 1.67	18.7 ± 9.04	2.72 ± 1.64
	Ours	47.7 ± 27.0	3.12 ± 1.67	18.8 ± 9.02	2.68 ± 1.65

Table 3 contains the mean and standard deviation of the estimated translation (t_x,err, t_y,err), rotation (r_x,err, r_y,err), and scale error (α _err ) for the monocular RWHEC study. As is the case for the standard RWHEC experiment, our method and LOM have very similar accuracy when LOM is accurately initialized, but the performance of LOM degrades when poorly initialized. In contrast, our method finds a global minimum and therefore does not require an accurate initial estimate of X, Y, or α. Even though monocular RWHEC is ostensibly more challenging than RWHEC, both monocular RWHEC solvers return more accurate estimates than their corresponding RWHEC methods. ¹¹

Table 3.

Calibration results for the experiments in Robot arm poses on a sphere with unknown scale. The values in each row are estimated by a different algorithm. The mean error magnitude and standard deviation are given in each cell. The close initialization of LOM is with parameters 10° and 10 cm from the ground truth values and a scale of 1.

Noise level	Method (Init.)	tx,err [mm]	rx,err [deg]	ty,err [mm]	ry,err [deg]	α err [%]
κ = 125, σ = 1 cm	LOM (random)	578 ± 579	237 ± 163	630 ± 637	237 ± 164	9.86e-1 ± 8.82e-1
	LOM (close)	4.61 ± 2.39	0.51 ± 0.21	3.82 ± 1.93	0.22 ± 0.12	1.78e-3 ± 1.54e-3
	Ours	4.71 ± 2.43	0.51 ± 0.22	3.82 ± 1.91	0.250 ± 0.113	1.76e-3 ± 1.54e-3
κ = 125, σ = 5 cm	LOM (random)	522 ± 538	227 ± 166	559 ± 595	227 ± 166	9.53e-1 ± 8.98e-1
	LOM (close)	23.0 ± 11.2	0.94 ± 0.44	19.1 ± 11.4	0.85 ± 0.40	8.45e-3 ± 7.03e-3
	Ours	23.8 ± 11.3	1.11 ± 0.53	19.0 ± 11.4	1.06 ± 0.52	8.41e-3 ± 7.02e-3
κ = 12, σ = 1 cm	LOM (random)	482 ± 540	214 ± 170	528 ± 601	214 ± 171	9.38e-1 ± 8.98e-1
	LOM (close)	4.52 ± 1.97	1.57 ± 0.59	3.78 ± 1.89	0.20 ± 0.10	1.88e-3 ± 1.37e-3
	Ours	4.50 ± 1.99	1.54 ± 0.57	3.77 ± 1.88	0.206 ± 0.098	1.87e-3 ± 1.37e-3
κ = 12, σ = 5 cm	LOM (random)	606 ± 609	234 ± 163	659 ± 675	234 ± 164	9.27e-1 ± 8.77e-1
	LOM (close)	23.2 ± 14.1	1.90 ± 0.81	20.0 ± 11.0	1.07 ± 0.54	9.24e-3 ± 7.69e-3
	Ours	24.4 ± 14.3	1.99 ± 0.81	19.6 ± 11.0	1.25 ± 0.58	9.16e-3 ± 7.69e-3

Many cameras observing a hand-mounted target

In this study, we compare the estimated parameter accuracy of standard and monocular RWHEC algorithms for systems with multiple Xs and one Y. The simulated system consists of a robotic manipulator with a hand-mounted target viewed by four fixed cameras. Figure 3 shows a diagram of the base-camera and hand-target transformations for one camera. Each variable ${\mathbf{X}}_i={\mathbf{T}}_{b{c}_i}$ relates the manipulator base frame $\underrightarrow{{\mathcal{F}}_b}$ and the ith camera frame $\underrightarrow{{\mathcal{F}}_{c_i}}$ . The unknown transformation from the robot hand to the target is represented by the variable Y = T _ht , where $\underrightarrow{{\mathcal{F}}_h}$ and $\underrightarrow{{\mathcal{F}}_t}$ are the manipulator hand and target reference frames, respectively. Each simulated trajectory consists of 108 manipulator poses, ensures that the target is always visible to every fixed camera, and has a unique solution by satisfying the requirements of Corollary 4.

Figure 3.

A diagram of a robot arm with a hand-mounted target that is observed by a stationary camera. In this diagram, the base, joint 1, hand, camera j, and target reference frames are labeled $\underrightarrow{{\mathcal{F}}_b}$ , $\underrightarrow{{\mathcal{F}}_1}$ , $\underrightarrow{{\mathcal{F}}_h}$ , $\underrightarrow{{\mathcal{F}}_{c_j}}$ , and $\underrightarrow{{\mathcal{F}}_t}$ , respectively. The red arrows indicate the axes of joint rotation. At time i, we use the forward kinematics of the manipulator to estimate the transformation from the manipulator base to the hand, A _i . Further, we can measure the transformation from the target to camera j, B _ij .

The results of standard and monocular RWHEC studies are shown in Tables 4 and 5, which contain the mean and standard deviation of the estimated translation (t_x,err, t_y,err) and rotation (r_x,err, r_y,err) error. The errors associated with X (i.e., t_x,err and r_x,err) are the mean and standard deviation of all four estimated X transformations. Table 5 also displays the mean and standard deviation of the estimated scale error (α _err ). As with the previous experiments, our method and LOM with intelligent initialization produced the most accurate parameter estimates, but the gap between the performance of LOM with random initialization and our method is larger when compared with the single-sensor case. The method of Wang et al. (2022) returns approximate closed-form solutions using a linearized system and the singular value decomposition, yet our experiments demonstrate that it is more accurate than the globally optimal method of Horn et al. (2023). Recalling that there is a double cover from the unit quaternions to SO(3), we hypothesize that the use of dual quaternions by the method in Horn et al. (2023) leads to its deteriorated performance in the multi-X case, as each measurement (i.e., A or B) must be assigned one of two equivalent unit DQ representations of SE(3). Finally, as in our previous monocular RWHEC study, our algorithm and LOM initialized to within 10 cm and 10° of the true solution achieved similar accuracy. The performance between randomly initialized LOM and our method is even larger than the known-scale case.

Table 4.

Calibration results for the multi-sensor experiments in Many cameras observing a hand-mounted target with known scale. The values in each row are estimated by a different algorithm. The mean error magnitude and standard deviation are given in each cell. LOM is once again initialized with the results from Wang or random poses. Note that we have no a priori estimates of ${\mathbf{t}}_{{\mathbf{X}}_i}$ .

Noise level	Method (Init.)	tx,err [mm]	rx,err [deg]	ty,err [mm]	ry,err [deg]
κ = 125, σ = 1 cm	Wang	2.32 ± 1.56	0.469 ± 0.200	2.71 ± 1.37	0.273 ± 0.120
	Horn	12.11 ± 6.49	0.455 ± 0.191	5.43 ± 2.04	0.165 ± 0.067
	LOM (random)	303 ± 841	21.9 ± 58.2	154 ± 418	21.4 ± 58.1
	LOM (Wang)	0.994 ± 0.416	0.454 ± 0.190	0.572 ± 0.296	0.022 ± 0.010
	Ours	0.992 ± 0.416	0.454 ± 0.189	0.570 ± 0.294	0.021 ± 0.010
κ = 125, σ = 5 cm	Wang	4.95 ± 2.20	0.470 ± 0.196	3.52 ± 1.57	0.260 ± 0.115
	Horn	12.4 ± 6.4	4.55 ± 1.91	5.42 ± 2.33	1.76 ± 0.69
	LOM (random)	457 ± 995	32.7 ± 68.8	233 ± 494	32.1 ± 68.7
	LOM (Wang)	4.57 ± 1.96	0.460 ± 0.191	2.74 ± 1.34	0.120 ± 0.056
	Ours	4.56 ± 1.96	0.459 ± 0.191	2.69 ± 1.32	0.111 ± 0.051
κ = 12, σ = 1 cm	Wang	6.05 ± 4.84	1.54 ± 0.63	7.63 ± 4.04	0.812 ± 0.364
	Horn	69 ± 121	24.6 ± 94.4	48.5 ± 82.5	13.0 ± 76.8
	LOM (random)	177 ± 658	13.9 ± 45.2	90.7 ± 330	12.5 ± 45.8
	LOM (Wang)	0.933 ± 0.370	1.52 ± 0.64	0.537 ± 0.262	0.021 ± 0.010
	Ours	0.933 ± 0.370	1.50 ± 0.62	0.537 ± 0.262	0.021 ± 0.010
κ = 12, σ = 5 cm	Wang	7.85 ± 4.34	1.52 ± 0.70	9.58 ± 4.46	0.868 ± 0.340
	Horn	77 ± 146	26 ± 106	57 ± 110	14.8 ± 83.9
	LOM (random)	331 ± 870	24.5 ± 59.5	168 ± 430	23.2 ± 60.1
	LOM (Wang)	4.57 ± 1.97	1.51 ± 0.682	2.93 ± 1.34	0.104 ± 0.047
	Ours	4.57 ± 1.97	1.48 ± 0.67	2.92 ± 1.32	0.103 ± 0.046

Table 5.

Calibration results for the multi-sensor experiments in Many cameras observing a hand-mounted target with unknown scale. The values in each row are estimated by a different algorithm. The mean error magnitude and standard deviation are given in each cell. In the close initialization scheme for LOM, the parameters are initialized within 10° and 10 cm from the ground truth values and a scale of 1. For the random initialization of LOM, the poses are drawn from a uniform distribution over SO(3) × [−1,1]³.

Noise level	Method (Init.)	tx,err [mm]	rx,err [deg]	ty,err [mm]	ry,err [deg]	α err [%]
κ = 125, σ = 1 cm	LOM (random)	1.87e3 ± 2.18e3	226 ± 172	2.47e3 ± 2.53e3	224 ± 170	8.68e-1 ± 6.65e-1
	LOM (close)	1.05 ± 0.45	0.469 ± 0.185	0.631 ± 0.249	0.030 ± 0.013	2.71e-4 ± 1.98e-4
	Ours	1.05 ± 0.45	0.468 ± 0.185	0.597 ± 0.256	0.026 ± 0.012	2.71e-4 ± 1.98e-4
κ = 125, σ = 5 cm	LOM (random)	1.74e3 ± 2.22e3	195 ± 178	2.23e3 ± 2.56e3	192 ± 176	7.39e-1 ± 6.81e-1
	LOM (close)	5.25 ± 2.20	0.457 ± 0.195	3.15 ± 1.26	0.179 ± 0.079	1.37e-3 ± 1.03e-3
	Ours	5.21 ± 2.18	0.454 ± 0.194	3.03 ± 1.24	0.159 ± 0.071	1.37e-3 ± 1.03e-3
κ = 12, σ = 1 cm	LOM (random)	1.81e3 ± 2.13e3	217 ± 172	2.39e3 ± 2.46e3	213 ± 173	8.26e-1 ± 6.74e-1
	LOM (close)	1.02 ± 0.44	1.50 ± 0.62	0.530 ± 0.284	0.021 ± 0.010	2.58e-4 ± 2.02e-4
	Ours	1.02 ± 0.44	1.46 ± 0.61	0.530 ± 0.284	0.021 ± 0.010	2.57e-4 ± 2.02e-4
κ = 12, σ = 5 cm	LOM (random)	2.04e3 ± 2.17e3	242 ± 166	2.60e3 ± 2.48e3	238 ± 166	9.22e-1 ± 6.46e-1
	LOM (close)	5.20 ± 2.27	1.48 ± 0.64	3.13 ± 1.56	0.156 ± 0.075	1.18e-3 ± 8.63e-4
	Ours	5.18 ± 2.26	1.44 ± 0.626	3.02 ± 1.51	0.144 ± 0.069	1.18e-3 ± 8.62e-4

Real-world experiment

In this section, we discuss our real-world data collection system, data preprocessing procedure, and calibration results.

Data collection and data preprocessing

In our real world experiment, we use infrared motion capture markers placed on a mobile sensor system to produce measurements A_(j,k),i and fiducial markers in the environment with unknown poses X _j to form the RWHEC problem shown in Figure 4.

Figure 4.

A diagram of the measurements for camera k and target j at time i. The reference frames for the cameras, targets, OptiTrack world, and rig reference frames are. $\underrightarrow{{\mathcal{F}}_{c_k}}$ , $\underrightarrow{{\mathcal{F}}_{t_j}}$ , $\underrightarrow{{\mathcal{F}}_w}$ , and $\underrightarrow{{\mathcal{F}}_r}$ , respectively. The OptiTrack rig constellation enables estimation of the transformation A_(j,k),i. The monocular camera observing the AprilTag enables estimation of the transformation B_(j,k),i.

$\underrightarrow{{\mathcal{F}}_{c_k}}$ , $\underrightarrow{{\mathcal{F}}_{t_j}}$ , $\underrightarrow{{\mathcal{F}}_w}$ , and $\underrightarrow{{\mathcal{F}}_r}$ , respectively. The OptiTrack rig constellation enables estimation of the transformation A_(j,k),i. The monocular camera observing the AprilTag enables estimation of the transformation B_(j,k),i.

Figures 5 and 6 show our mobile system and two images of our indoor experimental environment, respectively. Our mobile system is equipped with eight Point Grey Blackfly S USB cameras, OptiTrack markers, and a VectorNav VN-100 inertial measurement unit (IMU). We place a sufficient number of OptiTrack markers on the system to enable estimation of the relative transformation between the OptiTrack reference frame and a frame fixed to the mobile system. To validate our estimated camera calibration parameters, we approximate ground truth extrinsics Y _k with an estimate computed using the Kalibr toolbox (Rehder et al., 2016), which requires measurements from the IMU in addition to images of a checkerboard target taken by each camera. Kalibr uses a local method and therefore requires an initialization which we manually computed, therefore it should only be treated as a rough proxy for the true solution.

Figure 5.

Image of the real-world data collection rig. The data collection rig consists of eight hardware synchronized cameras facing a variety of different directions. Further, the data collection rig includes an IMU and OptiTrack markers. The OptiTrack markers enable us to estimate the rig pose relative to the OptiTrack reference frame.

Figure 6.

Images from the real-world experiment. These images are from camera 0 and show a subset of the AprilTags in the environment. The bottom left AprilTag in the image on the right has OptiTrack markers, so we can determine the ground truth pose of the AprilTag frame relative to the OptiTrack world frame.

A total of sixteen AprilTags (Olson, 2011) are mounted in our experimental environment for use as fiducial markers. To evaluate the accuracy of estimated AprilTag poses X _j , we place OptiTrack markers on a single AprilTag to establish ground truth measurements.

Figure 7 shows the trajectory of the mobile system in our experiment.

Figure 7.

Trajectory of the rig in the real-world experiment. Initially, the platform rotates about the y-axis and moves in the xz-plane. Following the planar motion, the system follows an unconstrained trajectory, which allows for rotation about the x and z axes.

In the first half of the data collection run, the system experiences purely planar motion, and each target is observed at least once. After the planar motion, the system rotates about all three axes and translates perpendicular to the plane of motion from the first half of the trajectory, ensuring sufficient excitation for the RWHEC problem to have a unique solution. Figure 8 is a grid where row k corresponds to camera k, and column j corresponds to AprilTag j. If the square in column j and row k is blue, then the data collected for that camera-target pair enables an identifiable RWHEC subproblem (i.e., an instance of RWHEC with a unique solution). A red square indicates that the data collected for that camera-target pair did not contain sufficient excitation, while a white square indicates that target j was not observed by camera j. Figure 8 indicates that each camera observed at least one target, and that the overall generalized RWHEC problem is described by a weakly connected bipartite directed graph.

Figure 8.

A grid describing the observability of each connection in the bipartite graph generated by our real-world experiment. Blue squares indicate that measurements between tag j and camera k are sufficient for an identifiable RWHEC problem. Red squares indicate that there is a connection between tag j and camera k, but the measurements are insufficient for the problem to be identifiable on its own. A white square indicates that there is no observation of tag j by camera k.

Our data preprocessing pipeline for this experiment consists of four steps designed to align measurements in time and remove outliers. ¹² First, we rectify the images using the camera intrinsic parameters estimated with Kalibr (see Rehder et al. (2016)). Second, we measure the camera-to-AprilTag transformations B_(j,k),i using an AprilTag detector. Third, we synchronize the camera and OptiTrack measurements by interpolating the OptiTrack system measurements to the camera measurement timestamps with the Lie-algebraic method in Chapter 8 of Barfoot (2024). Finally, we use a RANSAC (Fischler and Bolles, 1981) scheme to reject gross outliers. For each ground truth estimate of Y _k computed with Kalibr, the RWHEC geometric constraint at each measurement time labeled by i ∈ [N_(j,k)] becomes

${\mathbf{X}}_j={\mathbf{A}}_{(j,k),i}^{-1}{\mathbf{Y}}_k{\mathbf{B}}_{(j,k),i}.$

(66)

Consequently, we can use RANSAC on the model in equation (66) to determine the A_(j,k),i-B_(j,k),i pairs that result in a consistent X _j transformation. For this RANSAC procedure, our minimum inlier set size is one third of the number of A_(j,k),i-B_(j,k),i pairs for camera k and target j. Data from camera-target pairs with only one A_(j,k),i-B_(j,k),i measurement are rejected because they cannot be validated using this scheme. An A_(j,k),i-B_(j,k),i pair is considered an inlier if it is within 0.6 m and 60° of the estimated pose X _j . The inlier A_(j,k),i-B_(j,k),i pairs are then used for our RWHEC problem. To extend this outlier rejection scheme to monocular RWHEC, we assume that the AprilTag size is known within 10% of the actual value, which we empirically found to be sufficient for reliable outlier rejection. Finally, we assumed that the noise parameters σ and κ were equal to one for all measurements from all sensor-target pairs. This simplifying assumption removed the need for a parameter fitting step, and allows us to fairly compare our method and LOM to the method in Wang et al. (2022), which does not support multiple measurement uncertainties.

Experimental results

Using the preprocessed data, we obtain the calibration results shown in Table 6.

Table 6.

Calibration results for our real world dataset with known scale. The values in each row are estimated by a different algorithm. The mean error magnitude and standard deviation for ${\mathbf{t}}_{c_0}^{c_i{c}_0}$ and ${\mathbf{R}}_{c_0{c}_i}$ are given in each cell. The error in the estimated transformation between AprilTag 20 and the OptiTrack reference frame is also provided. LOM is initialized with the solution from Wang et al. (2022).

Method (Init.)	${\mathbf{t}}_{c_0}^{c_i{c}_0}$ Error [cm]	${\mathbf{R}}_{c_0{c}_i}$ Error [deg]	${\mathbf{t}}_w^{t_{20}w}$ Error [cm]	${\mathbf{R}}_{w{t}_{20}}$ Error [deg]
Wang	3.46 ± 1.69	1.38 ± 0.81	11.7	5.30
LOM (Wang)	3.34 ± 2.44	0.88 ± 0.49	11.4	4.50
Ours	3.20 ± 1.88	0.99 ± 0.52	11.5	4.73
LOM with Unknown Scale (Wang)	3.28 ± 2.42	0.86 ± 0.47	7.92	4.50
Ours with Unknown Scale	2.87 ± 1.94	0.99 ± 0.52	7.94	4.72

We assume that our hand-measured AprilTag sizes are approximately correct, so the local standard and monocular RWHEC solvers are initialized with the parameters estimated using the method in Wang et al. (2022). Our estimated AprilTag translation is within 12 cm (or 8% of the ground-truth distance) and 6° of the ground truth transformation measured with OptiTrack. The estimated camera calibration parameters are, on average, within about 3 cm and 1° of the parameters estimated by Kalibr. We did not expect our algorithm to return the same values as Kalibr because collecting a dedicated calibration dataset for each camera should result in more accurate calibration parameters. As expected, the inexpensive approximation technique in Wang et al. (2022) returned the least accurate rotation estimates. As in our simulation studies, our method and LOM compute parameters with similar accuracy. Interestingly, estimating the scale of the AprilTags improves accuracy. The estimated AprilTag scale is 2.5% smaller than the hand-measured value. From our simulation studies, the estimated scale error is often within 0.01% of ground truth value, so a scale error this large is unexpected. This scaling error suggests that our manual measurement of the AprilTags may be off by approximately 3 mm. This correction appears to have improved our estimated camera calibration parameters and AprilTag translations by approximately 0.5 cm and 4 cm, respectively. However, additional experiments beyond the scope of this work are needed to rigorously determine whether our monocular method can reliably improve estimation accuracy in standard RWHEC problems with noisy fiducial marker size estimates. This line of inquiry also motivates future work on a maximum a posteriori (MAP) estimation scheme that leverages prior estimates of parameters including marker sizes in a principled fashion.

Certification

As described in Certifiably globally optimal extrinsic calibration, we can numerically certify that our convex method achieves the global minimum. Weak duality tells us that

$d\le d^{\star }\le p^{\star }\le p,$

(67)

where d and p are the dual and primal objective function values attained by candidate solutions ν and x , and d^⋆ and p^⋆ are the optima of the dual and primal objective function values attained by the optimal solutions ν ^⋆ and x ^⋆. The relative suboptimality of our solution x is

$\rho \triangleq \frac{p-p^{\star }}{p^{\star }},$

(68)

but we cannot directly compute ρ because do not have access to p^⋆. Instead, we note that equation (67) gives us the following upper bound on the relative suboptimality:

$\widehat{\rho }\triangleq \frac{p-d}{d}\ge \rho .$

(69)

For the standard RWHEC problem with known scale, $\widehat{\rho }=-6.41\mathrm{e}$ -9, and for the monocular case, $\widehat{\rho }=8.55\mathrm{e}$ -9. In both cases, the relative duality gap indicates that the primal solution returned by our method attains an objective value that is less than a millionth of a percent off of the lower bound provided by the dual solution. Finally, while weak duality means that $\widehat{\rho }<0$ is not possible in theory, our solutions were computed with floating point arithmetic which approximates $\mathbb{R}$ discretely and inevitably introduces roundoff errors. Therefore, the negative duality gap for the case with known scale is not large enough to cause concern.

Runtime

Since calibration is typically an offline procedure, we did not expend effort tuning solver parameters to reduce algorithm runtime. All experiments were run on a laptop with an Intel Core i7-8750H CPU and 16 GB of memory. The parameters used by COSMO and Ceres were selected to ensure accurate convergence, and the longest runtime observed was approximately 5 minutes for our global solver in the monocular real world experiment reported in Table 6. COSMO solved the synthetic problems with the sparsity pattern in Figure 9(b) substantially faster, taking at most 7 seconds. Unsurprisingly, the Ceres implementation of LOM took at most one second.

Figure 9.

Two directed graphs representing generalized RWHEC problems. Depending on the interpretation of variables discussed in Maximum likelihood estimation, it is possible to use either graph for the same RWHEC scenario. For example, the convention in Figure 1 treats X₁ as the hand-eye transformation and would correspond to Figure 9(a) when extended to include multiple targets. In contrast, the convention in Figure 3 treats Y₁ as the hand-eye transformation and therefore corresponds with Figure 9(b), leading to a sparser objective function (see Figure 10).

Conclusion

We have presented an efficient and certifiably globally optimal solution to a generalized formulation of multi-sensor extrinsic calibration. Our formulation builds on previous robot-world and hand-eye calibration methods by incorporating monocular cameras and arbitrary multi-sensor and target configurations. We have also characterized the subset of multi-sensor RWHEC problem instances which have a unique solution, and used this to prove that RWHEC admits a tight SDP relaxation when measurement noise is not too large. Our proof of this tightness result additionally required us to prove a general theorem on constraint qualification for systems with redundant constraints, including SO(3). Our experiments, which compare our method to a standard local solver and existing calibration methods, verify that global optimality is an important consideration for RWHEC, and that our MLE problem formulation using rotation matrices is superior to dual quaternion-based methods. Additionally, our method can be used as a certification step for other (potentially faster) generalized RWHEC solvers that lack formal guarantees (Yang et al., 2021).

We see our contributions as critical steps toward truly “power-on-and-go” sensor calibration algorithms for multi-sensor robotic systems. Realizing this vision will require extending our RWHEC solver so that it does not depend on specialized calibration targets and is therefore able to handle outliers caused by errors in data association. One potential option for mitigating the effect of outliers on generalized RWHEC is to use a robust truncated least squares objective function, which has been applied to other estimation problems while preserving their QCQP structure (Yang et al., 2021). Additionally, our current MLE formulation assumes that translation and rotation noise is isotropic. Future work can extend our problem formulation with guidance from the study of anisotropic SLAM in Holmes et al. (2024). Finally, truly autonomous sensor calibration will require an active perception strategy which can design trajectories that are information-theoretically optimal (Grebe et al., 2021) or seek out measurements that render parameters identifiable (Yang et al., 2023).