ICP

    Iterative Closest Point
PDF

    probability density function
6DoF

    six degrees of freedom
CNN

    convolutional neural network
EKF

    Extended Kalman Filter
UKF

    Unscented Kalman Filter
SGM

    Semi-Global Matching
RTB

    Robot Tracking Benchmark
ADD

    average-distance
ADD-S

    average-shortest-distance

# A Multi-body Tracking Framework -
From Rigid Objects to Kinematic Structures

Manuel Stoiber, Martin Sundermeyer, Wout Boerdijk, and Rudolph Triebel M.
Stoiber, M. Sundermeyer, W. Boerdijk, and R. Triebel are with the Institute of
Robotics and Mechatronics, German Aerospace Center (DLR), 82234 Wessling,
Germany and with the Department of Informatics, Technical University of Munich
(TUM), 80333 Munich, Germany.
E-mail<EMAIL_ADDRESS>
###### Abstract

Kinematic structures are very common in the real world. They range from simple
articulated objects to complex mechanical systems. However, despite their
relevance, most model-based 3D tracking methods only consider rigid objects.
To overcome this limitation, we propose a flexible framework that allows the
extension of existing 6DoF algorithms to kinematic structures. Our approach
focuses on methods that employ Newton-like optimization techniques, which are
widely used in object tracking. The framework considers both tree-like and
closed kinematic structures and allows a flexible configuration of joints and
constraints. To project equations from individual rigid bodies to a multi-body
system, Jacobians are used. For closed kinematic chains, a novel formulation
that features Lagrange multipliers is developed. In a detailed mathematical
proof, we show that our constraint formulation leads to an exact kinematic
solution and converges in a single iteration. Based on the proposed framework,
we extend ICG, which is a state-of-the-art rigid object tracking algorithm, to
multi-body tracking. For the evaluation, we create a highly-realistic
synthetic dataset that features a large number of sequences and various
robots. Based on this dataset, we conduct a wide variety of experiments that
demonstrate the excellent performance of the developed framework and our
multi-body tracker.

###### Index Terms:

Multi-body tracking, 3D object tracking, Pose estimation, Real-time

##  1 Introduction

Tracking objects in 3D space and estimating their six degrees of freedom
(6DoF) pose is an essential task in computer vision. The goal is to provide
the position and orientation with respect to the camera at high frequency,
given consecutive images and the 3D model. For kinematic structures, the
object’s configuration is also required. It directly corresponds to the 6DoF
pose of all individual rigid bodies in the system. Typical applications of 3D
object tracking range from augmented reality to robotics. For augmented
reality, the pose and configuration are used to superimpose digital
information on real-world objects. In robotics, continuous estimates are
needed to facilitate complex manipulation tasks and allow robots to react to
unpredicted changes in the environment. In many such applications, objects are
not rigid. Instead, they consist of multiple connected bodies that form
kinematic structures. Typical examples range from various tools, furniture,
and appliances to machinery and robots.

For 3D object tracking, multiple challenges exist, including occlusions,
cluttered environments, motion blur, textureless surfaces, object symmetries,
computational complexity, and many more. As a consequence, a wide variety of
approaches have been developed [1, 2]. Methods are typically differentiated by
their use of keypoints [3, 4], explicit edges [5, 6], direct optimization [7,
8], deep learning [9, 10], image regions [11, 12], and depth information [13,
14]. Depending on the application, all techniques have their own advantages
and disadvantages, showing different properties with respect to accuracy,
robustness, and runtime. However, most state-of-the-art methods only implement
the tracking of rigid objects without considering multi-body systems. While,
in theory, such algorithms could track bodies independently, in most cases,
the task is unfeasible without kinematic information. As a consequence, many
methods with desirable properties can not be used in applications that feature
kinematic structures.

To overcome this limitation, we propose a flexible framework that allows the
extension of rigid object tracking methods to kinematic structures. Our
approach focuses on Newton-like optimization, which is widely used in various
tracking techniques. The resulting framework allows a flexible configuration
of multi-body systems with various joints and constraints. To the best of our
knowledge, it is the first that is able to accurately model closed kinematic
chains. Based on the developed framework, we extend our previous work ICG [15]
to multi-body tracking. ICG combines region and depth information, is highly
efficient, and performs well for a wide variety of objects and scenarios.
Example images of the extended method are shown in Fig. 1.

Figure 1:  Real-world examples that show the tracking of kinematic structures.
To illustrate the predicted pose and configuration, the object model is
rendered as an overlay. The image on the left shows the tracking of the
medical robot MIRO, which consists of a 7 degrees-of-freedom chain structure.
In the right image, the palm, wrist, and forearm of the humanoid robot David
are tracked. The kinematic structure includes multiple closed chains and
consists of 12 individual bodies.

In a thorough evaluation on a novel, highly-realistic synthetic dataset, we
demonstrate the quality and consistency of the developed approach. In summary,
our contributions are as follows:

  * •

A unified framework that combines the projection to a minimal parameterization
and the application of pose constraints in a single formulation.

  * •

Constraint equations that consider pose differences and that directly operate
on the rotation vector, covering the entire space of possible rotations.

  * •

Thorough mathematical derivations with a proof that shows that constraints
enforce an exact solution for which pose differences converge in a single
iteration.

  * •

A multi-body 3D tracking method that extends ICG and considers both depth and
region information.

  * •

A realistic synthetic dataset that features a large number of sequences with
various moving robots.

In the following, we first discuss related work on multi-body object tracking.
Subsequently, our multi-body tracking framework is introduced, which allows to
extend tracking algorithms that use Newton-like optimization techniques. The
framework is formulated without considering a particular parameterization. We
then focus on the popular axis-angle representation and derive all equations
required for joints and constraints. In addition, properties of our constraint
formulation are analyzed mathematically. This is followed by a description of
all modifications to ICG that are required for multi-body tracking. Finally,
after introducing the created dataset, a thorough evaluation of the developed
framework and multi-body tracker is presented. Note that both the source code
of our tracker and the developed dataset are publicly
available.111https://zenodo.org/record/7548537,222https://github.com/DLR-
RM/3DObjectTracking

##  2 Related Work

In the following section, we provide a detailed overview of trackers for
multi-body objects with a focus on gradient-based techniques and rigid bodies.
Algorithms for elastic objects, such as human hands or bodies, are only
briefly discussed. While some early methods [16, 17] employed search
algorithms to determine the pose and configuration of objects, Lowe [18]
proposed the use of nonlinear least-squares optimization. He showed that the
approach is both robust and efficient, and allows to determine the state of
articulated objects for large frame-to-frame motion. Similar techniques were
later adopted for model-based hand tracking [19] and to recover human body
configurations [20]. Also, [21] proposed the tracking of articulated objects
using feature points in an  Extended Kalman Filter (EKF). Finally, a more
formal derivation motivated by the Lagrange-d’Alembert formulation in
classical physics was also presented [22]. In all those methods, Jacobian
matrices that describe the variation of individual points or measurements for
a minimal set of parameters are derived.

In contrast to the use of a minimal parameterization, Drummond and Cipolla [6]
proposed an approach that adopts Lagrange multipliers in combination with a
Lie group formalism. Their method first predicts the 6DoF pose variation of
all rigid bodies independently and later ensures compatibility using velocity
constraints. A detailed comparison of this postimposed approach compared to
preimposed methods using Jacobians was later presented [23]. The experiments
demonstrate that results for the two techniques are identical. Also, they
showed that while postimposed constraints are very efficient for simple
kinematic chains and allow temporary constraints, preimposed methods scale
better for more complex tree-like kinematic structures. In contrast to those
mathematically exact constraints, approaches that minimize the Mahalanobis
distance [24] or that introduce soft revolute joints [25] were also proposed.

Based on the presented principles to model kinematic structures, various
methods that employ different tracking techniques have been developed.
Approaches that consider depth information using the  Iterative Closest Point
(ICP) algorithm [26] were proposed by [27] and [28]. Later, [29] fused
information from region fitting, dense optical flow, and SIFT features [30].
Similarly, [31] combined SIFT features and dense color information with ICP-
like depth measurements. Information from depth sensors and joint encoders was
utilized by [32]. Also, joint positions that are predicted using pixel-wise
part classification on depth images were used [33]. Similarly, [34] employed
pixel-wise part classification but directly considered depth values instead of
joint positions. Lately, an algorithm that fuses region and dense color
information was also presented [35]. With DART [13, 36], a general user-
friendly method was developed that extends 6DoF tracking with signed distance
functions [37] to articulated objects. Later, the integration of DART with a
tactile sensor was studied by [38]. A similarly general method that uses dense
optical flow and ICP-like depth measurements was developed by [39]. It was
later renamed to SimTrack [40]. Except for SimTrack, which implements the
postimposed constraints of Drummond and Cipolla [6], most methods use
preimposed formulations similar to the approach of Lowe [18].

Apart from gradient-based methods, other optimization techniques were also
adopted for the tracking of multi-body objects. In the work of [41], an
Unscented Kalman Filter (UKF) was used to fuse different sources of
information. A method that employs a Random Forest to directly regress to
joint angles was proposed by [42]. Also, [43] used the Coordinate Particle
Filter [44] to track robot manipulators, considering information from both
depth images and joint measurements. Finally, deep learning was employed to
recognize 2D keypoints on a robot [45] or use a render-and-compare strategy
for pose estimation [46]. In addition to methods for rigid objects, a large
amount of work on the tracking of human hands and bodies exists. While deep
learning has become highly popular for those tasks [47, 48, 49], some model-
based methods still use the original formulation of Lowe to consider the
underlying kinematic structure [50, 51, 52]. However, because techniques like
keypoint detection and regularization are highly optimized for their
respective domains, there is no straightforward application of such algorithms
to arbitrary multi-body systems.

##  3 Multi-body Tracking

In the following, we derive a flexible framework that allows to extend
tracking algorithms from rigid objects to kinematic structures. For this, the
general architecture of methods that are compatible with our approach is
discussed. This is followed by an extension to tree-like structures and,
finally, to closed kinematic structures. The provided formulation is very
general and allows different parameterizations of poses, joints, and
constraints.

###  3.1 Rigid Objects

For the task of 3D object tracking, a wide variety of algorithms that use
different sources of information have been developed. Based on the considered
data, most methods derive an energy function $E(\boldsymbol{\theta})$ or
probability density function (PDF) $p(\boldsymbol{\theta})$ that depends on
the pose variation vector $\boldsymbol{\theta}$. Given such a function, the
pose that best explains the considered data is found by minimizing the energy
or maximizing the probability. While different techniques have been employed
for this optimization task, the following framework focuses on Newton-like
methods.

In general, Newton-like optimization iteratively estimates the variation
vector and updates the object pose. The estimated pose variation
$\boldsymbol{\hat{\theta}}$ is thereby calculated as follows

$\boldsymbol{\hat{\theta}}=-\boldsymbol{H}^{-1}\boldsymbol{g},$ (1)

where the gradient vector $\boldsymbol{g}$ and the Hessian matrix
$\boldsymbol{H}$ are the first- and second-order derivatives of the energy
function $E(\boldsymbol{\theta})$ or the negative logarithmic probability
$-\ln(p(\boldsymbol{\theta}))$. In the following, we derive our approach for
the energy function $E(\boldsymbol{\theta})$. Note, however, that with the
relation $E(\boldsymbol{\theta})=-\ln(p(\boldsymbol{\theta}))$, exactly the
same equations can be used for PDFs. Finally, (1) is not only limited to
classical Newton optimization with an exact Hessian matrix but also allows the
use of approximations from Gauss-Newton or quasi-Newton methods. As a
consequence, many popular algorithms, such as RAPID [5], PWP3D [11], or the
object tracker employed in Slam++ [53], are compatible with the developed
framework.

###  3.2 Tree-like Structures

In general, tree-like kinematic structures consist of individual bodies that
are connected by joints. All bodies, except for the root, have a single
parent. Based on those connections, the 6DoF pose variation
$\boldsymbol{\theta}$ of each body is described by joint variations
$\boldsymbol{\theta}_{\textrm{j}}\in\mathbb{R}^{n_{\textrm{j}}}$ along the
kinematic chain from the root. The number $n_{\textrm{j}}\in[0..6]$ describes
how many degrees of freedom a particular joint allows for a body relative to
its parent. Finally, the full variation of a kinematic structure with $n$
bodies is described by a combination of all joint variations
$\boldsymbol{\theta}_{\textrm{k}}^{\top}=\begin{bmatrix}\boldsymbol{\theta}_{\textrm{j}0}^{\top}\dots\boldsymbol{\theta}_{\textrm{j}n}^{\top}\end{bmatrix}$.
Note that, for the root body, the joint variation
$\boldsymbol{\theta}_{\textrm{j}0}$ directly describes the pose variation
$\boldsymbol{\theta}_{0}$. An example that shows the relation between
individual parameters in a kinematic structure is given in Fig. 2.

$\boldsymbol{\theta}_{\textrm{0}}$$\boldsymbol{\theta}_{\textrm{1}}$$\boldsymbol{\theta}_{\textrm{2}}$$\boldsymbol{\theta}_{\textrm{3}}$$\boldsymbol{\theta}_{\textrm{4}}$$\boldsymbol{\theta}_{\textrm{5}}$$\boldsymbol{\theta}_{\textrm{j0}}$$\boldsymbol{\theta}_{\textrm{j1}}$$~{}\boldsymbol{\theta}_{\textrm{j2}}$$\boldsymbol{\theta}_{\textrm{j3}}$$\boldsymbol{\theta}_{\textrm{j4}}$$\,\boldsymbol{\theta}_{\textrm{j5}}$$\boldsymbol{b}_{\textrm{34}}$$\boldsymbol{b}_{\textrm{14}}~{}~{}~{}$$\boldsymbol{b}_{\textrm{25}}$
Figure 2:  Relation between variation parameters in a kinematic structure. The
6DoF pose variations $\boldsymbol{\theta}_{\textrm{0}}$ to
$\boldsymbol{\theta}_{\textrm{5}}$ of bodies are illustrated as vertices in a
graph. For the root body, the vertex is colored yellow. Connections to parent
bodies and the associated joint variations $\boldsymbol{\theta}_{\textrm{j1}}$
to $\boldsymbol{\theta}_{\textrm{j5}}$ are indicated by blue arrows. For the
root body, which does not have a parent, the joint variation
$\boldsymbol{\theta}_{\textrm{j0}}$ directly describes the pose variation
$\boldsymbol{\theta}_{\textrm{0}}$. Finally, dashed red lines indicate the
constraints $\boldsymbol{b}_{\textrm{14}}$, $\boldsymbol{b}_{\textrm{34}}$,
and $\boldsymbol{b}_{\textrm{25}}$ between bodies.

Starting from the energy functions $E_{i}(\boldsymbol{\theta}_{i})$ of
individual bodies, the combined energy of the kinematic structure can be
calculated as the sum of the energy of all bodies

$E_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})=\sum_{i=1}^{n}E_{i}(\boldsymbol{\theta}_{i}),$
(2)

where variation vectors $\boldsymbol{\theta}_{i}$ of individual bodies are
functions of the combined variation $\boldsymbol{\theta}_{\textrm{k}}$. Given
the definitions of gradient vectors and Hessian matrices, we can now write

$\displaystyle\boldsymbol{g}_{\textrm{k}}^{\top}$
$\displaystyle=\frac{\partial
E_{\textrm{k}}}{\partial\boldsymbol{\theta}_{\textrm{k}}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\sum_{i=1}^{n}\frac{\partial
E_{i}}{\partial\boldsymbol{\theta}_{i}}\frac{\partial\boldsymbol{\theta}_{i}}{\partial\boldsymbol{\theta}_{\textrm{k}}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\sum_{i=1}^{n}\boldsymbol{g}_{i}^{\top}\boldsymbol{J}_{i},$
(3) $\displaystyle\begin{aligned}
\mathllap{\boldsymbol{H}_{\textrm{k}}}=\frac{\partial^{2}E_{\textrm{k}}}{\partial{\boldsymbol{\theta}_{\textrm{k}}}^{\\!2}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}&\approx\sum_{i=1}^{n}\frac{\partial\boldsymbol{\theta}_{i}}{\partial\boldsymbol{\theta}_{\textrm{k}}}^{\\!\top}\frac{\partial^{2}E_{i}}{\partial{\boldsymbol{\theta}_{i}}^{\\!2}}\frac{\partial\boldsymbol{\theta}_{i}}{\partial\boldsymbol{\theta}_{\textrm{k}}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}\\\
&\approx\sum_{i=1}^{n}\boldsymbol{J}_{i}^{\top}\boldsymbol{H}_{i}\,\boldsymbol{J}_{i},\end{aligned}$
(4)

with $\boldsymbol{g}_{i}$ and $\boldsymbol{H}_{i}$ the gradient vector and the
Hessian matrix of the rigid body $i$. Note that second-order derivatives of
pose variations $\boldsymbol{\theta}_{i}$ with respect to
$\boldsymbol{\theta}_{\textrm{k}}$ were neglected. The Jacobian matrices
$\boldsymbol{J}_{i}$ describe how the variation of the entire kinematic
structure affects the pose of individual bodies. A detailed derivation of the
required body Jacobians is given in Section 4.2. One major difference of our
formulation in comparison to the approach of Lowe [18] is that we do not
calculate the change of each measurement with respect to the combined
variation $\boldsymbol{\theta}_{\textrm{k}}$. Instead, the change of a body’s
measurements is first computed for the minimal 6DoF pose variation
$\boldsymbol{\theta}_{i}$ and then projected to the full kinematic structure.
This has the advantage that the same gradient vectors and Hessian matrices as
for single object tracking can be used. In addition, for kinematic structures
with large numbers of bodies and measurements, it is more efficient.

###  3.3 Closed Kinematic Structures

To model closed kinematic structures, we start from the previously defined
tree-like structures and include additional constraints. A constraint
describes any relation between bodies that can be expressed by a constraint
equation $\boldsymbol{b}(\boldsymbol{\theta}_{\textrm{k}})=\boldsymbol{0}$. A
simple example is a rotational joint, where translational differences normal
to the rotation axis have to be zero. An illustration of the topology of a
tree-like kinematic structure with constraints between individual bodies is
shown in Fig. 2. Given $n$ constraints, the full constraint equation for the
kinematic structure is written as
$\boldsymbol{b}_{\textrm{k}}^{\top}=\begin{bmatrix}\boldsymbol{b}_{0}^{\top}\dots\boldsymbol{b}_{n}^{\top}\end{bmatrix}$.
To integrate those constraints into the combined energy function
$E_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})$, we write the following
Lagrangian function

$\mathcal{L}(\boldsymbol{\theta}_{\textrm{k}},\boldsymbol{\lambda})=E_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})+\boldsymbol{b}_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})^{\top}\boldsymbol{\lambda},$
(5)

where $\boldsymbol{\lambda}$ is the vector of Lagrange multipliers. To find a
solution for $\boldsymbol{\theta}_{\textrm{k}}$ that satisfies the imposed
constraints and minimizes the energy function, the following equation has to
be solved

$\nabla\mathcal{L}(\boldsymbol{\theta}_{\textrm{k}},\boldsymbol{\lambda})=\begin{bmatrix}\boldsymbol{g}_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})+\boldsymbol{B}_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})^{\top}\boldsymbol{\lambda}\\\
\boldsymbol{b}_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})\end{bmatrix}\overset{!}{=}\boldsymbol{0},$
(6)

where the constraint Jacobian $\boldsymbol{B}_{\textrm{k}}$ is the first-order
derivative of $\boldsymbol{b}_{\textrm{k}}$ with respect to
$\boldsymbol{\theta}_{\textrm{k}}$. Detailed derivations of the constraint
function and Jacobian are given in Section 4.3.

Analogous to the minimization of the nonlinear energy function
$E(\boldsymbol{\theta})$ of a single object, we want to linearize (6) around
$\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}$ and iteratively find a
solution. For this, we use the first-order approximations
$\boldsymbol{g}_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})\approx\boldsymbol{g}_{\textrm{k}}+\boldsymbol{H}_{\textrm{k}}\boldsymbol{\theta}_{\textrm{k}}$
and
$\boldsymbol{b}_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})\approx\boldsymbol{b}_{\textrm{k}}+\boldsymbol{B}_{\textrm{k}}\boldsymbol{\theta}_{\textrm{k}}$
to derive the following linear equation

$\begin{bmatrix}\boldsymbol{\hat{\theta}}_{\textrm{k}}\\\
\boldsymbol{\hat{\lambda}}\end{bmatrix}=-\begin{bmatrix}\boldsymbol{H}_{\textrm{k}}&\boldsymbol{B}_{\textrm{k}}^{\top}\\\
\boldsymbol{B}_{\textrm{k}}&\boldsymbol{0}\end{bmatrix}^{-1}\begin{bmatrix}\boldsymbol{g}_{\textrm{k}}\\\
\boldsymbol{b}_{\textrm{k}}\end{bmatrix}.$ (7)

Similar to (1), the derived linear equation is used to estimate
$\boldsymbol{\hat{\theta}}_{\textrm{k}}$ and to iteratively minimize
$E_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})$ while, at the same time,
considering the constraints
$\boldsymbol{b}_{\textrm{k}}(\boldsymbol{\theta}_{\textrm{k}})=\boldsymbol{0}$.
Note that, in general, given a positive definite Hessian
$\boldsymbol{H}_{\textrm{k}}$ and unique and non-contradicting constraint
equations that lead to non-zero and linearly independent row vectors in the
constraint Jacobian $\boldsymbol{B}_{\textrm{k}}$, the matrix in (7) is
invertible and a unique solution exists for the estimated variation vector
$\boldsymbol{\hat{\theta}}_{\textrm{k}}$.

Finally, in contrast to the postimposed method of Drummond and Cipolla [6],
which first predicts unconstrained poses and applies constraints later, our
formulation directly estimates a variation vector that is compatible with
constraints. In addition, the approach is not limited to velocity constraints.
Together with our formulation for tree-like kinematic structures, the
developed multi-body tracking framework supports an efficient minimal
parameterization while simultaneously providing the flexibility to add
constraints for closed chains and temporary limitations. It, therefore,
combines the best of both worlds.

##  4 Parameterization

In this section, we derive all equations that are required to define a
kinematic structure. For the pose parameterization, we thereby focus on the
axis-angle representation. Note that while tracking algorithms based on Euler
angles, twist coordinates, or quaternions have also been developed, in our
experience, this parameterization is the most popular. After an introduction
to general mathematical concepts, Jacobian matrices and constraint equations
are derived. In both cases, we allow to lock or release the rotation and
translation along individual coordinate axes, facilitating various different
joints and constraints. Finally, we describe how to update the pose of
individual bodies.

###  4.1 Preliminaries

To define the pose between two reference frames A and B, the following
transformation matrix
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}\in\mathbb{SE}(3)$ is used

${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}=\begin{bmatrix}{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}&{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}\\\
\boldsymbol{0}&1\end{bmatrix},$ (8)

where ${}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\in\mathbb{SO}(3)$ is a
rotation matrix and
${}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}\in\mathbb{R}^{3}$ is a translation
vector. They together describe the transformation from B to A. For the
variation of poses with a minimal set of parameters, we adopt the axis-angle
representation for the rotation and write

$\boldsymbol{T}(\boldsymbol{\theta})=\begin{bmatrix}\exp([\boldsymbol{\theta}_{\textrm{r}}]_{\times})&\boldsymbol{\theta}_{\textrm{t}}\\\
\boldsymbol{0}&1\end{bmatrix},$ (9)

where $\boldsymbol{\theta}_{\textrm{r}}\in\mathbb{R}^{3}$ and
$\boldsymbol{\theta}_{\textrm{t}}\in\mathbb{R}^{3}$ are the rotational and
translational components of the variation vector
$\boldsymbol{\theta}^{\top}=\begin{bmatrix}\boldsymbol{\theta}_{\textrm{r}}^{\top}&\boldsymbol{\theta}_{\textrm{t}}^{\top}\end{bmatrix}$,
and $[\boldsymbol{\theta}_{\textrm{r}}]_{\times}$ is the skew-symmetric cross-
product matrix of $\boldsymbol{\theta}_{\textrm{r}}$. Finally, to project the
variation vector $\boldsymbol{\theta}$ from reference frame B to A, the
following adjoint representation is used

$\operatorname{Ad}({}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}})=\begin{bmatrix}{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}&\boldsymbol{0}\\\
[{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}]_{\times}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}&{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\end{bmatrix}.$
(10)

Based on those general mathematical concepts, we are able to describe
kinematic structures and derive the required body Jacobians and constraint
equations.

###  4.2 Body Jacobians

Each body has a defined location relative to its joint and corresponding
parent. In the following, we introduce a general class of joints that allow
motion along a user-defined set of rotational and translational axes. Based on
the joint reference frame J, motion along these directions is modeled by the
variation vector $\boldsymbol{\theta}_{\textrm{j}}$. The full 6DoF pose
variation $\boldsymbol{\theta}$ of a body’s model frame M given the variation
of its parent and joint is then defined as follows

$\boldsymbol{T}(\boldsymbol{\theta})={}_{\textrm{M}}\boldsymbol{T}_{\textrm{P}}\,\boldsymbol{T}(\boldsymbol{\theta}_{\textrm{p}})\,{}_{\textrm{P}}\boldsymbol{T}_{\textrm{J}}\,\boldsymbol{T}(\boldsymbol{\bar{\theta}}_{\textrm{j}})\,{}_{\textrm{J}}\boldsymbol{T}_{\textrm{M}},$
(11)

where P is the reference frame of the parent and
$\boldsymbol{\theta}_{\textrm{p}}$ the corresponding 6DoF variation vector. An
illustration of the transformations is shown in Fig. 3.

P
---
$\boldsymbol{\theta}_{\textrm{p}}$
---
M
---
J
---
$\boldsymbol{\bar{\theta}}_{\textrm{j}}$
---
$\boldsymbol{\theta}$
---
${}_{\textrm{M}}\boldsymbol{T}_{\textrm{P}}$
---
${}_{\textrm{J}}\boldsymbol{T}_{\textrm{M}}$
---
${}_{\textrm{P}}\boldsymbol{T}_{\textrm{J}}$
---
Body
---
Parent
---
Figure 3:  Visualization of a body that is connected to its parent by a joint.
Model, parent, and joint coordinate frames M, P, and J are illustrated in
yellow, blue, and red, respectively. For the coordinate frames, the
corresponding variation vectors $\boldsymbol{\theta}$,
$\boldsymbol{\bar{\theta}}_{\textrm{j}}$, and
$\boldsymbol{\theta}_{\textrm{p}}$, as well as the relative homogeneous
transformations ${}_{\textrm{J}}\boldsymbol{T}_{\textrm{M}}$,
${}_{\textrm{P}}\boldsymbol{T}_{\textrm{J}}$, and
${}_{\textrm{M}}\boldsymbol{T}_{\textrm{P}}$, are shown.

The bar above the joint variation vector
$\boldsymbol{\bar{\theta}}_{\textrm{j}}$ indicates the extension of
$\boldsymbol{\theta}_{\textrm{j}}$, which has $n_{\textrm{j}}$ dimensions, to
a 6DoF vector. For a typical joint that only allows movement along specified
axes of the joint frame, we simply set fixed directions to zero.

To project equations of individual bodies to the multi-body system, Jacobians
are used. Given a body’s joint and parent, we are able to formulate a
recursive strategy to calculate the Jacobian. Based on the Jacobian of the
parent body $\boldsymbol{J}_{\textrm{p}}$, we calculate

$\boldsymbol{J}=\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{k}}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{p}}}\boldsymbol{J}_{\textrm{p}}+\begin{bmatrix}\boldsymbol{0}&\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{j}}}&\boldsymbol{0}\end{bmatrix}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}},$
(12)

where the partial derivative
$\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{j}}}$
has to be added at the correct location that corresponds to the position of
$\boldsymbol{\theta}_{\textrm{j}}$ in the vector
$\boldsymbol{\theta}_{\textrm{k}}$. The advantage of this recursive strategy
is that it is not necessary to assemble the derivatives of all individual
joints along the kinematic chain. Instead, we simply consider the kinematic
chain using the Jacobian of the parent. For the root body, the parent Jacobian
$\boldsymbol{J}_{\textrm{p}}$ is simply zero. To calculate the Jacobian, we
now only require the partial derivatives of the body variation with respect to
the variations of the joint and parent.

Starting from (11) and knowing that the adjoint representation in (10) can be
used to project variation vectors between reference frames, we write the
following relations

$\displaystyle\boldsymbol{\theta}|_{\boldsymbol{\theta}_{\textrm{j}}=\boldsymbol{0}}$
$\displaystyle=\operatorname{Ad}({}_{\textrm{M}}\boldsymbol{T}_{\textrm{P}})\,\boldsymbol{\theta}_{\textrm{p}},$
(13)
$\displaystyle\boldsymbol{\theta}|_{\boldsymbol{\theta}_{\textrm{p}}=\boldsymbol{0}}$
$\displaystyle=\operatorname{Ad}({}_{\textrm{M}}\boldsymbol{T}_{\textrm{J}})\,\boldsymbol{\bar{\theta}}_{\textrm{j}}.$
(14)

As can be seen from those equations, the required first-order derivatives are

$\displaystyle\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{p}}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}$
$\displaystyle=\operatorname{Ad}({}_{\textrm{M}}\boldsymbol{T}_{\textrm{P}}),$
(15)
$\displaystyle\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\bar{\theta}}_{\textrm{j}}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}$
$\displaystyle=\operatorname{Ad}({}_{\textrm{M}}\boldsymbol{T}_{\textrm{J}}).$
(16)

Note that (16) considers the derivative with respect to the extended variation
vector $\boldsymbol{\bar{\theta}}_{\textrm{j}}$. To get the first-order
derivative
$\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{j}}}\big{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}$
with respect to the variation vector $\boldsymbol{\theta}_{\textrm{j}}$, one
simply assembles the columns from the partial derivative in (16) that are
considered by $\boldsymbol{\theta}_{\textrm{j}}$. With the derived
formulation, we are able to model various joints, including revolute,
prismatic, or spherical joints. For more specific joints, such as screw
connections, it is typically also straightforward to derive the necessary
first-order derivatives as a combination of the motion along individual axes.
Instead of directly assembling columns from (16), one then simply multiplies
with the joint’s partial derivative
$\frac{\partial\boldsymbol{\bar{\theta}}_{\textrm{j}}}{\partial\boldsymbol{\theta}_{\textrm{j}}}$
to get the derivative
$\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{j}}}=\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\bar{\theta}}_{\textrm{j}}}\frac{\partial\boldsymbol{\bar{\theta}}_{\textrm{j}}}{\partial\boldsymbol{\theta}_{\textrm{j}}}$
required for the body Jacobian.

###  4.3 Constraint Equations

In the following, we derive constraints that consider rotational and
translational differences between two bodies along individual axes.
Differences are calculated with respect to the constraint reference frames A
and B, which are defined for the corresponding bodies $a$ and $b$. Constrained
differences between those two frames are enforced to be zero. The relative
pose between frames A and B, given the pose variation vectors
$\boldsymbol{\theta}_{a}$ and $\boldsymbol{\theta}_{b}$, is written as

${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})={}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{a}}\,\boldsymbol{T}(\boldsymbol{\theta}_{a})^{-1}\,{}_{\textrm{M}_{a}}\boldsymbol{T}_{\textrm{M}_{b}}\,\boldsymbol{T}(\boldsymbol{\theta}_{b})\,{}_{\textrm{M}_{b}}\boldsymbol{T}_{\textrm{B}},$
(17)

where $\textrm{M}_{a}$ and $\textrm{M}_{b}$ are the model frames of bodies $a$
and $b$. An illustration of the transformation is shown in Fig. 4.

$\textrm{M}_{b}$
---
$\boldsymbol{\theta}_{b}$
---
$\textrm{M}_{a}$
---
A
---
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})$
---
$\boldsymbol{\theta}_{a}$
---
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{a}}$
---
${}_{\textrm{M}_{a}}\boldsymbol{T}_{\textrm{M}_{b}}$
---
Body a
---
Body b
---
B
---
${}_{\textrm{M}_{b}}\boldsymbol{T}_{\textrm{B}}$
---
Figure 4:  Illustration of a constraint between two bodies $a$ and $b$. The
model coordinate frames $\textrm{M}_{a}$ and $\textrm{M}_{b}$, as well as the
variation vectors $\boldsymbol{\theta}_{a}$ and $\boldsymbol{\theta}_{b}$ are
illustrated in yellow and blue, respectively. The constraint coordinate frames
$A$ and $B$ associated with bodies $a$ and $b$ are colored in red. The
relative transformation
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})$
is calculated from ${}_{\textrm{M}_{b}}\boldsymbol{T}_{\textrm{B}}$,
${}_{\textrm{M}_{a}}\boldsymbol{T}_{\textrm{M}_{b}}$, and
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{a}}$ and is considered in the
constraint equation.

Based on the relative pose
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})$,
we define the extended 6DoF constraint equation for all rotational and
translational axes as follows

$\boldsymbol{\bar{b}}_{ab}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})=\begin{bmatrix}{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})\\\
{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})\end{bmatrix},$
(18)

where ${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$ is the rotation vector, and
${}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}$ is the translation vector of the
transformation matrix ${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}$. The
rotation vector and the rotation matrix are related by the exponential map
${}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}=\exp([{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}]_{\times})$.
Note that, depending on the directions that should be constrained, only
individual elements of the extended constraint equation
$\boldsymbol{\bar{b}}_{ab}$ are used. The resulting reduced constraint
equation is denoted $\boldsymbol{b}_{ab}$.

Given a constraint equation, the corresponding constraint Jacobian can be
calculated using body Jacobians $\boldsymbol{J}$ that were derived in the
previous section as follows

$\boldsymbol{B}_{ab}=\frac{\partial\boldsymbol{b}_{ab}}{\partial\boldsymbol{\theta}_{\textrm{k}}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\frac{\partial\boldsymbol{b}_{ab}}{\partial\boldsymbol{\theta}_{a}}\boldsymbol{J}_{a}+\frac{\partial\boldsymbol{b}_{ab}}{\partial\boldsymbol{\theta}_{b}}\boldsymbol{J}_{b}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}.$
(19)

For the required partial derivatives of the extended constraint equation
$\boldsymbol{\bar{b}}_{ab}$ with respect to the pose variations
$\boldsymbol{\theta}_{a}$ and $\boldsymbol{\theta}_{b}$, the calculation is
quite lengthy. This is especially the case for the rotational constraint,
which considers the nontrivial change of the rotation vector
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$ for the variation of the two
bodies. Consequently, we provide the full derivation in Appendix A and only
state the following final results

$\displaystyle\frac{\partial\boldsymbol{\bar{b}}_{ab}}{\partial\boldsymbol{\theta}_{a}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}$
$\displaystyle=\begin{bmatrix}-\boldsymbol{C}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}&\boldsymbol{0}\\\
{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,[{}_{\textrm{M}_{a}}\boldsymbol{t}_{\textrm{B}}]_{\times}&-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\end{bmatrix},$
(20)
$\displaystyle\frac{\partial\boldsymbol{\bar{b}}_{ab}}{\partial\boldsymbol{\theta}_{b}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}$
$\displaystyle=\begin{bmatrix}\boldsymbol{C}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}&\boldsymbol{0}\\\
-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\,[{}_{\textrm{M}_{b}}\boldsymbol{t}_{\textrm{B}}]_{\times}&{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\end{bmatrix}.$
(21)

To obtain the derivatives for the reduced constraint equation
$\boldsymbol{b}_{ab}$, which are required in (19), one simply assembles the
rows from (20) and (21) that correspond to the constrained axes of coordinate
frame A. Finally, given the Jacobians $\boldsymbol{B}_{ab}$ of individual
constraints, the full constraint Jacobian $\boldsymbol{B}_{\textrm{k}}$ can be
assembled by row-wise concatenation of those matrices.

The matrix $\boldsymbol{C}$, which is used in the calculated first-order
derivatives and which we call variation matrix, is defined as

$\boldsymbol{C}=\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{I}-\frac{\alpha}{2}[\boldsymbol{e}]_{\times}+\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}\boldsymbol{e}^{\top},$
(22)

where $\alpha\in\mathbb{R}$ is the rotation angle and
$\boldsymbol{e}\in\mathbb{R}^{3}$ with $\lVert\boldsymbol{e}\rVert_{2}=1$ is
the rotation axis of the rotation vector
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}=\alpha\boldsymbol{e}$ evaluated at
$\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}$. The matrix describes how
the rotation vector ${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$ changes with
the variation of a subsequent infinitesimal rotation. It is derived in
Appendix A as part of the calculation of (20) and (21). In Appendix B, we show
that the variation matrix $\boldsymbol{C}$ is a normal, non-symmetric, and
non-orthogonal matrix that is directly related to the rotation matrix by

${}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}=\boldsymbol{C}^{\top}\boldsymbol{C}^{-1}=\boldsymbol{C}^{-1}\boldsymbol{C}^{\top}.$
(23)

Also, for the rotation vector ${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$, we
are able to prove that

${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}=\boldsymbol{C}\,{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}=\boldsymbol{C}^{\top}{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}.$
(24)

This shows that the rotation vector
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$ is an eigenvector of both the
original and transposed variation matrix $\boldsymbol{C}$.

For the approach in Section 3.2, which uses body Jacobians to project
equations to a minimal parameterization, only exact solutions are possible. At
the same time, for the constraint approach in Section 3.3 and the equations
proposed in this section, kinematic compliance is not apparent. Especially for
the rotation, which is highly non-linear in Euclidean space, one could expect
that multiple iterations of the Newton optimization are required to converge
to a kinematically accurate result. However, in Appendix C, we are able to
prove mathematically that this is not the case. In our proof, the developed
constraint equations and Jacobians are introduced into the linear equations of
the Newton optimization. Also, the previously developed property that
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}=\boldsymbol{C}{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$
is used. Based on this, we calculate the relative pose change per iteration.
The results demonstrate that both rotational and translational constraints
converge to a kinematically compliant result in a single iteration. While the
proof in Appendix C assumes that constraint coordinate frames are equal to
model frames, experiments in Section 6.4 demonstrate that results also hold
for more general cases.

If constraint equations are fulfilled and
$\boldsymbol{\bar{b}}_{ab}=\boldsymbol{0}$, the coordinate frames A and B are
equal, and the variation matrix $\boldsymbol{C}$ reduces to the identity
matrix. As a consequence, the derivatives in (20) and (21) turn into the
adjoint representations
$-\operatorname{Ad}({}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{a}})$ and
$\operatorname{Ad}({}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{b}})$. A short
proof of this is given in Appendix D. The derived expressions directly
correspond to the formulation of Drummond and Cipolla [6]. This equivalence
demonstrates that our approach is an extension of their method. However, the
big advantage of our generalization is that we directly operate on pose
differences instead of velocities. This ensures that even if the pose
constraint is not fulfilled initially, our method will automatically converge
to a consistent result. As a consequence, it is possible to model closed
chains or add temporary constraints that are not currently fulfilled. Both
cases are not viable using velocity constraints. In addition, we would like to
highlight the benefits of directly using the rotation vector in the constraint
equations. Compared to other options, such as using orthogonal axes, our
formulation has the advantage that it provides consistent results over the
entire space of possible rotations. We are, therefore, able to successfully
recover even from maximum rotational differences of $\alpha=\pi$ in a single
iteration.

###  4.4 Pose Update

Given the derived Jacobians and constraint equations, we are able to estimate
the variation vector of the kinematic structure
$\boldsymbol{\theta}_{\textrm{k}}^{\top}=\begin{bmatrix}\boldsymbol{\theta}_{\textrm{j}0}^{\top}\dots\boldsymbol{\theta}_{\textrm{j}n}^{\top}\end{bmatrix}$
using the linear relation of the Newton optimization in (7). Similar to the
calculation of body Jacobians, we formulate a recursive strategy for the pose
update. With the estimated joint variation
$\boldsymbol{\hat{\theta}}_{\textrm{j}}$ and the pose of the parent, the body
pose is updated as follows

${}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}}^{+}={}_{\textrm{A}}\boldsymbol{T}_{\textrm{P}}\,{}_{\textrm{P}}\boldsymbol{T}_{\textrm{J}}\,\boldsymbol{T}(\boldsymbol{\hat{\bar{\theta}}}_{\textrm{j}})\,{}_{\textrm{J}}\boldsymbol{T}_{\textrm{M}},$
(25)

where A is some arbitrary frame of reference such as the camera frame. Like in
(11), the full 6DoF variation vector
$\boldsymbol{\hat{\bar{\theta}}}_{\textrm{j}}$ has to be used for the
calculation of the transformation matrix. Again, it is simply created by
adding zeros to the fixed axes of our joint variation. Note that for the pose
of the joint coordinate frame J, either the transformation with respect to the
body ${}_{\textrm{J}}\boldsymbol{T}_{\textrm{M}}$ or the parent
${}_{\textrm{P}}\boldsymbol{T}_{\textrm{J}}$ is fixed. The other is inferred
from the previous pose estimates of the body and parent, as well as the fixed
transformation. While most of the time, both options are valid, for some
joints, which allow, for example, rotation around two axes, it makes a
difference if ${}_{\textrm{J}}\boldsymbol{T}_{\textrm{M}}$ or
${}_{\textrm{P}}\boldsymbol{T}_{\textrm{J}}$ is fixed.

Finally, to update the root body, which does not have a parent, the following
strategy is adopted

${}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{0}}^{+}={}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{0}}\,{}_{\textrm{M}_{0}}\boldsymbol{T}_{\textrm{J}_{0}}\,\boldsymbol{T}(\boldsymbol{\hat{\bar{\theta}}}_{\textrm{j}0})\,{}_{\textrm{J}_{0}}\boldsymbol{T}_{\textrm{M}_{0}}.$
(26)

It considers the variation with respect to the previous pose estimate in the
joint’s frame of reference. In summary, the developed approach allows us to
estimate all body poses in the kinematic structure, ensuring that variation
vectors comply with constraints and poses are only updated along free joint
directions.

##  5 Implementation

The following section considers the extension of our work ICG [15] to multi-
body tracking. ICG is a highly-efficient probabilistic tracker that considers
sparse region and depth information for the tracking of multiple independent
objects. For an efficient representation of geometry that avoids online
rendering, it utilizes a so-called sparse viewpoint model that stores randomly
sampled contour and surface points, as well as other information, for views
all around the object. For the region modality, color histograms are used to
differentiate between the object and the background. During tracking, contour
points are projected into the image to define the location of so-called
correspondence lines. For each line, a discrete probability distribution for
the contour location is computed based on the probability that pixels belong
to the foreground or background. For the depth modality, an ICP-like approach
is implemented that computes the distance between surface model points and
closest correspondence points from the depth image. It uses the point-to-plane
error metric and assumes a normal distribution. For the maximization of the
joint PDF, ICG employs Newton optimization with Tikhonov regularization. To
parameterize the pose variation, it adopts the axis-angle representation. ICG,
therefore, fits perfectly into the developed multi-body tracking framework.
For the implementation, we directly build on the available source
code333https://github.com/DLR-RM/3DObjectTracking. If not stated otherwise,
parameter values from the corresponding publication [15] are used. In the
following, we describe modifications that are required for the extension to
multi-body tracking.

For kinematic structures, it is very common that individual bodies intersect
or that they are very close together. Also, different bodies often belong to
the same region. As a consequence, one of the main modifications of ICG
considers the validation of contour and surface points. To validate surface
points from depth modalities, a silhouette image is rendered that contains all
bodies. Based on this image, surface points of a body are considered valid if
they fall on the corresponding silhouette. Note that the same rendering can be
employed for multiple modalities that use the same camera. For contour points
from region modalities, we also render a silhouette image. However, instead of
using a unique ID for each body, a region ID is assigned to each body, which
is rendered to the respective silhouette. Bodies with the same region ID
belong to the same region and consider similar color statistics. As described
in [15], it is essential that, for each correspondence line, the body region
on the inside and the non-body region on the outside of the contour are not
interrupted for a minimum continuous distance along the line, which was
defined as $3$ segments. Consequently, for each contour point, we check if
rendered pixels along the normal vector fulfill this rule. If it is violated,
the contour point is rejected. An illustration of the described validation
strategies is shown in Fig. 5.

Region Modality
---
Depth Modality
---
Figure 5:  Validation strategies for the region and depth modality. Valid
points are visualized with a circle, while invalid points are marked by a
cross. For the region modality, the body in the center and lower right corner
are rendered with the same region ID. To check if a contour point is valid,
pixels on a line along the normal vector are evaluated. If the distance to the
inside is interrupted by another silhouette or the distance to the outside is
interrupted by the same silhouette, the corresponding contour point is
rejected. For the depth modality, all bodies are rendered with unique values.
Any surface point that is not on the body’s own silhouette is invalid.

In addition to contour point validation, the silhouette image is also used to
check whether pixel values that should be assigned to color histograms are on
the correct silhouette. If they are on the wrong silhouette, they are ignored.
Both the rendering and validation are repeated for each correspondence search.

In addition to validation strategies, we allow individual region modalities to
share color histograms. This is especially beneficial if individual bodies are
occluded while other bodies that model the same region are still visible.
Also, color histograms consider more information and, therefore, perform
slightly better. Another modification required for multi-body tracking
considers the number of correspondence lines and points each modality deploys.
Since the size of bodies varies widely, the number has to be dynamically
adapted to uniformly cover the contour and surface of all bodies. We,
therefore, scale the number of lines and points according to the surface area
and contour length of a body’s current silhouette relative to a reference area
and length. In the evaluation, we use the maximum contour length and surface
area of all views and bodies within an object as reference. Also, except for
the Medical Robot, which is shown in Fig. 6, a maximum of $300$ correspondence
lines and points is deployed per body. For the Medical Robot, which consists
of equally sized bodies, we only use $100$ lines and points to improve
computational efficiency.

The last modification considers the maximization of the joint probability
using the Newton method. Similar to ICG, the required variation vector
$\hat{\boldsymbol{\theta}}_{\textrm{k}}$ is estimated using the linear
relation in (7). However, in contrast to the original implementation, we do
not add regularization to each body separately. Instead, we directly add the
diagonal regularization matrix $\boldsymbol{\Sigma}$ to the Hessian of the
kinematic structure $\boldsymbol{H}_{\textrm{k}}$. The diagonal elements of
$\boldsymbol{\Sigma}$ consist of the rotational and translational parameters
$\lambda_{\textrm{r}}=100$ and $\lambda_{\textrm{t}}=1000$. They are applied
to the respective rotational and translational components of the full
variation vector $\boldsymbol{\theta}_{\textrm{k}}$. Finally, the constraint
Jacobian $\boldsymbol{B}_{\textrm{k}}$ and the Hessian matrix
$\boldsymbol{H}_{\textrm{k}}$ have very different orders of magnitude. To
ensure stability when solving the linear equation of the optimization, we use
a robust Cholesky decomposition with pivoting.

Since, in the evaluation, our tracker deals with large frame-to-frame pose
differences, we conduct $6$ iterations in which correspondences are
established. For the scale parameter of the region modality, we use
$s=\\{9,7,5,2\\}$. Also, the correspondence point threshold of the depth
modality, which is given in millimeters, is defined as
$r_{\textrm{t}}=\\{100,80,50\\}$, and is sampled with a stride of
$8\,\mathrm{mm}$. Note that values for the thresholds $r_{\textrm{t}}$ and the
stride are scaled according to the depth of model points and are defined at a
reference distance of $1\,\mathrm{m}$. In addition to those parameters, we use
the standard deviations $\sigma_{\textrm{r}}=\\{25,15,10\\}$ and
$\sigma_{\textrm{d}}=\\{50,30,20\\}$, which are given in pixels and
millimeters, respectively. The provided sets define parameters for each
iteration, with the last value employed in all remaining iterations. Finally,
with the developed framework and the described modifications, our Multi-body
ICG algorithm can be used for the tracking of a wide variety of open and
closed kinematic structures.

##  6 Evaluation

In the following section, we provide a detailed evaluation of the developed
framework and Multi-body ICG tracker. For this, a newly created dataset and
the used evaluation metrics are introduced. Based on those sequences, we
assess different configurations of kinematic structures with respect to
quality and efficiency. After this, the convergence of constraints is
analyzed. In addition, while the focus of this paper is a general framework
that can be used with various methods, a comparison to another algorithm is
also provided. Finally, we discuss limitations of our approach. All
experiments are conducted on a computer with an Intel Core i9-11900K CPU and a
NVIDIA RTX A5000 GPU. Qualitative results in real-world applications, as well
as sequences from our dataset, are provided in the supplementary
video.444https://www.youtube.com/watch?v=0ORZvDDbDjA Note that both the source
code for our tracker and the dataset are publicly available.1,2

###  6.1 Robot Tracking Benchmark

In the past, evaluations of multi-body tracking and pose estimation methods
considered only a very limited number of sequences. Consequently, experiments
were typically of a more qualitative nature [31, 32, 33, 54, 40, 36]. The main
reason is that diverse real-world data with moving cameras and high-quality
pose annotations is hard to obtain, even if object configurations can be
measured. However, for a reliable evaluation, a sufficient number of realistic
sequences with accurate ground truth is essential.

We therefore created the  Robot Tracking Benchmark (RTB). It is a novel
synthetic dataset that was developed using the procedural rendering pipeline
BlenderProc555https://github.com/DLR-RM/BlenderProc. Example images of the
dataset are shown in Fig. 6.

GripperMedical PliersMedical RobotPicker RobotRobot FingersRobot Wrist Figure
6:  Multi-body objects included in the RTB dataset, featuring the Gripper,
Medical Pliers, Medical Robot, Picker Robot, Robot Fingers, and Robot Wrist.
The Gripper, Picker Robot, and Robot Wrist contain closed kinematic chains.
Note that for the Robot Fingers, the wrist is locked, and only finger joints
are moving, while the Robot Wrist features locked fingers and a movable wrist.

To facilitate the creation of the dataset, we extended BlenderProc to load
robot models from URDF files. Also, for the generation of articulated motions,
forward and backward kinematics were integrated. Our open-source pipeline
produces photo-realistic sequences with HDRi
lighting666https://polyhaven.com/hdris and physically-based materials. Perfect
ground truth annotations for camera and robot trajectories are provided in the
BOP format [55]. Many physical effects, such as motion blur, rolling shutter,
and camera shaking, are accurately modeled to reflect real-world conditions.
For each frame, we also generated the following four depth qualities: Ground
Truth, Azure Kinect, Active Stereo, and Stereo. They simulate sensors with
different characteristics. While the first quality provides perfect ground
truth, the second considers measurements with the distance-dependent noise
characteristics of the Azure Kinect time-of-flight sensor [56]. Smoothed depth
is thereby modeled using random Gaussian shifting, and, at very dark surfaces,
missing measurements are considered. Finally, for the Active Stereo and Stereo
qualities, two stereo RGB images with and without a pattern from a simulated
dot projector were rendered. Depth images were then reconstructed using the
Semi-Global Matching (SGM) algorithm [57]. Examples of the four depth
qualities are shown in Fig. 7.

Ground TruthAzure KinectActive StereoStereo Figure 7:  Depth image qualities
provided in the RTB dataset, featuring Ground Truth with perfect measurements,
noise characteristics from an Azure Kinect camera, and stereo reconstructions
from SGM. For Active Stereo, the pattern from a dot projector is simulated to
add additional texture information, while Stereo operates without a pattern.

The benchmark features six robotic systems with different kinematics, ranging
from simple open-chain and tree topologies to structures with complex closed
kinematics. Example images of all multi-body objects included in the dataset
are shown in Fig. 6. An overview of their kinematic structure is given in Tab.
I.

TABLE I:  Characteristic numbers and thresholds for objects
included in the RTB dataset.  |  Gripper |  Medical Pliers |  Medical Robot |  Picker Robot |  Robot Fingers |  Robot Wrist
---|---|---|---|---|---|---
Number of Bodies |  9 |  5 |  8 |  11 |  16 |  12
Degrees of Freedom |  2 |  4 |  7 |  3 |  15 |  2
Closed Chains |  4 |  0 |  0 |  6 |  0 |  4
Error Threshold $e_{\textrm{t}}$ [m] |  0.01 |  0.01 |  0.1 |  0.1 |  0.05 |  0.05

For each robotic system, we provide three difficulty levels: Easy, Medium, and
Hard. In all sequences, the kinematic system is in motion. However, while for
Easy sequences, the camera is mostly static with respect to the robot, Medium
and Hard sequences feature faster and shakier motions for both the robot and
camera. Consequently, motion blur increases, which also reduces the quality of
stereo matching. Finally, for each object, difficulty level, and depth image
quality, $10$ sequences with $150$ frames were rendered. In total, this
results in $108.000$ frames that feature different kinematic structures,
motion patterns, depth measurements, scenes, and lighting conditions. In
summary, given the diverse data, the Robot Tracking Benchmark allows to
extensively measure, compare, and ablate the performance of multi-body
tracking algorithms, which is essential for further progress in the field.

###  6.2 Metrics

For the evaluation on the RTB dataset, the ADD and ADD-S area-under-curve
scores [58] are used, which we modify for multi-body structures. The average-
distance (ADD) error $e_{\textrm{ADD}}$ and average-shortest-distance (ADD-S)
error $e_{\textrm{ADD-S}}$ are thereby computed for each body and frame as
follows

$\displaystyle e_{\text{ADD}}$
$\displaystyle=\frac{1}{n_{\textrm{v}}}\sum_{i=1}^{n_{\textrm{v}}}\big{\lVert}\big{(}{}_{\text{M}}\boldsymbol{\widetilde{X}}_{i}-{}_{\text{M}}\boldsymbol{T}_{\text{M}_{\text{GT}}}\,{}_{\text{M}}\boldsymbol{\widetilde{X}}_{i}\big{)}_{3\times
1}\big{\rVert}_{2},$ (27) $\displaystyle e_{\text{ADD{-}S}}$
$\displaystyle=\frac{1}{n_{\textrm{v}}}\sum_{i=1}^{n_{\textrm{v}}}\min_{j\in[n_{\textrm{v}}]}\big{\lVert}\big{(}{}_{\text{M}}\boldsymbol{\widetilde{X}}_{i}-{}_{\text{M}}\boldsymbol{T}_{\text{M}_{\text{GT}}}\,{}_{\text{M}}\boldsymbol{\widetilde{X}}_{j}\big{)}_{3\times
1}\big{\rVert}_{2},$ (28)

where ${}_{\textrm{M}}\boldsymbol{T}_{\textrm{M}_{\textrm{GT}}}$ is the
transformation between the ground truth and estimated model pose,
$\boldsymbol{\widetilde{X}}_{i}$ is a vertex from the 3D mesh of the body
written in homogeneous coordinates, $n_{\textrm{v}}$ is the number of
vertices, and $()_{3\times 1}$ denotes the first three elements of a vector.
Note that, in the case that a rigid body is composed of multiple 3D meshes,
one simply averages the conventional and shortest distance error over all
meshes. Based on the errors of individual bodies and frames $e_{ij}$, we
compute the area-under-curve score for an entire kinematic structure and
sequence as follows

$s=\frac{1}{n_{\textrm{b}}n_{\textrm{f}}}\sum_{i=1}^{n_{\textrm{b}}}\sum_{j=1}^{n_{\textrm{f}}}\max\Big{(}1-\frac{e_{ij}}{e_{\textrm{t}}},0\Big{)},$
(29)

with $n_{\textrm{b}}$ the number of bodies, $n_{\textrm{f}}$ the number of
frames, and $e_{\textrm{t}}$ an error threshold. For the computation of ADD
and ADD-S area-under-curve scores $s_{\textrm{ADD}}$ and $s_{\textrm{ADD-S}}$,
we use the error metrics $e_{\textrm{ADD}}$ and $e_{\textrm{ADD-S}}$,
respectively. Error thresholds $e_{\textrm{t}}$ that are used for the
considered multi-body objects are given in Tab. I. They are defined to be
approximately ten percent of each object’s diameter. The resulting score
considers each body equally important, takes into account the size of the
kinematic structure, and rewards accuracy while limiting the impact of bad
predictions.

###  6.3 Kinematic Configuration

Using the RTB dataset, we thoroughly evaluate the proposed multi-body tracking
framework. For this, we compare four kinematic configurations per object. We
thereby differentiate between (i) independently tracked bodies, (ii)
projection to a minimal parameterization with Jacobians, (iii) constraints
using Lagrange multipliers, and (iv) a combination of projection and
constraints. In the combined case, the main configuration is equal to the
projected scenario. However, all chains are closed using additional
constraints from the constrained version.

Results of the evaluation are shown in Tab. II.

TABLE II:  Comparison of different kinematic configurations on the RTB dataset, showing ADD-S area-under-curve scores in percent and average runtimes in milliseconds. Tree-like structures are indicated by a ⋆.  Config-urations |  Gripper |  Medical Pliers⋆ |  Medical Robot⋆ |  Picker Robot |  Robot Fingers⋆ |  Robot Wrist |  Average |  Runtime
[ms]
---|---|---|---|---|---|---|---|---
Independent |  21.6 |  34.6 |  52.1 |  21.0 |  28.5 |  28.3 |  31.0 |  13.2
Projected |  71.1 |  81.7 |  95.1 |  54.0 |  90.0 |  86.1 |  79.7 |  13.5
Constrained |  87.4 |  79.9 |  92.9 |  93.7 |  84.3 |  96.8 |  89.2 |  16.2
Combined |  87.9 |  81.7 |  95.1 |  94.7 |  90.0 |  97.4 |  91.1 |  13.8

The experiments clearly demonstrate that independent tracking of bodies is not
an option for kinematic structures. Also, while the projection to a minimal
parameterization works very well for tree-like structures, for objects with
closed kinematic chains, such as the Gripper, Picker Robot, and Robot Wrist,
it is a significant disadvantage to not use the full kinematic information.
Finally, what is most interesting is the comparison of the constrained and
combined configurations. Even though both include the full kinematic
information, the combined version performs better than the constrained. The
main reason for this is a difference in regularization. For the constrained
configuration, the regularization considers the full pose variation of each
body independently. On the other hand, for the combined case, relative joint
variations between bodies are regularized. Since bodies in a kinematic
structure move relative to each other and the entire structure moves with
respect to the camera, the relative regularization of the combined
configuration is closer to reality. Consequently, using projection and
combining it with constraints to model closed chains works best.

In addition to tracking quality, we also compare computational efficiency.
Results in Tab. II show that runtimes for the independent, projected, and
combined configurations are almost equal. For the constrained configuration,
it is, however, noticeably larger. To analyze this difference in more detail,
we create an additional experiment that compares the projected and constrained
configurations. We thereby consider a kinematic chain with one rotational link
per body and a variable number of bodies. For the comparison, we measure the
runtime required for a single iteration of the optimization and the pose
update. Results over the number of bodies are given in Fig. 8.

$0$$10$$20$$30$$40$$50$$0$$2$$4$$6$Number of BodiesOptimization Time
[$ms$]ProjectedConstrained Figure 8:  Optimization time per iteration for the
projected and constrained configuration over the number of bodies in a
kinematic chain.

The plots show a significant difference between the two configurations. The
main reason is that, while for the constrained version, $11$ unknowns are
added per additional body, with $6$ parameters for the body pose and $5$ for
the constraint, only $1$ unknown is required in the projected scenario.
Together with the Cholesky decomposition’s computational complexity of
$\mathcal{O}(n^{3})$, which is used to solve the linear system of equations,
this explains the obtained results. Also, while the difference between the two
formulations decreases for joints with more than one degree of freedom, the
constrained configuration is never more efficient than the projected. In
conclusion, experiments in this section demonstrate that using projection
where possible and combining it with constraints where necessary works best
both for quality and efficiency.

###  6.4 Constraint Convergence

In Appendix C we proved mathematically that constraints converge in a single
iteration for the special case of equal model and constraint coordinate
frames. In the following, we analyze the more general case of unequal
coordinate frames. For this, random transformations
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{a}}$ and
${}_{\textrm{B}}\boldsymbol{T}_{\textrm{M}_{b}}$ are defined. Also, we start
with an initial random difference ${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}$
between constraint reference frames A and B. Transformations are thereby
generated using normalized 3D rotation and translation vectors with a random
orientation from a uniform distribution. The length of those rotation and
translation vectors is then sampled from uniform distributions on the
intervals $[-1,1]$ and $[-\pi,\pi]$, with translations in meters and rotations
in radians. Given an initial pose difference
${}_{\textrm{A}}\boldsymbol{T}_{\textrm{B}}$, four Newton iterations are
conducted. For each iteration, absolute rotation and translation errors
$\lVert{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}\rVert_{2}$ and
$\lVert{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}\rVert_{2}$ are computed. In
total, we evaluate $100.000$ random cases and report error percentiles.
Obtained results are visualized in Fig. 9.

Rotation Vector Constraints
(Ours)$0$$1$$2$$3$$4$0$\frac{\pi}{2}$$\pi$$0$$1$$2$$3$$4$$0$$0.5$$1$Orthogonality
Constraints$0$$1$$2$$3$$4$0$\frac{\pi}{2}$$\pi$$0$$1$$2$$3$$4$$0$$0.5$$1$Rotational
errors $\lVert{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}\rVert_{2}$ in [rad]
over iterations Translational errors
$\lVert{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}\rVert_{2}$ in [m] over
iterations Figure 9:  Convergence plots showing rotational and translational
errors over iterations of the Newton method. Initial errors are uniformly
distributed with a maximum of
$\lVert{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}\rVert_{2}=\pi$ and
$\lVert{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}\rVert_{2}=1\,\mathrm{m}$.
For the translation, the constraint
${}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}=\boldsymbol{0}$ is used. For the
rotation, we differentiate between our constraints, which restrict the
rotation vector using
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}=\boldsymbol{0}$, and orthogonality
constraints that ensure that axes of reference frame A are orthogonal to axes
of reference frame B. Dark lines visualize error deciles while bright lines
show error percentiles.  TABLE III:  Comparison of tracking algorithms on the
RTB dataset, showing ADD and ADD-S area-under-curve scores in percent
and average runtimes in milliseconds.8 Objects with tree-like kinematic
structures are indicated by a ⋆.

Object |  | Gripper | Medical Pliers⋆ | Medical Robot⋆ | Picker Robot | Robot Fingers⋆ | Robot Wrist | Average
---|---|---|---|---|---|---|---|---
App. |  Level |  ADD |  ADD-S |  ADD |  ADD-S |  ADD |  ADD-S |  ADD |  ADD-S |  ADD |  ADD-S |  ADD |  ADD-S |  ADD |  ADD-S
DART[13] |  Easy |  33.9 |  57.2 |  29.7 |  44.9 |  75.1 |  86.3 |  27.9 |  33.8 |  54.3 |  64.8 |  51.6 |  63.3 |  45.4 |  58.4
Medium |  1.4 |  3.1 |  11.0 |  25.5 |  12.1 |  20.7 |  3.2 |  5.4 |  2.9 |  4.7 |  4.9 |  8.3 |  5.9 |  11.3
Hard |  1.4 |  3.1 |  8.1 |  20.1 |  3.6 |  7.8 |  1.0 |  2.2 |  1.2 |  2.1 |  1.4 |  2.6 |  2.8 |  6.3
All |  12.2 |  21.2 |  16.2 |  30.1 |  30.2 |  38.3 |  10.7 |  13.8 |  19.5 |  23.9 |  19.3 |  24.7 |  18.0 |  25.3
Runtime | 4.1 ms | 3.7 ms | 5.8 ms | 9.6 ms | 12.8 ms | 11.4 ms | 7.9 ms
Mb-ICG (Ours) |  Easy |  92.1 |  94.1 |  78.3 |  86.8 |  93.1 |  97.0 |  96.0 |  97.3 |  95.6 |  96.8 |  97.9 |  98.5 |  92.2 |  95.1
Medium |  81.5 |  86.7 |  64.6 |  79.0 |  86.5 |  94.2 |  90.8 |  93.6 |  86.9 |  91.0 |  96.6 |  97.8 |  84.4 |  90.4
Hard |  78.6 |  83.0 |  62.9 |  79.2 |  81.8 |  94.1 |  90.7 |  93.0 |  76.5 |  82.2 |  94.0 |  95.9 |  80.7 |  87.9
All |  84.1 |  87.9 |  68.6 |  81.7 |  87.1 |  95.1 |  92.5 |  94.7 |  86.3 |  90.0 |  96.2 |  97.4 |  85.8 |  91.1
Runtime | 9.7 ms | 6.8 ms | 20.5 ms | 11.4 ms | 16.7 ms | 17.5 ms | 13.8 ms

The plots demonstrate that, like in our proof, both rotational and
translational constraints converge in a single iteration. Also, while we do
not report them here, similar results are obtained if individual components of
the rotation and translation are constrained.

To provide additional context, we compare our results to convergence plots
that use orthogonality constraints for the rotation. One thereby enforces
orthogonality between the axes of the constraint coordinate frames A and B as
follows

$b_{\textrm{r}ijab}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})={}_{\textrm{A}}\boldsymbol{e}_{i}^{\top}{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})\,{}_{\textrm{B}}\boldsymbol{e}_{j},$
(30)

with the tuple
$(i,j)\in\\{(\textrm{x},\textrm{y}),(\textrm{y},\textrm{z}),(\textrm{z},\textrm{x})\\}$.
Derivatives that are required for the implementation are given in Appendix E.
Convergence plots of the experiment are again shown in Fig. 9. The results
demonstrate that multiple iterations are required to converge to a
kinematically compliant result. However, what is even worse is that the
orthogonality requirement is not unique over the space of possible rotations.
In total, $7$ additional pose configurations exist that fulfill the defined
orthogonality constraints. In Fig. 9, those cases are visible in the
convergence towards a rotational error of $\pi$ and $\frac{2}{3}\pi$. In
conclusion, our experiments demonstrate that, in contrast to other approaches,
the developed constraints converge to a kinematically exact solution over the
entire space of possible rotations while requiring only a single iteration.

###  6.5 Multi-body ICG

In addition to the framework’s evaluation, we compare our Multi-body ICG
tracker to DART [13, 36]. DART is one of the few general-purpose algorithms
for the tracking of kinematic structures that is publicly available. It uses
signed distance functions to consider depth information and allows the
configuration of kinematic structures with both rotational and prismatic
joints. For our evaluation, we use the available source
code777https://github.com/tschmidt23/dart and leave all parameters at their
default values. Except for the Picker Robot, all experiments are conducted on
the same computer.888 Due to memory requirements of the Picker Robot, the
object’s evaluation with DART had to be conducted on a computer with an Intel
Xeon Gold 6254 CPU and a NVIDIA RTX A6000 GPU.  Results of the evaluation are
shown in Tab. 6.4. The comparison emphasizes the performance of our Multi-body
ICG tracker. While for Easy sequences, DART is still able to track some
objects well, for Medium and Hard, most sequences are impossible. The main
reason is that, due to the large pose differences between frames, the
algorithm quickly loses most objects. This suggests that using depth alone and
not being able to model closed kinematic structures significantly limits
tracking performance. In comparison, our Multi-body ICG approach is able to
successfully track almost all objects and achieves relatively high scores even
for Hard sequences.

With respect to runtime, both DART and our Multi-body ICG tracker are able to
operate well above $30\,\mathrm{Hz}$, which is the frequency of most consumer
RGB-D cameras. For DART, the largest runtime is observed for the Robot
Fingers, which has the maximum number of bodies. For our approach, most
resources are required for the tracking of the Medical Robot, which uses a
large number of correspondence lines and points. With its highly-optimized
CUDA implementation and the used NVIDIA RTX A5000 GPU, DART is even faster
than our approach. At the same time, for our method, the GPU is only required
to validate contour and surface points. Consequently, it remains mostly idle.
Also, the algorithm runs on a single CPU core. In summary, while differences
with respect to runtime and required computational resources exist, both DART
and Multi-body ICG are highly efficient and provide real-time performance.

Finally, in addition to difficulty levels, we evaluate both approaches with
respect to different depth image qualities provided in the RTB dataset.
Results of the experiments are given in Tab. IV.

TABLE IV:  Tracking results on the RTB dataset for different depth image qualities, with ADD and ADD-S area-under-curve scores in percent.  Approach | DART | Mb-ICG (Ours)
---|---|---
Depth Quality |  ADD |  ADD-S |  ADD |  ADD-S
Ground Truth |  21.1 |  29.0 |  88.7 |  93.0
Azure Kinect |  16.2 |  22.4 |  87.9 |  92.5
Active Stereo |  18.0 |  25.3 |  86.2 |  91.5
Stereo |  16.9 |  24.5 |  80.4 |  87.6

For DART, the evaluation shows a notable difference between Ground Truth and
other depth image qualities that are more realistic and imperfect. In
comparison, for our approach, differences between Ground Truth, Azure Kinect,
and Active Stereo are less severe, and results are mostly comparable. Only for
Stereo, for which depth images often have missing measurements and provide
considerably less information, the tracking performance is lower. In summary,
the results demonstrate that our Multi-body ICG approach does not require
perfect depth images. Instead, its combination of region and depth information
ensures high-quality tracking even in challenging scenarios with imperfect
depth measurements. Note that additional experiments that demonstrate the
performance for real-world sequences are provided in the supplementary video.

###  6.6 Limitations

While our framework allows to model various kinematic structures and is
compatible with a wide variety of algorithms, some limitations remain. First,
it focuses on approaches that employ Newton-like optimization techniques such
as Gauss-Newton, Levenberg-Marquardt, or quasi-Newton methods. The framework
is, therefore, not compatible with particle-based methods or existing deep
learning-based techniques. Also, in this work, we only derived equations based
on the axis-angle representation. Note, however, that it is relatively easy to
derive equations required for other parameterizations. Another limitation
comes from the Newton method itself. Because each iteration considers the
energy function only at a single point, the algorithm can get stuck in local
minima. Like for most tracking methods, this limits maximum configuration
changes and pose differences between frames. Also, algorithms that should be
extended with our framework have to deal with multiple bodies at the same
time. Typical challenges thereby include bodies that are very close together
or even intersect. Like in our extension of ICG, it might be necessary to
modify the original algorithm to successfully deal with such cases.

Finally, if our framework is applied to a specific approach, strengths and
weaknesses of the original method remain mostly unchanged. For example, the
original ICG algorithm requires objects to be distinguishable from the
background and needs reasonable depth measurements for the object’s surface.
In addition, the algorithm might struggle in situations where the object
geometry is not conclusive, such as, for example, in the case of a
rotationally symmetric cylinder. Naturally, the same characteristics also
apply to our extension. Fortunately, strengths of the original algorithm are
also preserved. Like ICG, the developed Multi-body ICG tracker is therefore
highly efficient, accurate, and robust, without requiring object texture.

##  7 Conclusion

In this work, we developed a framework that allows to extend existing 6DoF
algorithms to multi-body object tracking. It combines projection to a minimal
parameterization and the application of pose constraints into a single
formulation. With the efficiency and realistic regularization of projection
and the ability to model temporary connections and additional constraints, it
combines the best of both worlds. The resulting formulation allows to model a
wide variety of kinematic structures and, to the best of our knowledge, is the
first that accurately considers closed kinematic chains. In a detailed
mathematical proof, as well as in experiments, we were able to show that the
developed constraints enforce an exact kinematic solution and converge in a
single iteration of the Newton optimization. In contrast to previous work, our
constraints directly operate on pose differences instead of velocities. This
ensures that kinematic errors are minimized and can not accumulate over time.

Based on the developed framework, we extended the 6DoF object tracker ICG [15]
to multi-body tracking. For a thorough, quantitative evaluation, we created
the RTB dataset. It features highly-realistic sequences and multiple robots,
both with open and closed kinematic chains. The dataset also provides numerous
sequences in various settings with distinct difficulty levels and depth
qualities. In a detailed comparison, we demonstrated that our Multi-body ICG
approach significantly outperforms DART [13], which is a state-of-the-art
articulated object tracker. With the obtained results and new possibilities
for quantitative evaluations, we are confident that both our tracker and
dataset will find many applications in robotics and augmented reality. Also,
given the versatility of our framework and its compatibility with a wide
variety of existing and potential future methods, we hope that more approaches
will move from rigid objects to kinematic structures.

##  Appendix A Derivatives of Constraint Equations

In the following, we derive the first-order derivatives of the extended
constraint equation
$\boldsymbol{\bar{b}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})$ with
respect to $\boldsymbol{\theta}_{a}$ and $\boldsymbol{\theta}_{b}$. We thereby
consider the rotational constraint
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})$
and the translational constraint
${}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})$
separately. Constraint equations are introduced in Section 4.3.

###  A.1 Rotational Constraint

Starting from (17), the variated rotation between the coordinate frames A and
B can be expressed using the exponential map as follows

$\begin{split}\exp\big{(}[{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta}_{\textrm{r}a},\boldsymbol{\theta}_{\textrm{r}b})]_{\times}\big{)}=\,&\exp\big{(}[-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,\boldsymbol{\theta}_{\textrm{r}a}]_{\times}\big{)}\\\
&\exp\big{(}[{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\,\boldsymbol{\theta}_{\textrm{r}b}]_{\times}\big{)}\\\
&\exp\big{(}[{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}]_{\times}\big{)},\end{split}$
(31)

The vectors $\boldsymbol{\theta}_{\textrm{r}a}$ and
$\boldsymbol{\theta}_{\textrm{r}b}$ are the rotational components of the pose
variation vectors $\boldsymbol{\theta}_{a}$ and $\boldsymbol{\theta}_{b}$. The
constant rotation vector ${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$ is
evaluated at
$\boldsymbol{\theta}_{\textrm{r}a}=\boldsymbol{\theta}_{\textrm{r}b}=\boldsymbol{0}$.
Note that for the calculation of (31), the following relation was used

$\boldsymbol{R}\exp([\boldsymbol{x}]_{\times})\boldsymbol{R}^{-1}=\exp([\boldsymbol{R}\boldsymbol{x}]_{\times}).$
(32)

Knowing that we only need to calculate the derivative with respect to either
$\boldsymbol{\theta}_{\textrm{r}a}$ or $\boldsymbol{\theta}_{\textrm{r}b}$
while the other is zero, we reduce (31) to take a single variation vector
$\boldsymbol{\theta}$ and write

$\exp\big{(}[{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta})]_{\times}\big{)}=\exp\big{(}[\boldsymbol{\theta}]_{\times}\big{)}\exp\big{(}[{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}]_{\times}\big{)}.$
(33)

For the vector $\boldsymbol{\theta}$, one can then simply substitute one of
the rotated variation vectors
$\boldsymbol{\theta}=-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,\boldsymbol{\theta}_{\textrm{r}a}$
or
$\boldsymbol{\theta}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\,\boldsymbol{\theta}_{\textrm{r}b}$.

While the relation in (33) looks relatively simple, it is not trivial to
calculate the rotation vector
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta})$ from
$\boldsymbol{\theta}$ and ${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$.
Fortunately, a closed-form solution exists that dates back to Olinde
Rodrigues. For this, each rotation vector has to be split into an axis and an
angle. Using the angles $\gamma,\theta,\alpha\in\mathbb{R}$ and the axes
$\boldsymbol{n},\boldsymbol{v},\boldsymbol{e}\in\mathbb{R}^{3}$ with
$\lVert\boldsymbol{n}\rVert_{2}=\lVert\boldsymbol{v}\rVert_{2}=\lVert\boldsymbol{e}\rVert_{2}=1$,
we define
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta})=\gamma\boldsymbol{n}$,
$\boldsymbol{\theta}=\theta\boldsymbol{v}$, and
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}=\alpha\boldsymbol{e}$. Based on
those values, one can then define the following equations

$\cos\Big{(}\frac{\gamma}{2}\Big{)}=\cos\Big{(}\frac{\theta}{2}\Big{)}\cos\Big{(}\frac{\alpha}{2}\Big{)}-\sin\Big{(}\frac{\theta}{2}\Big{)}\sin\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{v}^{\top}\boldsymbol{e},$
(34)
$\begin{split}\sin\Big{(}\frac{\gamma}{2}\Big{)}\boldsymbol{n}&=\sin\Big{(}\frac{\theta}{2}\Big{)}\cos\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{v}+\cos\Big{(}\frac{\theta}{2}\Big{)}\sin\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{e}\\\
&\quad\quad\quad+\sin\Big{(}\frac{\theta}{2}\Big{)}\sin\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{v}\times\boldsymbol{e}.\end{split}$
(35)

For a detailed derivation and geometrical explanation of this relation, we
point interested readers to [59]. To express the required vector
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta})$, we define
two variables $x=\cos(\frac{\gamma}{2})$ and
$\boldsymbol{y}=\sin(\frac{\gamma}{2})\boldsymbol{n}$. They are the right-hand
sides of (34) and (35). Based on their reformulation $\gamma=2\cos^{-1}(x)$
and $\boldsymbol{n}=\sin(\frac{\gamma}{2})^{-1}\boldsymbol{y}$, one can write

${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta})=\gamma\boldsymbol{n}=\frac{2\cos^{-1}(x)}{\sin(\cos^{-1}(x))}\,\boldsymbol{y}=\frac{2\cos^{-1}(x)}{\sqrt{1-x^{2}}}\,\boldsymbol{y}.$
(36)

Finally, starting from this definition, we calculate the first-order
derivative with respect to the scalar parameter $\theta$, which is the
magnitude of the variation vector $\boldsymbol{\theta}$

$\begin{split}\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\theta}&=\frac{\frac{-2}{\sqrt{1-x^{2}}}\sqrt{1-x^{2}}-2\cos^{-1}(x)\frac{-x}{\sqrt{1-x^{2}}}}{1-x^{2}}\frac{\partial
x}{\partial\theta}\boldsymbol{y}\\\
&\quad\quad\quad+\frac{2\cos^{-1}(x)}{\sqrt{1-x^{2}}}\frac{\partial\boldsymbol{y}}{\partial{\theta}},\end{split}$
(37)

Also, based on the right-hand sides of (34) and (35), we calculate

$\displaystyle\mathllap{\frac{\partial
x}{\partial\theta}}=-\frac{\sin(\frac{\theta}{2})\cos(\frac{\alpha}{2})}{2}-\frac{\cos(\frac{\theta}{2})\sin(\frac{\alpha}{2})}{2}\boldsymbol{v}^{\top}\boldsymbol{e},$
(38) $\displaystyle\begin{aligned}
\mathllap{\frac{\partial\boldsymbol{y}}{\partial{\theta}}}&=\frac{\cos(\frac{\theta}{2})\cos(\frac{\alpha}{2})}{2}\boldsymbol{v}-\frac{\sin(\frac{\theta}{2})\sin(\frac{\alpha}{2})}{2}\boldsymbol{e}\\\
&\quad\quad\quad+\frac{\cos(\frac{\theta}{2})\sin(\frac{\alpha}{2})}{2}\boldsymbol{v}\times\boldsymbol{e}.\end{aligned}$
(39)

By evaluating the derivatives at $\theta=0$, for which, according to (34) and
(35), $x=\cos(\frac{\alpha}{2})$ and
$\boldsymbol{y}=\sin(\frac{\alpha}{2})\boldsymbol{e}$, the following equations
are obtained

$\displaystyle\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\theta}\bigg{|}_{\theta=0}=\,$
$\displaystyle\frac{-2+\alpha\cot(\frac{\alpha}{2})}{\sin(\frac{\alpha}{2})}\boldsymbol{e}\frac{\partial
x}{\partial\theta}+\frac{\alpha}{\sin(\frac{\alpha}{2})}\frac{\partial\boldsymbol{y}}{\partial{\theta}}\bigg{|}_{\theta=0},$
(40) $\displaystyle\frac{\partial x}{\partial\theta}\bigg{|}_{\theta=0}=\,$
$\displaystyle-\frac{1}{2}\sin\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{v}^{\top}\boldsymbol{e},$
(41)
$\displaystyle\frac{\partial\boldsymbol{y}}{\partial\theta}\bigg{|}_{\theta=0}=\,$
$\displaystyle\frac{1}{2}\cos\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{v}+\frac{1}{2}\sin\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{v}\times\boldsymbol{e}.$
(42)

Introducing (41) and (42) in (40) and extracting the pose variation’s rotation
axis $\boldsymbol{v}$ results in the final expression for the first-order
derivative

$\displaystyle\begin{aligned}
\mathllap{\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\theta}\bigg{|}_{\theta=0}}&=\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}\boldsymbol{v}^{\top}\boldsymbol{e}\\\
&\quad\quad\quad+\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{v}+\frac{\alpha}{2}\boldsymbol{v}\times\boldsymbol{e}\end{aligned}$
(43) $\displaystyle\begin{aligned}
&=\bigg{(}\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{I}-\frac{\alpha}{2}[\boldsymbol{e}]_{\times}\\\
&\quad\quad\quad+\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}\boldsymbol{e}^{\top}\bigg{)}\boldsymbol{v}.\end{aligned}$
(44)

Equation (44) shows that the derivative with respect to the rotation angle
$\theta$ consists of a linear combination of column vectors with the rotation
axis $\boldsymbol{v}$. For the rotation vector
$\boldsymbol{\theta}=\theta\boldsymbol{v}$, the rotation axis $\boldsymbol{v}$
projects the scalar angle $\theta$ in a similar fashion. Knowing that

$\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\theta}=\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\boldsymbol{\theta}}\frac{\partial\boldsymbol{\theta}}{\partial\theta}=\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\boldsymbol{\theta}}\,\boldsymbol{v},$
(45)

we are finally able to extract the first-order derivative with respect to the
rotation vector $\boldsymbol{\theta}$ from (44) and write

$\displaystyle\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\boldsymbol{\theta}}\bigg{|}_{\boldsymbol{\theta}=\boldsymbol{0}}$
$\displaystyle=\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{I}-\frac{\alpha}{2}[\boldsymbol{e}]_{\times}$
(46)
$\displaystyle\quad\quad\quad+\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}\boldsymbol{e}^{\top}.$

For the originally required derivatives of the rotational constraint
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})$
with respect to the rotational pose variation vectors
$\boldsymbol{\theta}_{\textrm{r}a}$ and $\boldsymbol{\theta}_{\textrm{r}b}$,
one simply substitutes
$\boldsymbol{\theta}=-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,\boldsymbol{\theta}_{\textrm{r}a}$
and
$\boldsymbol{\theta}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\,\boldsymbol{\theta}_{\textrm{r}b}$.
Considering the partial derivatives
$\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{r}a}}=-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}$
and
$\frac{\partial\boldsymbol{\theta}}{\partial\boldsymbol{\theta}_{\textrm{r}b}}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}$,
we then obtain the following final results for the first-order derivatives of
the rotational constraint

$\displaystyle\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\boldsymbol{\theta}_{a}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\,$
$\displaystyle\begin{bmatrix}-\boldsymbol{C}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}&\boldsymbol{0}\end{bmatrix},$
(47)
$\displaystyle\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\boldsymbol{\theta}_{b}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\,$
$\displaystyle\begin{bmatrix}\boldsymbol{C}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}&\boldsymbol{0}\end{bmatrix}.$
(48)

The matrix $\boldsymbol{C}$, which we call variation matrix, is thereby
defined as

$\boldsymbol{C}=\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{I}-\frac{\alpha}{2}[\boldsymbol{e}]_{\times}+\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}\boldsymbol{e}^{\top}.$
(49)

It is the partial derivative of
$\frac{\partial{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}}{\partial\boldsymbol{\theta}}\big{|}_{\boldsymbol{\theta}=\boldsymbol{0}}$
and is computed using the angle and axis of the current rotation vector
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}=\alpha\boldsymbol{e}$. The
variation matrix $\boldsymbol{C}$ describes how the rotation vector
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$ changes for the variation with a
subsequent infinitesimal rotation. Properties of the matrix are derived in
Appendix B.

###  A.2 Translational Constraint

For the translational constraint, we again start from (17) to formulate the
variated translation between the coordinate frames A and B. Using the
knowledge that derivatives are either calculated with respect to
$\boldsymbol{\theta}_{\textrm{a}}$ or $\boldsymbol{\theta}_{\textrm{b}}$, one
can write

$\displaystyle{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}(\boldsymbol{\theta}_{a})$
$\displaystyle={}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\big{(}\boldsymbol{R}(-\boldsymbol{\theta}_{\textrm{r}a})({}_{\textrm{M}_{a}}\boldsymbol{t}_{\textrm{B}}-\boldsymbol{\theta}_{\textrm{t}a})\big{)}+{}_{\textrm{A}}\boldsymbol{t}_{\textrm{M}_{a}},$
(50)
$\displaystyle{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}(\boldsymbol{\theta}_{b})$
$\displaystyle={}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\big{(}\boldsymbol{R}(\boldsymbol{\theta}_{\textrm{r}b})\,{}_{\textrm{M}_{b}}\boldsymbol{t}_{\textrm{B}}+\boldsymbol{\theta}_{\textrm{t}b}\big{)}+{}_{\textrm{A}}\boldsymbol{t}_{\textrm{M}_{b}},$
(51)

where the vectors $\boldsymbol{\theta}_{\textrm{t}a}$ and
$\boldsymbol{\theta}_{\textrm{t}b}$ are the translational components of the
variation vectors $\boldsymbol{\theta}_{a}$ and $\boldsymbol{\theta}_{b}$.

Using the Taylor series expansion of the exponential map and the knowledge
that
$[\boldsymbol{x}]_{\times}\boldsymbol{y}=-[\boldsymbol{y}]_{\times}\boldsymbol{x}$,
we define the following general relation

$\displaystyle\boldsymbol{R}(\boldsymbol{\theta})\boldsymbol{t}$
$\displaystyle=\exp(\boldsymbol{\theta})\boldsymbol{t}$ (52)
$\displaystyle=(\boldsymbol{I}+[\boldsymbol{\theta}]_{\times}+\mathcal{O}(\boldsymbol{\theta}^{2}))\boldsymbol{t}$
(53)
$\displaystyle=\boldsymbol{t}-[\boldsymbol{t}]_{\times}\boldsymbol{\theta}+\mathcal{O}(\boldsymbol{\theta}^{2})\boldsymbol{t},$
(54)

where the expression $\mathcal{O}(\boldsymbol{\theta}^{2})$ models higher-
order terms. Using this relation in (50) and (51) and neglecting higher-order
terms, it is straightforward to calculate the final results for the first-
order derivatives of the translational constraint

$\displaystyle\frac{\partial{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}}{\partial\boldsymbol{\theta}_{a}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\,$
$\displaystyle\begin{bmatrix}{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,[{}_{\textrm{M}_{a}}\boldsymbol{t}_{\textrm{B}}]_{\times}&-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\end{bmatrix},$
(55)
$\displaystyle\frac{\partial{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}}{\partial\boldsymbol{\theta}_{b}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}=\,$
$\displaystyle\begin{bmatrix}-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\,[{}_{\textrm{M}_{b}}\boldsymbol{t}_{\textrm{B}}]_{\times}&{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\end{bmatrix}.$
(56)

##  Appendix B Properties of the Variation Matrix

The variation matrix $\boldsymbol{C}$, which was derived in Appendix A.1
describes how the rotation vector ${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$
changes with the variation of a subsequent infinitesimal rotation. It is
computed according to (49) and depends on the rotational difference between
the coordinate frames A and B. In the following, we investigate its relation
to the rotation matrix ${}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}$ and the
rotation vector ${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$. For brevity, we
drop the subscripts A and B in the notation and simply write $\boldsymbol{R}$
and $\boldsymbol{r}$.

Given the rotation vector $\boldsymbol{r}=\alpha\boldsymbol{e}$ with the angle
$\alpha$ and the normalized axis $\boldsymbol{e}$, the rotation matrix can be
computed using the Rodrigues formula

$\boldsymbol{R}=\boldsymbol{I}+\sin(\alpha)[\boldsymbol{e}]_{\times}+2\sin\Big{(}\frac{\alpha}{2}\Big{)}^{2}[\boldsymbol{e}]_{\times}^{2}.$
(57)

With the knowledge that
$[\boldsymbol{e}]_{\times}\boldsymbol{e}=\boldsymbol{0}$, we are able to
calculate the following product for the rotation and variation matrix

$\begin{split}\boldsymbol{R}\boldsymbol{C}=\,&\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{I}-\frac{\alpha}{2}[\boldsymbol{e}]_{\times}+\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}\boldsymbol{e}^{\top}\\\
&\quad+\frac{\alpha}{2}\sin(\alpha)\cot\Big{(}\frac{\alpha}{2}\Big{)}[\boldsymbol{e}]_{\times}-\frac{\alpha}{2}\sin(\alpha)[\boldsymbol{e}]_{\times}^{2}\\\
&\quad+\alpha\sin\Big{(}\frac{\alpha}{2}\Big{)}^{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}[\boldsymbol{e}]_{\times}^{2}-\alpha\sin\Big{(}\frac{\alpha}{2}\Big{)}^{2}[\boldsymbol{e}]_{\times}^{3}.\end{split}$
(58)

Using the relations

$\displaystyle\frac{\alpha}{2}\sin(\alpha)\cot\Big{(}\frac{\alpha}{2}\Big{)}$
$\displaystyle=\alpha-\alpha\sin\Big{(}\frac{\alpha}{2}\Big{)}^{2},$ (59)
$\displaystyle\alpha\sin\Big{(}\frac{\alpha}{2}\Big{)}^{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}$
$\displaystyle=\frac{\alpha}{2}\sin(\alpha),$ (60)
$\displaystyle[\boldsymbol{e}]_{\times}^{3}$
$\displaystyle=-[\boldsymbol{e}]_{\times},$ (61)

one can simplify (58) and write

$\displaystyle\boldsymbol{R}\boldsymbol{C}$
$\displaystyle=\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\boldsymbol{I}+\frac{\alpha}{2}[\boldsymbol{e}]_{\times}+\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}\boldsymbol{e}^{\top}$
(62) $\displaystyle=\boldsymbol{C}^{\top},$ (63)

which shows that the product of the rotation and variation matrix is simply
the transposed variation matrix. Looking at (34) and (35), one notices that
changing the order of the rotations $\boldsymbol{r}$ and $\boldsymbol{\theta}$
only changes the sign in the cross product term in (35). Deriving the
corresponding variation matrix then leads to the same expression as in (62),
which is simply the transposed of the original variation matrix
$\boldsymbol{C}$. The transposed variation matrix $\boldsymbol{C}^{\top}$
therefore describes how the rotation vector $\boldsymbol{r}$ changes with the
variation of a preceding infinitesimal rotation. Based on this interpretation,
(63) shows that rotating the variation matrix $\boldsymbol{C}$ with the
rotation matrix $\boldsymbol{R}$ changes the order in which the infinitesimal
rotation $\boldsymbol{\theta}$ is applied to the rotation $\boldsymbol{r}$.

Similarly, transposing the relation in (63) and using the knowledge that for
valid rotation matrices $\boldsymbol{R}^{\top}=\boldsymbol{R}^{-1}$, one is
able to write

$\boldsymbol{R}\boldsymbol{C}=\boldsymbol{C}^{\top}\;\Leftrightarrow\;\boldsymbol{C}^{\top}\boldsymbol{R}^{\top}=\boldsymbol{C}\;\Leftrightarrow\;\boldsymbol{C}^{\top}=\boldsymbol{C}\boldsymbol{R}.$
(64)

This shows that $\boldsymbol{R}$ and $\boldsymbol{C}$ are commutative. Hence,
applying the rotation matrix $\boldsymbol{R}$ on either side of the variation
matrix $\boldsymbol{C}$ changes the location of the infinitesimal rotation
from a subsequent to a preceding rotation. Finally, based on the relations in
(64), we are able to express the rotation matrix with respect to the variation
matrix as follows

$\boldsymbol{R}=\boldsymbol{C}^{\top}\boldsymbol{C}^{-1}=\boldsymbol{C}^{-1}\boldsymbol{C}^{\top}.$
(65)

The equation shows that because $\boldsymbol{C}$ and $\boldsymbol{C}^{\top}$
consider the variation with respect to the frames before and after the
rotation, the matrix $\boldsymbol{R}$ is implicitly encoded in the variation
matrix $\boldsymbol{C}$. Also, (65) shows that the variation matrix is normal
with
$\boldsymbol{C}\boldsymbol{C}^{\top}=\boldsymbol{C}^{\top}\boldsymbol{C}$.
However, at the same time, experiments show that, with
$\boldsymbol{C}^{\top}\neq\boldsymbol{C}$ and
$\boldsymbol{C}^{\top}\neq\boldsymbol{C}^{-1}$, it is neither symmetric nor
orthogonal.

Finally, we also want to analyze how the rotation vector $\boldsymbol{r}$ is
connected to the variation matrix $\boldsymbol{C}$. For this, the product
$\boldsymbol{C}\boldsymbol{r}$ is computed. With the relations
$[\boldsymbol{e}]_{\times}\boldsymbol{e}=\boldsymbol{0}$ and
$\boldsymbol{e}^{\top}\boldsymbol{e}=1$, we calculate

$\displaystyle\boldsymbol{C}\boldsymbol{r}$
$\displaystyle=\alpha\Big{(}\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e}+\alpha\Big{(}1-\frac{\alpha}{2}\cot\Big{(}\frac{\alpha}{2}\Big{)}\Big{)}\boldsymbol{e},$
(66) $\displaystyle=\boldsymbol{r}.$ (67)

Given the knowledge that the rotation vector $\boldsymbol{r}$ is the
eigenvector of the rotation matrix $\boldsymbol{R}$ for the eigenvalue
$\lambda=1$, one is able to define the relation
$\boldsymbol{r}=\boldsymbol{R}\boldsymbol{r}$. Together with the results from
(63), we can therefore show that

$\boldsymbol{C}\boldsymbol{r}=\boldsymbol{r}\;\Leftrightarrow\;\boldsymbol{C}\boldsymbol{R}\boldsymbol{r}=\boldsymbol{r}\;\Leftrightarrow\;\boldsymbol{C}^{\top}\boldsymbol{r}=\boldsymbol{r}.$
(68)

This demonstrates that the rotation vector $\boldsymbol{r}$ is an eigenvector
of both the original and transposed variation matrix $\boldsymbol{C}$. Hence,
it is an eigenvector of both the variation matrix for a subsequent and
preceding infinitesimal rotation.

##  Appendix C Constraint Convergence

Given the developed constraint equations and first-order derivatives, in the
following, we analyze how fast the Newton method converges to a compliant
kinematic result. For this, two bodies with an initial rotational or
translational difference of ${}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}$ or
${}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}$ are considered. Based on this
initial error, the relative pose update for a full rotational or translational
constraint is analyzed. Experiments, which demonstrate that the obtained
mathematical results also hold for more general cases, are provided in Section
6.4.

###  C.1 Rotational Constraint

Without loss of generality, we assume that constraint coordinate frames are
equal to model frames with $\textrm{A}=\textrm{M}_{a}$ and
$\textrm{B}=\textrm{M}_{b}$. Also, the translation of both bodies is fixed and
the combined variation
$\boldsymbol{\theta}_{\textrm{k}}^{\top}=\begin{bmatrix}\boldsymbol{\theta}_{\textrm{r}a}^{\top}&\boldsymbol{\theta}_{\textrm{r}b}^{\top}\end{bmatrix}$
only considers rotation. Based on those assumptions, the constraint equation
and constraint Jacobian can be written as

$\displaystyle\boldsymbol{b}_{\textrm{k}}$
$\displaystyle={}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}},$ (69)
$\displaystyle\boldsymbol{B}_{\textrm{k}}$
$\displaystyle=\begin{bmatrix}-\boldsymbol{C}&\boldsymbol{C}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\end{bmatrix}.$
(70)

Introducing both statements in (7) and given some arbitrary Hessian matrices
and gradient vectors for body $a$ and $b$, we obtain the linear equation for a
single Newton step

$\begin{bmatrix}-\boldsymbol{H}_{a}&\boldsymbol{0}&\boldsymbol{C}^{\top}\\\
\boldsymbol{0}&-\boldsymbol{H}_{b}&-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}^{\top}\,\boldsymbol{C}^{\top}\\\
\boldsymbol{C}&-\boldsymbol{C}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}&\boldsymbol{0}\end{bmatrix}\begin{bmatrix}\boldsymbol{\theta}_{\textrm{r}a}\\\
\boldsymbol{\theta}_{\textrm{r}b}\\\
\boldsymbol{\lambda}\end{bmatrix}=\begin{bmatrix}\boldsymbol{g}_{a}\\\
\boldsymbol{g}_{b}\\\
{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}\end{bmatrix}.$ (71)

Based on the first two lines of the equation, one can directly write

$\displaystyle\boldsymbol{\theta}_{\textrm{r}a}$
$\displaystyle=\boldsymbol{H}_{a}^{-1}(\boldsymbol{C}^{\top}\boldsymbol{\lambda}-\boldsymbol{g}_{a}),$
(72) $\displaystyle\boldsymbol{\theta}_{\textrm{r}b}$
$\displaystyle=\boldsymbol{H}_{b}^{-1}(-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}^{\top}\,\boldsymbol{C}^{\top}\boldsymbol{\lambda}-\boldsymbol{g}_{b}),$
(73)

where the only unknown is $\boldsymbol{\lambda}$. Subsequently, introducing
(72) and (73) into the last line of (71) leads to the following expression

$\boldsymbol{C}(\boldsymbol{X}+\boldsymbol{Y})\boldsymbol{C}^{\top}\boldsymbol{\lambda}+\boldsymbol{C}(-\boldsymbol{x}+\boldsymbol{y})={}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}},$
(74)

where the matrices $\boldsymbol{X}=\boldsymbol{H}_{a}^{-1}$ and
$\boldsymbol{Y}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,\boldsymbol{H}_{b}^{-1}{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}^{\top}$,
as well as the vectors
$\boldsymbol{x}=\boldsymbol{H}_{a}^{-1}\boldsymbol{g}_{a}$ and
$\boldsymbol{y}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,\boldsymbol{H}_{b}^{-1}\boldsymbol{g}_{b}$
are used for conciseness. Rewriting this equation leads to an expression for
$\boldsymbol{\lambda}$ that only depends on gradient vectors, Hessian
matrices, and the current rotational difference

$\boldsymbol{\lambda}=\boldsymbol{C}^{-\top}(\boldsymbol{X}+\boldsymbol{Y})^{-1}(\boldsymbol{x}-\boldsymbol{y}+\boldsymbol{C}^{-1}{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}).$
(75)

Finally, using the proof from Appendix B, which shows that
${}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$ is an eigenvector of the
variation matrix $\boldsymbol{C}$ and that
$\boldsymbol{C}^{-1}{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}={}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}$,
we can further simplify and write

$\boldsymbol{\lambda}=\boldsymbol{C}^{-\top}(\boldsymbol{X}+\boldsymbol{Y})^{-1}(\boldsymbol{x}-\boldsymbol{y}+{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}).$
(76)

To assess the convergence of our method, we compute how the rotational
difference changes for a single Newton step. Using the axis-angle
representation, the change in rotation is thereby calculated as

$\Delta\boldsymbol{R}=\exp\big{(}[-\boldsymbol{\theta}_{\textrm{r}a}]_{\times}\big{)}\exp\big{(}[{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,\boldsymbol{\theta}_{\textrm{r}b}]_{\times}\big{)}.$
(77)

By introducing (72) and (73), one can then write

$\Delta\boldsymbol{R}=\exp\big{(}[-\boldsymbol{X}\boldsymbol{C}^{\top}\boldsymbol{\lambda}+\boldsymbol{x}]_{\times}\big{)}\exp\big{(}[-\boldsymbol{Y}\boldsymbol{C}^{\top}\boldsymbol{\lambda}-\boldsymbol{y}]_{\times}\big{)}.$
(78)

Extending this equation with $\boldsymbol{Y}-\boldsymbol{Y}$ and
$\boldsymbol{X}-\boldsymbol{X}$ leads to

$\begin{split}\Delta\boldsymbol{R}&=\exp\Big{(}\mbox{\large$\frac{1}{2}$}\big{[}(\boldsymbol{Y}-\boldsymbol{X}-\boldsymbol{X}-\boldsymbol{Y})\boldsymbol{C}^{\top}\boldsymbol{\lambda}+2\boldsymbol{x}\big{]}_{\times}\Big{)}\\\
&\quad\quad\exp\Big{(}\mbox{\large$\frac{1}{2}$}\big{[}(\boldsymbol{X}-\boldsymbol{Y}-\boldsymbol{X}-\boldsymbol{Y})\boldsymbol{C}^{\top}\boldsymbol{\lambda}-2\boldsymbol{y}\big{]}_{\times}\Big{)}.\end{split}$
(79)

Subsequently, we introduce $\boldsymbol{\lambda}$ from (76) and multiply it
with the expression $(-\boldsymbol{X}-\boldsymbol{Y})\boldsymbol{C}^{\top}$ in
(79) to obtain

$\begin{split}\Delta\boldsymbol{R}&=\exp\Big{(}\mbox{\large$\frac{1}{2}$}\big{[}(\boldsymbol{Y}-\boldsymbol{X})\boldsymbol{C}^{\top}\boldsymbol{\lambda}+\boldsymbol{x}+\boldsymbol{y}-{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}\big{]}_{\times}\Big{)}\\\
&\quad\quad\exp\Big{(}\mbox{\large$\frac{1}{2}$}\big{[}(\boldsymbol{X}-\boldsymbol{Y})\boldsymbol{C}^{\top}\boldsymbol{\lambda}-\boldsymbol{x}-\boldsymbol{y}-{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}\big{]}_{\times}\Big{)},\end{split}$
(80)

which has the following highly symmetric structure

$\Delta\boldsymbol{R}=\exp\Big{(}\mbox{\large$\frac{1}{2}$}[\boldsymbol{z}-{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}]_{\times}\Big{)}\exp\Big{(}\mbox{\large$\frac{1}{2}$}[-\boldsymbol{z}-{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}]_{\times}\Big{)}.$
(81)

Finally, observing that the two exponents are commutative and knowing that
commutative exponents can be summed, we get the following result for the
rotational difference

$\Delta\boldsymbol{R}=\exp\big{(}[-{}_{\textrm{A}}\boldsymbol{r}_{\textrm{B}}]_{\times}\big{)}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}^{-1}$
(82)

This shows that a single iteration of the Newton method is enough to minimize
an initial rotational error of ${}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}$ to
zero and obtain an exact kinematic solution.

###  C.2 Translational Constraint

Without loss of generality, we again assume that constraint coordinate frames
are equal to model frames with $\textrm{A}=\textrm{M}_{a}$ and
$\textrm{B}=\textrm{M}_{b}$. Furthermore, the rotation of both bodies is fixed
and the combined variation vector
$\boldsymbol{\theta}_{\textrm{k}}^{\top}=\begin{bmatrix}\boldsymbol{\theta}_{\textrm{t}a}^{\top}&\boldsymbol{\theta}_{\textrm{t}b}^{\top}\end{bmatrix}$
only considers translation. The constraint equation and constraint Jacobian
can then be written as

$\displaystyle\boldsymbol{b}_{\textrm{k}}$
$\displaystyle={}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}},$ (83)
$\displaystyle\boldsymbol{B}_{\textrm{k}}$
$\displaystyle=\begin{bmatrix}-\boldsymbol{I}&{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\end{bmatrix}.$
(84)

Introducing both statements in (7), we obtain the following linear equation
for a single Newton step

$\begin{bmatrix}-\boldsymbol{H}_{a}&\boldsymbol{0}&\boldsymbol{I}^{\top}\\\
\boldsymbol{0}&-\boldsymbol{H}_{b}&-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}^{\top}\\\
\boldsymbol{I}&-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}&\boldsymbol{0}\end{bmatrix}\begin{bmatrix}\boldsymbol{\theta}_{\textrm{t}a}\\\
\boldsymbol{\theta}_{\textrm{t}b}\\\
\boldsymbol{\lambda}\end{bmatrix}=\begin{bmatrix}\boldsymbol{g}_{a}\\\
\boldsymbol{g}_{b}\\\
{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}\end{bmatrix}.$ (85)

Based on the first two rows of this relation, variation vectors are computed
as

$\displaystyle\boldsymbol{\theta}_{\textrm{t}a}$
$\displaystyle=\boldsymbol{H}_{a}^{-1}(\boldsymbol{\lambda}-\boldsymbol{g}_{a}),$
(86) $\displaystyle\boldsymbol{\theta}_{\textrm{t}b}$
$\displaystyle=\boldsymbol{H}_{b}^{-1}(-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}^{\top}\,\boldsymbol{\lambda}-\boldsymbol{g}_{b}).$
(87)

For conciseness, we again define the matrices
$\boldsymbol{X}=\boldsymbol{H}_{a}^{-1}$ and
$\boldsymbol{Y}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,\boldsymbol{H}_{b}^{-1}{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}^{\top}$,
as well as the vectors
$\boldsymbol{x}=\boldsymbol{H}_{a}^{-1}\boldsymbol{g}_{a}$ and
$\boldsymbol{y}={}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,\boldsymbol{H}_{b}^{-1}\boldsymbol{g}_{b}$.
We then introduce (86) and (87) into the last row of (85) to compute the
following relation for $\boldsymbol{\lambda}$ that includes only known
variables

$\boldsymbol{\lambda}=(\boldsymbol{X}+\boldsymbol{Y})^{-1}(\boldsymbol{x}-\boldsymbol{y}+{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}).$
(88)

To assess the convergence, we consider the change of the translational
difference between frame A and B and write

$\Delta\boldsymbol{t}=-\boldsymbol{\theta}_{\textrm{t}a}+{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,\boldsymbol{\theta}_{\textrm{t}b}.$
(89)

Introducing the variation vectors from (86) and (87) then leads to the
relation

$\Delta\boldsymbol{t}=-(\boldsymbol{X}+\boldsymbol{Y})\boldsymbol{\lambda}+\boldsymbol{x}-\boldsymbol{y}.$
(90)

Finally, with the definition of $\boldsymbol{\lambda}$ from (88), we are able
to write the following result

$\Delta\boldsymbol{t}=-{}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}.$ (91)

Similar to the rotational case, this demonstrates that a single iteration of
the Newton method is enough to overcome the translational error
${}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}$ and obtain an exact kinematic
solution.

##  Appendix D Adjoint Equivalence

In the following, we demonstrate that if the constraint coordinate frames A
and B are equal, the first-order derivatives of the constraint equation in
(20) and (21) reduce to an adjoint representation. Given that in such a case
the variation matrix $\boldsymbol{C}$ is equal to the identity matrix, only
the partial derivatives of the translational difference
${}_{\textrm{A}}\boldsymbol{t}_{\textrm{B}}$ with respect to the rotational
variation vectors $\boldsymbol{\theta}_{\textrm{r}a}$ and
$\boldsymbol{\theta}_{\textrm{r}b}$ differ from the adjoint representation
defined in (10). However, we can show that

$\displaystyle{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,[{}_{\textrm{M}_{a}}\boldsymbol{t}_{\textrm{B}}]_{\times}$
$\displaystyle={}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,[{}_{\textrm{M}_{a}}\boldsymbol{t}_{\textrm{A}}]_{\times}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}^{-1}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}$
(92)
$\displaystyle=[{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,{}_{\textrm{M}_{a}}\boldsymbol{t}_{\textrm{A}}]_{\times}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}$
(93)
$\displaystyle=-[{}_{\textrm{A}}\boldsymbol{t}_{\textrm{M}_{a}}]_{\times}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}},$
(94)

and similarly that

$-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\,[{}_{\textrm{M}_{b}}\boldsymbol{t}_{\textrm{B}}]_{\times}=[{}_{\textrm{A}}\boldsymbol{t}_{\textrm{M}_{b}}]_{\times}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}.$
(95)

This demonstrates that if A and B are equal, the derivatives in (20) and (21)
turn into $-\operatorname{Ad}({}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{a}})$
and $\operatorname{Ad}({}_{\textrm{A}}\boldsymbol{T}_{\textrm{M}_{b}})$.

##  Appendix E Derivatives of Orthogonality Constraints

In the following, we calculate the first-order derivatives of the rotational
orthogonality constraint equation. They are required for the calculation of
the constraint Jacobian in (19). Starting from (30), the constraint equation
is defined as

$\begin{split}b_{\textrm{r}ijab}(\boldsymbol{\theta}_{a},\boldsymbol{\theta}_{b})={}_{\textrm{A}}\boldsymbol{e}_{i}^{\top}&\exp\big{(}[-{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}}\,\boldsymbol{\theta}_{\textrm{r}a}]_{\times}\big{)}\\\
&\exp\big{(}[{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}\,\boldsymbol{\theta}_{\textrm{r}b}]_{\times}\big{)}\\\
&{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,{}_{\textrm{B}}\boldsymbol{e}_{j},\end{split}$
(96)

with the tuple
$(i,j)\in\\{(\textrm{x},\textrm{y}),(\textrm{y},\textrm{z}),(\textrm{z},\textrm{x})\\}$.
Similar to (53) and (54), we first linearize using the Taylor series expansion
$\exp([\boldsymbol{\theta}]_{\times})\approx\boldsymbol{I}+[\boldsymbol{\theta}]_{\times}$
and then use
$[\boldsymbol{x}]_{\times}\boldsymbol{y}=-[\boldsymbol{y}]_{\times}\boldsymbol{x}$.
With this, the following first-order derivatives can be calculated

$\displaystyle\frac{\partial{}b_{\textrm{r}ijab}}{\partial\boldsymbol{\theta}_{\textrm{r}a}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}$
$\displaystyle={}_{\textrm{A}}\boldsymbol{e}_{i}^{\top}[{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,{}_{\textrm{B}}\boldsymbol{e}_{j}]_{\times}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{a}},$
(97)
$\displaystyle\frac{\partial{}b_{\textrm{r}ijab}}{\partial\boldsymbol{\theta}_{\textrm{r}b}}\bigg{|}_{\boldsymbol{\theta}_{\textrm{k}}=\boldsymbol{0}}$
$\displaystyle={}_{\textrm{A}}\boldsymbol{e}_{i}^{\top}[{}_{\textrm{A}}\boldsymbol{R}_{\textrm{B}}\,{}_{\textrm{B}}\boldsymbol{e}_{j}]_{\times}\,{}_{\textrm{A}}\boldsymbol{R}_{\textrm{M}_{b}}.$
(98)

Knowing that first-order derivatives with respect to the translational
variations $\boldsymbol{\theta}_{\textrm{t}a}$ and
$\boldsymbol{\theta}_{\textrm{t}b}$ are zero, we are able to compute the
constraint Jacobians $\boldsymbol{B}_{ab}$ required in the Newton
optimization.

## References

  * [1] V. Lepetit and P. Fua, _Monocular Model-Based 3D Tracking of Rigid Objects: A Survey_. Foundations and Trends in Computer Graphics and Vision, 2005.
  * [2] A. Yilmaz, O. Javed, and M. Shah, “Object tracking: A survey,” _ACM Computing Surveys_ , vol. 38, no. 4, p. 13, 2006.
  * [3] L. Vacchetti, V. Lepetit, and P. Fua, “Stable real-time 3D tracking using online and offline information,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 26, no. 10, pp. 1385–1391, 2004.
  * [4] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “ORB: An efficient alternative to SIFT or SURF,” in _IEEE International Conference on Computer Vision_ , 2011, pp. 2564–2571.
  * [5] C. Harris and C. Stennett, “RAPID - A video rate object tracker,” in _British Machine Vision Conference_ , 1990, pp. 15.1–15.6.
  * [6] T. Drummond and R. Cipolla, “Real-time visual tracking of complex structures,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 24, no. 7, pp. 932–946, 2002.
  * [7] B. D. Lucas and T. Kanade, “An iterative image registration technique with an application to stereo vision,” in _Proceedings of the 7th International Joint Conference on Artificial Intelligence_ , vol. 2, 1981, pp. 674–679.
  * [8] A. Crivellaro and V. Lepetit, “Robust 3D tracking with descriptor fields,” in _IEEE Conference on Computer Vision and Pattern Recognition_ , 2014, pp. 3414–3421.
  * [9] B. Wen, C. Mitash, B. Ren, and K. E. Bekris, “se(3)-TrackNet: Data-driven 6D pose tracking by calibrating image residuals in synthetic domains,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2020, pp. 10 367–10 373.
  * [10] X. Deng, A. Mousavian, Y. Xiang, F. Xia, T. Bretl, and D. Fox, “PoseRBPF: A Rao–Blackwellized particle filter for 6-D object pose tracking,” _IEEE Transactions on Robotics_ , vol. 37, no. 5, pp. 1328–1342, 2021.
  * [11] V. A. Prisacariu and I. D. Reid, “PWP3D: Real-time segmentation and tracking of 3D objects,” _International Journal of Computer Vision_ , vol. 98, no. 3, pp. 335–354, 2012.
  * [12] M. Stoiber, M. Pfanne, K. H. Strobl, R. Triebel, and A. Albu-Schäffer, “SRT3D: A sparse region-based 3D object tracking approach for the real world,” _International Journal of Computer Vision_ , vol. 130, no. 4, p. 1008–1030, 2022.
  * [13] T. Schmidt, R. Newcombe, and D. Fox, “DART: Dense articulated real-time tracking with consumer depth cameras,” _Autonomous Robots_ , vol. 39, no. 3, pp. 239–258, 2015.
  * [14] J. Issac, M. Wüthrich, C. G. Cifuentes, J. Bohg, S. Trimpe, and S. Schaal, “Depth-based object tracking using a robust gaussian filter,” in _IEEE International Conference on Robotics and Automation_ , 2016, pp. 608–615.
  * [15] M. Stoiber, M. Sundermeyer, and R. Triebel, “Iterative corresponding geometry: Fusing region and depth for highly efficient 3D tracking of textureless objects,” in _IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2022, pp. 6855–6865.
  * [16] D. Hogg, “Model-based vision: A program to see a walking person,” _Image and Vision Computing_ , vol. 1, no. 1, pp. 5–20, 1983.
  * [17] D. Gavrila and L. Davis, “3-D model-based tracking of humans in action: A multi-view approach,” in _IEEE Computer Society Conference on Computer Vision and Pattern Recognition_ , 1996, pp. 73–80.
  * [18] D. G. Lowe, “Fitting parameterized three-dimensional models to images,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 13, no. 5, p. 441–450, 1991.
  * [19] J. M. Rehg and T. Kanade, “Visual tracking of high dof articulated structures: An application to human hand tracking,” in _European Conference on Computer Vision_ , J.-O. Eklundh, Ed., 1994, pp. 35–46.
  * [20] C. Bregler and J. Malik, “Tracking people with twists and exponential maps,” in _IEEE Computer Society Conference on Computer Vision and Pattern Recognition_ , 1998, pp. 8–15.
  * [21] K. Nickels and S. Hutchinson, “Model-based tracking of complex articulated objects,” _IEEE Transactions on Robotics and Automation_ , vol. 17, no. 1, pp. 28–36, 2001.
  * [22] A. I. Comport, Éric Marchand, and F. Chaumette, “Kinematic sets for real-time robust articulated object tracking,” _Image and Vision Computing_ , vol. 25, no. 3, pp. 374–391, 2007.
  * [23] T. E. de Campos, B. J. Tordoff, and D. W. Murray, “Recovering articulated pose: A comparison of two pre and postimposed constraint methods,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 28, no. 1, pp. 163–168, 2006.
  * [24] D. Demirdjian, T. Ko, and T. Darrell, “Constraining human body tracking,” in _Ninth IEEE International Conference on Computer Vision_ , 2003, pp. 1071–1078 vol.2.
  * [25] L. Mündermann, S. Corazza, and T. P. Andriacchi, “Accurately measuring human movement using articulated ICP with soft-joint constraints and a repository of articulated models,” in _IEEE Conference on Computer Vision and Pattern Recognition_ , 2007, pp. 1–6.
  * [26] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 14, no. 2, pp. 239–256, 1992.
  * [27] G. Dewaele, F. Devernay, and R. Horaud, “Hand motion from 3D point trajectories and a smooth surface model,” in _European Conference on Computer Vision_ , T. Pajdla and J. Matas, Eds., 2004, pp. 495–507.
  * [28] S. Pellegrini, K. Schindler, and D. Nardi, “A generalisation of the ICP algorithm for articulated bodies,” in _British Machine Vision Conference_ , 2008, pp. 87.1–87.10.
  * [29] T. Brox, B. Rosenhahn, J. Gall, and D. Cremers, “Combined region and motion-based 3D tracking of rigid and articulated objects,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 32, no. 3, pp. 402–415, 2010.
  * [30] D. G. Lowe, “Distinctive image features from scale-invariant keypoints,” _International Journal of Computer Vision_ , vol. 60, no. 2, pp. 91–110, 2004\.
  * [31] M. Krainin, P. Henry, X. Ren, and D. Fox, “Manipulator and object tracking for in-hand 3D object modeling,” _The International Journal of Robotics Research_ , vol. 30, no. 11, pp. 1311–1327, 2011.
  * [32] M. Klingensmith, T. Galluzzo, C. M. Dellin, M. Kazemi, J. A. Bagnell, and N. Pollard, “Closed-loop servoing using real-time markerless arm tracking,” 2013\.
  * [33] J. Bohg, J. Romero, A. Herzog, and S. Schaal, “Robot arm pose estimation through pixel-wise part classification,” in _IEEE International Conference on Robotics and Automation_ , 2014, pp. 3143–3150.
  * [34] C. Rauch, T. Hospedales, J. Shotton, and M. Fallon, “Visual articulated tracking in the presence of occlusions,” in _IEEE International Conference on Robotics and Automation_ , 2018, pp. 643–650.
  * [35] Y. Liu and A. Namiki, “Articulated object tracking by high-speed monocular RGB camera,” _IEEE Sensors Journal_ , vol. 21, no. 10, pp. 11 899–11 915, 2021.
  * [36] T. Schmidt, K. Hertkorn, R. Newcombe, Z. Marton, M. Suppa, and D. Fox, “Depth-based tracking with physical constraints for robot manipulation,” in _IEEE International Conference on Robotics and Automation_ , 2015, pp. 119–126.
  * [37] A. W. Fitzgibbon, “Robust registration of 2D and 3D point sets,” _Image and Vision Computing_ , vol. 21, no. 13-14, pp. 1145–1153, 2003.
  * [38] G. Izatt, G. Mirano, E. Adelson, and R. Tedrake, “Tracking objects with point clouds from vision and touch,” in _International Conference on Robotics and Automation_ , 2017, pp. 4000–4007.
  * [39] K. Pauwels, L. Rubio, and E. Ros, “Real-time model-based articulated object pose detection and tracking with variable rigidity constraints,” in _IEEE Conference on Computer Vision and Pattern Recognition_ , 2014, pp. 3994–4001.
  * [40] K. Pauwels and D. Kragic, “SimTrack: A simulation-based framework for scalable real-time object pose detection and tracking,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2015, pp. 1300–1307.
  * [41] P. Hebert, N. Hudson, J. Ma, T. Howard, T. Fuchs, M. Bajracharya, and J. Burdick, “Combined shape, appearance and silhouette for simultaneous manipulator and object tracking,” in _International Conference on Robotics and Automation_ , 2012, pp. 2405–2412.
  * [42] F. Widmaier, D. Kappler, S. Schaal, and J. Bohg, “Robot arm pose estimation by pixel-wise regression of joint angles,” in _IEEE International Conference on Robotics and Automation_ , 2016, pp. 616–623.
  * [43] C. G. Cifuentes, J. Issac, M. Wüthrich, S. Schaal, and J. Bohg, “Probabilistic articulated real-time tracking for robot manipulation,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 2, pp. 577–584, 2017\.
  * [44] M. Wüthrich, J. Bohg, D. Kappler, C. Pfreundt, and S. Schaal, “The coordinate particle filter - A novel particle filter for high dimensional systems,” in _IEEE International Conference on Robotics and Automation_ , 2015, pp. 2454–2461.
  * [45] Y. Zuo, W. Qiu, L. Xie, F. Zhong, Y. Wang, and A. L. Yuille, “CRAVES: Controlling robotic arm with a vision-based economic system,” in _IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 4209–4218.
  * [46] Y. Labbé, J. Carpentier, M. Aubry, and J. Sivic, “Single-view robot pose and joint angle estimation via render & compare,” in _IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2021, pp. 1654–1663.
  * [47] C. Zimmermann and T. Brox, “Learning to estimate 3D hand pose from single RGB images,” in _IEEE International Conference on Computer Vision_ , 2017, pp. 4913–4921.
  * [48] A. Kanazawa, M. J. Black, D. W. Jacobs, and J. Malik, “End-to-end recovery of human shape and pose,” in _IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2018, pp. 7122–7131.
  * [49] L. Ge, Z. Ren, Y. Li, Z. Xue, Y. Wang, J. Cai, and J. Yuan, “3D hand shape and pose estimation from a single RGB image,” in _IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 10 825–10 834.
  * [50] D. J. Tan, T. Cashman, J. Taylor, A. Fitzgibbon, D. Tarlow, S. Khamis, S. Izadi, and J. Shotton, “Fits like a glove: Rapid and reliable hand shape personalization,” in _IEEE Conference on Computer Vision and Pattern Recognition_ , 2016, pp. 5610–5619.
  * [51] J. Taylor, L. Bordeaux, T. Cashman, B. Corish, C. Keskin, T. Sharp, E. Soto, D. Sweeney, J. Valentin, B. Luff, A. Topalian, E. Wood, S. Khamis, P. Kohli, S. Izadi, R. Banks, A. Fitzgibbon, and J. Shotton, “Efficient and precise interactive hand tracking through joint, continuous optimization of pose and correspondences,” _ACM Transactions on Graphics_ , vol. 35, no. 4, 2016.
  * [52] S. Han, B. Liu, R. Cabezas, C. D. Twigg, P. Zhang, J. Petkau, T.-H. Yu, C.-J. Tai, M. Akbay, Z. Wang, A. Nitzan, G. Dong, Y. Ye, L. Tao, C. Wan, and R. Wang, “MEgATrack: Monochrome egocentric articulated hand-tracking for virtual reality,” _ACM Transactions on Graphics_ , vol. 39, no. 4, 2020.
  * [53] R. F. Salas-Moreno, R. A. Newcombe, H. Strasdat, P. H. Kelly, and A. J. Davison, “SLAM++: Simultaneous localisation and mapping at the level of objects,” in _IEEE Conference on Computer Vision and Pattern Recognition_ , 2013, pp. 1352–1359.
  * [54] F. Michel, A. Krull, E. Brachmann, M. Yang, S. Gumhold, and C. Rother, “Pose estimation of kinematic chain instances via object coordinate regression,” in _British Machine Vision Conference_ , 2015, pp. 181.1–181.11.
  * [55] T. Hodaň, M. Sundermeyer, B. Drost, Y. Labbé, E. Brachmann, F. Michel, C. Rother, and J. Matas, “BOP challenge 2020 on 6D object localization,” in _European Conference on Computer Vision Workshops_ , 2020, pp. 577–594.
  * [56] M. Tölgyessy, M. Dekan, L. Chovanec, and P. Hubinskỳ, “Evaluation of the azure kinect and its comparison to kinect v1 and kinect v2,” _Sensors_ , vol. 21, no. 2, 2021.
  * [57] H. Hirschmüller, “Accurate and efficient stereo processing by semi-global matching and mutual information,” in _IEEE Computer Society Conference on Computer Vision and Pattern Recognition_ , vol. 2, 2005, pp. 807–814.
  * [58] S. Hinterstoisser, V. Lepetit, S. Ilic, S. Holzer, G. Bradski, K. Konolige, and N. Navab, “Model based training, detection and pose estimation of texture-less 3D objects in heavily cluttered scenes,” in _Asian Conference on Computer Vision_ , 2013, pp. 548–562.
  * [59] S. L. Altmann, _Rotations, Quaternions, and Double Groups_. Courier Corporation, 2005.

|  Manuel Stoiber received a B.Sc. and M.Sc. degree from the Technical
University of Munich and spent time at the Queensland University of Technology
and ETH Zurich. Currently, he is a PhD student at the Technical University of
Munich and a research scientist at the German Aerospace Center, where he is
part of the Perception and Cognition department in the Institute of Robotics
and Mechatronics. In 2020, he received the Best Paper Saburo Tsuji Award from
the Asian Conference on Computer Vision. His research interests include
computer vision techniques and probabilistic methods for real-world robotic
applications.
---|---
|  Martin Sundermeyer is a PhD student at the Technical University of Munich
and a research scientist at the German Aerospace Center, where he is part of
the Perception and Cognition department in the Institute of Robotics and
Mechatronics. He also performed research at the University of Tokyo and
NVIDIA, received the Best Paper Award at ECCV 2018, is a core developer of
BlenderProc and organizer of 6D pose estimation workshops and challenges. His
research interests include computer vision methods for spatial robotic tasks
with known and unknown objects, self-supervised learning from demonstrations
and sim2real transfer.
---|---
|  Wout Boerdijk is a PhD student at the Technical University of Munich and a
research scientist at the German Aerospace Center, where he is part of the
Perception and Cognition department in the Institute of Robotics and
Mechatronics. His research interests include computer vision methods for
learning of and interacting with objects.
---|---
|  Rudolph Triebel leads the department of Perception and Cognition at the DLR
Institute of Robotics and Mechatronics. He received his PhD in computer
science in 2007 from the University of Freiburg, Germany and the habilitation
in 2015 from the Technical University of Munich. Before working at DLR, he was