Preparing to use manif in wolf
manif
is a library for Lie groups developed by @artivis and @jsola. See here for ref: https://github.com/artivis/manif
It can be used for frame composition, point and vector transformation, motion integration, and related math operations.
It allows wolf to handle these operations in a more consistent and elegant manner across 2D or 3D, rotations, or rigid body motions. It also allows for a neat implementation of the IMU preintegration.
In my opinion, 2D and 3D odometers in wolf are the first to be migrated to manif. However, there is a little problem:
Currently, odom2d takes 2 inputs (distance,angle) to compute the motion of one step. This is problematic because a 2D pose has 3DoF, and only 2 data are passed, with a 2x2 covariances matrix. The covariance of the pose delta is, therefore, singular (or rank deficient).
In wolf, we handle this by adding a small 3x3 diagonal matrix to the covariance. This I find quite inelegant.
A better way to go is the following: Consider motion u
expressed in the tangent space se(2) of the current robot pose increment D
, which is in the Lie group SE(2), of dimension 3; use the exponential map to convert it to a local pose increment d
; compose with the previous pose increment D
. This is explained below,
- define 3 variables for motion:
- Along the wheels direction: distance
d
- across the wheels direction: lateral slippage
s
- angle turned: angle
a
- Along the wheels direction: distance
- form the vector
u = (d,s,a)
, from which(d,a)
are measured with noise, ands
is just zero-mean noise. - Define a 3x3 covariance matrix for this motion,
Q_u=diag(s_d^2, s_s^2, s_a^2)
, from three different sigma valuess_d, s_s, s_a
. -
ProcessorOdom2D::computeCurrentDelta()
: compute the current deltad
with Lie exponential map, its JacobianJ_y_x=Dy/Dx
, and covarianceQ_d
,u // in se(2) d = exp(u) // in SE(2) J_d_u = D exp(u) / D u // 3x3 matrix Q_d = J_d_u * Q_u * J_d_u.tr // 3x3 matrix
-
ProcessorOdom2D::deltaPlusDelta()
: Compose this current delta onto an integrated deltaD
,D = D (+) d // in SE(2) J_D_D, J_D_d // 3x3 matrix Q_D = J_D_D * Q_D * J_D_D.tr + J_D_d * Q_d * J_D_d.tr // 3x3 matrix
More elaborated odometers like e.g. differential drive will just compute the original u
from a particular motion model, with model parameters subject to calibration. For this, we can just derive the class Odom2D just defined above, and overload the function computeCurrentDelta()
to account for the specific motion model and its calibration parameters,
-
ProcessorDiffDrive::computeCurrentDelta()
: Compute the tangent motion from motion data and model parameters,data = (a_l, a_r, s) // 3 vector: left wheel angle, right wheel angle, lateral slippage Q_data = [...] // 3x3 matrix params = (r_l, r_r, d) // n-vector, e.g. wheels radii r_l, r_r, wheel separation d u = f ( data, params ) // in se(2) J_u_data // 3x3 matrix J_u_params // 3xn matrix Q_u = J_u_data * Q_data * J_u_data.tr d = exp(u) [... as before]
- Proceed with the algorithm on top.
- Add the integration of the Jacobian of the integrated delta
D
wrtparams
, for the sake of self-calibration,// In ProcessorMotion: J_D_params = J_D_D * J_D_params + J_D_d * J_d_u * J_u_params // 3xn matrix