SE(3) constraints for robotics

2021-09-08

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, $S E (3)$ , and can be written in the homogeneous form.

\begin{array}{r} T_{4 \times 4} = [\begin{array}{cc} R_{3 \times 3} & t_{3 \times 1} \\ 0_{1 \times 3} & 1 \end{array}] \in S E (3) . \end{array}

The rotation matrix $R$ is in the special orthogonal group $S O (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 $T \in S E (3)$ may transform a 3D point $p \in R^{3}$ :

\begin{aligned} p_{3 \times 1} & = [\begin{array}{c} x \\ y \\ z \end{array}] . \end{aligned}

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

\begin{array}{r} p_{4 \times 1} = [\begin{array}{c} x \\ y \\ z \\ 1 \end{array}] \end{array}

so that points may be transformed rigidly

\begin{array}{r} T p \equiv R p + t . \end{array}

1.2 The Lie algebra

Each element in $S E (3)$ is associated with an element on the corresponding Lie algebra, $s e (3)$ :

\begin{aligned} ξ_{4 \times 4} & = [\begin{array}{cc} [ω]_{\times} & τ \\ 0_{1 \times 3} & 0 \end{array}] \in s e (3) \\ = [\begin{array}{cccc} 0 & - ω_{3} & ω_{2} & τ_{1} \\ ω_{3} & 0 & - ω_{1} & τ_{2} \\ - ω_{2} & ω_{1} & 0 & τ_{3} \\ 0 & 0 & 0 & 0 \end{array}] \in s e (3) \end{aligned}

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

\begin{array}{r} ξ_{6 \times 1} = [\begin{array}{c} ω_{3 \times 1} \\ τ_{3 \times 1} \end{array}] . \end{array}

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 $S E (3)$ and its algebra $s e (3)$ are related by the exponential map:

\begin{aligned} \exp & : s e (3) \to S E (3) \\ \log & : S E (3) \to s e (3) . \end{aligned}

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

\begin{array}{r} \exp (ξ) = I_{4 \times 4} + ξ_{4 \times 4} + \frac{1}{2!} ξ_{4 \times 4}^{2} + \frac{1}{3!} ξ_{4 \times 4}^{3} \dots \end{array}

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

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

\begin{array}{r} T \exp (δ) = \exp (Ad (T) δ) T . \end{array}

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

\begin{array}{r} Ad (T) = [\begin{array}{cc} R & 0_{3 \times 3} \\ [t]_{\times} R & R \end{array}] . \end{array}

1.4 Notation summary

In general,

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

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

Description	Notation
skew-symmetric cross product matrix $[\begin{array}{ccc} 0 & - t_{3} & t_{2} \\ t_{3} & 0 & - t_{1} \\ - t_{2} & t_{1} & 0 \end{array}]$	$[t]_{\times}$
translational part of $T \in S E (3)$	$t (T)$
rotational part of $T \in S E (3)$	$R (T)$
translational part of $ξ \in s e (3)$	$τ (ξ)$
rotational part of $ξ \in s e (3)$	$ω (ξ)$
compose $T_{1}, T_{2} \in S E (3)$	$T_{1} T_{2}$
exp $ξ \in s e (3)$	$\exp (ξ)$
log of $T \in S E (3)$	$\log (T)$
exp $ω \in s o (3)$	$\exp (ω)$
log of $R \in S O (3)$	$\log (R)$
inverse of $ξ \in s e (3)$	$- ξ$
inverse of $T \in S E (3)$	$T^{- 1}$
inverse of $ω \in s o (3)$	$- ω$
inverse of $R \in S O (3)$	$R^{T}$
adjoint of $T \in S E (3)$	$Ad (T)$
$i$ th element of ${ξ ∣ ξ \in s e (3)}$	$ξ_{i}$
$i$ th element of ${T ∣ T \in S E (3)}$	$T_{i}$
transform $p \in R^{3}$ by $T \in S E (3)$	$T p$
transform $p \in R^{3}$ by $ξ \in s e (3)$	$\exp (ξ) p$

Table 1 Summary of notation and implementation. For the array ones, operations are applied elementwise.

2 Derivatives

Here, we only differentiate with respect to $δ \in s e (3)$ around the point $δ = 0$ . In other words, we have a function $F (T)$ where $T \in S E (3)$ and we would like to perturb $T$ by a very small $δ$ .

Suppose $δ = {[\begin{array}{cc} ω & τ \end{array}]}^{T}$ .If $F$ is of dimension $n$ , then the resulting derivative is an $n \times 6$ Jacobian.

\begin{aligned} J_{n \times 6} \equiv {\frac{\partial F (T \oplus δ)}{\partial δ}]}_{δ = 0} & = [\begin{array}{cccccc} \frac{\partial F_{1}}{\partial ω_{1}} & \frac{\partial F_{1}}{\partial ω_{2}} & \frac{\partial F_{1}}{\partial ω_{3}} & \frac{\partial F_{1}}{\partial τ_{1}} & \frac{\partial F_{1}}{\partial τ_{2}} & \frac{\partial F_{1}}{\partial τ_{3}} \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ \frac{\partial F_{n}}{\partial ω_{1}} & \frac{\partial F_{n}}{\partial ω_{2}} & \frac{\partial F_{n}}{\partial ω_{3}} & \frac{\partial F_{n}}{\partial τ_{1}} & \frac{\partial F_{n}}{\partial τ_{2}} & \frac{\partial F_{n}}{\partial τ_{3}} \end{array}] \end{aligned}

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

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

\begin{aligned} \frac{\partial F (A \exp (δ) B)}{\partial δ} & = \frac{\partial F (\exp (Ad (A) δ) A B)}{\partial δ} \\ = \frac{\partial}{\partial δ} F (\exp (\underset{ϵ}{\underset{⏟}{Ad (A) δ}}) A B) \\ = \frac{\partial}{\partial ϵ} F (\exp (ϵ) A B) \frac{\partial ϵ}{\partial δ} \\ = \frac{\partial}{\partial ϵ} F (\exp (ϵ) A B) Ad (A) . \end{aligned}

We also have a derivative of the log function:

\begin{aligned} D_{log} (T) & \equiv {\frac{\partial}{\partial δ}]}_{δ = 0_{6 \times 1}} \log (\exp (δ) T) \\ = [\begin{array}{cc} D_{log} (ω) & 0_{3 \times 3} \\ - D_{log} (ω) B D_{log} (ω) & D_{log} (ω) \end{array}] \end{aligned}

where

\begin{aligned} D_{log} (ω) & = I - \frac{1}{2} [ω_{\times}] + e_{θ} [ω]_{\times}^{2} \\ B & \equiv b_{θ} [u]_{\times} + c_{θ} (ω u^{T} + u ω^{T}) + (ω^{t} u) \cdot W (ω) \\ θ & = ∥ ω ∥ \\ a_{θ} & = \frac{\sin θ}{θ} \\ b_{θ} & = \frac{1 - \cos θ}{θ^{2}} \\ c_{θ} & = \frac{1 - a_{θ}}{θ^{2}} \\ e_{θ} & = \frac{b_{θ} - 2 c_{θ}}{2 a_{θ}} \end{aligned}

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

Let $T \in S E (3)$ , $p \in 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.

Function	Derivative
$F (δ)$	${\frac{\partial F}{\partial δ}]}_{δ = 0}$
$\exp (δ) T p$	$[\begin{array}{cc} - [T p]_{\times} & I \end{array}]$
$T \exp (δ) p$	$R (T) [\begin{array}{cc} - [p]_{\times} & I \end{array}]$
$\log (\exp (δ) T)$	$D_{log} (T)$

Table 2 Summary of derivatives

3 Optimisation

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

\begin{array}{r} cost (S) = F (S)^{T} F (S) . \end{array}

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 $δ (t) \in s e (3)$ :

\begin{array}{r} S_{new} (t) = S (t) \oplus δ (t) \end{array}

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

\begin{array}{r} S \oplus δ \equiv \exp (δ) S \end{array}

or the right-update:

\begin{array}{r} S \oplus δ \equiv S \exp (δ) . \end{array}

Both are valid depending on numerical properties of the problem.

In any case, we linearise the problem around $δ = 0$ .

Let the Jacobian matrix be:

\begin{array}{r} J = {\frac{\partial}{\partial δ} F (S \oplus δ)]}_{δ = 0} . \end{array}

Then we can solve $δ$ using Gauss-Newton:

\begin{array}{r} J^{T} J δ_{GN} = - J^{T} F \end{array}

or steepest descent:

\begin{array}{r} δ_{s} = - J^{T} F \end{array}

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

\begin{array}{r} δ = c_{GN} δ_{GN} + c_{s} δ_{s} \end{array}

where scalar weights $c_{GN}$ , $c_{s}$ are chosen such that $∥ δ ∥ \leq Δ$ 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, $c_{GN}$ 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.1 Optimisation under uncertainty

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

\begin{array}{r} J^{T} Σ^{- 1} J δ = - J^{T} Σ^{- 1} F \end{array}

The matrix $Σ^{- 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 $W = Σ^{- \frac{1}{2}}$ , then,

\begin{array}{r} (W J)^{T} (W J) δ = - (W J)^{T} (W F) . \end{array}

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 $Σ$ :

\begin{array}{r} Σ = Q Λ Q^{T} \end{array}

where $Q \in S O (n)$ is the square matrix whose columns are the orthonormal eigenvectors of $Σ$ and $Λ$ is the diagonal matrix whose entries are the eigenvalues. Then,

\begin{array}{r} W = Λ^{- \frac{1}{2}} Q^{T} . \end{array}

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

\begin{array}{r} L = \prod \exp (- f_{i}^{2}) \end{array}

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

\begin{array}{r} cost = - \log L = \sum f_{i}^{2} . \end{array}

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:

\begin{array}{r} cost = \sum ρ (f_{i})^{2} \end{array}

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

\begin{array}{r} r_{i} \equiv \frac{\partial ρ (f_{i})}{\partial f_{i}} . \end{array}

Name	Definition
`TrivialLoss`	$ρ (x) = x$
`CauchyLoss`	$ρ (x) = \log (1 + x)$
`HuberLoss`	$ρ (x) = {\begin{cases} x & x < k \\ 2 k \sqrt{x} - k^{2} & x \geq k^{2} \end{cases}$

Table 3 Some loss functions for

x = ∣ f ∣

4 Trajectory representation

The goal is to recover the trajectory of a vehicle as a parametric curve $S (t) \in S E (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 $S (t)$ in constant time.
Local perturbations $δ (t)$ may be applied to the curve, so that $S_{new} (t) = S (t) \oplus δ (t)$ , such that the perturbation $δ (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 $S_{i}$ that represent $S (t_{i})$ for some $t_{i}$ sampled with even spacing at a high frequency (say, 100~Hz).To evaluate $S (t)$ , we use a geodesic interpolation for $t_{i} \leq t < t_{i + 1}$ .

\begin{array}{r} S (t) = \exp ((t - t_{i}) \log (S_{i + 1} S_{i}^{- 1})) S_{i} . \end{array}

To perturb this spline with a curve $δ (t)$ , we update each $S_{i}$ , like so:

\begin{array}{r} S_{i, new} = S_{i} \oplus δ (t_{i}) \end{array}

Alternative approaches for parameterising trajectories include:

5 Parameterisation 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 $S_{new}$ remains twice-differentiable, we parameterise the perturbation $δ$ by a finite vector $ξ$ .The vector $ξ$ is the concatenation many six-dimensional vectors $ξ_{i} \in s e (3)$ , such that

\begin{array}{r} δ (t) = \sum_{i}^{n} ξ_{i} β ((t - i) Δ t) . \end{array}

Notice that, since $δ (t)$ only applies small local perturbations, it is possible to add together the $ξ_{i}$ treating them as vectors in $R^{6}$ .The scalar-valued function $β$ 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:

\begin{array}{r} β (t) = {\begin{cases} \frac{1}{6} t^{3} & t \in [0, 1] \\ \frac{1}{6} (- 3 t^{3} + 12 t^{2} - 12 t + 4) & t \in [1, 2] \\ \frac{1}{6} (3 t^{3} - 24 t^{2} + 60 t - 44) & t \in [2, 3] \\ \frac{1}{6} (- t^{3} + 12 t^{2} - 48 t + 64) & t \in [3, 4] \\ 0 & t \notin [0, 4] \end{cases} \end{array}

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

\begin{array}{r} J = {\frac{\partial}{\partial ξ} F (S \oplus δ) ∣}_{δ = 0} . \end{array}

Since $β$ is a constant,

\begin{array}{r} \frac{\partial}{\partial ξ_{i}} = β ((t - i) Δ t) \frac{\partial}{\partial δ (t)} . \end{array}

The elements $ξ_{i} \in s e (3)$ are known as the spline knots. Since each knot is a $6 \times 1$ vector, the number of columns of $J$ is $6 \times n_{knots}$ where $n_{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

\begin{array}{r} \frac{\partial}{\partial δ (t)} \end{array}

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 $ξ$ .

6 Constraints

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

\begin{array}{r} F = [\begin{array}{c} F_{position} \\ F_{loop} \\ F_{gravity} \\ ⋮ \end{array}] \end{array}

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 $p (t)$ measured at time $t$ .

6.1.1 Residual

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

\begin{array}{r} F_{position} = t (T (t)) - p (t) \end{array}

Recall from the notation section that $t (T)$ is the translational component of the $S E (3)$ element $T$ .

6.1.2 Left Jacobian

The left Jacobian is $3 \times 6$ :

\begin{aligned} J_{left} \equiv \frac{\partial F_{position}}{\partial δ} & = \frac{\partial t (\exp (δ) T (t))}{\partial δ} \\ = [\begin{array}{cc} - [t (T (t))]_{\times} & I_{3 \times 3} \end{array}] \end{aligned}

The trick is to view $t (T) = T p$ where $p = 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$ :

\begin{aligned} J_{right} \equiv \frac{\partial F_{position}}{\partial δ} & = \frac{\partial t (T (t) \exp (δ))}{\partial δ} \\ = [\begin{array}{cc} 0_{3 \times 3} & R (T (t)) \end{array}] \end{aligned}

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 $A$ .

6.2.1 Residual

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

\begin{array}{r} F_{loop closure} = \log (\underset{trajectory}{\underset{⏟}{T (t_{1})^{- 1} T (t_{2})}} A) \end{array}

6.2.2 Left Jacobians

\begin{aligned} J_{left, t_{1}} & \equiv \frac{\partial F_{loop closure}}{\partial δ (t_{1})} \\ = \frac{\partial \log ((\exp (δ) T (t_{1}))^{- 1} T (t_{2}) A)}{\partial δ} \\ = \frac{\partial \log (T (t_{1})^{- 1} \exp (- δ) T (t_{2}) A)}{\partial δ} \\ = - \frac{\partial \log (T (t_{1})^{- 1} \exp (δ) T (t_{2}) A)}{\partial δ} \\ = - \frac{\partial \log (\exp (Ad (T (t_{1})^{- 1}) δ) T (t_{1})^{- 1} T (t_{2}))}{\partial δ} \\ = - \frac{\partial \log (\exp (ϵ) T (t_{1})^{- 1} T (t_{2}) A)}{\partial ϵ} Ad (T (t_{1})^{- 1}) \\ = - D_{log} (T (t_{1})^{- 1} T (t_{2}) A) Ad (T (t_{1})^{- 1}) \end{aligned}

\begin{aligned} J_{left, t_{2}} & \equiv \frac{\partial F_{loop closure}}{\partial δ (t_{2})} \\ = \frac{\partial \log (T (t_{1})^{- 1} \exp (δ) T (t_{2}) A)}{\partial δ} \\ = D_{log} (T (t_{1})^{- 1} T (t_{2}) A) Ad (T (t_{1})^{- 1}) \end{aligned}

6.2.3 Right Jacobians

\begin{aligned} J_{right, t_{1}} & \equiv \frac{\partial F_{loop closure}}{\partial δ (t_{1})} \\ = \frac{\partial \log ((T (t_{1}) \exp (δ))^{- 1} T (t_{2}) A)}{\partial δ} \\ = \frac{\partial \log (\exp (- δ) T (t_{1})^{- 1} T (t_{2}) A)}{\partial δ} \\ = - D_{log} (T (t_{1})^{- 1} T (t_{2}) A) \end{aligned}

\begin{aligned} J_{right, t_{2}} & \equiv \frac{\partial F_{loop closure}}{\partial δ (t_{2})} \\ = \frac{\partial \log ((T (t_{1})^{- 1} T (t_{2}) \exp (δ) A)}{\partial δ} \\ = D_{log} (T (t_{1})^{- 1} T (t_{2}) A) Ad (T (t_{1})^{- 1} T (t_{2})) \end{aligned}

6.3 Gravity constraint

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

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

We can also use the accelerometer reading to obtain a different $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:

\begin{array}{r} F_{gravity} = (R (T (t)) u - u_{true}) \end{array}

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 $ω (δ)$ , which we hereafter write as $ω$ .

\begin{aligned} J_{left} \equiv \frac{\partial F_{gravity}}{\partial ω} & = \frac{\partial \exp (ω) R (T (t)) u}{\partial ω} \\ = - [R (T (t)) u]_{\times} \end{aligned}

6.3.3 Right Jacobian

\begin{aligned} J_{right} \equiv \frac{\partial F_{gravity}}{\partial ω} & = \frac{\partial R (T (t)) \exp (ω) u}{\partial ω} \\ = - R (T (t)) [u]_{\times} \end{aligned}

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

6.4 Point constraint

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

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

If you are registering the point to a line, you can let $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 $A$ be $1 \times 3$ .

6.4.1 Residual

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

\begin{array}{r} F_{point} = A (T m - s) . \end{array}

6.4.2 Left Jacobian

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

\begin{aligned} J_{left} & = A [\begin{array}{cc} - [T m]_{\times} & I \end{array}] \end{aligned}

6.4.3 Right Jacobian

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

\begin{aligned} J_{right} & = A R (T) [\begin{array}{cc} - [m]_{\times} & I \end{array}] \end{aligned}

Recall that $R (T)$ is the rotational component of the pose $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 $Δ$ such that

\begin{aligned} F_{velocity} & = h \log (Δ) - ξ_{velocity} \\ = h \log (T (t_{2}) T (t_{1})^{- 1}) - ξ_{velocity} \end{aligned}

6.5.2 Left Jacobian

Consider differentiating with respect to left-updates of $T (t_{1})$ .

The Jacobian is $6 \times 6$ .

\begin{aligned} J_{left} (t_{1}) \equiv \frac{\partial F_{velocity}}{\partial δ (t_{1})} & = \frac{\partial}{\partial δ} h \log (T (t_{2}) {(\exp (δ) T (t_{1}))}^{- 1}) \\ = \frac{\partial}{\partial δ} h \log (\underset{Δ}{\underset{⏟}{T (t_{2}) T (t_{1})^{- 1}}} \exp (- δ)) \\ = \frac{\partial}{\partial δ} h \log (\exp (\underset{ϵ}{\underset{⏟}{- Ad (Δ) δ}}) Δ) \\ = \frac{\partial}{\partial ϵ} h \log (\exp (ϵ) Δ) \frac{\partial ϵ}{\partial δ} \\ = - h D_{\log} (Δ) Ad (Δ) \\ = - h D_{\log} (T (t_{2}) T (t_{1})^{- 1}) Ad (T (t_{2}) T (t_{1})^{- 1}) . \end{aligned}

Now, consider differentiating with respect to left-updates to $T (t_{2})$ .

\begin{aligned} J_{left} (t_{2}) \equiv \frac{\partial F_{velocity}}{\partial δ (t_{2})} & = \frac{\partial}{\partial δ} h \log (\exp (δ) T (t_{2}) T (t_{1})^{- 1}) \\ = h D_{\log} (T (t_{2}) T (t_{1})^{- 1}) . \end{aligned}

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$ :

\begin{array}{r} F_{regulariser} = ξ . \end{array}

6.6.2 Jacobian

The Jacobian is the identity:

\begin{array}{r} J_{regulariser} = I . \end{array}

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

2026年3月

C/C++ log 头文件

NAS 信息

机器人学的SE(3)约束: https://daniel.lawrence.lu/blog/2021-09-08-se-3-constraints-for-robotics/

1 Transformation parameterisation

1.1 Transforming a point

1.2 The Lie algebra

1.3 The exponential map

1.4 Notation summary

2 Derivatives

3 Optimisation

3.1 Optimisation under uncertainty

3.2 Robust loss functions

4 Trajectory representation

5 Parameterisation of the perturbation

6 Constraints

6.1 Position constraint

6.1.1 Residual

6.1.2 Left Jacobian

6.1.3 Right Jacobian

6.2 Loop closure constraint

6.2.1 Residual

6.2.2 Left Jacobians

6.2.3 Right Jacobians

6.3 Gravity constraint

6.3.1 Residual

6.3.2 Left Jacobian

6.3.3 Right Jacobian

6.4 Point constraint

6.4.1 Residual

6.4.2 Left Jacobian

6.4.3 Right Jacobian

6.5 Velocity constraint

6.5.1 Residual

6.5.2 Left Jacobian

6.6 Regularisation constraint

6.6.1 Residual

6.6.2 Jacobian

7 References

李群、李代数 基本示例程序

点云全局配准算法：Quatro++

最新文章

最近回复

分类

归档

其它

李群、李代数基本示例程序