Gal3

The Gal3 class implements two related but distinct concepts: a kinematic state on a manifold, and the Galilean group of spacetime transformations SGal(3). Unlike a Pose3 which describes a static pose (position and orientation), a Gal3 state describes a full kinematic state, including velocity and time. It is a 10-dimensional manifold. For users familiar with NavState, which also models attitude, position, and velocity, Gal3 is different in that it forms a complete Lie group, with a well-defined composition rule that correctly propagates states through time.

import gtsam
from gtsam import Gal3, Rot3, Point3, Event
import numpy as np

`Gal3` as a Manifold¶

First, we can view Gal3 as a manifold representing the kinematic state of an object. An element on this manifold is a tuple $(C, p, v, t)$ where:

$C \in SO(3)$ : The attitude (orientation).
$p \in \mathbb{R}^3$ : The position.
$v \in \mathbb{R}^3$ : The velocity.
$t \in \mathbb{R}$ : The time.

This is useful for representing the state of a dynamic system in estimation problems like trajectory optimization.

# Identity element (state at the origin)
g_identity = gtsam.Gal3()
print(f"Identity State:\n{g_identity}\n")

# A custom kinematic state
attitude = Rot3.Yaw(np.pi / 6)
position = Point3(10, 20, 30)
velocity = np.array([1, 2, 3])
time = 5.0
G1 = Gal3(attitude, position, velocity, time)
print(f"Kinematic State G1:\n{G1}")

Identity State:
R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
r: 0 0 0
v: 0 0 0
t: 0


Kinematic State G1:
R: [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]
r: 10 20 30
v: 1 2 3
t: 5

The state’s components can be accessed using accessor methods that align with this manifold view.

print(f"Attitude:\n{G1.attitude()}\n")
print(f"Position: {G1.position()}")
print(f"Velocity: {G1.velocity()}")
print(f"Time: {G1.time()}")

Attitude:
R: [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]


Position: [10. 20. 30.]
Velocity: [1. 2. 3.]
Time: 5.0

The Lie Group SGal(3)¶

The Gal3 class also implements the 3D Galilean group of spacetime transformations. When viewed as a transform, an element $g=(R, r, v, t)$ maps spacetime coordinates from one frame to another. In this context, we often refer to the components as rotation and translation, and Gal3 provides aliases for this purpose:

rotation() is an alias for attitude().
translation() is an alias for position().

The group operation defines how to compose two such transformations. For two transforms $g_1=(R_1, r_1, v_1, t_1)$ and $g_2=(R_2, r_2, v_2, t_2)$ , their composition $g_{comp} = g_1 \cdot g_2$ is:

R_{comp} = R_1 R_2

(1)

v_{comp} = R_1 v_2 + v_1

(2)

r_{comp} = R_1 r_2 + t_2 v_1 + r_1

(3)

t_{comp} = t_1 + t_2

(4)

This composition rule correctly integrates the kinematic states.

# Create a second transform
G2 = Gal3(Rot3.Yaw(np.pi / 12), Point3(5, 5, 5), np.array([4, 2, 5]), 2.0)

# Inverse
g_inv = G1.inverse()
print(f"G1.inverse():\n{g_inv}\n")

# Composition
g_composed = G1 * G2
print(f"G1 * G2:\n{g_composed}\n")

G1.inverse():
R: [
	0.866025, 0.5, 0;
	-0.5, 0.866025, 0;
	0, 0, 1
]
r: -9.33013 -6.16025      -15
v: -1.86603 -1.23205       -3
t: -5


G1 * G2:
R: [
	0.707107, -0.707107, 0;
	0.707107, 0.707107, 0;
	0, 0, 1
]
r: 13.8301 30.8301      41
v:  3.4641 5.73205       8
t: 7

Group Action on Spacetime Events¶

The natural action for a Galilean transform $g=(R,r,v,t)$ is on a spacetime Event, which is a point in space and time $e=(p_{in}, t_{in})$ . The act method transforms an event into a new reference frame. The new event $(p_{out}, t_{out})$ is calculated as:

t_{out} = t + t_{in}

(5)

p_{out} = R \cdot p_{in} + v \cdot t_{in} + r

(6)

This action describes how the coordinates of a spacetime event change under the Galilean transformation.

# Create a spacetime event
event = Event(10.0, Point3(2, 4, 6))

# Apply the Gal3 transform to the event
transformed_event = G1.act(event)
print(f"Transformed Event:\n{transformed_event}")

Transformed Event:
{'time':15, 'location': 19.7321 44.4641      66}

Lie Algebra and Manifold Operations¶

The Lie algebra of SGal(3), sgal(3), is the 10D tangent space at the identity. A tangent vector xi is a 10D vector xi = [ρ, ν, θ, τ] where ρ (displacement), ν (velocity change), θ (rotation vector) are 3D vectors and τ is a scalar time change. The Expmap and Logmap functions convert between this Lie algebra representation and the Gal3 group element.

As with other Lie groups in GTSAM, Gal3 is a manifold, and the retract and localCoordinates methods are used for optimization. By default, they are implemented using the group’s Expmap and Logmap.

# Create a vector in the Lie algebra [rho, nu, theta, tau]
xi = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.1, 0.05, 0.1, 0.7])

# Exponential map: from Lie algebra to group
G_exp = Gal3.Expmap(xi)
print(f"Expmap(xi):\n{G_exp}\n")

# Logarithm map: from group to Lie algebra
xi_log = Gal3.Logmap(G_exp)
print(f"Logmap(G_exp) = {np.round(xi_log, 4)}")

Expmap(xi):
R: [
	0.993762, -0.0971301, 0.0548033;
	0.102121, 0.990019, -0.0971301;
	-0.0448221, 0.102121, 0.993762
]
r: 0.235734 0.362209 0.520662
v: 0.390601 0.489186 0.614805
t: 0.7


Logmap(G_exp) = [0.1  0.2  0.3  0.4  0.5  0.6  0.1  0.05 0.1  0.7 ]

p = Gal3(Rot3.Yaw(np.pi / 4), Point3(15, 30, 50), np.array([-2, 0, 0]), 8.0)
q = G1 # from the first example

# Find the tangent vector to go from p to q
v = p.localCoordinates(q)
print(f"localCoordinates(q) from p = {np.round(v, 3)}\n")

# Move from p along v to get back to q
q_retracted = p.retract(v)
print(f"p.retract(v):\n{q_retracted}")

localCoordinates(q) from p = [ -9.429  -1.365 -15.5     3.608  -0.24    3.      0.      0.     -0.262
  -3.   ]

p.retract(v):
R: [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]
r: 10 20 30
v: 1 2 3
t: 5

Gal3 as a Manifold¶

The Lie Group SGal(3)¶

Group Action on Spacetime Events¶

Lie Algebra and Manifold Operations¶

`Gal3` as a Manifold¶