ExtendedPose3

The ExtendedPose3<K> class is the generic matrix Lie group $SE_K(3)$ : the semi-direct product of SO(3) with K copies of $\mathbb{R}^3$ .

This notebook focuses on Python usage of ExtendedPose36 (the K=6 instantiation), including construction, group operations, and manifold/Lie operations.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

# Install GTSAM from pip if running in Google Colab
try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass # Not in Colab

import gtsam
import numpy as np
from gtsam import Rot3

ExtendedPose36 = gtsam.ExtendedPose36

Lie Group Math Overview¶

An element of $SE_K(3)$ is $X=(R, x_1, \ldots, x_K)$ with $R \in SO(3)$ and $x_i \in \mathbb{R}^3$ .

Group composition is:

(R, x_i) \cdot (S, y_i) = (RS, x_i + R y_i).

(1)

Group inverse is:

(R, x_i)^{-1} = (R^T, -R^T x_i).

(2)

The tangent vector is ordered as $\xi=[\omega, \rho_1, \ldots, \rho_K] \in \mathbb{R}^{3+3K}$ . The exponential and logarithm maps are:

X = \operatorname{Exp}(\xi), \quad \xi = \operatorname{Log}(X).

(3)

For the matrix-Lie-group view, $\operatorname{Hat}(\xi)$ maps tangent vectors to Lie algebra matrices and $\operatorname{Vee}(\cdot)$ is the inverse map:

\operatorname{Vee}(\operatorname{Hat}(\xi)) = \xi.

(4)

The adjoint map transports perturbations between frames:

\operatorname{Ad}_X \eta = \operatorname{AdjointMap}(X) \; \eta.

(5)

As a manifold, local updates use retraction and local coordinates:

X' = X \oplus \delta = \operatorname{Retract}_X(\delta), \quad \delta = \operatorname{Local}_X(X').

(6)

Constructing `ExtendedPose36`¶

ExtendedPose36 stores one Rot3 and six 3-vectors $x_1,\dots,x_6$ . The manifold dimension is 3 + 3*K = 21, and the homogeneous matrix is (3+K) x (3+K) = 9 x 9.

X_identity = ExtendedPose36()
print(f"k = {X_identity.k()}, dim = {X_identity.dim()}, Dim = {ExtendedPose36.Dim()}")
print("matrix shape:", X_identity.matrix().shape)
print(X_identity.matrix())

k = 6, dim = 21, Dim = 21
matrix shape: (9, 9)
[[1. 0. 0. 0. 0. 0. 0. 0. 0.]
 [0. 1. 0. 0. 0. 0. 0. 0. 0.]
 [0. 0. 1. 0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 1. 0. 0. 0. 0. 0.]
 [0. 0. 0. 0. 1. 0. 0. 0. 0.]
 [0. 0. 0. 0. 0. 1. 0. 0. 0.]
 [0. 0. 0. 0. 0. 0. 1. 0. 0.]
 [0. 0. 0. 0. 0. 0. 0. 1. 0.]
 [0. 0. 0. 0. 0. 0. 0. 0. 1.]]

Construct from rotation + 3xK block¶

R = Rot3.Yaw(np.deg2rad(20.0))
X_block = np.array([
    [1.0, 4.0, -1.0, 0.5, 2.1, -0.3],
    [2.0, 5.0,  0.5, -1.2, 3.3,  0.8],
    [3.0, 6.0,  2.0, 1.4, -2.7, 0.6],
])

X1 = ExtendedPose36(R, X_block)
print("x(0):", np.array(X1.x(0)))
print("xMatrix:", X1.xMatrix())

x(0): [1. 2. 3.]
xMatrix: [[ 1.   4.  -1.   0.5  2.1 -0.3]
 [ 2.   5.   0.5 -1.2  3.3  0.8]
 [ 3.   6.   2.   1.4 -2.7  0.6]]

Construct from homogeneous matrix¶

T = X1.matrix()
X1_from_T = ExtendedPose36(T)
print("equals:", X1.equals(X1_from_T, 1e-9))

equals: True

Group operations¶

Composition, inverse, and between follow the same Pose3-style API.

xi2 = np.array([
    0.02, -0.01, 0.03,
    0.1, 0.2, -0.1,
    -0.2, 0.3, 0.4,
    0.5, -0.6, 0.2,
    -0.3, 0.1, 0.2,
    0.4, 0.2, -0.5,
    -0.1, 0.7, 0.2,
])
X2 = ExtendedPose36.Expmap(xi2)

X12 = X1.compose(X2)
print("compose == matrix multiply:", np.allclose(X12.matrix(), X1.matrix() @ X2.matrix(), atol=1e-9))

X_between = X1.between(X2)
X_between_expected = X1.inverse().compose(X2)
print("between == inverse-compose:", X_between.equals(X_between_expected, 1e-9))

compose == matrix multiply: True
between == inverse-compose: True

Expmap / Logmap and tangent vectors¶

xi = np.array([
    0.11, -0.07, 0.05,
    0.30, -0.40, 0.10,
    -0.20, 0.60, -0.50,
    0.70, -0.10, 0.20,
    -0.40, 0.30, 0.80,
    0.50, -0.20, -0.30,
    0.10, 0.20, -0.60,
])

X = ExtendedPose36.Expmap(xi)
xi_round_trip = ExtendedPose36.Logmap(X)
print("log(exp(xi)) ~= xi:", np.allclose(xi_round_trip, xi, atol=1e-9))

log(exp(xi)) ~= xi: True

Hat / Vee¶

X_hat = ExtendedPose36.Hat(xi)
xi_from_hat = ExtendedPose36.Vee(X_hat)
print("vee(hat(xi)) ~= xi:", np.allclose(xi_from_hat, xi, atol=1e-9))

vee(hat(xi)) ~= xi: True

Manifold operations¶

As with Pose3, optimization-facing operations are retract and localCoordinates.

delta = 1e-2 * np.ones(21)
X_perturbed = X1.retract(delta)
delta_back = X1.localCoordinates(X_perturbed)
print("localCoordinates(retract(delta)) ~= delta:", np.allclose(delta_back, delta, atol=1e-7))

localCoordinates(retract(delta)) ~= delta: True

Adjoint¶

eta = np.linspace(-0.3, 0.3, 21)
Ad = X1.AdjointMap()
eta_adjoint = X1.Adjoint(eta)
print("AdjointMap shape:", Ad.shape)
print("Adjoint(eta) == AdjointMap @ eta:", np.allclose(eta_adjoint, Ad @ eta, atol=1e-9))

AdjointMap shape: (21, 21)
Adjoint(eta) == AdjointMap @ eta: True

Notes and references¶

ExtendedPose3<K> is meaning-agnostic: it only defines the group structure. Semantic interpretation of each $x_i$ (e.g., position, velocity, bias, contact points) is left to derived/application-level code.

For an invariant-filter application context (including walking-robot style state design ideas), see:

For Pose3-specific behavior and conventions, see:

Pose3 notebook