2.1. Matrix Lie groups

The special orthogonal group, representing rotations, is the set of valid rotation matrices:

(1) \begin{equation}SO(3) = \left\{ \textbf{C} \in \mathbb{R}^{3\times3} \; | \; \textbf{C} \textbf{C}^T = \textbf{1}, \mbox{det} \,\textbf{C} = 1 \right\},\end{equation}

where $\textbf{1}$ is the identity matrix. It is common to map a vector, $\boldsymbol{\phi} \in \mathbb{R}^3$ , to a rotation matrix, $\textbf{C}$ , through the matrix exponential,

(2) \begin{equation}\textbf{C}(\boldsymbol{\phi}) = \exp\left( \boldsymbol{\phi}^\wedge \right),\end{equation}

where $(\cdot)^\wedge$ is the skew-symmetric operator,

(3) \begin{equation}\boldsymbol{\phi}^\wedge = \left( \begin{array}{c} \phi_1 \\ \phi_2 \\ \phi_3 \end{array}\right)^\wedge = \left( \begin{array}{c@{\quad}c@{\quad}c} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{array}\right).\end{equation}

The mapping is surjective only, meaning every $\textbf{C}$ can be produced by many different values for $\boldsymbol{\phi}$ .

The special Euclidean group, representing poses (i.e., translation and rotation), is the set of valid transformation matrices:

(4) \begin{equation}SE(3) = \left\{ \textbf{T} = \left( \begin{array}{c@{\quad}c} \textbf{C} & \textbf{r} \\ \\[-7pt] \;\,\textbf{0}^T & 1 \end{array}\right) \in \mathbb{R}^{4\times4} \; \Biggl| \; \textbf{C} \in SO(3), \, \textbf{r} \in \mathbb{R}^3 \right\}.\end{equation}

It is again common to map a vector, $\boldsymbol{\xi} \in \mathbb{R}^6$ , to a transformation matrix, $\textbf{T}$ , through the matrix exponential,

(5) \begin{equation}\textbf{T}(\boldsymbol{\xi}) = \exp\left( \boldsymbol{\xi}^\wedge \right),\end{equation}

where

(6) \begin{equation}\boldsymbol{\xi}^\wedge = \left( \begin{array}{c} \boldsymbol{\rho} \\ \\[-7pt] \boldsymbol{\phi} \end{array}\right)^\wedge = \left( \begin{array}{c@{\quad}c} \boldsymbol{\phi}^\wedge & \boldsymbol{\rho} \\ \\[-7pt] \textbf{0}^T & 0 \end{array}\right).\end{equation}

As is common practice [Reference Barfoot5], we have broken the pose vector, $\boldsymbol{\xi}$ , into a translational component, $\boldsymbol{\rho}$ , and a rotational component, $\boldsymbol{\phi}$ . The mapping is also surjective only, meaning every $\textbf{T}$ can be produced by many different values for $\boldsymbol{\xi}$ .

Finally, the adjoint of pose is given by

(7) \begin{equation}\;{\boldsymbol{\mathcal{T}}}(\boldsymbol{\xi}) = \mbox{Ad}\left( \textbf{T} \right) = \left( \begin{array}{c@{\quad}c} \textbf{C}(\boldsymbol{\phi}) & \textbf{r}^\wedge \textbf{C}(\boldsymbol{\phi}) \\ \\[-7pt] \textbf{0} & \textbf{C}(\boldsymbol{\phi}) \end{array}\right),\end{equation}

which is now $6 \times 6$ . We will refer to the set of adjoints as $\mbox{Ad}(SE(3))$ . We can map a vector, $\boldsymbol{\xi} \in \mathbb{R}^6$ , to an adjoint transformation matrix again through the matrix exponential map:

(8) \begin{equation}\;{\boldsymbol{\mathcal{T}}}(\boldsymbol{\xi}) = \exp\left( \boldsymbol{\xi}^\curlywedge \right),\end{equation}

where

(9) \begin{equation}\boldsymbol{\xi}^\curlywedge = \left( \begin{array}{c} \boldsymbol{\rho} \\ \\[-7pt] \boldsymbol{\phi} \end{array}\right)^\curlywedge = \left( \begin{array}{c@{\quad}c} \boldsymbol{\phi}^\wedge & \boldsymbol{\rho}^\wedge \\ \\[-7pt] \textbf{0} & \boldsymbol{\phi}^\wedge \end{array}\right).\end{equation}

The mapping is again surjective only, meaning every $\boldsymbol{\mathcal{T}}$ can be produced by many different values for $\boldsymbol{\xi}$ .

2.2. Key identities

We will be exploring mappings (including the exponential map) from vectors to Lie groups and will have occasion to work with series expressions of rotation and transformation matrices. Each group has associated with it an identity that can be used to limit the number of terms in such series, related to the Cayley–Hamilton theorem.

For rotations, we have that the characteristic equation of $\boldsymbol{\phi}^\wedge$ is

(10) \begin{equation}\left| \lambda \textbf{1} - \boldsymbol{\phi}^\wedge \right| = \lambda ( \lambda^2 + \underbrace{\phi_1^2 + \phi_2^2 + \phi_3^2}_{\phi^2} ) = \lambda^3 + \phi^2 \lambda = 0,\end{equation}

where $\lambda$ are the eigenvalues of $\boldsymbol{\phi}^\wedge$ . From the Cayley–Hamilton theorem, we can claim that

(11) \begin{equation} \boldsymbol{\phi}^{\wedge^3} + \phi^2 \boldsymbol{\phi}^\wedge \equiv \textbf{0},\end{equation}

since $\boldsymbol{\phi}^\wedge$ must satisfy its own characteristic equation.

For poses, we have that the characteristic equation of $\boldsymbol{\xi}^\wedge$ is

(12) \begin{equation}\left| \lambda \textbf{1} - \boldsymbol{\xi}^\wedge \right| = \left| \begin{matrix} \lambda \textbf{1} - \boldsymbol{\phi}^\wedge & -\boldsymbol{\rho} \\ \\[-7pt] \textbf{0}^T & \lambda \end{matrix} \right| = \lambda^4 + \phi^2 \lambda^2 = 0.\end{equation}

Again, from the Cayley–Hamilton theorem we can claim that

(13) \begin{equation}\boldsymbol{\xi}^{\wedge^4} + \phi^2 \boldsymbol{\xi}^{\wedge^2} \equiv \textbf{0},\end{equation}

since $\boldsymbol{\xi}^\wedge$ must satisfy its own characteristic equation.

Finally, for adjoint poses we have that the characteristic equation of $\boldsymbol{\xi}^\curlywedge$ is

(14) \begin{equation}\left| \lambda \textbf{1} - \boldsymbol{\xi}^\curlywedge \right| = \left| \begin{matrix} \lambda \textbf{1} - \boldsymbol{\phi}^\wedge & -\boldsymbol{\rho}^\wedge \\ \\[-7pt] \textbf{0} & \lambda \textbf{1} - \boldsymbol{\phi}^\wedge \end{matrix} \right| = \left( \lambda^3 + \phi^2 \lambda \right)^2 = \lambda^6 + 2 \phi^2 \lambda^4 + \phi^4 \lambda^2 = 0.\end{equation}

We could again employ the Cayley–Hamilton theorem to create a similar identity to the other two cases, but it turns out that the minimal polynomial of $\boldsymbol{\xi}^\curlywedge$ is actually [ Reference Barfoot5,Reference D’Eleuterio and Barfoot16],

(15) \begin{equation}\boldsymbol{\xi}^{\curlywedge^5} + 2\phi^2 \boldsymbol{\xi}^{\curlywedge^3} + \phi^4 \boldsymbol{\xi}^\curlywedge \equiv \textbf{0},\end{equation}

which is one order lower than the characteristic equation in this case. This is because both the algebraic and geometric multiplicities of eigenvalue $\lambda=0$ in (14) are two (two Jordan blocks of size one), allowing us to drop one copy when constructing the minimal polynomial; in (12), the algebraic multiplicity of $\lambda = 0$ is two, while the geometric multiplicity is one (one Jordan block of size two).

3.1. Vector mappings for rotations

It is well known that there exist many different ways to parameterize the group of three-dimensional rotations, SO(3). Naturally, such rotations have three degrees of freedom. Representations with three parameters are known to always have singularities, while representations with more than three parameters have constraints to keep the degrees of freedom at three [ Reference Hughes8,Reference Stuelpnagel17]. Bauchau and Trainelli [Reference Bauchau and Trainelli9] synthesized previous work on three-parameter representations that can be considered vectors; they show that all such parameterizations can be written as

(16) \begin{equation}\boldsymbol{\phi} = \phi(\varphi) \, \textbf{a} \in \mathbb{R}^3,\end{equation}

where $\textbf{a}$ is the unit-length axis of rotation, $\varphi$ the angle of rotation, and $\phi(\varphi)$ a generating function. The scalar generating function is an odd function of the rotation angle that satisfies

(17) \begin{equation}\lim_{\varphi \rightarrow 0} \frac{\phi(\varphi)}{\varphi} = \kappa,\end{equation}

with $\kappa$ a real normalization constant. We will assume $\kappa = 1$ in what follows as this makes it easier to compare different generating functions directly. The generating function is essentially a nonlinear warping of one angle, $\varphi$ , to new angle, $\phi$ .

Some common examples of vector parameterizations of rotation are

(18) \begin{equation}\underbrace{\boldsymbol{\phi} = \varphi \textbf{a}}_{\mbox{rotation vector}}, \quad \underbrace{\boldsymbol{\phi} = 2 \tan \frac{\varphi}{2} \textbf{a}}_{\mbox{Cayley-Gibbs-Rodrigues}}, \quad \underbrace{\boldsymbol{\phi} = 4 \tan \frac{\varphi}{4} \textbf{a}}_{\mbox{modified Rodrigues}},\end{equation}

where we have ensured $\kappa = 1$ in (17) by including normalization constants where necessary. Notably, the Euler angle sequences, a common three-parameter representation, do not fit the vector definition since they are not parallel to the axis of rotation. Vector parameterizations come along with singularities but these will not be a roadblock for the way in which we intend to use them. Figure 1 depicts several generating functions.

Figure 1.

Example generating functions, $\phi(\varphi)$ , that map rotation angle, $\varphi$ , to different vector representations of rotation and pose. The independent variable is shown on the vertical axis to save space. We see that the $2\tan\frac{\varphi}{2}$ function is quite a bit more extreme than the others.

Bauchau and Trainelli [Reference Bauchau and Trainelli9] show that the corresponding rotation matrix, $\textbf{C} \in SO(3)$ , can be constructed from the general vector parameterization as follows:

(19) \begin{equation}\textbf{C}(\boldsymbol{\phi}) = \textbf{1} + \frac{\nu^2}{\epsilon}\boldsymbol{\phi}^\wedge + \frac{\nu^2}{2}\boldsymbol{\phi}^{\wedge^2},\end{equation}

where

(20) \begin{equation}\nu(\varphi) = \frac{2 \sin(\varphi/2)}{\phi(\varphi)}, \quad \epsilon(\varphi) = \frac{2 \tan(\varphi/2)}{\phi(\varphi)}.\end{equation}

The group-mapped-from-vector function is a surjective-only mapping from $\mathbb{R}^3$ to SO(3), which means every rotation matrix can be generated by many vectors (owing to the rotation angle wrapping around).

The expression for the SO(3) Jacobian, $\textbf{J}(\boldsymbol{\phi})$ , that maps the derivative of the vector parameters to angular velocity,

(21) \begin{equation}\boldsymbol{\omega} = \textbf{J}(\boldsymbol{\phi}) \dot{\boldsymbol{\phi}},\end{equation}

is given by ref. [Reference Bauchau and Trainelli9]

(22) \begin{equation}\textbf{J}(\boldsymbol{\phi}) = \mu \textbf{1} + \frac{\nu^2}{2} \boldsymbol{\phi}^\wedge + \frac{1}{\phi^2}\left( \mu - \frac{\nu^2}{\epsilon}\right) \boldsymbol{\phi}^{\wedge^2},\end{equation}

where

(23) \begin{equation}\mu(\varphi) = \left(\frac{d\phi(\varphi)}{d\varphi}\right)^{-1}.\end{equation}

The inverse of this Jacobian is given by ref. [Reference Bauchau and Trainelli9]

(24) \begin{equation}\textbf{J}(\boldsymbol{\phi})^{-1} = \frac{1}{\mu} \textbf{1} - \frac{1}{2} \boldsymbol{\phi}^\wedge - \frac{1}{\phi^2} \left( \frac{1}{\epsilon} - \frac{1}{\mu} \right)\boldsymbol{\phi}^{\wedge^2},\end{equation}

which we will require when dealing with SE(3).

As the rotation angle, $\varphi$ , becomes small, we have the following infinitesimal expressions:

(25) \begin{equation}\textbf{C}(\boldsymbol{\phi}) \approx \textbf{1} + \boldsymbol{\phi}^\wedge, \quad \textbf{J}(\boldsymbol{\phi}) \approx \textbf{1} + \frac{1}{2} \boldsymbol{\phi}^\wedge.\end{equation}

These will become important when considering perturbations.

The result from this section is general, but it will be instructive to consider some specific examples. Table 1 provides some common vector parameterizations and their associated mappings to SO(3). The first three mappings are appealing in that they do not require calculating the angle of rotation, $\varphi$ , and instead can be built directly from $\boldsymbol{\phi}$ .

Table 1.

Example vector parameterizations of rotation and the associated mappings to SO(3). Note the connection to the Cayley transformation in the $\tan$ parameterizations. The entries in the table are ordered from steepest to shallowest generating function (right to left in Figure 1)

3.2. Vector mappings for poses

We next extend the idea of generalized vector mappings to SE(3). As pose changes have six degrees of freedom, our vector parameterization will take the form

(26) \begin{equation}\boldsymbol{\xi} = \left( \begin{array}{c} \boldsymbol{\rho} \\ \\[-7pt] \boldsymbol{\phi} \end{array}\right) \in \mathbb{R}^6,\end{equation}

where the rotational parameters, $\boldsymbol{\phi} \in \mathbb{R}^3$ , are the same as for SO(3) and $\boldsymbol{\rho} \in \mathbb{R}^3$ are the translational parameters (not necessarily equivalent to the actual translation in Euclidean space, $\textbf{r}$ ). We would like to generalize the SO(3) result such that we have a surjective-only mapping from $\mathbb{R}^6$ to SE(3).

Motivated by the Rodrigues-type form of (19) above, we define a general vector pose mapping to be of the form

(27) \begin{equation}\textbf{T}(\boldsymbol{\xi}) = \textbf{1} + a\, \boldsymbol{\xi}^{\wedge} + b\, \boldsymbol{\xi}^{\wedge^2} + c\, \boldsymbol{\xi}^{\wedge^3},\end{equation}

for some a, b, and c such that the result is an element of SE(3). We know that higher-order powers of $\boldsymbol{\xi}^\wedge$ are unnecessary owing to (13), which comes from the Cayley–Hamilton theorem.

It is not immediately obvious that this definition of a pose vector parameterization for SE(3) is consistent with Bauchau [Reference Bauchau12, Section 14.4.1], who notes that we can define such a mapping for $\mbox{Ad}(SE(3))$ as one that is parallel to an eigenvector of the resulting adjoint pose matrix associated with the unit eigenvalue. It turns out that the two definitions are equivalent and we will demonstrate this shortly after first examining the conditions on a, b, and c to ensure $\textbf{T} \in SE(3)$ .

Using the definition of $\boldsymbol{\xi}^\wedge$ , we can manipulate our pose mapping into the form,

(28) \begin{equation}\textbf{T}(\boldsymbol{\xi}) = \left( \begin{array}{c@{\quad}c} \textbf{C}(\boldsymbol{\phi}) & \textbf{D}(\boldsymbol{\phi}) \boldsymbol{\rho} \\ \\[-7pt] \textbf{0}^T & 1 \end{array}\right) = \left( \begin{array}{c@{\quad}c} \textbf{1} + (a - \phi^2 c ) \,\boldsymbol{\phi}^\wedge + b \,\boldsymbol{\phi}^{\wedge^2} & \left( a \textbf{1} + b \,\boldsymbol{\phi}^\wedge + c \,\boldsymbol{\phi}^{\wedge^2} \right) \boldsymbol{\rho} \\ \\[-7pt] \textbf{0}^T & 1 \end{array}\right),\end{equation}

where we used (11) to simplify the top-left entry.

Looking to (4), the top-left entry in (28) must be the rotation matrix:

(29) \begin{equation}\textbf{C}(\boldsymbol{\phi}) = \textbf{1} + (a - \phi^2 c) \,\boldsymbol{\phi}^\wedge + b \,\boldsymbol{\phi}^{\wedge^2}.\end{equation}

Comparing to (19), the following conditions must hold:

(30) \begin{equation}a - \phi^2 c = \frac{\nu^2}{\epsilon}, \quad b = \frac{\nu^2}{2}.\end{equation}

The second condition leaves no choice, while the first provides some flexibility.

Turning to the top-right entry of (28), the matrix

(31) \begin{equation}\textbf{D}(\boldsymbol{\phi}) = a \textbf{1} + b \,\boldsymbol{\phi}^\wedge + c \,\boldsymbol{\phi}^{\wedge^2},\end{equation}

is the coupling matrix between the translational and rotational degrees of freedom. Because there is flexibility in the choice of a and c, this means there are an infinite number of possibilities for $\textbf{D}(\boldsymbol{\phi})$ that satisfy our pose mapping definition in (27).

Under the conditions in (30), the defined properties of the generating function, and the further condition that $\textbf{D}(\boldsymbol{\phi})$ must be invertible (except at some singularities), we can claim that our chosen mapping in (27) is surjective only from $\mathbb{R}^6$ to SE(3). This is not too hard to see since every $\textbf{C}(\boldsymbol{\phi})$ can be generated by many $\boldsymbol{\phi}$ and every $\textbf{r}$ can be generated by some $\boldsymbol{\rho}$ provided $\textbf{D}(\boldsymbol{\phi})$ invertible. The inverse map is discussed further in Section 3.6.

Returning to the question of consistency of our definition of pose vector parameterizations with that of Bauchau [Reference Bauchau12], we want to show that $\boldsymbol{\xi}$ is an eigenvector of $\boldsymbol{\mathcal{T}} = \mbox{Ad}(\textbf{T})$ associated with a unit eigenvalue, that is,

(32) \begin{equation}\;{\boldsymbol{\mathcal{T}}} \boldsymbol{\xi} = \boldsymbol{\xi}.\end{equation}

Filling in the details of $\boldsymbol{\mathcal{T}}$ from (7), we have

(33) \begin{equation}\;{\boldsymbol{\mathcal{T}}} \boldsymbol{\xi} = \left( \begin{array}{c@{\quad}c} \textbf{C} & \left( \textbf{D} \boldsymbol{\rho} \right)^\wedge \textbf{C} \\ \\[-7pt] \textbf{0} & \textbf{C} \end{array}\right) \left( \begin{array}{c} \boldsymbol{\rho} \\ \\[-7pt] \boldsymbol{\phi} \end{array}\right) = \left( \begin{array}{c} \left( \textbf{C} - \boldsymbol{\phi}^\wedge \textbf{D} \right) \boldsymbol{\rho} \\ \\[-7pt] \boldsymbol{\phi} \end{array}\right),\end{equation}

where we have used $\textbf{C} \boldsymbol{\phi} = \boldsymbol{\phi}$ . Using (29), (31), and (11), we see that $\textbf{C} - \boldsymbol{\phi}^\wedge \textbf{D} = \textbf{1}$ so that $\boldsymbol{\mathcal{T}} \boldsymbol{\xi} = \boldsymbol{\xi}$ . Thus, our SE(3) definition of pose vector parameterizations is equivalent to that of Bauchau [Reference Bauchau12], for $\mbox{Ad}(SE(3))$ .

The nonuniqueness of the pose parameterization that we discussed earlier can also be explained from the eigenstructure, since there are two linearly independent eigenvectors for $\boldsymbol{\mathcal{T}}$ associated with its two unit eigenvalues [ Reference Bauchau12,Reference D’Eleuterio and Barfoot16]; the SE(3) pose vector parameterization therefore has a free parameter, as we showed by another means.

3.3. Screws, pose adjoints, and commutative mappings

As discussed in the previous section, we must choose values for the parameters, a, b, and c to construct the rotation matrix in (29). However, we have a free parameter in making this choice, as long as the parameters satisfy (30). One possibility is to choose

(34) \begin{equation}a = \mu, \quad b = \frac{\nu^2}{2}, \quad c = \frac{1}{\phi^2}\left( \mu - \frac{\nu^2}{\epsilon}\right),\end{equation}

which satisfy the conditions in (30) and importantly this particular choice means that we can write our vector pose parameter as [5, eq. 14.34]

(35) \begin{equation}\boldsymbol{\xi} = \phi(\varphi) \, \textbf{s},\end{equation}

where $\textbf{s}$ is the constant screw axis of the Mozzi–Chasles theorem. The screw can be written in the form

(36) \begin{equation}\textbf{s} = \left( \begin{array}{c} p \,\textbf{a} + \textbf{m}^\wedge \textbf{a} \\ \\[-7pt] \textbf{a} \end{array}\right),\end{equation}

where p is the (scalar) pitch, $\textbf{a}$ is the (unit) axis of rotation, and $\textbf{m}$ is the moment arm; $(\textbf{m}^\wedge \textbf{a}, \textbf{a})$ are the Plücker coordinates. The relationship in (35) parallels the SO(3) definition of ref. [Reference Bauchau and Trainelli9] captured in (16); we have the product of the generating function and a (screw) axis. In the case that $\phi(\varphi) = \varphi$ , then $\boldsymbol{\xi} = \varphi \textbf{s}$ is the familiar twist. We underscore that it is the specific selection of a and c in (34) that has made this connection; other choices for a and c will not result in a constant screw.

The selection of a and c in (34) means that the pose mapping becomes

(37) \begin{equation}\textbf{T}(\boldsymbol{\xi}) = \textbf{1} + \mu \, \boldsymbol{\xi}^{\wedge} + \frac{\nu^2}{2} \boldsymbol{\xi}^{\wedge^2} + \frac{1}{\phi^2}\left( \mu - \frac{\nu^2}{\epsilon}\right) \boldsymbol{\xi}^{\wedge^3}.\end{equation}

As the rotation angle, $\varphi$ , becomes small, we have the infinitesimal expression,

(38) \begin{equation}\textbf{T}(\boldsymbol{\xi}) \approx \textbf{1} + \boldsymbol{\xi}^\wedge,\end{equation}

which will become important when considering perturbations.

Choosing a and c as in (34) also has the effect of selecting

(39) \begin{equation}\textbf{D}(\boldsymbol{\phi}) = \textbf{J}(\boldsymbol{\phi}),\end{equation}

the Jacobian of SO(3) for the chosen parameterization [Reference Bauchau12], which can be verified by substituting into (22). Choosing $\textbf{D}(\boldsymbol{\phi}) = \textbf{J}(\boldsymbol{\phi})$ gives our SE(3) mapping an important property related to pose adjoints. With these definitions, we can establish the following desirable commutative relationship:

(40)

This means we can transform our vector parameterization of pose to its adjoint in two equivalent ways.

To verify this commutative property, we can write the series form for the adjoint mapping as

(41) \begin{equation}\;{\boldsymbol{\mathcal{T}}}(\boldsymbol{\xi}) = \textbf{1} + d \, \boldsymbol{\xi}^\curlywedge + e \, \boldsymbol{\xi}^{\curlywedge^2} + f \, \boldsymbol{\xi}^{\curlywedge^3} + g \, \boldsymbol{\xi}^{\curlywedge^4},\end{equation}

for some unknowns, d, e, f, g. We do not need higher-order terms in this Rodrigues-type series owing to (15), which can be used to reduce quintic and higher terms. Multiplying out and comparing terms between (7) and (41), we find that

(42) \begin{equation}d = a + \phi^2 (f - c), \quad e = \phi^2 g + b, \quad f = \frac{1}{2}\left( ab-c \right), \quad g = \frac{1}{2} \left( b^2 - c(a-\phi^2 c) \right),\end{equation}

in terms of the earlier choices for a, b, and c. There may also be other choices for a and c (recall these are not unique) that impose the commutative relationship for SE(3), but this was not explored further.

Owing to smoothness of the generating function and its convergence to $\varphi$ in the limit of an infinitesimally small rotation, one might be able to argue that our vector mappings offer alternatives (to the exponential map) for the local diffeomorphism from the Lie algebra, $\mathfrak{se}(3)$ , to the Lie group, SE(3). In such a case, by changing the map we are merely “warping” how the Lie algebra maps onto the Lie group.

3.4. Cayley transformations

It is known [ Reference Bauchau and Trainelli9,Reference Borri, Trainelli and Bottasso13] that for the Cayley–Gibbs–Rodrigues parameterization of rotation, $\phi(\varphi) = 2 \tan \frac{\varphi}{2}$ , we can write the rotation matrix in terms of the Cayley transformation:

(43) \begin{equation}\textbf{C}(\boldsymbol{\phi}) = \left( \textbf{1} - \frac{1}{2}\boldsymbol{\phi}^\wedge \right)^{-1} \left( \textbf{1} + \frac{1}{2}\boldsymbol{\phi}^\wedge \right).\end{equation}

In fact, this can be generalized to the case where $\phi(\varphi) = 2m \tan \frac{\varphi}{2m}$ with m a positive integer using higher-order Cayley transformations [Reference Bauchau and Trainelli9]:

(44) \begin{equation}\textbf{C}(\boldsymbol{\phi}) = \left( \textbf{1} - \frac{1}{2m}\boldsymbol{\phi}^\wedge \right)^{-m} \left( \textbf{1} + \frac{1}{2m}\boldsymbol{\phi}^\wedge \right)^m.\end{equation}

However, things become somewhat more complicated for poses.

Borri et al. [Reference Borri, Trainelli and Bottasso13] and later Selig [Reference Selig14] demonstrated that the Cayley transformation can be used to map pose vectors to SE(3) according to

(45) \begin{equation}\textbf{T}(\boldsymbol{\xi}) = \left( \textbf{1} - \frac{1}{2m}\boldsymbol{\xi}^\wedge \right)^{-m} \left( \textbf{1} + \frac{1}{2m}\boldsymbol{\xi}^\wedge \right)^m,\end{equation}

where we have generalized to higher-order transformations. Borri et al. [Reference Borri, Trainelli and Bottasso13] and Selig [Reference Selig14] also show that the Cayley transformation can be used to map pose vectors to $\mbox{Ad}(SE(3))$ according to

(46) \begin{equation}\;{\boldsymbol{\mathcal{T}}}(\boldsymbol{\xi}) = \left( \textbf{1} - \frac{1}{2m}\boldsymbol{\xi}^\curlywedge \right)^{-m} \left( \textbf{1} + \frac{1}{2m}\boldsymbol{\xi}^\curlywedge \right)^m,\end{equation}

where we have generalized again to higher-order transformations. Disappointingly, Selig [Reference Selig14] demonstrates that starting from the same $\boldsymbol{\xi}$ and applying (45) and (46) does not result in an equivalent transformation, that is, $\boldsymbol{\mathcal{T}}(\boldsymbol{\xi}) \neq \mbox{Ad}(\textbf{T}(\boldsymbol{\xi}))$ ; the commutative property for adjoints does not hold.

We can actually say something more than Selig [Reference Selig14]. It turns out that (45) is not equivalent to (37), but (46) is equivalent to (41), for the family of rotation parameterizations, $\phi(\varphi) = 2m \tan \frac{\varphi}{2m}$ . The explanation for this lies in the choice of $\textbf{D}(\boldsymbol{\phi})$ , discussed earlier. For the $6 \times 6$ case, choosing $\textbf{D}(\boldsymbol{\phi}) = \textbf{J}(\boldsymbol{\phi})$ results in the Cayley transformation, while for the $4 \times 4$ case it does not; to connect to the Cayley transformation in the $4 \times 4$ case for $m=1$ , we must pick $\textbf{D}(\boldsymbol{\phi}) = \frac{1}{2}(\textbf{C}(\boldsymbol{\phi}) + \textbf{1}) \neq \textbf{J}(\boldsymbol{\phi})$ , as shown by Selig [Reference Selig14].

To show that (46) holds, consider the $m=1$ case. If we left-multiply by the inverse of the first factor we should have

(47) \begin{equation}\left( \textbf{1} - \frac{1}{2}\boldsymbol{\xi}^\curlywedge \right) \;{\boldsymbol{\mathcal{T}}}(\boldsymbol{\xi}) = \textbf{1} + \frac{1}{2}\boldsymbol{\xi}^\curlywedge.\end{equation}

Multiplying out the left side, we have

(48) \begin{equation}\textbf{1} + \left(d-\frac{1}{2}(1-\phi^4 g)\right) \boldsymbol{\xi}^\curlywedge + \left(e-\frac{1}{2}d\right) \boldsymbol{\xi}^{\curlywedge^2} + \left(f-\frac{1}{2}(e-2\phi^2 g)\right) \boldsymbol{\xi}^{\curlywedge^3} + \left(g-\frac{1}{2}f \right) \boldsymbol{\xi}^{\curlywedge^4} = \textbf{1} + \frac{1}{2}\boldsymbol{\xi}^\curlywedge,\end{equation}

where we have made use of (15) once to reduce a quintic term. Comparing the coefficients of each term it must be that

(49) \begin{equation}d-\frac{1}{2}(1-\phi^4 g) = \frac{1}{2}, \quad e-\frac{1}{2}d = 0, \quad f-\frac{1}{2}(e-2\phi^2 g) = 0, \quad g-\frac{1}{2}f = 0.\end{equation}

Substituting (42) and then (34) confirms these conditions for $\phi(\varphi) = 2 \tan \frac{\varphi}{2}$ .

3.5. Cayley-type factorizations

Following the previous section that focussed on the $\phi(\varphi) = 2m \tan \frac{\varphi}{2m}$ family, we next consider some novel Cayley-type mappings for different generating functions. To our knowledge, these have not been presented before.

It turns out that we can factor any of the vectorial rotation parameterizations according to

(50) \begin{equation}\textbf{C}(\boldsymbol{\phi}) = \left( \textbf{1} - \lambda \boldsymbol{\phi}^\wedge \right)^{-1} \left( \textbf{1} + \gamma \boldsymbol{\phi}^\wedge \right) = \left( \textbf{1} - \gamma \boldsymbol{\phi}^\wedge \right)^{-1} \left( \textbf{1} + \lambda \boldsymbol{\phi}^\wedge \right),\end{equation}

where

(51a) \begin{eqnarray}\gamma & = & \frac{1}{2} \epsilon, \end{eqnarray}

(51b) \begin{eqnarray}\lambda & = & \frac{\nu^2}{\epsilon} - \frac{1}{2} \epsilon \left( 1 - \frac{1}{2} \phi^2 \nu^2 \right).\end{eqnarray}

In the case that $\phi(\varphi) = 2 \tan \frac{\varphi}{2}$ , we recover $\gamma = \lambda = \frac{1}{2}$ . The inverse of $\textbf{C}(\boldsymbol{\phi})$ is obtained by flipping the signs in the factorization:

(52) \begin{equation}\textbf{C}(\boldsymbol{\phi})^{-1} = \textbf{C}(\boldsymbol{\phi})^T = \left( \textbf{1} + \lambda \boldsymbol{\phi}^\wedge \right)^{-1} \left( \textbf{1} - \gamma \boldsymbol{\phi}^\wedge \right) = \left( \textbf{1} + \gamma \boldsymbol{\phi}^\wedge \right)^{-1} \left( \textbf{1} - \lambda \boldsymbol{\phi}^\wedge \right).\end{equation}

The relationships can be verified by simply multiplying out and comparing to (19).

We can also factor some SE(3) parameterizations in an analogous way:

(53a) \begin{eqnarray}\textbf{T}(\boldsymbol{\xi}) & = & \left( \textbf{1} - \lambda \boldsymbol{\xi}^\wedge \right)^{-1} \left( \textbf{1} + \gamma \boldsymbol{\xi}^\wedge \right) = \left( \textbf{1} - \gamma \boldsymbol{\xi}^\wedge \right)^{-1} \left( \textbf{1} + \lambda \boldsymbol{\xi}^\wedge \right), \end{eqnarray}

(53b) \begin{eqnarray}\textbf{T}(\boldsymbol{\xi})^{-1} & = & \left( \textbf{1} + \lambda \boldsymbol{\xi}^\wedge \right)^{-1} \left( \textbf{1} - \gamma \boldsymbol{\xi}^\wedge \right) = \left( \textbf{1} + \gamma \boldsymbol{\xi}^\wedge \right)^{-1} \left( \textbf{1} - \lambda \boldsymbol{\xi}^\wedge \right),\end{eqnarray}

where $\gamma$ and $\lambda$ are again given by (51). To achieve this factorization, we must select $c = \frac{1}{4} \nu^2 \epsilon$ in (30), which consequently selects a $\textbf{D}(\boldsymbol{\phi}) \neq \textbf{J}(\boldsymbol{\phi})$ .

Finally, the pattern holds for $\mbox{Ad}(SE(3))$ where we have

(54a) \begin{eqnarray}\;{\boldsymbol{\mathcal{T}}}(\boldsymbol{\xi}) & = & \left( \textbf{1} - \lambda \boldsymbol{\xi}^\curlywedge \right)^{-1} \left( \textbf{1} + \gamma \boldsymbol{\xi}^\curlywedge \right) = \left( \textbf{1} - \gamma \boldsymbol{\xi}^\curlywedge \right)^{-1} \left( \textbf{1} + \lambda \boldsymbol{\xi}^\curlywedge \right), \end{eqnarray}

(54b) \begin{eqnarray}\;{\boldsymbol{\mathcal{T}}}(\boldsymbol{\xi})^{-1} & = & \left( \textbf{1} + \lambda \boldsymbol{\xi}^\curlywedge \right)^{-1} \left( \textbf{1} - \gamma \boldsymbol{\xi}^\curlywedge \right) = \left( \textbf{1} + \gamma \boldsymbol{\xi}^\curlywedge \right)^{-1} \left( \textbf{1} - \lambda \boldsymbol{\xi}^\curlywedge \right),\end{eqnarray}

where $\gamma$ and $\lambda$ are yet again given by (51). To achieve this, we must select

(55) \begin{equation}d = \phi^2 f + \frac{\nu^2}{\epsilon}, \quad e = \frac{1}{2} \epsilon \phi^2 f + \frac{\nu^2}{2}, \quad f = \frac{\nu^2 \epsilon}{4 + \phi^2 \epsilon^2}, \quad g = \frac{1}{2}\epsilon f,\end{equation}

in (41). As discussed in the previous section, when $\phi(\varphi) = 2 \tan \frac{\varphi}{2}$ this happens to select $\textbf{D}(\boldsymbol{\phi}) = \textbf{J}(\boldsymbol{\phi})$ , but this is not true for the other generating functions.

3.6. Inverse mappings

We briefly discuss how to go the other way from $\textbf{T}$ , or $\boldsymbol{\mathcal{T}}$ , back to $\boldsymbol{\xi}$ in the general case. The first step is to extract $\textbf{C}(\boldsymbol{\phi})$ and $\textbf{r}$ from $\textbf{T}$ , or $\boldsymbol{\mathcal{T}}$ . From $\textbf{C}(\boldsymbol{\phi})$ , we can exploit the facts that

(56) \begin{equation}\mbox{tr}(\textbf{C}(\boldsymbol{\phi})) = 2 \cos\varphi + 1, \quad \textbf{C}(\boldsymbol{\phi}) \textbf{a} = \textbf{a},\end{equation}

which are true independent of the chosen vector parameterization. The first gives us the rotation angle, $\varphi$ , while the second is an eigenproblem that can be solved for axis $\textbf{a}$ (unit-length eigenvector corresponding to eigenvalue of 1); see Hughes [Reference Hughes8] for an explicit formula. Owing to the singularity of the vector parameterization, we choose $\varphi \in (-\pi,\pi]$ . We can then calculate $\boldsymbol{\phi} = \phi(\varphi) \, \textbf{a}$ .

Once we have $\boldsymbol{\phi}$ , we can calculate $\boldsymbol{\rho}$ according to

(57) \begin{equation}\boldsymbol{\rho} = \textbf{D}(\boldsymbol{\phi})^{-1} \textbf{r},\end{equation}

and then assemble $\boldsymbol{\rho}$ and $\boldsymbol{\phi}$ into $\boldsymbol{\xi}$ . In the case of $ \textbf{D}(\boldsymbol{\phi}) = \textbf{J}(\boldsymbol{\phi})$ , the inverse, $\textbf{J}(\boldsymbol{\phi})^{-1}$ , is given by (24) and naturally has singularities of which to be aware.

3.7. Compounding poses

For SO(3), we seek to compound two rotations such that

(58) \begin{equation}\textbf{C}(\boldsymbol{\phi}) = \textbf{C}(\boldsymbol{\phi}_2) \textbf{C}(\boldsymbol{\phi}_1).\end{equation}

Bauchau and Trainelli [Reference Bauchau and Trainelli9] show that the vector parameterization can be directly compounded according to

(59a) \begin{eqnarray}\varphi & = & 2 \cos^{-1} \left( \nu(\varphi_1)\nu(\varphi_2) \left( \frac{1}{\epsilon(\varphi_1)\epsilon(\varphi_2)} - \frac{1}{4} \, \boldsymbol{\phi}_1^T \boldsymbol{\phi}_2 \right)\right), \end{eqnarray}

(59b) \begin{eqnarray}\boldsymbol{\phi} & = & \frac{\nu(\varphi_1) \nu(\varphi_2)}{\nu(\varphi)} \left( \frac{1}{\epsilon(\varphi_2)}\boldsymbol{\phi}_1 + \frac{1}{\epsilon(\varphi_1)}\boldsymbol{\phi}_2 - \frac{1}{2} \boldsymbol{\phi}_1^\wedge \boldsymbol{\phi}_2 \right) .\end{eqnarray}

The first of these is used to calculate the compound rotation angle, $\varphi$ , while the second provides the complete vectorial parameterization, $\boldsymbol{\phi}$ .

For SE(3), Condurache and Ciureanu [Reference Condurache and Ciureanu18] recently discussed a closed-form solution for compounding two pose vectors in the case that $\boldsymbol{\phi} = \varphi \textbf{a}$ . Bauchau and Choi (2003) showed that we can easily generalize this result to any of our vectorial pose parameterizations. The compounding of two poses is

(60) \begin{align}\textbf{T}(\boldsymbol{\xi}) = \left( \begin{array}{c@{\quad}c} \textbf{C}(\boldsymbol{\phi}) & \textbf{D}(\boldsymbol{\phi}) \boldsymbol{\rho} \\ \\[-7pt] \textbf{0}^T & 1 \end{array}\right) = \textbf{T}(\boldsymbol{\xi}_2) \textbf{T}(\boldsymbol{\xi}_1) & = \left( \begin{array}{c@{\quad}c} \textbf{C}(\boldsymbol{\phi}_2) & \textbf{D}(\boldsymbol{\phi}_2) \boldsymbol{\rho}_2 \\ \\[-7pt] \textbf{0}^T & 1 \end{array}\right) \left( \begin{array}{c@{\quad}c} \textbf{C}(\boldsymbol{\phi}_1) & \textbf{D}(\boldsymbol{\phi}_1) \boldsymbol{\rho}_1 \\ \\[-7pt] \textbf{0}^T & 1 \end{array}\right)\nonumber\\ &= \left( \begin{array}{c@{\quad}c} \textbf{C}(\boldsymbol{\phi}_2) \textbf{C}(\boldsymbol{\phi}_1) & \textbf{C}(\boldsymbol{\phi}_2) \textbf{D}(\boldsymbol{\phi}_1)\boldsymbol{\rho}_1 + \textbf{D}(\boldsymbol{\phi}_2) \boldsymbol{\rho}_2 \\ \\[-7pt] \textbf{0}^T & 1 \end{array}\right). \end{align}

Comparing entries, we see that

(61a) \begin{eqnarray}\textbf{C}(\boldsymbol{\phi}) & =& \textbf{C}(\boldsymbol{\phi}_2) \textbf{C}(\boldsymbol{\phi}_1),\qquad\qquad\qquad\qquad\qquad\quad\qquad \end{eqnarray}

(61b) \begin{eqnarray}\boldsymbol{\rho} & = & \textbf{D}(\boldsymbol{\phi})^{-1} \textbf{C}(\boldsymbol{\phi}_2) \textbf{D}(\boldsymbol{\phi}_1)\boldsymbol{\rho}_1 + \textbf{D}(\boldsymbol{\phi})^{-1} \textbf{D}(\boldsymbol{\phi}_2) \boldsymbol{\rho}_2 .\end{eqnarray}

We can use the approach of Bauchau and Trainelli [Reference Bauchau and Trainelli9] to calculate the compound rotation, $\boldsymbol{\phi}$ , and then calculate $\boldsymbol{\rho}$ afterwards. We require an expression for $ \textbf{D}(\boldsymbol{\phi})^{-1}$ , which in the case of $ \textbf{D}(\boldsymbol{\phi}) = \textbf{J}(\boldsymbol{\phi})$ is given by (24).

3.8. Additional useful properties of vector mappings

There are a few other important properties of our vector mappings of which we frequently make use when manipulating expressions in state-estimation problems. Owing to the well-known identity [Reference Barfoot5],

(62) \begin{equation}\left( \textbf{C} \textbf{v} \right)^\wedge \equiv \textbf{C} \textbf{v}^\wedge \textbf{C}^T,\end{equation}

where $\textbf{C} \in SO(3)$ and $\textbf{v} \in \mathbb{R}^3$ , we have that

(63) \begin{equation}\textbf{C}_1 \textbf{C}_2(\boldsymbol{\phi}_2) \textbf{C}_1^T \equiv \textbf{C}_2\left( \textbf{C}_1 \boldsymbol{\phi}_2 \right).\end{equation}

The proof is as follows:

(64) \begin{align}\textbf{C}_1 \textbf{C}_2(\boldsymbol{\phi}_2) \textbf{C}_1^T = \textbf{C}_1 \left( \textbf{1} + \alpha \, \boldsymbol{\phi}_2^\wedge + \beta \boldsymbol{\phi}_2^{\wedge^2} \right) \textbf{C}_1^T & = \textbf{1} + \alpha \, \textbf{C}_1 \boldsymbol{\phi}_2^\wedge \textbf{C}_1^T + \beta \, \textbf{C}_1 \boldsymbol{\phi}_2^\wedge \textbf{C}_1 \, \textbf{C}_1^T \boldsymbol{\phi}_2^\wedge \textbf{C}_1^T \nonumber\\ &= \textbf{1} + \alpha \left( \textbf{C}_1 \boldsymbol{\phi}_2 \right)^\wedge + \beta \left( \textbf{C}_1 \boldsymbol{\phi}_2 \right)^{\wedge^2} = \textbf{C}_2\left( \textbf{C}_1 \boldsymbol{\phi}_2 \right), \end{align}

where $\alpha$ and $\beta$ are standing in for the coefficients in (19).

For SE(3), we can use the identities [Reference Barfoot5],

(65) \begin{align}\left( \!\! \;{\boldsymbol{\mathcal{T}}} \textbf{x} \right)^\wedge \equiv \textbf{T} \textbf{x}^\wedge \textbf{T}^{-1}, \end{align}

(66) \begin{align} \left( \!\!\;{\boldsymbol{\mathcal{T}}} \textbf{x} \right)^\curlywedge \equiv \;{\boldsymbol{\mathcal{T}}} \textbf{x}^\curlywedge \;{\boldsymbol{\mathcal{T}}}^{-1}\end{align}

with $\textbf{T} \in SE(3)$ , $\boldsymbol{\mathcal{T}} = \mbox{Ad}(\textbf{T})$ , $\textbf{x} \in \mathbb{R}^6$ to establish

(67) \begin{align}\textbf{T}_1 \textbf{T}_2(\boldsymbol{\xi}_2) \textbf{T}_1^{-1} \equiv \textbf{T}_2 \left( \!\!\;{\boldsymbol{\mathcal{T}}}_1 \boldsymbol{\xi}_2 \right), \end{align}

(68) \begin{align} \;{\boldsymbol{\mathcal{T}}}_1 \;{\boldsymbol{\mathcal{T}}}_2(\boldsymbol{\xi}_2) \;{\boldsymbol{\mathcal{T}}}_1^{-1} \equiv \;{\boldsymbol{\mathcal{T}}}_2 \left( \!\!\;{\boldsymbol{\mathcal{T}}}_1 \boldsymbol{\xi}_2 \right),\end{align}

where the proofs are similar to (65).

4.1. Interpolation

Pose interpolation has been discussed extensively in the literature; Han and Bauchau [ Reference Han and Bauchau19,Reference Han and Bauchau20] provide some representative examples. Here our goal is not to claim a new method of pose interpolation but rather to show the effect of simply changing the generating function on a basic interpolation scheme defined in the vector space.

We can define a “linear” interpolation scheme for poses using any of our vector parameterizations:

(69) \begin{equation}\boldsymbol{\xi} = (1-s) \, \boldsymbol{\xi}_{\textrm{final}} + s \, \boldsymbol{\xi}_{\textrm{initial}},\end{equation}

where $\boldsymbol{\xi}_\textrm{initial}$ is an initial pose, $\boldsymbol{\xi}_{\textrm{final}}$ a final pose, and $s \in [0,1]$ the interpolation variable. As we vary s from 0 to 1, the vector, $\boldsymbol{\xi}$ , will vary and so too will $\textbf{T}(\boldsymbol{\xi})$ . This allows the smooth transition from one pose to another.

Figure 2 provides a qualitative example of this scheme for all five generating functions, $\phi(\varphi)$ , listed in Table 1 with $\textbf{D}(\boldsymbol{\phi}) = \textbf{J}(\boldsymbol{\phi})$ . The paths taken each correspond to a geodesic in $\mathbb{R}^6$ , but when mapped to SE(3) are quite different. Notably, none of the paths are straight lines in Euclidean space owing to the coupling between translation and rotation. As the rotational motion becomes smaller, the paths followed by the different generating functions look more similar owing to (17). Looking more closely at Figure 2, we can also observe that the spacing of the steps is quite different for the different generating functions, even though we stepped s uniformly. The $\tan$ generating functions take larger steps at the beginning but smaller ones at the end, while the $\sin$ ones do the opposite, and the $\phi(\varphi) = \varphi$ function keeps the steps even throughout. Naturally, we can make the steps in the interpolation variable, s, as small as we like to obtain smooth interpolation; we chose fairly large steps to keep the plot in Figure 2 from becoming too cluttered.

Figure 2.

An example of “linear” interpolation between an initial pose (green) and final pose (red) for different vector parameterizations of pose. The paths are straight lines in $\mathbb{R}^6$ but when mapped to SE(3) produce different curves. Each parameterization takes a unique path with the $\phi(\varphi) = 2\tan\frac{\varphi}{2}$ generating function being the most distinct. The figure compares the path taken by each of the generating functions from Figure 1 (same color scheme) as well as displays the full pose trajectory individually for three generating functions.

4.2. Pose servoing control

Pose servoing is also a well-studied topic. Here, we simply want to show the effect of changing the pose mapping on a simple control law defined in the vector space. We define a “linear” control law to servo onto a reference path. Consider a reference pose, $\textbf{T}_{\textrm{ref}}$ , that we want to track. We can compare our current pose, $\textbf{T}(\boldsymbol{\xi})$ , to this reference and put it through the inverse mapping to create an “error vector”:

(70) \begin{equation}\boldsymbol{\xi}_{\textrm{error}} \leftarrow \textbf{T}(\boldsymbol{\xi}) \textbf{T}_{\textrm{ref}}^{-1},\end{equation}

where the left arrow stands in for the inverse of any of our pose mappings. We can then adjust our pose to be slightly closer to the reference pose (which is moving) using a simple control law:

(71) \begin{equation}\dot{\boldsymbol{\xi}} = -\kappa \, \boldsymbol{\xi}_{\textrm{error}}.\end{equation}

with $\kappa >0$ a gain. As the error $\boldsymbol{\xi}_{\textrm{error}}$ approaches zero, the moving pose, $\textbf{T}(\boldsymbol{\xi})$ , will approach the reference pose, $\textbf{T}_{\textrm{ref}}$ .

Figure 3 provides an example of servoing to a reference pose that is moving straight along the x-axis, starting from a poor initial pose that is translated and rotated from the reference. We kept the vector control law (including the gain) identical and varied only the choice of pose mapping. As expected, we see different transient behavior resulting from the different pose mappings as they seek the reference.

Figure 3.

An example of pose servoing control using a “linear” controller in pose vector space. Once mapped to SE(3), we see different behaviors from the different pose mappings. In this example, the reference pose is moving along the straight black line and there is an initial error. The figure compares the path taken to join the reference by each of the generating functions from Figure 1 (same color scheme) as well as displays the full pose trajectory individually for three generating functions.

4.3. Pointcloud alignment

We use this section to discuss aligning two clouds of three-dimensional points. Our problem is similar to that of Barfoot et al. [Reference Barfoot, Forbes and Furgale21], but formulated directly on SE(3).

4.3.1. Problem setup

Let us consider that we have two sets of observations of some three-dimensional points, $\textbf{p}_j$ and $\textbf{q}_j$ , expressed in two different reference frames. We assume the points are expressed in homogeneous format,

(72) \begin{equation}\textbf{p} = \left( \begin{array}{c} \textbf{v} \\ 1 \end{array}\right), \quad \textbf{v} = \left( \begin{array}{c} x \\ y \\ z \end{array}\right) .\end{equation}

Our goal is to find the rigid-body transformation, $\textbf{T}$ , that minimizes the following sum-of-squares cost function,

(73) \begin{equation}J(\textbf{T}) = \frac{1}{2} \sum_j \textbf{e}_j^T \textbf{W}_j \textbf{e}_j,\end{equation}

where

(74) \begin{equation}\textbf{e}_j = \textbf{q}_j - \textbf{T} \, \textbf{p}_j,\end{equation}

and $\textbf{W}_j > 0$ is a matrix weight for each associated point pair.

4.3.2. General perturbation solution

Owing to the matrix weights, there is no closed-form solution to this optimization problem and we must turn to an iterative scheme [Reference Barfoot5]. If we have some initial guess (or operating point) for our pose solution, $\textbf{T}_\textrm{op}$ , we can seek an optimal perturbation, $\delta\textbf{T}(\boldsymbol{\xi})$ , that iteratively reduces our cost function. Any of the pose mappings discussed earlier could be used in this regard:

(75) \begin{equation}\textbf{T} = \delta\textbf{T}(\boldsymbol{\xi}) \, \textbf{T}_\textrm{op} \approx \left( \textbf{1} + \boldsymbol{\xi}^\wedge \right) \textbf{T}_\textrm{op},\end{equation}

where notably the infinitesimal expression is the same for all our pose mappings. Inserting this into the error, we have

(76) \begin{equation}\textbf{e}_j \approx \textbf{q}_j - \left( \textbf{1} + \boldsymbol{\xi}^\wedge \right) \textbf{T}_\textrm{op} \textbf{p}_j = \underbrace{\textbf{q}_j - \textbf{T}_\textrm{op} \textbf{p}_j}_{\textbf{e}_{j,\textrm{op}}} - \left( \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \boldsymbol{\xi} = \textbf{e}_{j,\textrm{op}} - \left( \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \boldsymbol{\xi},\end{equation}

where we have made use of $\boldsymbol{\xi}^\wedge \textbf{p} \equiv \textbf{p}^\odot \boldsymbol{\xi}$ [ Reference Barfoot5,Reference Barfoot and Furgale6] with

(77) \begin{equation}\textbf{p}^\odot = \left( \begin{array}{c@{\quad}c} \textbf{1} & -\textbf{v}^\wedge \\ \\[-7pt] \textbf{0}^T & \textbf{0}^T \end{array}\right).\end{equation}

Our approximate error expression is now linear and inserting it into the cost function we have

(78) \begin{equation}J(\boldsymbol{\xi}) \approx \frac{1}{2} \sum_j \left( \textbf{e}_{j,\textrm{op}} - \left( \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \boldsymbol{\xi} \right)^T \textbf{W}_j \left( \textbf{e}_{j,\textrm{op}} - \left( \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \boldsymbol{\xi} \right),\end{equation}

which is exactly quadratic in $\boldsymbol{\xi}$ , and can be minimized by solving the following linear system of equations for $\boldsymbol{\xi}$ :

(79) \begin{equation}\left(\sum_j \left( \textbf{T}_\textrm{op} \textbf{p}_j \right)^{\odot^T} \textbf{W}_j \left( \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \right) \, \boldsymbol{\xi} = \sum_j \left( \textbf{T}_\textrm{op} \textbf{p}_j \right)^{\odot^T} \textbf{W}_j \textbf{e}_{j,\textrm{op}}.\end{equation}

After solving for the optimal $\boldsymbol{\xi}$ , we can update our initial guess using our pose mapping of choice,

(80) \begin{equation}\textbf{T}_\textrm{op} \leftarrow \delta\textbf{T}(\boldsymbol{\xi}) \, \textbf{T}_\textrm{op},\end{equation}

and iterate to convergence. This approach is discussed in detail by Barfoot [Reference Barfoot5] for the $\phi(\varphi) = \varphi$ case, while here we show it generalizes to any vectorial mapping of pose.

4.3.3. Alternate Cayley perturbation solution

The previous section relied on using the infinitesimal expression for pose at each iteration. Here, we take a slightly different strategy based on the Cayley transformation that is similar in spirit to that of refs. [ Reference Junkins, Majji, Macomber, Davis, Doebbler and Nosterk22–Reference Wong, Singla, Lee and Majji27]. We will refer to the resulting algorithm as CayPer. If we choose the $\phi(\varphi) = 2 \tan \frac{\varphi}{2}$ generating function, we can then let our perturbation to the operating point be

(81) \begin{equation}\delta \textbf{T}(\boldsymbol{\xi}) = \left( \textbf{1} - \frac{1}{2} \boldsymbol{\xi}^\wedge \right)^{-1} \left( \textbf{1} + \frac{1}{2} \boldsymbol{\xi}^\wedge \right).\end{equation}

Our point error is then

(82) \begin{equation}\textbf{e}_j = \textbf{q}_j - \left( \textbf{1} - \frac{1}{2} \boldsymbol{\xi}^\wedge \right)^{-1} \left( \textbf{1} + \frac{1}{2} \boldsymbol{\xi}^\wedge \right) \textbf{T}_\textrm{op} \, \textbf{p}_j.\end{equation}

We can then define a new error, $\textbf{e}_j^\prime$ , by premultiplying by $ \left( \textbf{1} - \frac{1}{2} \boldsymbol{\xi}^\wedge \right)$ :

(83) \begin{equation}\textbf{e}_j^\prime = \left( \textbf{1} - \frac{1}{2} \boldsymbol{\xi}^\wedge \right) \textbf{e}_j = \left( \textbf{1} - \frac{1}{2} \boldsymbol{\xi}^\wedge \right) \textbf{q}_j - \left( \textbf{1} + \frac{1}{2} \boldsymbol{\xi}^\wedge \right) \textbf{T}_\textrm{op} \textbf{p}_j = \textbf{e}_{j,\textrm{op}} - \frac{1}{2}\left( \textbf{q}_j + \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \, \boldsymbol{\xi}.\end{equation}

Importantly, we have that

(84) \begin{equation}\lim_{\boldsymbol{\xi} \rightarrow \textbf{0}} \textbf{e}^\prime_j = \textbf{e}_j.\end{equation}

This suggests that we can define a new cost function,

(85) \begin{align}J^\prime(\boldsymbol{\xi}) &= \frac{1}{2} \sum_j \textbf{e}_j^{\prime^T} \textbf{W}_j \textbf{e}_j^\prime \nonumber\\ &\approx \frac{1}{2} \sum_j \left( \textbf{e}_{j,\textrm{op}} - \frac{1}{2}\left( \textbf{q}_j + \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \, \boldsymbol{\xi} \right)^T \textbf{W}_j \left( \textbf{e}_{j,\textrm{op}} - \frac{1}{2}\left( \textbf{q}_j + \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \, \boldsymbol{\xi} \right), \end{align}

which we minimize at each iteration instead of (78). The optimal perturbation, $\frac{1}{2}\boldsymbol{\xi}$ , is this time the solution to the linear system,

(86) \begin{equation}\left(\sum_j \left( \textbf{q}_j + \textbf{T}_\textrm{op} \textbf{p}_j \right)^{\odot^T} \textbf{W}_j \left( \textbf{q}_j + \textbf{T}_\textrm{op} \textbf{p}_j \right)^\odot \right) \, \frac{1}{2} \boldsymbol{\xi} = \sum_j \left( \textbf{q}_j + \textbf{T}_\textrm{op} \textbf{p}_j \right)^{\odot^T} \textbf{W}_j \textbf{e}_{j,\textrm{op}}.\end{equation}

The update to the operating point is then

(87) \begin{equation}\textbf{T}_\textrm{op} \leftarrow \left( \textbf{1} - \frac{1}{2} \boldsymbol{\xi}^\wedge \right)^{-1} \left( \textbf{1} + \frac{1}{2} \boldsymbol{\xi}^\wedge \right) \, \textbf{T}_\textrm{op},\end{equation}

which requires only linear algebraic operations (e.g., no trigonometric functions). Notably, on the first iteration with $\textbf{T}_\textrm{op} = \textbf{1}$ , the CayPer approach becomes a one-shot weighted optimal linear attitude and translation estimator [ Reference Junkins, Majji, Macomber, Davis, Doebbler and Nosterk22–Reference Wong, Singla, Lee and Majji27] directly on SE(3). However, we can also continue to iterate to convergence of the original problem.

4.3.4. Experiment

We conducted a simulated pointcloud alignment experiment motivated by aligning two sets of matched features from a stereo camera. Our approach is very similar in nature to that of Maimone et al. [Reference Maimone, Cheng and Matthies28] and Barfoot et al. [Reference Barfoot, Forbes and Furgale21] in that we align matched stereo points in Euclidean space rather than image space.

We generated feature locations in image space, $\textbf{y}$ , by observing a set of static landmarks using a stereo camera model:

(88) \begin{equation}\textbf{y} = \left( \begin{array}{c} u_\ell \\\\[-7pt] v_\ell \\ \\[-7pt] u_r \\ \\[-7pt] v_r \end{array}\right) = \underbrace{\left( \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} f & 0 & c_u & f \frac{b}{2} \\ \\[-7pt] 0 & f & c_v & 0 \\ \\[-7pt] f & 0 & c_u & -f\frac{b}{2} \\ \\[-7pt] 0 & f & c_v & 0 \end{array}\right)}_{\textbf{M}} \, \frac{1}{z} \left( \begin{array}{c} x \\ \\[-7pt] y \\ \\[-7pt] z \\ \\[-7pt] 1 \end{array}\right) + \textbf{n},\end{equation}

where $\textbf{n} \sim \mathcal{N}(\textbf{0}, \textbf{R})$ and $\textbf{M}$ is a combined parameter matrix for the stereo rig. We used focal length $f = 200$ (pixels), stereo baseline $b = 0.25$ (m), optical center $(c_u, c_v) = (0,0)$ (pixels), and measurement covariance $\textbf{R} = 0.25^2 \textbf{1}$ (pixels $^2$ ). This model was used to generate two noisy pointclouds where the second frame is transformed by groundtruth pose, $\textbf{T}_{\textrm{gt}}$ , from the first. Once the noisy feature locations were generated in the two different stereo image frames, we transformed these back to two Euclidean pointclouds, $\textbf{p}_j$ and $\textbf{q}_j$ , using the inverse stereo camera model. The alignment problem is then to recover $\textbf{T}_{\textrm{gt}}$ using $\textbf{p}_j$ and $\textbf{q}_j$ . We assume the points have already been correlated correctly and do not treat the outlier rejection problem here.

To generate the weight matrices, $\textbf{W}_j$ , for our cost function, we mapped the isotropic covariance in image space, $\textbf{R}$ , through the inverse stereo camera model to produce an (inverse) covariance of our error in Euclidean space:

(89) \begin{equation}\textbf{W}_j^{-1} = \mathbb{E} [ \textbf{e}_j \textbf{e}_j^T ].\end{equation}

The resulting $\textbf{W}_j^{-1}$ is a long, skinny uncertainty ellipsoid in the depth direction of the camera as established by Matthies and Shafer [Reference Matthies and Shafer29]. This anisotropic measurement covariance makes this problem particularly challenging. As far we know, there is no practical algorithm that can guarantee finding the global minimum of the cost function, J, in this anisotropic situation. Yang et al. [Reference Yang, Shi and Carlone30] show how to certify finding the global minimum in the case that $\textbf{W}_j^{-1}$ is isotropic (i.e., a scalar times the identity matrix) even with significant outliers, but this does not apply in the chosen situation.

We selected a pyramid distribution of 12 landmarks in the field of view from 5 to 15 m away from the camera. The camera was translated 1 m in the depth, z, direction to generate the two noisy pointclouds. We then ran many trials where we varied (i) the random draw of the measurement noise, and (ii) the random draw of the initial guess for the alignment algorithms. Our goal was to see if choosing different vector parameterizations of pose would result in different paths to get from the initial guess to the groundtruth pose.

Figure 4.

Results of pointcloud alignment problem for both the exponential map and CayPer algorithm. The left plot shows the convergence history of the cost function J for 100 trials where the initial pose was randomly selected. We see that the exponential map regularly takes a long time to converge and often becomes stuck in a local minimum when the initial pose is far from the groundtruth. CayPer always converged in 20 iterations or less and always ended at the global minimum. The right plot shows a histogram of the costs, J, at the final iteration for 1000 trials where the initial pose was randomly selected. Again, we see that in approximately 20% of the trials the exponential map failed to converge to the global minimum while CayPer succeeded in all trials.

Figure 5.

Two trials of the pointcloud alignment problem. In both cases, CayPer successfully finds the correct minimum while the exponential map succeeds in the top case but not the bottom where it becomes stuck in an undesirable local minimum. Red box is the initial guess, green box is the groundtruth pose, black dots are the groundtruth landmark locations, green dots are the first noisy pointcloud (static), magenta box (hidden by green box) is the CayPer final pose, blue box (when not hidden by green box) is the exponential map final pose, magenta dots are the second noisy pointcloud (transformed) at the final CayPer pose, and blue dots are the second noisy pointcloud (transformed) at the final exponential map pose.