# SE(3) constraints for robotics

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

# 1Transformation parameterisation

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

The rotation matrix is in the special orthogonal group , which means that it is orthogonal (its columns are normal and orthogonal to each other) and it has determinant .

## 1.1Transforming a point

An element may transform a 3D point :

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

so that points may be transformed rigidly

## 1.2The Lie algebra

Each element in is associated with an element on the corresponding Lie algebra, :

where is the skew symmetric form of the cross product by . We may also write it as a vector.

We will use the and 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.3The exponential map

The group and its algebra are related by the exponential map:

The definition of is based on the Taylor series:

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

Note that is not commutative. The adjoint action relates things in different orders:

## 1.4Notation summary

In general,

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

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

# 2Derivatives

Here, we only differentiate with respect to around the point . In other words, we have a function where and we would like to perturb by a very small .

Suppose . If is of dimension , then the resulting derivative is an Jacobian.

where in the case of left-perturbations and 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

Let , , 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.

# 3Optimisation

Suppose we have a trajectory 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 :

where the 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 .

Let the Jacobian matrix be:

Then we can solve using Gauss-Newton:

or steepest descent:

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

where scalar weights , are chosen such that where the scalar parameter is the radius of the trust region. When is small, the problem is behaving badly and the quadratic approximation that Gauss-Newton uses is not very valid. In this case, is zero, allowing the optimiser to take timid steps along the steepest descent direction. When 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 and when to decrease it.

## 3.1Optimisation under uncertainty

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

The matrix 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 , 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 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 :

where is the square matrix whose columns are the orthonormal eigenvectors of and 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.2Robust 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 , we optimise:

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

# 4Trajectory representation

The goal is to recover the trajectory of a vehicle as a parametric curve as a function of time .

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 within the domain of the curve, we can efficiently evaluate in constant time.
• Local perturbations may be applied to the curve, so that , such that the perturbation 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 that represent for some sampled with even spacing at a high frequency (say, 100~Hz). To evaluate , we use a geodesic interpolation for .

To perturb this spline with a curve , we update each , like so:

Alternative approaches for parameterising trajectories include:

# 5Parameterisation of the perturbation

When applying perturbations, the curve 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 remains twice-differentiable, we parameterise the perturbation by a finite vector . The vector is the concatenation many six-dimensional vectors , such that

Notice that, since only applies small local perturbations, it is possible to add together the treating them as vectors in . The scalar-valued function is a basis function with compact support, which means that it is nonzero for a finite contiguous segment of 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 is a constant,

The elements are known as the spline knots. Since each knot is a vector, the number of columns of is where is the number of spline knots.

As you can see, the derivative of the trajectory at any point 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 , to the actual Jacobian where we are optimizing the spline knots .

# 6Constraints

The function is known as the residual. It consists of many constraints:

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

## 6.1Position 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 measured at time .

### 6.1.1Residual

The residual is a vector:

Recall from the notation section that is the translational component of the element .

### 6.1.2Left Jacobian

The left Jacobian is :

The trick is to view where . Then we can apply the Jacobian for transforming a point in the derivatives table.

### 6.1.3Right Jacobian

The right Jacobian is :

## 6.2Loop 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 .

### 6.2.1Residual

The residual is a vector:

## 6.3Gravity constraint

The gravity constraint seeks to penalise the distance between up vector of the car and the true up vector .

For example, if the sensor were mounted perfectly upright, then .

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

### 6.3.1Residual

The residual is a vector:

### 6.3.2Left Jacobian

Since the gravity constraint only depends on rotation and not on translation, we only care about the derivative with respect to , which we hereafter write as .

### 6.3.3Right Jacobian

Recall that is the rotational component of the pose .

## 6.4Point constraint

We are aligning a “moving” or “map” point to a “static” or “scene” point by transforming the moving point with the pose .

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

• If you are registering the point to a line, you can let be . 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 be .

### 6.4.1Residual

The residual is a vector:

### 6.4.2Left Jacobian

The left Jacobian is , where is .

### 6.4.3Right Jacobian

The right Jacobian is , where is .

Recall that is the rotational component of the pose .

## 6.5Velocity 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 and . Let .

### 6.5.1Residual

The residual is a vector. For concise notation let us define the matrix such that

### 6.5.2Left Jacobian

Consider differentiating with respect to left-updates of .

The Jacobian is .

Now, consider differentiating with respect to left-updates to .

## 6.6Regularisation 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.1Residual

The residual is :

### 6.6.2Jacobian

The Jacobian is the identity:

# 7References

• jlblanco Blanco, J.L. (2020) A tutorial on SE(3) transformation parameterizations and on-manifold optimization. link