On the covariance of X in AX = XB

TL;DR

This paper derives a rigorous method to compute the covariance of the hand-eye calibration transformation in robot vision, enhancing the understanding of uncertainty in robot perception tasks.

Contribution

It introduces a covariance propagation approach in SE(3) for the AX=XB problem, providing precise uncertainty estimates for the transformation.

Findings

Accurately predicts covariance of hand-eye transformation.

Validated with synthetic and real data.

Offers a tool for high-precision robot perception applications.

Abstract

Hand-eye calibration, which consists in identifying the rigid- body transformation between a camera mounted on the robot end-effector and the end-effector itself, is a fundamental problem in robot vision. Mathematically, this problem can be formulated as: solve for X in AX = XB. In this paper, we provide a rigorous derivation of the covariance of the solution X, when A and B are randomly perturbed matrices. This fine-grained information is critical for applications that require a high degree of perception precision. Our approach consists in applying covariance propagation methods in SE(3). Experiments involving synthetic and real calibration data confirm that our approach can predict the covariance of the hand-eye transformation with excellent precision.

Equations

$$\text{prescript}\mathbf{b}{\mathbf{T}}_1^{\mathbf{e}}\text{prescript}\mathbf{e}{\mathbf{T}}^{\mathbf{c}}\text{prescript}\mathbf{c}{\mathbf{T}}_1^{\mathbf{o}}=\text{prescript}\mathbf{b}{\mathbf{T}}^{\mathbf{o}}=\text{prescript}\mathbf{b}{\mathbf{T}}_2^{\mathbf{e}}\text{prescript}\mathbf{e}{\mathbf{T}}^{\mathbf{c}}\text{prescript}\mathbf{c}{\mathbf{T}}_2^{\mathbf{o}},$$

$${\text{prescript}\mathbf{b}{\mathbf{T}}_2^{\mathbf{e}}}^{-1}\text{prescript}\mathbf{b}{\mathbf{T}}_1^{\mathbf{e}}\text{prescript}\mathbf{e}{\mathbf{T}}^{\mathbf{c}}=\text{prescript}\mathbf{e}{\mathbf{T}}^{\mathbf{c}}\text{prescript}\mathbf{c}{\mathbf{T}}_2^{\mathbf{o}}{\text{prescript}\mathbf{c}{\mathbf{T}}_1^{\mathbf{o}}}^{-1},$$

$${\mathbf{R}}_{{\mathbf{A}}_i}=\exp ([{\mathbf{\xi }}_{{\mathbf{R}}_{\mathbf{A}}i}]){\bar{\mathbf{R}}}_{{\mathbf{A}}_i},$$

$${\mathbf{R}}_{{\mathbf{B}}_i}=\exp ([{\mathbf{\xi }}_{{\mathbf{R}}_{\mathbf{B}}i}]){\bar{\mathbf{R}}}_{{\mathbf{B}}_i},$$

$${\mathbf{t}}_{{\mathbf{A}}_i}={\mathbf{\xi }}_{{\mathbf{t}}_{\mathbf{A}}i}+{\bar{\mathbf{t}}}_{{\mathbf{A}}_i},$$

$${\mathbf{t}}_{{\mathbf{B}}_i}={\mathbf{\xi }}_{{\mathbf{t}}_{\mathbf{B}}i}+{\bar{\mathbf{t}}}_{{\mathbf{B}}_i},$$

$${\mathbf{\Sigma }}_{\mathbf{f}}=\mathcal{J}\mathbf{\Sigma }{\mathcal{J}}^{\top },$$

$${\mathbf{P}}^{\ast }:=\underset{\mathbf{P}}{\min }\Vert \mathbf{V}-\mathbf{f}(\mathbf{P}){\Vert }_{{\mathbf{\Sigma }}_{\mathbf{V}}}.$$

$$\Vert \mathbf{V}-\mathbf{f}(\mathbf{P}){\Vert }_{{\mathbf{\Sigma }}_{\mathbf{V}}}=\Vert (\mathbf{V}-\bar{\mathbf{V}})-\mathcal{J}(\mathbf{P}-\bar{\mathbf{P}}){\Vert }_{{\mathbf{\Sigma }}_{\mathbf{V}}}.$$

$${\mathbf{P}}^{\ast }-\bar{\mathbf{P}}=({\mathcal{J}}^{\top }{{\mathbf{\Sigma }}_{\mathbf{V}}}^{-1}\mathcal{J}{)}^{-1}{\mathcal{J}}^{\top }{{\mathbf{\Sigma }}_{\mathbf{V}}}^{-1}(\mathbf{V}-\bar{\mathbf{V}}).$$

$${\mathbf{\Sigma }}^{\ast }$$

$${\mathbf{R}}_{\mathbf{A}i}\mathbf{R}=\mathbf{R}{\mathbf{R}}_{\mathbf{B}i},$$

$${\mathbf{R}}_{\mathbf{A}i}\mathbf{t}+{\mathbf{t}}_{\mathbf{A}i}=\mathbf{R}{\mathbf{t}}_{\mathbf{B}i}+\mathbf{t},$$

$$[{\mathbf{\alpha }}_i]:=\log {\mathbf{R}}_{\mathbf{A}i},[{\mathbf{\beta }}_i]:=\log {\mathbf{R}}_{\mathbf{B}i}.$$

$${\mathbf{\Sigma }}_{{\mathbf{\alpha }}_i}=\mathbf{J}({\mathbf{\alpha }}_i{)}^{-1}{\mathbf{\Sigma }}_{{\mathbf{R}}_{\mathbf{A}}i}\mathbf{J}({\mathbf{\alpha }}_i{)}^{-1\top },$$

$${\mathbf{\Sigma }}_{{\mathbf{\beta }}_i}=\mathbf{J}({\mathbf{\beta }}_i{)}^{-1}{\mathbf{\Sigma }}_{{\mathbf{R}}_{\mathbf{B}}i}\mathbf{J}({\mathbf{\beta }}_i{)}^{-1\top },$$

$$\log {\mathbf{R}}_{\mathbf{A}i}=\log \mathbf{R}{\mathbf{R}}_{\mathbf{B}i}{\mathbf{R}}^{\top }=\mathbf{R}[{\mathbf{\beta }}_i]{\mathbf{R}}^{\top }.$$

$${\mathbf{\alpha }}_i=\mathbf{R}{\mathbf{\beta }}_i.$$

$$\mathbf{R}=\exp ([{\mathbf{\xi }}_{\mathbf{R}}])\hat{\mathbf{R}}.$$

$$\underbrace{\left(\begin{array}[]{c}\bm{\beta}_{1}\\ \bm{\alpha}_{1}\\ \vdots\\ \bm{\beta}_{k}\\ \bm{\alpha}_{k}\\ \end{array}\right)}_{\bm{V}}=\underbrace{\left(\begin{array}[]{c}\bm{\beta}_{1}\\ \exp([\bm{\xi}_{\bm{R}}])\hat{\bm{R}}\bm{\beta}_{1}\\ \vdots\\ \bm{\beta}_{k}\\ \exp([\bm{\xi}_{\bm{R}}])\hat{\bm{R}}\bm{\beta}_{k}\\ \end{array}\right)}_{\bm{f}(\bm{P})}.$$

$${\mathbf{\Sigma }}_{\mathbf{V}}:=\mathrm{diag}({\mathbf{\Sigma }}_{{\mathbf{V}}_1},{\mathbf{\Sigma }}_{{\mathbf{V}}_2},...,{\mathbf{\Sigma }}_{{\mathbf{V}}_k}),$$

$$\underset{\mathbf{P}}{\min }\Vert \mathbf{V}-\mathbf{f}(\mathbf{P})\Vert =\underset{\mathbf{P}}{\min }\sum _i^k({\mathbf{V}}_i-\mathbf{f}(\mathbf{P}{)}_i{)}^{\top }{\mathbf{\Sigma }}_{{\mathbf{V}}_i}^{-1}({\mathbf{V}}_i-\mathbf{f}(\mathbf{P}{)}_i).$$

$${\hat{\mathbf{R}}}_{(j+1)}$$

$${\hat{\mathbf{\beta }}}_{i(j+1)}$$

$${\mathcal{J}}_{\mathbf{f}}^{\top }{\mathbf{\Sigma }}_{\mathbf{V}}^{-1}{\mathcal{J}}_{\mathbf{f}}\mathbf{\delta }={\mathcal{J}}_{\mathbf{f}}^{\top }{\mathbf{\Sigma }}_{\mathbf{V}}^{-1}(\mathbf{V}-\mathbf{f}(\mathbf{P})).$$

$${\mathcal{J}}_{\mathbf{f}}$$

$$\text{where }\bm{\mathcal{J}}^{\bm{\xi}_{\bm{R}}}_{i}:=\left[\begin{array}[]{c}\bf 0\\ -[\hat{\bm{R}}{\hat{\bm{\beta}}}_{i}]\end{array}\right],\bm{\mathcal{J}}^{\bm{\beta}}_{i}:=\left[\begin{array}[]{c}\mathbb{I}\\ \hat{\bm{R}}\end{array}\right].$$

$$\displaystyle\left[\begin{array}[]{cc}\bm{\mathcal{J}}^{\bm{\xi}_{\bm{R}}\top}{\bf{\Sigma{}}}_{\bm{V}}^{-1}\bm{\mathcal{J}}^{\bm{\xi}_{\bm{R}}}&\bm{\mathcal{J}}^{\bm{\xi}_{\bm{R}}\top}{\bf{\Sigma{}}}_{\bm{V}}^{-1}\bm{\mathcal{J}}^{\bm{\beta}}\\ \bm{\mathcal{J}}^{\bm{\beta}\top}{\bf{\Sigma{}}}_{\bm{V}}^{-1}\bm{\mathcal{J}}^{\bm{\xi}_{\bm{R}}}&\bm{\mathcal{J}}^{\bm{\beta}\top}{\bf{\Sigma{}}}_{\bm{V}}^{-1}\bm{\mathcal{J}}^{\bm{\beta}}\end{array}\right]\left(\begin{array}[]{c}\bm{\xi}_{\bm{R}}\\ \bm{\delta}_{\bm{\beta}}\end{array}\right)$$

$$\displaystyle=\left(\begin{array}[]{c}\bm{\mathcal{J}}^{\bm{\xi}_{\bm{R}}\top}{\bf{\Sigma{}}}_{\bm{V}}^{-1}(\bm{V}-\bm{f}(\bm{P}))\\ \bm{\mathcal{J}}^{\bm{\beta}\top}{\bf{\Sigma{}}}_{\bm{V}}^{-1}(\bm{V}-\bm{f}(\bm{P}))\end{array}\right).$$

$$\mathbf{U}:=$$

$$\mathbf{W}:=$$

$$\mathbf{Z}:=$$

$$\text{ where }{\mathbf{W}}_i:=$$

$${\mathbf{ϵ}}_{{\mathbf{\xi }}_{\mathbf{R}}}:=$$

$$=$$

Full text

I Introduction

Hand-eye calibration, which consists in identifying the rigid-body transformation between a camera (eye) mounted on the robot end-effector and the end-effector (hand) itself, is a fundamental problem in robot vision. Mathematically, this problem can be formulated as: solve for $\bm{X}$ in $\bm{A}\bm{X}=\bm{X}\bm{B}$, where $\bm{X}$ is the unknown $4\times 4$ hand-eye transformation matrix and $\bm{A}$ and $\bm{B}$ are known $4\times 4$ transformation matrices (see details in Section II-A). Starting from the late 1980’s, a large amount of literature has been devoted to this problem, and a number of efficient methods have been developed, see e.g. [1, 2, 3, 4, 5, 6].

In this paper, we are interested, not merely in solving for $\bm{X}$, but more comprehensively, in evaluating the covariance of $\bm{X}$ from those of $\bm{A}$ and $\bm{B}$, where $\bm{A}$ and $\bm{B}$ are now randomly perturbed transformation matrices. This fine-grained information is critical in high-precision robotics applications for several reasons.

Motivations

The uncertainty of the object pose estimation comes from three main sources: (i) the uncertainty of the object pose estimation in the camera frame, (ii) the uncertainty of the hand-eye calibration, and (iii) the uncertainty of the robot end-effector positioning. In practice, source (ii) arguably contributes the most: for instance, a tiny orientation error of 0.05 degrees in the hand-eye calibration already implies an error of 0.6 mm in object position if the latter is 70 cm away from the camera (typical viewing distance for commodity 3D cameras). In turn, having a precise knowledge of the uncertainty of the object pose estimation is critical:

• •

In high-precision manufacturing, it is important, not only to know the pose of an object, but also to guarantee that the pose estimation error is within some tolerance. For instance, when drilling holes in the fuselage of an aircraft, the hole position tolerance is 0.5 mm – which would be violated by an error of 0.05 degrees in the hand-eye calibration, even when assuming that the object pose estimation in the camera frame is perfect (see above);

• •

The precise knowledge of the object pose covariance matrix allows one to intelligently refine the object pose estimation by other perception modes. For instance, in visuo-tactile sensor fusion [7], knowing that the covariance of the object pose is comparatively large in the translation along, say, the X-axis will prompt us to touch the object along that axis in order to best reduce the uncertainty.

In addition, knowing the covariance of $\bm{X}$ allows improving the calibration process itself, by e.g. choosing the appropriate number of measurements to achieve a desired level of precision, or choosing the appropriate matrices $\bm{A}$ and $\bm{B}$ that minimize the covariance of $\bm{X}$.

Related works

Finding the covariance of $\bm{X}$ is challenging for several reasons. First, as $\bm{X}$, $\bm{A}$ and $\bm{B}$ represent rigid-body transformations, they live in $\bm{S}\bm{E}(3)$, a subset of the space of $4\times 4$ matrices endowed with a non-trivial Lie group structure [8]. Second, how to represent and calculate uncertainties in $\bm{S}\bm{E}(3)$ is by itself a complex issue, which has prompted advanced mathematical developments [9]. Finally, merely solving for $\bm{X}$ in $\bm{A}\bm{X}=\bm{X}\bm{B}$ is already a difficult problem [3, 4, 5], let alone evaluating the uncertainty of the solution.

There are a number of works dealing with the uncertainty of hand-eye calibration. In [10], based on a sensitivity analysis of closed-form solutions, some critical factors and criteria influencing the accuracy of the result are analyzed. For instance, one may try to maximize the angle between rotation axes of relative movement to reduce the influence on error in rotation, or to minimize the distance between the optical center of the camera and the calibration pattern to reduce the influence on error in translation. Based on this analysis, Shi et al. [11] present a algorithm to select movement pairs automatically from a series of measurements to reduce the error of the estimate. Schmidt et al. also introduce similar approach based on a vector quantization method [12]. In [13], Aron et al. present an error estimation method of the rotation part of $\bm{X}$ based on an Euler angles parameterization. The authors do not discuss how that error propagates to the translation part of $\bm{X}$ and their vision tracking measurements are also assumed to be noise-free. More fundamentally, the Euler angles formulation, as opposed to the $\bm{S}\bm{E}(3)$ formulation, is well-known to involve singularities.

The idea of estimating explicitly uncertainties in the system is by no means new. Many have studied the problem of uncertainty in the camera model (intrinsic and extrinsic parameters) [14] and the propagation of uncertainties through the camera model [9]. However, we stress that this work is different in that it focuses on the hand-eye transformation and its uncertainty.

Contribution and organization of the paper

It can be noted that none of the aforementioned works has provided a derivation of the covariance of $\bm{X}$, which is ultimately the most generic and relevant quantification of the uncertainty of the hand-eye calibration process. The goal of this paper is to rigorously work out such a derivation. Specifically, we transpose methods for forward and backward propagation of covariance [14] into the framework of uncertainty in $\bm{S}\bm{E}(3)$ proposed by Barfoot and Furgale [9]. The structure of the hand-eye calibration equation raises specific technical difficulties, which we shall address in detail.

The remainder of the paper is organized as follows. In Section II, we state the hand-eye calibration problem and introduce the mathematical background of the work, which includes the representation of uncertainty in $\bm{S}\bm{E}(3)$, and methods for forward and backward propagation of covariance. In Section III, we present our method to estimate the rotation and translation parts of the hand-eye transformation matrix and their associated covariance matrices. In Section IV, we show that the method can indeed predict with excellent precision these covariances in synthetic and real calibration datasets, and uses this information to compute the covariance of the object pose estimation in a real setting. Finally, in Section V, we conclude by discussing the advantages and drawbacks of our approach and sketch some future research directions.

II Background

II-A Formulation of the hand-eye calibration problem

The classical hand-eye calibration method consists in looking at a fixed pattern from two different viewpoints, say 1 and 2, giving rise to the following equation

[TABLE]

where

• •

$\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{i}$ is the transformation of the end-effector with respect to the fixed robot base at configuration $i$;

• •

$\prescript{}{\bm{e}}{\bm{T}}^{\bm{c}}$ is the constant transformation of the camera with respect to the end-effector;

• •

$\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{i}$ is the transformation of the pattern (object) with respect to the camera at configuration $i$;

• •

$\prescript{}{\bm{b}}{\bm{T}}^{\bm{o}}$ is the constant transformation of the pattern with respect to the robot base (see Fig. 1).

Next, one can transform the above equation into

[TABLE]

which has the form of $\bm{A}\bm{X}=\bm{X}\bm{B}$, where $\bm{X}:=\prescript{}{\bm{e}}{\bm{T}}^{\bm{c}}$ is the unknown hand-eye transformation, and $\bm{A}:={\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{2}}^{-1}\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{1}$ and $\bm{B}:=\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{2}{\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{1}}^{-1}$ can be computed from respectively the robot kinematics and pattern pose estimation [3]. Next, if the fixed pattern is viewed from a large number of viewpoints, one can collect many different $\bm{A}$’s and $\bm{B}$’s. Suppose that we have a set of $k$ measurements ${(\bm{A}_{1},\bm{B}_{1}),(\bm{A}_{2},\bm{B}_{2}),...,(\bm{A}_{k},\bm{B}_{k})}$. Since in practice these measurements are perturbed by actuator/sensor noise, the exact solution for the set of $k$ equations $\bm{A}_{i}\bm{X}=\bm{X}\bm{B}_{i}$ will not exist. Instead, the problem is commonly framed as an optimization problem in which $\bm{X}$ is found as the transformation that “best” fits the $k$ equalities.

Note that sometimes the camera may not be mounted on the end-effector but on a fixed stand. In this case, finding the relative transformation between the camera and the robot base can also be formulated as the $\bm{A}\bm{X}=\bm{X}\bm{B}$ problem and can be treated by the same method.

II-B *Representation of rigid-body transformations and of their

uncertainties*

We choose to represent rigid-body transformations as elements of the Special Euclidean group $\bm{S}\bm{E}(3)$ [8]. To model the uncertainty on $\bm{S}\bm{E}(3)$, we adopt the framework proposed in [9]. As there is in general no bi-invariant distance on $\bm{S}\bm{E}(3)$ [15], solving for the rotation and translation components of $\bm{X}$ simultaneously would in any case require an arbitrary rotation/translation weighting. Instead, we choose to solve them separately, which entails a number of simplifications [3]. As a consequence, the uncertainties of the rotation and the translation parts are also modeled separately.

Specifically, we assume that the rotation parts of the observations $\bm{A}_{i}$ and $\bm{B}_{i}$ are corrupted by Gaussian noise as follows

[TABLE]

where $\bar{\bm{R}}_{\bm{A}_{i}},\bar{\bm{R}}_{\bm{B}_{i}}\in\bm{S}\bm{O}(3)$ are the means of $\bm{R}_{\bm{A}_{i}},\bm{R}_{\bm{B}_{i}}$, and $\bm{\xi}_{\bm{R}_{\bm{A}}i},\bm{\xi}_{\bm{R}_{\bm{B}}i}\in\mathbb{R}^{3}$ are zero-mean Gaussian perturbations with covariance matrices ${\bf{\Sigma{}}}_{\bm{R}_{\bm{A}}i},{\bf{\Sigma{}}}_{\bm{R}_{\bm{B}}i}$, respectively.

The translation parts of the $\bm{A}_{i}$ and $\bm{B}_{i}$ are corrupted as follows

[TABLE]

where $\bar{\bm{t}}_{\bm{A}_{i}},\bar{\bm{t}}_{\bm{B}_{i}}\in\mathbb{R}^{3}$ are the means of $\bm{t}_{\bm{A}_{i}},\bm{t}_{\bm{B}_{i}}$, and $\bm{\xi}_{\bm{t}_{\bm{A}}i},\bm{\xi}_{\bm{t}_{\bm{B}}i}\in\mathbb{R}^{3}$ are zero-mean Gaussian perturbations with covariance matrices ${\bf{\Sigma{}}}_{\bm{t}_{\bm{A}}i},{\bf{\Sigma{}}}_{\bm{t}_{\bm{B}}i}$, respectively.

Note that the above assumptions imply that rotation and translation noises are independent.

II-C Forward and backward propagation of covariance

Forward propagation. Let $\bm{P}$ be a random vector in $\mathbb{R}^{M}$ with mean $\bar{\bm{P}}$ and covariance matrix ${\bf{\Sigma{}}}$. Consider a function $\bm{f}:\mathbb{R}^{M}\rightarrow\mathbb{R}^{N}$ that is differentiable in a neighbourhood of $\bar{\bm{P}}$. Then, at the first order of approximation, $\bm{f}(\bm{P})$ is a random variable with mean $\bm{f}(\bar{\bm{P}})$ and covariance matrix

[TABLE]

where $\bm{\mathcal{J}}$ is the Jacobian matrix of $\bm{f}$ at $\bar{\bm{P}}$.

Backward propagation. Assume now that $\bm{P}$ (the parameter) is unknown, but that $\bm{V}:=\bm{f}(\bm{P})$ (the measurement) is known and determined to be a random variable with mean $\bar{\bm{V}}$ and covariance matrix ${\bf{\Sigma{}}}_{\bm{V}}$. Then the best estimate for $\bm{P}$ is given by

[TABLE]

To estimate the covariance of $\bm{P}$, one can approximate $\bm{f}$ by an affine function $\bm{f}(\bm{P})=\bm{f}(\bar{\bm{P}})+\bm{\mathcal{J}}(\bm{P}-\bar{\bm{P}})$, which yields

[TABLE]

Using the weighted pseudo-inverse, one has

[TABLE]

From (7), the covariance of $\bm{P}$ can now be approximated at the first order by

[TABLE]

In practice, when performing an iterative least-squares optimization, one can use (10) at the last iteration to obtain the estimation of the covariance of $\bm{P}$.

Note that the quality of the approximations given by Equations (7) and (10) depends in particular on the quality of the linear approximation of $\bm{f}$.

III Derivation of the covariance of $\bm{X}$

Equation $\bm{A}_{i}\bm{X}=\bm{X}\bm{B}_{i}$ can be decomposed as

[TABLE]

where $\bm{R},\bm{t}$ denote respectively the rotation and translation parts of $\bm{X}$.

III-A Covariance of the rotation part of $\bm{X}$

We first consider the rotation part $\bm{R}$ of $\bm{X}$. Let $[\bm{\alpha}_{i}],[\bm{\beta}_{i}]\in\bm{s}\bm{o}(3)$ denote the logarithms of $\bm{R}_{\bm{A}i}$ and $\bm{R}_{\bm{B}i}$ respectively, i.e.

[TABLE]

Note that the covariance matrices of $\bm{\alpha}_{i}$ and $\bm{\beta}_{i}$ can be obtained by applying the forward propagation of covariance

[TABLE]

where $\bm{J}(\bm{\alpha}_{i})$ denotes the (left) Jacobian of $\bm{S}\bm{O}(3)$ at $\bm{\alpha}_{i}$, see [9] for more details.

Next, via logarithm mapping, equation (11) can be written as

[TABLE]

Applying the rule $\bm{R}[\bm{\omega}]\bm{R}^{\top}=[\bm{R}\bm{\omega}]$ for $\bm{R}\in\bm{S}\bm{O}(3)$ and $[\bm{\omega}]\in\bm{s}\bm{o}(3)$, one has

[TABLE]

In order to use the uncertainty model in $\bm{S}\bm{O}(3)$, we define a random variable $\bm{\xi}_{\bm{R}}$ that represents the difference between $\bm{R}$ and the current estimate $\hat{\bm{R}}$ by

[TABLE]

Next, to apply the backward propagation of covariance, one needs the measurement vectors $\bm{\alpha}_{i}$ and $\bm{\beta}_{i}$ to appear on the same side of the equation. To achieve this without making it too complex, we use a trick from [14], which consists in “copying” the $\bm{\beta}_{i}$’s on both sides, as follows

[TABLE]

Now, the measurement vector is given by $\bm{V}:=(\bm{V}_{1},\dots,\bm{V}_{k})$, where $\bm{V}_{i}:=(\bm{\beta}_{i},\bm{\alpha}_{i})$, and the parameter vector is given by $\bm{P}:=(\bm{\xi}_{\bm{R}},\bm{\beta}_{1},\dots,\bm{\beta}_{k})$.

Since the noise of $\bm{\alpha}_{i}$’s and $\bm{\beta}_{i}$’s are independent ($\bm{\alpha}$ is caused by robot kinematics while $\bm{\beta}_{i}$ is caused by object pose estimation in the camera frame), the covariance matrix of the measurement vector is given by

[TABLE]

with ${\bf{\Sigma{}}}_{\bm{V}_{i}}:=\mathrm{diag}({\bf{\Sigma{}}}_{\bm{\beta}_{i}},{\bf{\Sigma{}}}_{\bm{\alpha}_{i}})$.

Now, the covariance-weighted minimization is given by

[TABLE]

This minimization problem can be solved by iteratively updating the estimate of the parameter vector by the rules

[TABLE]

where at each step $(j)$ the update vector $\bm{\delta}:=(\bm{\xi}_{\bm{R}},\bm{\delta}_{\bm{\beta}_{1}},\dots,\bm{\delta}_{\bm{\beta}_{k}})$ is found by solving the normal equation

[TABLE]

The Jacobian of $\bm{f}$ has the form

[TABLE]

[TABLE]

The set of equations (22) may now written in block form as

[TABLE]

To simplify the left-hand side of (36), let

[TABLE]

As for the right-hand side of (36), let

[TABLE]

To solve equations (36), one can left-multiply both sides by \left[\begin{array}[]{cc}\mathbb{I}&\bm{W}\bm{Z}^{-1}\\ \bf 0&\mathbb{I}\end{array}\right], which yields

[TABLE]

The above equations can now be solved to find the updating vectors $\bm{\xi}_{\bm{R}}$ and $\bm{\delta}_{\bm{\beta}}$.

Applying backward propagation of covariance, a first-order approximation of the covariance of $\bm{P}$ is given the following matrix, taken at the last iteration,

[TABLE]

The covariance of $\bm{\xi}_{\bm{R}}$ is given by the top-left block of ${\bf{\Sigma{}}}^{*}$, that is:

[TABLE]

III-B Covariance of the translation part of $\bm{X}$

We now consider the translation part $\bm{t}$ of $\bm{X}$. Let $\bm{q}_{i}:=\bm{R}\bm{t}_{\bm{B}_{i}}-\bm{t}_{\bm{A}_{i}}$. Equations (12) can be written as

[TABLE]

Note that the covariance matrices of $\bm{q}_{i}$ can be approximated by applying the forward propagation of covariance

[TABLE]

where ${\bm{R}^{*}}$ is the optimal rotation found in the previous section, and ${\bf{\Sigma{}}}_{\bm{R}}$ is the corresponding covariance.

Applying the same trick as previously, we “copy” the $\bm{R}_{\bm{A}i}$’s on both sides of the equation, as follows

[TABLE]

Now the measurement vector is given by $\bm{V}:=(\bm{V}_{1},\dots,\bm{V}_{k})$, where $\bm{V}_{i}:=(\bm{R}_{\bm{A}_{i}},\bm{q}_{i})$, and the parameter vector is given by $\bm{P}:=(\bm{t},\bm{R}_{\bm{A}_{1}},\dots,\bm{R}_{\bm{A}_{k}})$.

Since computing the cross-variance of $\bm{R}_{\bm{A}_{i}}$ and $\bm{q}_{i}$ would be too complex, we simply assume them to be independent. The covariance matrix of the measurement vector is then given by

[TABLE]

where ${{\bf{\Sigma{}}}_{\bm{V}_{i}}}:=\mathrm{diag}({\bf{\Sigma{}}}_{{\bm{R}_{\bm{A}}}_{i}},{\bf{\Sigma{}}}_{\bm{q}_{i}})$.

Now, the covariance-weighted minimization is given by

[TABLE]

We solve this by iteratively updating the estimate of the parameter vector by the rules

[TABLE]

where at each step $(j)$ the update vector $\bm{\delta}:=(\bm{\delta}_{\bm{t}},{\bm{\xi}_{\bm{R}_{\bm{A}}}})=(\bm{\delta}_{\bm{t}},\bm{\xi}_{\bm{R}_{\bm{A}_{1}}},\dots,\bm{\xi}_{\bm{R}_{\bm{A}_{k}}})$ is found by solving the normal equation

[TABLE]

The Jacobian matrix has the form

[TABLE]

For the rest of the derivation, we following the same procedure as previously derived. One thus can obtain the update vectors from

[TABLE]

[TABLE]

At the last iteration, a first-order approximation of the covariance matrix of $\bm{t}$ is given by

[TABLE]

IV Experiments

We now validate the proposed method by comparing the covariance predicted by the method and that obtained from Monte-Carlo simulations, using synthetic and real calibration data. Using the covariance of $\bm{X}$, we are then in a position to compute the covariance of the object pose estimation in a real setting. Our implementation is open-source and is available at https://github.com/dinhhuy2109/python-cope.

IV-A Synthetic calibration data

To generate synthetic data, we start by selecting a random transformation matrix $\bar{\bm{X}}=(\bar{\bm{R}},\bar{\bm{t}})$, which serves as the true hand-eye transformation. We then generate $M=1000$ dataset, each dataset comprising $k=30$ corrupted pairs $(\bm{A}_{i},\bm{B}_{i})_{i\in[1,k]}$. Each corrupted pair is generated as follows. First, we generate a random uncorrupted pair $(\bar{\bm{A}}_{i},\bar{\bm{B}}_{i})$, which verifies $\bar{\bm{A}}_{i}\bar{\bm{X}}=\bar{\bm{X}}\bar{\bm{B}}_{i}$ exactly. Next, we add noise to $\bar{\bm{A}}_{i}$ and $\bar{\bm{B}}_{i}$ as explained in Section II-B. The covariance matrices of the noise are chosen arbitrarily as

[TABLE]

where $\lambda\in\mathbb{R}$ is a scaling parameter that allows us to change the magnitude of the uncertainties.

At each noise level $\lambda$, we evaluate the covariance of $\bm{X}$ following two methods

• •

Our method: For some dataset $m\in[1,M]$, we compute the covariance matrices ${\bf{\Sigma{}}}_{\bm{R}_{m}},{\bf{\Sigma{}}}_{\bm{t}_{m}}$ using the proposed method (PRedicted). In fact, the ${\bf{\Sigma{}}}_{\bm{R}_{m}},{\bf{\Sigma{}}}_{\bm{t}_{m}}$ are nearly identical across the $M$ datasets, so the particular value of $m$ did not matter;

• •

Monte-Carlo: For each dataset $m\in[1,M]$, we find the rotation and translation $\hat{\bm{R}}_{m}$, $\hat{\bm{t}}_{m}$ that optimally fit the $k$ hand-eye equations following the proposed method. We then compute the covariance matrices ${\bf{\Sigma{}}}_{\bm{R}}^{\mathrm{MC}}$, ${\bf{\Sigma{}}}_{\bm{t}}^{\mathrm{MC}}$ by the Monte-Carlo method across the $M$ datasets as

[TABLE]

where $\bm{\xi}_{\bm{R}_{m}}:=(\log(\hat{\bm{R}}_{m}\bar{\bm{R}}^{-1}))^{\vee}$111The $[\leavevmode\nobreak\ ]$ operator turns $\bm{\xi}\in\mathbb{R}^{3}$ into a member of the Lie algebra $\bm{s}\bm{e}(3)$ (see equation (13)), we use $\vee$ as the inverse operation of $[\leavevmode\nobreak\ ]$., $\bm{\xi}_{\bm{t}_{m}}:=\hat{\bm{t}}_{m}-\bar{\bm{t}}$.

Fig. 2 shows projections of the covariance ellipsoids on pairs of axes for the Monte-Carlo method and our method, when $\lambda=10^{-5}$. It can be noted that the proposed method provides an excellent estimation of the covariance of the hand-eye transformation.

To gauge the performance at different noise levels, we use the following metrics

[TABLE]

Fig 3 shows that our algorithm can cope well with increasing magnitudes of the measurement uncertainty. The estimation errors remain low overall, and increases slightly with the magnitude of the noise, since larger noise levels increase the number of local minima at each iteration. Note also that the errors in the covariances of the translation parts tend to be larger than that of the rotation parts. This is because, in our method, the errors in the estimation of the rotation propagate to that of the translation. Regarding the computation cost, our method is naturally several magnitude faster than the Monte-Carlo method.

It is also worth noting that the closed form solution in [3] always yields slightly higher covariance as compared to our method. This is because our method does optimally minimize the error of the estimated transformation by taking to account the measurement noise.

IV-B Real calibration data

We now validate the proposed method on actual calibration data obtained from our robot system, which consists of a 3D camera mounted on a 6-DOF industrial manipulator, as shown in Fig. 1.

IV-B1 Covariances of $\bm{A}$ and $\bm{B}$ in the actual system

We first need to empirically estimate the covariances of the $\bm{A}_{i}$’s and $\bm{B}_{i}$’s in our system, so that we can give them as inputs to our method.

As the industrial manipulator has a very high precision (0.2 mm of repeatability), we assume that the noise on the $\bm{A}_{i}$’s is negligible.

Regarding the $\bm{B}_{i}$’s, the $\bm{B}_{1},\dots,\bm{B}_{k}$ are assumed to have the same noise distributions: $\forall i,\ {\bf{\Sigma{}}}_{\bm{R}_{\bm{B}_{i}}}={\bf{\Sigma{}}}_{\bm{R}_{\bm{B}}},{\bf{\Sigma{}}}_{\bm{t}_{\bm{B}_{i}}}={\bf{\Sigma{}}}_{\bm{t}_{\bm{B}}}$. We experimentally collect 500 pairs of $\bm{A}_{i}$ and $\bm{B}_{i}$ from our system. Next, we generate M = 400 datasets, each dataset comprising $k=30$ pairs $(\bm{A}_{i},\bm{B}_{i})$ randomly selected from the collected pairs.

The rotation and translation errors of $\bm{B}_{i}$’s are then computed as

[TABLE]

where the ground truth is $\bar{\bm{B}}_{i}={\bar{\bm{X}}}^{-1}{\bm{A}}_{i}\bar{\bm{X}}$.

Since the true transformation $\bar{\bm{X}}$ is unknown in the real system, we use

[TABLE]

as the ground truth. Note that estimating $\hat{\bm{R}}_{m},\hat{\bm{t}}_{m}$ using our method would require information of $\bm{A}_{i}$’s, $\bm{B}_{i}$’s noise, therefore we use [3] instead.

After obtaining the rotation and translation errors of $\bm{B}_{i}$’s, the empirical covariance matrices of $\bm{B}_{i}$ can be estimated similarly to the equations (79,80).

IV-B2 Validation

To validate our method, we collect another 500 pairs of $\bm{A}_{i}$ and $\bm{B}_{i}$ from our system. We constrain the robot motion so that it covers the same area as that used for determining the noise on $\bm{B}_{i}$. We then generate $M=400$ datasets, each dataset comprising $k=30$ pairs $(\bm{A}_{i},\bm{B}_{i})$ randomly selected from the collected pairs. The covariance matrices are computed from these datasets using the Monte-Carlo method and our method, in the same manner as previously.

Fig. 4 provides projections of the covariance ellipsoids on pairs of axes shown for two methods. We see that the proposed method delivers a good estimation of the covariances. We do not believe there has been another methods of estimating uncertainty of the hand-eye transformation. Moreover, the proposed method is also relatively easy to replicate and use in practical applications.

IV-C Covariance of the object pose estimation

Using the covariance of $\bm{X}$ previously obtained, we are now in a position to predict the covariance of the object pose estimation, which is our ultimate goal. Here, we demonstrate the propagation of uncertainties to the object pose estimation using the same robot system as previously (see Fig. 1).

Recall that the constant transformation of the pattern (object) with respect to the robot base is given by

[TABLE]

The covariances of $\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{i}$ and $\prescript{}{\bm{e}}{\bm{T}}^{\bm{c}}$ $(=\bm{X})$ can be estimated using the procedure proposed in Section IV-B. Thus, to predict the mean and the covariance of $\bm{Y}$, one needs now to estimate the covariances of $\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{i}$.

Suppose that $\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{1},...,\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{k}$ have the same noise distribution, i.e. $\forall i,\ {\bf{\Sigma{}}}_{\bm{R}_{\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}i}}={\bf{\Sigma{}}}_{\bm{R}_{\bm{c}\bm{o}}},{\bf{\Sigma{}}}_{\bm{t}_{\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}i}}={\bf{\Sigma{}}}_{\bm{t}_{\bm{c}\bm{o}}}$. We begin by experimentally collecting 500 pairs of $\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{i},\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{i}$ from our system. The rotation and translation errors of $\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{i}$’s are then computed similar to (82, 83), where the ground truth is $\bar{\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{i}}=\bar{\bm{Y}}\bar{\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{i}}\bar{\bm{X}}$. As discussed earlier, we will use (84,85) instead of the true value of $\bar{\bm{X}}$. Regarding $\bar{\bm{Y}}$, one can transform (86) into

[TABLE]

which has the form of $\bm{A}^{\prime}_{i}\bm{Y}=\bm{Y}\bm{B}^{\prime}_{i}$, where $\bm{A}^{\prime}\coloneqq{\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{p}}{\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{q}}^{-1}$ and $\bm{B}^{\prime}\coloneqq{\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{p}}^{-1}{\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{q}}$. Hence, $\bar{\bm{Y}}$ can also be computed in the same manner as computing $\bar{\bm{X}}$.

We now collect 500 pairs of $\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{i},\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{i}$ from our system. We then generate $M=400$ datasets, each dataset comprising $k=30$ $(\bm{A}_{i},\bm{B}_{i})$ pairs computed from $(\prescript{}{\bm{b}}{\bm{T}}^{\bm{e}}_{i},\prescript{}{\bm{c}}{\bm{T}}^{\bm{o}}_{i})$ pairs randomly selected from the 500 collected pairs.

Next, we evaluate the covariance of $\bm{Y}$ following two methods

• •

Our method: For some dataset $m\in[1,M]$, we compute the covariance matrices ${\bf{\Sigma{}}}_{\bm{R}_{\bm{Y}m}},{\bf{\Sigma{}}}_{\bm{t}_{\bm{Y}m}}$ using the propagation method described in Appendix A (PRedicted). In fact, the ${\bf{\Sigma{}}}_{\bm{R}_{\bm{Y}m}},{\bf{\Sigma{}}}_{\bm{t}_{\bm{Y}m}}$ are nearly identical across the $M$ datasets, so the particular value of $m$ did not matter;

• •

Monte-Carlo: For each dataset $m\in[1,M]$, we compute $\bm{Y}_{m}=\prescript{}{\bm{b}}{\bm{T}}^{\bm{o}}_{m}=\prescript{}{\bm{b}}{\bm{\mathring{missing}}{T}}^{\bm{e}}\hat{\prescript{}{\bm{e}}{\bm{T}}^{\bm{c}}_{m}}\prescript{}{\bm{c}}{\bm{\mathring{missing}}{T}}^{\bm{o}}$, where $\prescript{}{\bm{b}}{\bm{\mathring{missing}}{T}}^{\bm{e}}$ and $\prescript{}{\bm{c}}{\bm{\mathring{missing}}{T}}^{\bm{o}}$ are randomly selected from the collected pairs; the rotation and translation of $\hat{\prescript{}{\bm{e}}{\bm{T}}^{\bm{c}}_{m}}$ together with their covariances are computed using our method as proposed in Section III (see equations (49, 74)). Next, the covariances are computed by the Monte-Carlo method as ${\bf{\Sigma{}}}_{\bm{R}_{\bm{Y}}}^{\mathrm{MC}}:=\frac{1}{M}\sum_{m}^{M}{\bm{\xi}_{\bm{R}_{\bm{Y}_{m}}}}\bm{\xi}^{\top}_{\bm{R}_{\bm{Y}_{m}}}$ and ${\bf{\Sigma{}}}_{\bm{t}_{\bm{Y}}}^{\mathrm{MC}}:=\frac{1}{M}\sum_{m}^{M}\bm{\xi}_{\bm{t}_{\bm{Y}_{m}}}{\bm{\xi}^{\top}_{\bm{t}_{\bm{Y}_{m}}}},$ where $\bm{\xi}_{\bm{R}_{\bm{Y}_{m}}}:=(\log(\hat{\bm{R}}_{\bm{Y}_{m}}{\bar{\bm{R}}_{\bm{Y}}}^{-1}))^{\vee}$, $\bm{\xi}_{\bm{t}_{\bm{Y}_{m}}}:=\hat{\bm{t}}_{\bm{Y}_{m}}-\bar{\bm{t}}_{\bm{Y}}$.

Fig. 5 shows the one-standard-deviation covariance ellipsoids shown for two methods. One can see that our prediction matches very well the covariances estimated by the Monte-Carlo method.

In absolute values, the covariance of the hand-eye calibration compounds with that of the object pose estimation in the camera frame, resulting in a relatively large overall covariance for the object pose estimation in the robot frame, around 1cm in standard deviation. This again emphasizes the need of having access to the covariance of the hand-eye transformation. This fine-grained information tells us how confident we can be regarding the object pose estimation and shall also enable us to design new perception algorithms and methods for reaching higher precision, by e.g. visuo-tactile sensor fusion.

V Conclusion

In this paper, we have presented a rigorous derivation of the covariance of the solution $\bm{X}$, when $\bm{A}$ and $\bm{B}$ are randomly perturbed matrices. Our approach consists in transposing methods for forward and backward propagation of covariance into the framework of uncertainty in $\bm{S}\bm{E}(3)$. Experiments involving synthetic and real calibration data show that our approach can predict the covariance of the hand-eye transformation with excellent precision.

While these estimates could also be provided by Monte-Carlo simulations, such a method would require collecting a large number of samples, which is not practical. Furthermore, the Monte-Carlo method yields no insights into how the uncertainties on the measurements of $\bm{A}$ and $\bm{B}$ propagate to the uncertainty of the hand-eye transformation. By contrast, in our method, by analyzing critical factors influencing the covariance of $\bm{X}$, for instance, based on the formulae (49) and (74), one may be able to refine the calibration process to achieve a higher precision, by e.g. determining the appropriate number of sample viewpoints or choosing their optimal distribution, which is the object of our future research.

Acknowledgment

This work was partially supported by NTUitive Gap Fund NGF-2016-01-028.

Appendix A Propagating uncertainties when rotation and translation

are decoupled

In this Section, we present our extension of the covariance propagation method of [9] to the case where rotation and translation are decoupled.

Consider two noisy poses $\bm{T}_{1}$ and $\bm{T}_{2}$, whose nominal values and associated uncertainties are $\{{\bar{\bm{R}}}_{1},{\bf{\Sigma{}}}_{\bm{R}_{1}}\},\{{\bar{\bm{t}}}_{1},{\bf{\Sigma{}}}_{\bm{t}_{1}}\}$ and $\{{\bar{\bm{R}}}_{2},{\bf{\Sigma{}}}_{\bm{R}_{2}}\},\{{\bar{\bm{t}}}_{2},{\bf{\Sigma{}}}_{\bm{t}2}\}$ respectively.

Let $\bm{T}_{12}=\bm{T}_{1}\bm{T}_{2}$ be the compounded pose, we have

[TABLE]

Similar to [9] (Section III), the covariance matrix of the rotation can be estimated by:

[TABLE]

with $\bm{M},\bm{N}\in\mathbb{R}^{n\times n}$.

Regarding the translation vector, its covariance matrix can be estimated simply by using the forward propagation method of Section II-C:

[TABLE]

In summary, to compound two poses, we propagate the means using (88,89) and the covariances using (90,95).