Similarity3 - GTSAM Docs

A Similarity3 represents a similarity transformation in 3D space, which is a combination of a rotation, a translation, and a uniform scaling. It is an element of the special similarity group Sim(3), which is also a Lie group. Its 2-dimensional analog is Similarity2. It is included in the top-level gtsam package.

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

import gtsam
from gtsam import Similarity3, Rot3, Point3, Pose3
import numpy as np

Initialization and Properties¶

A Similarity3 can be initialized in several ways:

With no arguments to create an identity transform (R=I, t=0, s=1).
With a Rot3 for rotation, a Point3 for translation, and a float for scale.

# Identity transform
s_identity = gtsam.Similarity3()
print(f"Identity:\n{s_identity}")

# Transform with 30-degree yaw, translation (10, 20, 30), and scale 2
R = Rot3.Yaw(np.pi / 6)
t = Point3(10, 20, 30)
s = 2.0
S1 = Similarity3(R, t, s)
print(f"S1:\n{S1}")

Identity:


R:
 [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t: 0 0 0 s: 1

S1:


R:
 [
	0.866025, -0.5, 0;
	0.5, 0.866025, 0;
	0, 0, 1
]
t: 10 20 30 s: 2

The transform’s properties can be accessed using rotation(), translation(), and scale(). The 4x4 matrix representation can be obtained with matrix().

print(f"Rotation:\n{S1.rotation()}")
print(f"Translation: {S1.translation()}")
print(f"Scale: {S1.scale()}")
print(f"Matrix:\n{np.round(S1.matrix(), 3)}")

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

Translation: [10. 20. 30.]
Scale: 2.0
Matrix:
[[ 0.866 -0.5    0.    10.   ]
 [ 0.5    0.866  0.    20.   ]
 [ 0.     0.     1.    30.   ]
 [ 0.     0.     0.     0.5  ]]

Mathematical Representation¶

GTSAM’s Similarity3 implementation follows a convention similar to that described in Ethan Eade’s “Lie Groups for 2D and 3D Transformations”. An element of Sim(3) is a tuple $(R, t, s)$ where:

\mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3, s \in \mathbb{R}

(1)

It can be represented by a $4 \times 4$ matrix:

T = \begin{pmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & s^{-1} \end{pmatrix} \in \text{Sim(3)}

(2)

The composition of two transforms $T_1 = (R_1, t_1, s_1)$ and $T_2 = (R_2, t_2, s_2)$ is given by matrix multiplication:

T_1 \cdot T_2 = \begin{pmatrix} \mathbf{R}_1 & \mathbf{t}_1 \\ \mathbf{0} & s_1^{-1} \end{pmatrix} \begin{pmatrix} \mathbf{R}_2 & \mathbf{t}_2 \\ \mathbf{0} & s_2^{-1} \end{pmatrix} = \begin{pmatrix} \mathbf{R}_1 \mathbf{R}_2 & \mathbf{R}_1 \mathbf{t}_2 + s_2^{-1} \mathbf{t}_1 \\ \mathbf{0} & (s_1 s_2)^{-1} \end{pmatrix}

(3)

The inverse of a transform $T_1$ is:

T_1^{-1} = \begin{pmatrix} \mathbf{R}_1^T & -s_1 \mathbf{R}_1^T \mathbf{t}_1 \\ \mathbf{0} & s_1 \end{pmatrix}

(4)

The action of a transform $T$ on a 3D point $\mathbf{x}$ is defined for homogeneous points in $\mathbb{RP}^3$ . For a point $\mathbf{x} = (x, y, z, w)^T$ :

T \cdot \mathbf{x} = \begin{pmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & s^{-1} \end{pmatrix} \cdot \mathbf{x} = \begin{pmatrix} \mathbf{R} (x \ y \ z)^T + w\mathbf{t} \\ s^{-1}w \end{pmatrix} \thicksim \begin{pmatrix} s(\mathbf{R} (x \ y \ z)^T + w\mathbf{t}) \\ w \end{pmatrix}

(5)

For a standard 3D point (where $w=1$ ), this simplifies to the action $p' = s(Rp + t)$ .

Basic Operations¶

Similarity3 can transform Point3 and Pose3 objects.

# Transform a Point3
p1 = Point3(1, 1, 1)
p2 = S1.transformFrom(p1)
print(f"S1 * {p1} = {p2}")

# Transform a Pose3
pose1 = Pose3(Rot3.RzRyRx(0.1, 0.2, 0.3), Point3(5, 5, 5))
pose2 = S1.transformFrom(pose1)
print(f"S1.transformFrom(pose1) =\n{pose2}")

S1 * [1. 1. 1.] = [20.73205081 42.73205081 62.        ]
S1.transformFrom(pose1) =
R: [
	0.666039, -0.716453, 0.207576;
	0.718973, 0.69074, 0.0771696;
	-0.198669, 0.0978434, 0.97517
]
t: 23.6603 53.6603      70

Lie Group Sim(3)¶

Similarity3 implements the Lie group operations identity, inverse, compose, and between.

# Create a second transform
S2 = Similarity3(Rot3.Pitch(-np.pi/3), Point3(5, -5, 2), 0.5)

# Composition (group product)
s_composed = S1 * S2
print(f"S1 * S2 = \n{s_composed}")

# Inverse
s_inv = S1.inverse()
print(f"S1.inverse() = \n{s_inv}")

# Between (relative transform): S1.inverse() * S2
s_between = S1.between(S2)
print(f"S1.between(S2) = \n{s_between}")

S1 * S2 = 


R:
 [
	0.433013, -0.5, -0.75;
	0.25, 0.866025, -0.433013;
	0.866025, 0, 0.5
]
t: 26.8301 38.1699      62 s: 1

S1.inverse() = 


R:
 [
	0.866025, 0.5, 0;
	-0.5, 0.866025, 0;
	0, 0, 1
]
t: -37.3205  -24.641      -60 s: 0.5

S1.between(S2) = 


R:
 [
	0.433013, 0.5, -0.75;
	-0.25, 0.866025, 0.433013;
	0.866025, 0, 0.5
]
t: -72.8109 -56.1122     -118 s: 0.25

Lie Algebra¶

The Lie algebra of Sim(3) is the tangent space at the identity. It is represented by a 7D vector xi = [omega_x, omega_y, omega_z, v_x, v_y, v_z, lambda].

(omega_x, omega_y, omega_z) is the angular velocity.
(v_x, v_y, v_z) is the translational velocity.
lambda is the log of the rate of scale change.

The Expmap and Logmap functions convert between the Lie algebra and the Lie group.

# Create a vector in the Lie algebra
xi = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, np.log(1.5)])

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

# Logarithm map: from group to Lie algebra
xi_log = Similarity3.Logmap(S_exp)
print(f"Logmap(S_exp) = {np.round(xi_log, 5)}")

Expmap(xi) =


R:
 [
	0.935755, -0.283165, 0.210192;
	0.302933, 0.950581, -0.0680313;
	-0.18054, 0.127335, 0.97529
]
t:  0.31224 0.436154 0.482059 s: 1.5

Logmap(S_exp) = [0.1     0.2     0.3     0.4     0.5     0.6     0.40547]

Manifold Operations¶

Similarity3 is also a manifold. The retract operation maps a tangent vector v from the tangent space at a Similarity3 p to a new point on the manifold. The localCoordinates is the inverse operation.

For Lie groups in GTSAM, retract(v) is equivalent to p.compose(Expmap(v)), and localCoordinates(q) is Logmap(p.inverse().compose(q)).

p = Similarity3(Rot3.RzRyRx(0.1, 0.2, 0.3), Point3(1,2,3), 1.2)
q = Similarity3(Rot3.RzRyRx(0.4, 0.5, 0.6), Point3(8,-5,4), 2.1)

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

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

localCoordinates(q) from p = [ 0.193  0.359  0.201  3.857 -9.991  7.439  0.56 ]
p.retract(v) =


R:
 [
	0.7243, -0.365982, 0.584334;
	0.49552, 0.865602, -0.0720659;
	-0.479426, 0.341747, 0.808307
]
t:  8 -5  4 s: 2.1

Original q =


R:
 [
	0.7243, -0.365982, 0.584334;
	0.49552, 0.865602, -0.0720659;
	-0.479426, 0.341747, 0.808307
]
t:  8 -5  4 s: 2.1

Alignment¶

Similarity3 provides a static Align method that can compute the transformation that best aligns sets of corresponding pairs. This is useful for tasks like registering two point clouds or aligning coordinate frames.

# Example: Align two coordinate frames (e.g., world 'w' and egovehicle 'e')
# using pairs of poses of objects observed in both frames.
expected_wSe = Similarity3(Rot3.Ypr(np.pi, 0, 0), Point3(2, 3, 5), 2.0)

# Create source poses (objects in egovehicle frame 'e')
eTo1 = Pose3(Rot3(), Point3(0, 0, 0))
eTo2 = Pose3(Rot3(np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])), Point3(4, 0, 0))

# Create destination poses (same objects in world frame 'w')
wTo1 = expected_wSe.transformFrom(eTo1)
wTo2 = expected_wSe.transformFrom(eTo2)

# Create a list of corresponding pose pairs
correspondences = [(wTo1, eTo1), (wTo2, eTo2)]

# Recover the transformation wSe using Align
actual_wSe = Similarity3.Align(correspondences)
print(f"Expected wSe:\n{expected_wSe}")
print(f"Actual wSe from Align:\n{actual_wSe}")

Expected wSe:


R:
 [
	-1, -1.22465e-16, 0;
	1.22465e-16, -1, 0;
	-0, 0, 1
]
t: 2 3 5 s: 2

Actual wSe from Align:


R:
 [
	-1, -1.22465e-16, 0;
	1.22465e-16, -1, 0;
	0, 0, 1
]
t: 2 3 5 s: 2