Pose2

A Pose2 represents a position and orientation in 2D space. It is both a $\mathcal{SE}(2)$ pose manifold and a $SE(2)$ Lie group of transforms. It consists of a 2D position (variously represented as $r$ , $t$ , or $(x, y)$ depending on the context) and a rotation (similarly, $C$ , $R$ , or θ). Its 3-dimensional analog is a Pose3. It is included in the top-level gtsam package.

%pip install gtsam

import gtsam
from gtsam import Pose2, Point2, Rot2, Point3
import numpy as np

Initialization and properties¶

A Pose2 can be initialized with no arguments, which yields the identity pose, or it can be constructed with a position and rotation.

# Identity pose
p1 = Pose2()
print(p1)

R = Rot2.fromDegrees(90)
# Point2 is not an object, it is a utility function that creates a 2d float np.ndarray
t = Point2(1, 2) # or (1, 2) or [1, 2] or np.array([1, 2])
# Pose at (1, 2) and facing north
p2 = Pose2(R, t)
print(p2)

(0, 0, 0)

(1, 2, 1.5708)

The pose’s properties can be accessed using the following members:

x()
y()
translation() (which returns a two-element np.ndarray representing x, y)
theta()
rotation() (which returns a Rot2)
matrix()

The matrix() function returns the pose’s rotation and translation in the following form:

T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix}

(1)

print(f"Location: ({p2.x()}, {p2.y()}); also accessible with translation(): {p2.translation()}")

# .rotation() returns a Rot2 object; the float value can be accessed with .theta()
print(f"Rotation: {p2.theta()}; also accessible with rotation(): {p2.rotation().theta()}")

print(f"Position-rotation 3x3 matrix:\n{p2.matrix()}")

Location: (1.0, 2.0); also accessible with translation(): [1. 2.]
Rotation: 1.5707963267948966; also accessible with rotation(): 1.5707963267948966
Position-rotation 3x3 matrix:
[[ 6.123234e-17 -1.000000e+00  1.000000e+00]
 [ 1.000000e+00  6.123234e-17  2.000000e+00]
 [ 0.000000e+00  0.000000e+00  1.000000e+00]]

Basic operations¶

Points in the global space can be transformed to and from the local space of the Pose2 using transformTo and transformFrom.

global_point = Point2(5, 5)
origin = Pose2(Rot2.fromAngle(np.pi), Point2(1, 1))

# For a point at (1, 1) that is rotated 180 degrees, a point at (5, 5) in global
# space is at (-4, -4) in local space.
transformed = origin.transformTo(global_point)
print(f"{global_point} transformed by {origin} into local space -> {transformed}")

reversed = origin.transformFrom(transformed)
print(f"{transformed} transformed by {origin} into global space -> {reversed}")

[5. 5.] transformed by (1, 1, 3.14159)
 into local space -> [-4. -4.]
[-4. -4.] transformed by (1, 1, 3.14159)
 into global space -> [5. 5.]

Bearings (angles) and ranges (distances) can be calculated to points using the associated functions bearing and range.

p1 = Pose2(Rot2.fromDegrees(90), Point2(-3, -3))
point1 = Point2(-2, -3)
print(f"Bearing from {p1} to {point1}: {p1.bearing(point1).theta()}")

p2 = Pose2(Rot2.fromDegrees(-45), Point2(1, 1))
p3 = Pose2(Rot2.fromDegrees(180), Point2(0, 2))
print(f"Bearing from {p2} to {p3.translation()}: {p2.bearing(p3.translation()).theta()}")

p4 = Pose2(Rot2.fromDegrees(-90), Point2(4, 0))
point2 = Point2(0, 3)
print(f"Range from {p4} to {point2}: {p4.range(point2)}")

Bearing from (-3, -3, 1.5708)
 to [-2. -3.]: -1.5707963267948966
Bearing from (1, 1, -0.785398)
 to [0. 2.]: 3.141592653589793
Range from (4, 0, -1.5708)
 to [0. 3.]: 5.0

Manifold $\mathcal{SE}(2)$ ¶

The manifold $\mathcal{SE}(2)$ represents poses in 2D space and is defined by elements $(C, r)$ where:

$C \in \mathcal{SO}(2)$ : The orientation or attitude.
$r \in \mathbb{R}^2$ : The position.

This manifold can be thought of as all possible values that can specify the position and orientation of a rigid body in some reference frame. It does not have a notion of composition; we would never think to compose two poses of two different rigid bodies.

Manifold operations¶

Local¶

The $\text{local}(p, q)$ function maps $q$ into the local coordinate system of $p$ . Essentially, it “subtracts” $p$ from $q$ on the manifold, giving the relative pose in the local frame of $p$ . In other words, it computes the difference between two points on a manifold and expresses the result in the tangent space. It is a form of the Lie group operation $p^{-1}q$ .

Mathematically, the local function is computed as follows:

\text{local}(p,q) = (C_p^TC_q, C_p^T(r_q-r_p))

(2)

where:

$p = (C_p, r_p)$
$q = (C_q, r_q)$

so:

$C_p^TC_q$ is the relative rotation.
$C_p^T(r_q-r_p)$ is the relative translation in the frame of $p$ .

Call the localCoordinates member function to use the local function in your code. In GTSAM, the result of the local function is interpreted as a vector in the tangent space, so localCoordinates returns a 3-element np.ndarray.

p = Pose2(Rot2.fromDegrees(90), Point2(-5, -3))
q = Pose2(Rot2.fromDegrees(-45), Point2(1, 4))

print(p.localCoordinates(q))

[ 7.         -6.         -2.35619449]

Retract¶

The $\text{retract}(p, v)$ function takes a point $p$ on the manifold and a perturbation $v$ from the tangent space and maps it back onto the manifold to get a new point. It is the inverse of the $\text{local}$ function, so it may be considered as $p + v$ .

Given:

An initial pose $p = (x, y, \theta)$
A perturbation $v = (v_x, v_y, v_\theta)$

The retract function updates the pose using the following formula:

\text{Retract}(T, v) = \begin{cases} \begin{bmatrix} x \\ y \end{bmatrix} + R(\theta) \begin{bmatrix} i \\ j \end{bmatrix}, \quad \theta + v_\theta, & \text{if } v_\theta \neq 0 \\ \begin{bmatrix} x + v_x \\ y + v_y \end{bmatrix}, \quad \theta + v_\theta, & \text{if } v_\theta \approx 0 \end{cases}

(3)

where:

$R(\theta)$ is the rotation matrix:
$R(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}$
(4)
$i, j$ are computed based on the motion model:
$i = \frac{v_x \sin v_\theta + v_y (1 - \cos v_\theta)}{v_\theta}$
(5)

$j = \frac{v_x (1 - \cos v_\theta) + v_y \sin v_\theta}{v_\theta}$
(6)

Call the retract member function to use the retract function in your code. Since $v$ is in the tangent space, it must be passed as a 3D vector (a 3-element np.ndarray). retract returns the adjusted $p$ .

p = Pose2(Rot2.fromDegrees(90), Point2(-5, -3))
v = Point3(2, -1, Rot2.fromDegrees(180).theta())
q = Pose2(Rot2.fromDegrees(-45), Point2(1, 4))

print(p.retract(v))

print(q)
# Applies local and then retract, which cancel out. This statement prints q given
# any p.
print(p.retract(p.localCoordinates(q)))

(-4, -1, -1.5708)

(1, 4, -0.785398)

(1, 4, -0.785398)

Optimization on $\mathcal{SE}(2)$ ¶

Many problems involve optimization over the pose manifold, such as:

Localization: Estimating the pose of a robot or sensor in a global or local frame.
SLAM (Simultaneous Localization and Mapping): Optimizing a graph of poses and landmarks to minimize error.

Examples of these problems can be found at the end of this page.

Lie group $SE(2)$ ¶

Group operations¶

A Pose2 implements the group operations identity, inverse, compose, and between. For more information on groups and their use here, see GTSAM concepts.

Identity¶

The Pose2 identity is $(0, 0, 0)$ .

print(Pose2.Identity())

(0, 0, 0)

Inverse¶

The inverse of a pose represents the transformation that undoes the pose. In other words, if you have a pose $T$ that moves from frame A to frame B, its inverse $T^{-1}$ moves from frame B back to frame A. The equation to compute the inverse is as follows:

T^{-1} = (-x \cos\theta - y \sin\theta, x \sin\theta - y \cos\theta, -\theta)

(7)

p5 = Pose2(0, Point2(-5, 2))
print(f"Inverse of {p5} -> {p5.inverse()}")

p6 = Pose2(Rot2.fromDegrees(45), Point2(6, 4))
print(f"Inverse of {p6} -> {p6.inverse()}")

Inverse of (-5, 2, 0)
 -> (5, -2, -0)

Inverse of (6, 4, 0.785398)
 -> (-7.07107, 1.41421, -0.785398)

Composition¶

The composition of two Pose2 objects follows the rules of $\mathcal{SE}(2)$ transformation.

Given two poses:

Pose A: $T_A = (x_A, y_A, \theta_A)$
Pose B: $T_B = (x_B, y_B, \theta_B)$

The composition of these two poses $T_C = T_A \cdot T_B$ results in:

x_C = x_A + \cos(\theta_A) x_B - \sin(\theta_A) y_B

(8)

y_C = y_A + \sin(\theta_A) x_B + \cos(\theta_A) y_B

(9)

\theta_C = \theta_A + \theta_B

(10)

Therefore:

T_C = ( x_A + \cos(\theta_A) x_B - \sin(\theta_A) y_B,\space y_A + \sin(\theta_A) x_B + \cos(\theta_A) y_B,\space \theta_A + \theta_B )

(11)

In other words:

The rotation of Pose A is applied to the translation of Pose B before adding it.
The final rotation is just the sum of the two rotations.

p7 = Pose2(0, Point2(8, 10))
p8 = Pose2(Rot2.fromDegrees(135), Point2(4, -7))

print(f"Composition: {p7} * {p8} -> {p7 * p8}")
print(f"Composition is not commutative: {p8} * {p7} = {p8 * p7}")

Composition: (8, 10, 0)
 * (4, -7, 2.35619)
 -> (12, 3, 2.35619)

Composition is not commutative: (4, -7, 2.35619)
 * (8, 10, 0)
 = (-8.72792, -8.41421, 2.35619)

Between¶

Given two poses $T_A$ and $T_B$ , the between function returns the pose of $T_B$ in the local coordinate frame of $A$ ; in other words, the transformation needed to move from $T_A$ to $T_B$ .

The between function is given by:

T_{T_A→T_B} = T_A^{-1} \cdot T_B

(12)

a = Pose2(Rot2.fromDegrees(0), Point2(1, 4))
b = Pose2(Rot2.fromDegrees(45), Point2(-3, 0))

print(a.between(b))

(-4, -4, 0.785398)

Lie group operations¶

A Pose2 also implements the Lie group operations for exponential mapping, log mapping, and adjoint mapping, as well as other Lie group functionalities. For more information on Lie groups and their use here, see GTSAM concepts.

Exponential mapping¶

The exponential map function expmap is used to convert a small motion, like a velocity or perturbation, in the Lie algebra (tangent space) into a Pose2 transformation in the Lie group $\mathcal{SE}(2)$ . It is used because optimization is easier in the tangent space; transformations behave like vectors there.

In tangent space, small motions are represented as:

\xi = (\nu_x, \nu_y, \omega)

(13)

where:

$\nu_x, \nu_y$ are small translations in the local frame.
ω is a small rotation.

The exponential map converts this small motion into a full pose:

T = \exp(\xi) = \begin{cases} (x, y, \theta) = (\nu_x, \nu_y, \omega) & \text{if } \omega = 0 \\ \left( \frac{\sin\omega}{\omega} \nu_x - \frac{1 - \cos\omega}{\omega} \nu_y, \frac{1 - \cos\omega}{\omega} \nu_x + \frac{\sin\omega}{\omega} \nu_y, \omega \right) & \text{otherwise} \end{cases}

(14)

This accounts for rotational effects when mapping from the tangent space back to $\mathcal{SE}(2)$ .

twist = gtsam.Point3(0.5, 0.5, Rot2.fromDegrees(90).theta())

print(Pose2.Expmap(twist))
# There is no pose.expmap(...), only Pose2.Expmap(...). If you are looking to
# convert a small motion to a pose relative to a pose, use retract.

(0, 0.63662, 1.5708)

Log mapping¶

The log map function logmap is used to convert a transformation in $\mathcal{SE}(2)$ (such as a Pose2) into a vector in tangent space. It can be used to convert a pose to its small motion representation or compute the difference between two poses.

For a pose $T = (x,y,\theta)$ , logmap finds the equivalent motion in tangent space:

\log(T) = \left( \begin{array}{c} V^{-1} \cdot t \\ \theta \\ \end{array} \right) = \xi = (\nu_x, \nu_y, \omega)

(15)

where:

$V^{-1} = \frac{1}{A^2+B^2} \left( \begin{array}{cc} A & B \\ -B & A \end{array} \right)$
$A = \frac{\sin(\theta)}{\theta}$
$B = \frac{1 - \cos(\theta)}{\theta}$
$t = (x, y)$

pose = Pose2(Rot2.fromDegrees(135), Point2(4, -7))
diff = Pose2(Rot2.fromDegrees(135), Point2(6, -7))

# Convert a pose to its small motion representation
print(Pose2.Logmap(pose))

# Compute the difference between two poses
print(pose.logmap(pose))
print(pose.logmap(diff))

[-6.29474529 -8.12827598  2.35619449]
[0. 0. 0.]
[-1.41421356 -1.41421356  0.        ]

Advanced concepts¶

Adjoint mapping¶

Jacobians¶

Example: SLAM¶

Pose2 can be used as the basis to perform simultaneous localization and mapping (SLAM), as seen in the example here.

Initialization and properties¶

Basic operations¶

Manifold SE(2)\mathcal{SE}(2)SE(2)¶

Manifold operations¶

Local¶

Retract¶

Optimization on SE(2)\mathcal{SE}(2)SE(2)¶

Lie group SE(2)SE(2)SE(2)¶

Group operations¶

Identity¶

Inverse¶

Composition¶

Between¶

Lie group operations¶

Exponential mapping¶

Log mapping¶

Advanced concepts¶

Adjoint mapping¶

Jacobians¶

Example: SLAM¶

Manifold $\mathcal{SE}(2)$ ¶

Optimization on $\mathcal{SE}(2)$ ¶

Lie group $SE(2)$ ¶