This document summarizes some common maths used for state estimation of rigid bodies such as in robotics.

1 Transformation parameterisation

Rigid transformations in 3 dimensions are known as the special Euclidean group, $SE(3)$, and can be written in the homogeneous form.

The rotation matrix $\mathbf R$ is in the special orthogonal group $SO(3)$, which means that it is orthogonal (its columns are normal and orthogonal to each other) and it has determinant $+1$.

1.1 Transforming a point

An element $\mathbf T \in SE(3)$ may transform a 3D point $\mathbf p \in \mathbb R^3$:

In this document we use it interchangeably with the homogeneous representation:

so that points may be transformed rigidly

1.2 The Lie algebra

Each element in $SE(3)$ is associated with an element on the corresponding Lie algebra, $\mathfrak{se}(3)$:

where $[\bm \omega]_\times$ is the skew symmetric form of the cross product by $\omega$. We may also write it as a $6\times 1$ vector.

We will use the $6\times 1$ and $4\times 4$ representations interchangeably depending on context.

Note that some other textbooks put the translational part on top and the rotational part below. It doesn’t matter much, but it will affect our notation for things like the adjoint action, differentials, etc below.

1.3 The exponential map

The group $SE(3)$ and its algebra $\mathfrak{se}(3)$ are related by the exponential map:

The definition of $\exp$ is based on the Taylor series:

Closed forms exist. See: J. L. Blanco’s report jlblanco.

Note that $SE(3)$ is not commutative. The adjoint action relates things in different orders:

The adjoint is a $6\times 6$ matrix:

1.4 Notation summary

In general,

• bold lowercase refers to vectors (e.g. translation $\mathbf t$)
• bold uppercase refers to matrices (e.g. transformation $\mathbf T$)
• non-bold lowerase refers to scalars (e.g. time $t$)

Here we define and summarise some notation for the following sections.

2 Derivatives

Here, we only differentiate with respect to $\bm \delta \in \mathfrak{se}(3)$ around the point $\bm \delta = \mathbf 0$. In other words, we have a function $\mathbf F(\mathbf T)$ where $\mathbf T \in SE(3)$ and we would like to perturb $\mathbf T$ by a very small $\bm \delta$.

Suppose $\bm \delta = \begin{bmatrix}\bm \omega & \bm \tau\end{bmatrix}^T$. If $\mathbf F$ is of dimension $n$, then the resulting derivative is an $n\times 6$ Jacobian.

where $\mathbf F(\mathbf T \oplus \bm \delta) = \mathbf F(\exp(\bm \delta)\mathbf T)$ in the case of left-perturbations and $\mathbf F(\mathbf T\exp(\bm \delta))$ in the case of right-perturbations.

The adjoint action can be used to relate the left and right derivatives by using the chain rule.

We also have a derivative of the log function:

where

The exact derivation is in Ethan Eade’s report eade.

Let $\mathbf T \in SE(3)$, $\mathbf p \in \mathbb R^3$, then the following table lists some useful derivatives.

As you will see later, these are basically all the derivatives you need, and all other derivatives can be easily derived from these, often together with using the adjoint.

3 Optimisation

Suppose we have a trajectory $\mathbf S(t) \in SE(3)$ that is a smooth curve. We would like to minimise some objective function:

We seek to reduce the cost as much as possible. This is a nonlinear least squares problem. During the optimisation process, we perturb it by a small perturbation $\bm \delta(t) \in \mathfrak{se}(3)$:

where the $\oplus$ operator can either be the left-update:

or the right-update:

Both are valid depending on numerical properties of the problem.

In any case, we linearise the problem around $\bm \delta = \mathbf 0$.

Let the Jacobian matrix be:

Then we can solve $\bm \delta$ using Gauss-Newton:

or steepest descent:

or more sophisticated algorithms. In practice, we use Powell’s Dog Leg. The update is

where scalar weights $c_\text{GN}$, $c_\text{s}$ are chosen such that $\|\bm \delta \| \leq \Delta$ where the scalar parameter $\Delta$ is the radius of the trust region. When $\Delta$ is small, the problem is behaving badly and the quadratic approximation that Gauss-Newton uses is not very valid. In this case, $c_\text{GN}$ is zero, allowing the optimiser to take timid steps along the steepest descent direction. When $\Delta$ is large, the quadratic approximation is good and we take bigger steps in the Gauss-Newton direction.

Heuristics are used to determine when to increase $\Delta$ and when to decrease it.

3.1 Optimisation under uncertainty

When uncertainty is present, the data is associated with some covariance matrix $\bm \Sigma$ which is an $n\times n$ matrix where $n$ is the sise of the residual. The Gauss-Newton update then becomes:

The matrix $\bm \Sigma^{-1}$ is also sometimes called the information matrix. In practice, it is useful to factor this matrix, for example, by taking the matrix square root. Let $\mathbf W = \bm \Sigma^{-\frac{1}{2}}$, then,

In other words, we just pre-multiply the Jacobian and residual by a weight matrix. This is a form of whitening. In the common case where $\bm \Sigma$ is a diagonal matrix, we simply divide each row of the Jacobian and the residual by the standard deviation.

Another common factorisation is the eigendecomposition of the covariance matrix $\bm \Sigma$:

where $\mathbf Q \in SO(n)$ is the square matrix whose columns are the orthonormal eigenvectors of $\bm \Sigma$ and $\bm \Lambda$ is the diagonal matrix whose entries are the eigenvalues. Then,

This is useful, for example, in the case of surfel matches. Recall that the covariance matrix is always symmetric positive semidefinite, allowing for easy eigendecomposition.

3.2 Robust loss functions

A least squares problem can be thought of as minimising the negative log likelihood function of a Gaussian. The likelihood function is of the form

Then, the log likelihood is of the least squares form:

However, in many cases, the random variable is not Gaussian. The Gaussian has very thin tails, making it incredibly unlikely to have outliers. In the real life, there are often many outliers that necessitate a more thick-tailed distribution. To model such situations, we need robust loss functions.

Instead of optimising $\sum f_i^2$, we optimise:

where $\rho$ is a robust loss function. We can then weigh each row or block of the Jacobian $\mathbf J$ and the residual $\mathbf F$ with a robust reweighting factor:

4 Trajectory representation

The goal is to recover the trajectory of a vehicle as a parametric curve $\mathbf S(t) \in SE(3)$ as a function of time $t$.

We may assume the curve does not oscillate faster than some frequency (say, 10~Hz).

Now we should find a trajectory representation that satisfies the following properties:

• For some finite period of time, the trajectory should be approximately correct and differentiable.
• For any given real number $t$ within the domain of the curve, we can efficiently evaluate $\mathbf S(t)$ in constant time.
• Local perturbations $\bm \delta (t)$ may be applied to the curve, so that $\mathbf S_\text{new}(t) = \mathbf S(t) \oplus \bm \delta(t)$, such that the perturbation $\bm \delta(t)$ may be parameterised with a finite number of parameters, each with compact support.
• The trajectory must be independent of the choice of the inertial reference frame.

Our solution to satisfy these requirements is a piecewise linear trajectory. The trajectory is represented as a sequence of elements $\mathbf S_i$ that represent $\mathbf S(t_i)$ for some $t_i$ sampled with even spacing at a high frequency (say, 100~Hz). To evaluate $\mathbf S(t)$, we use a geodesic interpolation for $t_i \leq t < t_{i + 1}$.

To perturb this spline with a curve $\bm \delta(t)$, we update each $\mathbf S_i$, like so:

Alternative approaches for parameterising trajectories include:

• Cumulative cubic B-splines.
• Hermite splines.

5 Parameterisation of the perturbation

When applying perturbations, the curve $\bm \delta$ is a smooth curve which may be thought of as a vector of infinite dimension.

To ensure that the problem is computationally tractable and that $\mathbf S_{\text{new}}$ remains twice-differentiable, we parameterise the perturbation $\bm \delta$ by a finite vector $\bm \xi$. The vector $\bm \xi$ is the concatenation many six-dimensional vectors $\bm \xi_i \in \mathfrak{se}(3)$, such that

Notice that, since $\bm \delta(t)$ only applies small local perturbations, it is possible to add together the $\bm \xi_i$ treating them as vectors in $\mathbb R^6$. The scalar-valued function $\beta$ is a basis function with compact support, which means that it is nonzero for a finite contiguous segment of $t$ and zero everywhere else. A good choice is the piecewise polynomial for a cardinal cubic B-spline:

We can now redefine the Jacobian to be with respect to the parameters:

Since $\beta$ is a constant,

The elements $\bm \xi_i \in \mathfrak{se}(3)$ are known as the spline knots. Since each knot is a $6\times 1$ vector, the number of columns of $\mathbf J$ is $6 \times n_\text{knots}$ where $n_\text{knots}$ is the number of spline knots.

As you can see, the derivative of the trajectory at any point $t$ in time is a linear combination of the derivatives of up to four spline knots.

In the next sections we will compute derivatives

knowing that each of these blocks will contribute up to four blocks, weighted with scalar weights $\beta$, to the actual Jacobian where we are optimizing the spline knots $\bm \xi$.

6 Constraints

The function $\mathbf F$ is known as the residual. It consists of many constraints:

For people familiar with the Ceres library, each constraint is a residual block.

6.1 Position constraint

The position constraint seeks to penalise the distance between the pose’s translational component and a 3D point.

For example, the 3D point may be the GPS position $\mathbf p(t)$ measured at time $t$.

6.1.1 Residual

The residual is a $3\times 1$ vector:

Recall from the notation section that $\mathbf t(\mathbf T)$ is the translational component of the $SE(3)$ element $\mathbf T$.

6.1.2 Left Jacobian

The left Jacobian is $3 \times 6$:

The trick is to view $\mathbf t(\mathbf T) = \mathbf T \mathbf p$ where $\mathbf p = \mathbf 0$. Then we can apply the Jacobian for transforming a point in the derivatives table.

6.1.3 Right Jacobian

The right Jacobian is $3 \times 6$:

6.2 Loop closure constraint

Suppose that we have aligned the poses from two points in time along a trajectory, e.g. using ICP.

This produces a relative transformation $\mathbf A$.

6.2.1 Residual

The residual is a $6\times 1$ vector:

6.2.2 Left Jacobians

6.2.3 Right Jacobians

6.3 Gravity constraint

The gravity constraint seeks to penalise the distance between up vector of the car $\mathbf R(\mathbf T(t)) \mathbf u$ and the true up vector $\mathbf u_\text{true} = [0, 0, 1]^T$.

For example, if the sensor were mounted perfectly upright, then $\mathbf u = \mathbf u_\text{true}$.

We can also use the accelerometer reading to obtain a different $\mathbf u$ at each point in time, as long as you remember to subtract the inertial acceleration.

6.3.1 Residual

The residual is a $3 \times 1$ vector:

6.3.2 Left Jacobian

Since the gravity constraint only depends on rotation and not on translation, we only care about the derivative with respect to $\bm \omega(\bm \delta)$, which we hereafter write as $\bm \omega$.

6.3.3 Right Jacobian

Recall that $\mathbf R(\mathbf T)$ is the rotational component of the pose $\mathbf T$.

6.4 Point constraint

We are aligning a “moving” or “map” point $\mathbf m$ to a “static” or “scene” point $\mathbf s$ by transforming the moving point with the pose $\mathbf T$.

The matrix $\mathbf A$ can be used to store the uncertainty of the point $\mathbf s$, i.e. an information matrix (the inverse of a covariance matrix).

• If you are registering the point to a line, you can let $\mathbf A$ be $2\times 3$. A common choice is to compute an orthonormal basis from the direction of the ray, e.g. using Duff et al’s approach. This is commonly used in visual odometry.
• If you are registering the point to a plane, you can let $\mathbf A$ be $1\times 3$.

6.4.1 Residual

The residual is a $3\times 1$ vector:

6.4.2 Left Jacobian

The left Jacobian is $k\times 6$, where $\mathbf A$ is $k\times 3$.

6.4.3 Right Jacobian

The right Jacobian is $k\times 6$, where $\mathbf A$ is $k\times 3$.

Recall that $\mathbf R(\mathbf T)$ is the rotational component of the pose $\mathbf T$.

6.5 Velocity constraint

The velocity constraint seeks to penalise deviations in vehicle velocity from the 6 degree of freedom velocity estimates from another source.

For ease of implementation, the vehicle velocity is obtained by numerical differentiation, e.g. by evaluating the pose at times $t_1$ and $t_2$. Let $h = 1 / (t_2 - t_1)$.

6.5.1 Residual

The residual is a $6\times 1$ vector. For concise notation let us define the $6\times 6$ matrix $\bm \Delta$ such that

6.5.2 Left Jacobian

Consider differentiating with respect to left-updates of $\mathbf T(t_1)$.

The Jacobian is $6\times 6$.

Now, consider differentiating with respect to left-updates to $\mathbf T(t_2)$.

6.6 Regularisation constraint

The regularisation constraint implements a basic Tikhonov regulariser. It seeks to dampen the problem to avoid divergent oscillations, overfitting, or other types of poor convergence.

6.6.1 Residual

The residual is $6\times 1$:

6.6.2 Jacobian

The Jacobian is the identity:

7 References

• jlblanco Blanco, J.L. (2020) A tutorial on SE(3) transformation parameterizations and on-manifold optimization. link
• eade Eade, E. (2018) Derivative of the Exponential map. link