Similarity2 - GTSAM Docs

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

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

See LICENSE for the license information

# 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 Similarity2, Rot2, Point2, Pose2
import numpy as np

Initialization and Properties¶

A Similarity2 can be initialized in several ways:

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

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

# Transform with 30-degree rotation, translation (10, 20), and scale 2
R = Rot2.fromDegrees(30)
t = Point2(10, 20)
s = 2.0
S1 = Similarity2(R, t, s)
print(f"S1:\n{S1}")

Identity:


R:
: 0
t: 0 0 s: 1

S1:


R:
: 0.523599
t: 10 20 s: 2

The transform’s properties can be accessed using rotation(), translation(), and scale(). The 3x3 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{S1.matrix()}")

Rotation:
theta: 0.523599

Translation: [10. 20.]
Scale: 2.0
Matrix:
[[ 0.8660254 -0.5       10.       ]
 [ 0.5        0.8660254 20.       ]
 [ 0.         0.         0.5      ]]

Mathematical Representation¶

GTSAM’s Similarity2 implementation follows a convention similar to that described in Ethan Eade’s “Lie Groups for 2D and 3D Transformations”. An element of Sim(2) is a tuple $(R, t, s)$ with $R \in SO(2)$ , $t \in \mathbb{R}^2$ , and $s \in \mathbb{R}$ .

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

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

(1)

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} R_1 & t_1 \\ 0 & s_1^{-1} \end{pmatrix} \begin{pmatrix} R_2 & t_2 \\ 0 & s_2^{-1} \end{pmatrix} = \begin{pmatrix} R_1 R_2 & R_1 t_2 + s_2^{-1} t_1 \\ 0 & (s_1 s_2)^{-1} \end{pmatrix}

(2)

The inverse of a transform $T_1$ is:

T_1^{-1} = \begin{pmatrix} R_1^T & -s_1 R_1^T t_1 \\ 0 & s_1 \end{pmatrix}

(3)

The action of a transform $T$ on a 2D point $p$ is defined as $p' = s(Rp + t)$ . This can be derived from the matrix form by applying it to a homogeneous point $\mathbf{x} = (p, 1)^T$ and re-normalizing:

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

(4)

Basic Operations¶

Similarity2 can transform Point2 and Pose2 objects.

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

# Transform a Pose2
pose1 = Pose2(Rot2.fromDegrees(45), Point2(5, 5))
pose2 = S1.transformFrom(pose1)
print(f"S1.transformFrom({pose1}) = {pose2}\n")

S1.transformFrom([1. 1.]) = [20.73205081 42.73205081]
S1.transformFrom((5, 5, 0.785398)
) = (23.6603, 53.6603, 1.309)

Lie Group Sim(2)¶

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

# Create a second transform
S2 = Similarity2(Rot2.fromDegrees(60), Point2(5, -5), 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:
: 1.5708
t: 26.8301 38.1699 s: 1

S1.inverse() = 


R:
: -0.523599
t: -37.3205  -24.641 s: 0.5

S1.between(S2) = 


R:
: 0.523599
t: -72.8109 -56.1122 s: 0.25

Lie Algebra¶

The Lie algebra of Sim(2) is the tangent space at the identity. It is represented by a 4D vector xi = [v_x, v_y, omega, lambda].

(v_x, v_y) is the translational velocity.
omega is the angular 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, np.pi/4, np.log(1.5)])

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

# Logarithm map: from group to Lie algebra
xi_log = Similarity2.Logmap(S_exp)
print(f"Logmap(S_exp) = {xi_log}")

Expmap([0.1        0.2        0.78539816 0.40546511]) =


R:
: 0.785398
t: 0.00791887   0.179002 s: 1.5

Logmap(S_exp) = [0.1        0.2        0.78539816 0.40546511]

Manifold Operations¶

Similarity2 is also a manifold. The retract operation maps a tangent vector v from the tangent space at a Similarity2 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 = Similarity2(Rot2.fromDegrees(10), Point2(1,2), 1.2)
q = Similarity2(Rot2.fromDegrees(80), Point2(8,-5), 2.1)

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

# 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.48657861 -13.38262603   1.22173048   0.55961579]
p.retract(v) =


R:
: 1.39626
t:  8 -5 s: 2.1

Original q =


R:
: 1.39626
t:  8 -5 s: 2.1