Skip to content

Commit 875ba9d

Browse files
dellaertdellaert
authored
Merge pull request borglab#2180 from borglab/feature/similarity
Updates to Similarity2 and Similarity3
2 parents da36c1f + 2c834fe commit 875ba9d

File tree

6 files changed

+1199
-31
lines changed

6 files changed

+1199
-31
lines changed

‎gtsam/geometry/doc/Similarity2.ipynb‎

Lines changed: 376 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,376 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "markdown",
5+
"metadata": {},
6+
"source": [
7+
"# Similarity2\n",
8+
"\n",
9+
"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."
10+
]
11+
},
12+
{
13+
"cell_type": "markdown",
14+
"metadata": {},
15+
"source": [
16+
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Similarity2.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
17+
]
18+
},
19+
{
20+
"cell_type": "code",
21+
"execution_count": 1,
22+
"metadata": {},
23+
"outputs": [],
24+
"source": [
25+
"# Install GTSAM and Plotly from pip if running in Google Colab\n",
26+
"try:\n",
27+
" import google.colab\n",
28+
" %pip install --quiet gtsam-develop \n",
29+
"except ImportError:\n",
30+
" pass # Not in Colab"
31+
]
32+
},
33+
{
34+
"cell_type": "code",
35+
"execution_count": 2,
36+
"metadata": {},
37+
"outputs": [],
38+
"source": [
39+
"import gtsam\n",
40+
"from gtsam import Similarity2, Rot2, Point2, Pose2\n",
41+
"import numpy as np"
42+
]
43+
},
44+
{
45+
"cell_type": "markdown",
46+
"metadata": {},
47+
"source": [
48+
"## Initialization and Properties\n",
49+
"\n",
50+
"A `Similarity2` can be initialized in several ways:\n",
51+
"- With no arguments to create an identity transform `(R=I, t=0, s=1)`.\n",
52+
"- With a `Rot2` for rotation, a `Point2` for translation, and a `float` for scale."
53+
]
54+
},
55+
{
56+
"cell_type": "code",
57+
"execution_count": 3,
58+
"metadata": {},
59+
"outputs": [
60+
{
61+
"name": "stdout",
62+
"output_type": "stream",
63+
"text": [
64+
"Identity:\n",
65+
"\n",
66+
"\n",
67+
"R:\n",
68+
": 0\n",
69+
"t: 0 0 s: 1\n",
70+
"\n",
71+
"S1:\n",
72+
"\n",
73+
"\n",
74+
"R:\n",
75+
": 0.523599\n",
76+
"t: 10 20 s: 2\n",
77+
"\n"
78+
]
79+
}
80+
],
81+
"source": [
82+
"# Identity transform\n",
83+
"s_identity = gtsam.Similarity2()\n",
84+
"print(f\"Identity:\\n{s_identity}\")\n",
85+
"\n",
86+
"# Transform with 30-degree rotation, translation (10, 20), and scale 2\n",
87+
"R = Rot2.fromDegrees(30)\n",
88+
"t = Point2(10, 20)\n",
89+
"s = 2.0\n",
90+
"S1 = Similarity2(R, t, s)\n",
91+
"print(f\"S1:\\n{S1}\")"
92+
]
93+
},
94+
{
95+
"cell_type": "markdown",
96+
"metadata": {},
97+
"source": [
98+
"The transform's properties can be accessed using `rotation()`, `translation()`, and `scale()`. The 3x3 matrix representation can be obtained with `matrix()`."
99+
]
100+
},
101+
{
102+
"cell_type": "code",
103+
"execution_count": 4,
104+
"metadata": {},
105+
"outputs": [
106+
{
107+
"name": "stdout",
108+
"output_type": "stream",
109+
"text": [
110+
"Rotation:\n",
111+
"theta: 0.523599\n",
112+
"\n",
113+
"Translation: [10. 20.]\n",
114+
"Scale: 2.0\n",
115+
"Matrix:\n",
116+
"[[ 0.8660254 -0.5 10. ]\n",
117+
" [ 0.5 0.8660254 20. ]\n",
118+
" [ 0. 0. 0.5 ]]\n"
119+
]
120+
}
121+
],
122+
"source": [
123+
"print(f\"Rotation:\\n{S1.rotation()}\")\n",
124+
"print(f\"Translation: {S1.translation()}\")\n",
125+
"print(f\"Scale: {S1.scale()}\")\n",
126+
"print(f\"Matrix:\\n{S1.matrix()}\")"
127+
]
128+
},
129+
{
130+
"cell_type": "markdown",
131+
"metadata": {},
132+
"source": [
133+
"### Mathematical Representation\n",
134+
"\n",
135+
"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}$.\n",
136+
"\n",
137+
"It can be represented by a $3 \\times 3$ matrix:\n",
138+
"$$\n",
139+
"T = \\begin{pmatrix} R & t \\\\ 0 & s^{-1} \\end{pmatrix} \\in \\text{Sim(2)}\n",
140+
"$$\n",
141+
"\n",
142+
"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:\n",
143+
"$$ \n",
144+
"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}\n",
145+
"$$\n",
146+
"\n",
147+
"The inverse of a transform $T_1$ is:\n",
148+
"$$ \n",
149+
"T_1^{-1} = \\begin{pmatrix} R_1^T & -s_1 R_1^T t_1 \\\\ 0 & s_1 \\end{pmatrix}\n",
150+
"$$\n",
151+
"\n",
152+
"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:\n",
153+
"$$ \n",
154+
"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}\n",
155+
"$$"
156+
]
157+
},
158+
{
159+
"cell_type": "markdown",
160+
"metadata": {},
161+
"source": [
162+
"## Basic Operations\n",
163+
"\n",
164+
"`Similarity2` can transform `Point2` and `Pose2` objects."
165+
]
166+
},
167+
{
168+
"cell_type": "code",
169+
"execution_count": 5,
170+
"metadata": {},
171+
"outputs": [
172+
{
173+
"name": "stdout",
174+
"output_type": "stream",
175+
"text": [
176+
"S1.transformFrom([1. 1.]) = [20.73205081 42.73205081]\n",
177+
"S1.transformFrom((5, 5, 0.785398)\n",
178+
") = (23.6603, 53.6603, 1.309)\n",
179+
"\n",
180+
"\n"
181+
]
182+
}
183+
],
184+
"source": [
185+
"# Transform a Point2\n",
186+
"p1 = Point2(1, 1)\n",
187+
"p2 = S1.transformFrom(p1)\n",
188+
"print(f\"S1.transformFrom({p1}) = {p2}\")\n",
189+
"\n",
190+
"# Transform a Pose2\n",
191+
"pose1 = Pose2(Rot2.fromDegrees(45), Point2(5, 5))\n",
192+
"pose2 = S1.transformFrom(pose1)\n",
193+
"print(f\"S1.transformFrom({pose1}) = {pose2}\\n\")"
194+
]
195+
},
196+
{
197+
"cell_type": "markdown",
198+
"metadata": {},
199+
"source": [
200+
"## Lie Group Sim(2)\n",
201+
"\n",
202+
"`Similarity2` implements the Lie group operations `identity`, `inverse`, `compose`, and `between`."
203+
]
204+
},
205+
{
206+
"cell_type": "code",
207+
"execution_count": 6,
208+
"metadata": {},
209+
"outputs": [
210+
{
211+
"name": "stdout",
212+
"output_type": "stream",
213+
"text": [
214+
"S1 * S2 = \n",
215+
"\n",
216+
"\n",
217+
"R:\n",
218+
": 1.5708\n",
219+
"t: 26.8301 38.1699 s: 1\n",
220+
"\n",
221+
"S1.inverse() = \n",
222+
"\n",
223+
"\n",
224+
"R:\n",
225+
": -0.523599\n",
226+
"t: -37.3205 -24.641 s: 0.5\n",
227+
"\n",
228+
"S1.between(S2) = \n",
229+
"\n",
230+
"\n",
231+
"R:\n",
232+
": 0.523599\n",
233+
"t: -72.8109 -56.1122 s: 0.25\n",
234+
"\n"
235+
]
236+
}
237+
],
238+
"source": [
239+
"# Create a second transform\n",
240+
"S2 = Similarity2(Rot2.fromDegrees(60), Point2(5, -5), 0.5)\n",
241+
"\n",
242+
"# Composition (group product)\n",
243+
"s_composed = S1 * S2\n",
244+
"print(f\"S1 * S2 = \\n{s_composed}\")\n",
245+
"\n",
246+
"# Inverse\n",
247+
"s_inv = S1.inverse()\n",
248+
"print(f\"S1.inverse() = \\n{s_inv}\")\n",
249+
"\n",
250+
"# Between (relative transform): S1.inverse() * S2 \n",
251+
"s_between = S1.between(S2)\n",
252+
"print(f\"S1.between(S2) = \\n{s_between}\")"
253+
]
254+
},
255+
{
256+
"cell_type": "markdown",
257+
"metadata": {},
258+
"source": [
259+
"### Lie Algebra\n",
260+
"\n",
261+
"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]`. \n",
262+
"- `(v_x, v_y)` is the translational velocity.\n",
263+
"- `omega` is the angular velocity.\n",
264+
"- `lambda` is the log of the rate of scale change.\n",
265+
"\n",
266+
"The `Expmap` and `Logmap` functions convert between the Lie algebra and the Lie group."
267+
]
268+
},
269+
{
270+
"cell_type": "code",
271+
"execution_count": 7,
272+
"metadata": {},
273+
"outputs": [
274+
{
275+
"name": "stdout",
276+
"output_type": "stream",
277+
"text": [
278+
"Expmap([0.1 0.2 0.78539816 0.40546511]) =\n",
279+
"\n",
280+
"\n",
281+
"R:\n",
282+
": 0.785398\n",
283+
"t: 0.00791887 0.179002 s: 1.5\n",
284+
"\n",
285+
"Logmap(S_exp) = [0.1 0.2 0.78539816 0.40546511]\n"
286+
]
287+
}
288+
],
289+
"source": [
290+
"# Create a vector in the Lie algebra\n",
291+
"xi = np.array([0.1, 0.2, np.pi/4, np.log(1.5)])\n",
292+
"\n",
293+
"# Exponential map: from Lie algebra to group\n",
294+
"S_exp = Similarity2.Expmap(xi)\n",
295+
"print(f\"Expmap({xi}) =\\n{S_exp}\")\n",
296+
"\n",
297+
"# Logarithm map: from group to Lie algebra\n",
298+
"xi_log = Similarity2.Logmap(S_exp)\n",
299+
"print(f\"Logmap(S_exp) = {xi_log}\")"
300+
]
301+
},
302+
{
303+
"cell_type": "markdown",
304+
"metadata": {},
305+
"source": [
306+
"## Manifold Operations\n",
307+
"\n",
308+
"`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.\n",
309+
"\n",
310+
"For Lie groups in GTSAM, `retract(v)` is equivalent to `p.compose(Expmap(v))`, and `localCoordinates(q)` is `Logmap(p.inverse().compose(q))`. "
311+
]
312+
},
313+
{
314+
"cell_type": "code",
315+
"execution_count": 8,
316+
"metadata": {},
317+
"outputs": [
318+
{
319+
"name": "stdout",
320+
"output_type": "stream",
321+
"text": [
322+
"localCoordinates(q) from p = [ 0.48657861 -13.38262603 1.22173048 0.55961579]\n",
323+
"p.retract(v) =\n",
324+
"\n",
325+
"\n",
326+
"R:\n",
327+
": 1.39626\n",
328+
"t: 8 -5 s: 2.1\n",
329+
"\n",
330+
"Original q =\n",
331+
"\n",
332+
"\n",
333+
"R:\n",
334+
": 1.39626\n",
335+
"t: 8 -5 s: 2.1\n",
336+
"\n"
337+
]
338+
}
339+
],
340+
"source": [
341+
"p = Similarity2(Rot2.fromDegrees(10), Point2(1,2), 1.2)\n",
342+
"q = Similarity2(Rot2.fromDegrees(80), Point2(8,-5), 2.1)\n",
343+
"\n",
344+
"# Find the tangent vector to go from p to q\n",
345+
"v = p.localCoordinates(q)\n",
346+
"print(f\"localCoordinates(q) from p = {v}\")\n",
347+
"\n",
348+
"# Move from p along v to get back to q\n",
349+
"q_retracted = p.retract(v)\n",
350+
"print(f\"p.retract(v) =\\n{q_retracted}\")\n",
351+
"print(f\"Original q =\\n{q}\")"
352+
]
353+
}
354+
],
355+
"metadata": {
356+
"kernelspec": {
357+
"display_name": "py312",
358+
"language": "python",
359+
"name": "python3"
360+
},
361+
"language_info": {
362+
"codemirror_mode": {
363+
"name": "ipython",
364+
"version": 3
365+
},
366+
"file_extension": ".py",
367+
"mimetype": "text/x-python",
368+
"name": "python",
369+
"nbconvert_exporter": "python",
370+
"pygments_lexer": "ipython3",
371+
"version": "3.12.6"
372+
}
373+
},
374+
"nbformat": 4,
375+
"nbformat_minor": 2
376+
}

0 commit comments

Comments
 (0)