Skip to content

Commit 1e10fee

Browse files
contagoncontagon
committed
Add in README image
1 parent 3874aae commit 1e10fee

File tree

4 files changed

+275
-1
lines changed

4 files changed

+275
-1
lines changed

‎.github/assets/readme.png‎

4.36 MB
Loading

‎README.md‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,9 @@
44
<!-- <a href="https://github.com/rpl-cmu/form/blob/master/LICENSE"><img src="https://img.shields.io/github/license/rpl-cmu/form" /></a> -->
55
</div>
66

7-
FORM is a LiDAR Odometry system that performs fixed-lag smoothing over a window of prior poses while repairing the map, all in real-time with minimal parameters.
7+
![](.github/assets/readme.png)
8+
9+
FORM is a LiDAR Odometry system that performs fixed-lag smoothing over a window of prior poses along with map reparations, all in real-time with minimal parameters.
810

911
## Building
1012

‎experiments/readme_viz.py‎

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
from pathlib import Path
2+
from typing import cast
3+
from evalio import datasets as ds, types as ty, pipelines as pl
4+
from tqdm import tqdm
5+
import pyvista as pv
6+
import seaborn as sns
7+
import numpy as np
8+
from time import time
9+
import pickle
10+
11+
from itertools import chain
12+
13+
from env import GRAPHICS_DIR
14+
15+
# ------------------------- Config! ------------------------- #
16+
dataset = ds.OxfordSpires.observatory_quarter_01
17+
18+
start = 450
19+
end = 572
20+
21+
window_size_big = [1000, 600]
22+
window_size_small = [600, 600]
23+
24+
sphere_config = {
25+
"theta_resolution": 16,
26+
"phi_resolution": 16,
27+
}
28+
sphere_map_radius = 0.07
29+
sphere_feat_radius = 0.1
30+
31+
cam_vals = {
32+
"distance": 90.0,
33+
"azimuth": 160.0,
34+
"elevation": 25.0,
35+
}
36+
37+
far_focal_point = (-7, 5, 0)
38+
zoomed_focal_point = (-16, 11.5, 0)
39+
zoom_big = 1.25
40+
zoom_small = 8.0
41+
42+
c = sns.color_palette("colorblind")[:3]
43+
44+
45+
def set_camera_position(
46+
pl: pv.Plotter,
47+
focal_point: tuple[float, float, float],
48+
distance: float,
49+
azimuth: float,
50+
elevation: float,
51+
):
52+
"""Set the camera position given spherical coordinates."""
53+
pl.camera.focal_point = focal_point
54+
azimuth = np.radians(azimuth)
55+
elevation = np.radians(elevation)
56+
x = focal_point[0] + distance * np.cos(elevation) * np.cos(azimuth)
57+
y = focal_point[1] + distance * np.cos(elevation) * np.sin(azimuth)
58+
z = focal_point[2] + distance * np.sin(elevation)
59+
pl.camera.position = (x, y, z)
60+
61+
62+
# ------------------------- Loop through to get map ------------------------- #
63+
cache = Path(".cache") / dataset.full_name / f"{start}_{end}.pkl"
64+
pipe = pl.get_pipeline("form")
65+
if isinstance(pipe, Exception):
66+
raise pipe
67+
out = ty.Experiment.from_pl_ds(pipe, dataset).setup()
68+
if isinstance(out, Exception):
69+
raise out
70+
pipe, dataset = out
71+
72+
global_map = {}
73+
features = None
74+
pose = None
75+
76+
if cache.exists():
77+
print(f"Loading from {cache}")
78+
with open(cache, "rb") as f:
79+
global_map, features, pose = pickle.load(f)
80+
81+
else:
82+
curr_idx = 0
83+
loop = tqdm(total=end)
84+
for data in dataset.lidar():
85+
if isinstance(data, ty.LidarMeasurement):
86+
# visualize in rerun
87+
if curr_idx > start:
88+
# feed through pipeline
89+
features = pipe.add_lidar(data)
90+
pose = pipe.pose()
91+
global_map = pipe.map()
92+
93+
curr_idx += 1
94+
loop.update(1)
95+
if curr_idx >= end:
96+
break
97+
98+
loop.close()
99+
100+
cache.parent.mkdir(parents=True, exist_ok=True)
101+
pickle.dump((global_map, features, pose), open(cache, "wb"))
102+
103+
if features is None or global_map is None or pose is None:
104+
raise ValueError("No features or map found")
105+
106+
# ------------------------- Visualize ------------------------- #
107+
start = time()
108+
pl = pv.Plotter(
109+
off_screen=True,
110+
window_size=window_size_big,
111+
lighting="three lights",
112+
)
113+
pose = pose * dataset.imu_T_lidar()
114+
R = pose.rot.toMat()
115+
t = pose.trans
116+
117+
# plot map
118+
map_sphere = pv.Sphere(radius=sphere_map_radius, **sphere_config) # type: ignore
119+
map_points = list(chain.from_iterable(global_map.values()))
120+
map_colors = np.vstack(
121+
[np.tile(c[p.col % len(c)], (map_sphere.n_points, 1)) for p in map_points]
122+
)
123+
map_points = np.array([[p.x, p.y, p.z] for p in map_points])
124+
map_pv = pv.PolyData(map_points).glyph(
125+
geom=map_sphere,
126+
scale=False,
127+
orient=False,
128+
)
129+
map_pv = cast(pv.PolyData, map_pv)
130+
pl.add_mesh(
131+
map_pv,
132+
scalars=map_colors,
133+
rgb=True,
134+
smooth_shading=True,
135+
render=False,
136+
)
137+
138+
# plot features
139+
feat_sphere = pv.Sphere(radius=sphere_feat_radius, **sphere_config) # type: ignore
140+
feat_points = list(chain.from_iterable(features.values()))
141+
feat_points = np.array([[p.x, p.y, p.z] for p in feat_points])
142+
feat_points = feat_points @ R.T + t
143+
feat_pv = pv.PolyData(feat_points).glyph(
144+
geom=feat_sphere,
145+
scale=False,
146+
orient=False,
147+
)
148+
feat_pv = cast(pv.PolyData, feat_pv)
149+
pl.add_mesh(
150+
feat_pv,
151+
color="k",
152+
smooth_shading=True,
153+
render=False,
154+
)
155+
156+
# set camera location
157+
pl.camera.zoom(zoom_big)
158+
set_camera_position(
159+
pl,
160+
focal_point=far_focal_point,
161+
**cam_vals,
162+
)
163+
# pl.show_axes() # type: ignore
164+
pl.screenshot(GRAPHICS_DIR / "readme_big.png")
165+
166+
pl.window_size = window_size_small
167+
set_camera_position(
168+
pl,
169+
focal_point=zoomed_focal_point,
170+
**cam_vals,
171+
)
172+
pl.camera.zoom(zoom_small)
173+
pl.update()
174+
pl.screenshot(GRAPHICS_DIR / "readme_small.png")
175+
176+
time_elapsed = time() - start
177+
print(f"Rendered in {time_elapsed:.2f} seconds")

‎experiments/readme_viz.tex‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
\documentclass{article}
2+
3+
% import tikz
4+
\usepackage{tikz}
5+
\usetikzlibrary{decorations.pathreplacing, positioning, calc, backgrounds, arrows.meta}
6+
7+
% make the document the exact size of the figure
8+
\usepackage[active,tightpage]{preview}
9+
\renewcommand\PreviewBbAdjust{0bp 0bp 0bp 0bp}
10+
\PreviewEnvironment[]{tikzpicture}
11+
\usepackage[T1]{fontenc}
12+
13+
% Colors - seaborn colorblind palette
14+
\definecolor{color0}{HTML}{0173b2} % blue
15+
\definecolor{color1}{HTML}{de8f05} % orange
16+
\definecolor{color2}{HTML}{029e73} % green
17+
\definecolor{color3}{HTML}{d55e00}
18+
\definecolor{color4}{HTML}{cc78bc}
19+
\definecolor{color5}{HTML}{ca9161}
20+
\definecolor{color6}{HTML}{fbafe4}
21+
\definecolor{color7}{HTML}{949494}
22+
\definecolor{color8}{HTML}{ece133}
23+
\definecolor{color9}{HTML}{56b4e9}
24+
25+
\begin{document}
26+
\begin{tikzpicture}[
27+
CIRCLE/.style={circle, draw, very thick, inner sep=0pt},
28+
FAC/.style={rectangle, minimum size=1mm, inner sep=0pt, draw, fill},
29+
OLD/.style={fill=color7!55, draw=color7!55},
30+
]
31+
% Setup all constants
32+
\coordinate (zoom) at (-0.6,-0.9);
33+
\pgfmathsetlengthmacro{\circleradius}{0.155\columnwidth};
34+
\pgfmathsetlengthmacro{\circlediameter}{2.0 * \circleradius};
35+
36+
% ------------------------- main image ------------------------- %
37+
\node[inner sep=1pt] (image) at (0,0) {
38+
\includegraphics[width=0.99\columnwidth]{../graphics/readme_big.png}
39+
};
40+
41+
% ------------------------- zoom in ------------------------- %
42+
\begin{scope}[shift={(image.south west)}, xshift=130mm, yshift=57mm]
43+
\begin{scope}
44+
\path[clip] (0,0) circle [x radius=\circleradius, y radius=\circleradius];
45+
\node[inner sep=0pt] {
46+
\includegraphics[width=\circlediameter]{../graphics/readme_small.png}
47+
};
48+
\end{scope}
49+
\node[CIRCLE, minimum size=\circlediameter] (zoom_big) {};
50+
\end{scope}
51+
52+
\node[circle, draw, thick, minimum size=9mm, inner sep=0pt] (zoom_small) at (zoom) {};
53+
\path[draw, thick] (zoom_big.north west) -- (zoom_small.north west);
54+
\path[draw, thick] (zoom_big.south) -- (zoom_small.south);
55+
56+
57+
% ------------------------- Factor graph ------------------------- %
58+
\node[CIRCLE, minimum size=\circlediameter, fill=white] (graph) [below=2mm of zoom_big] {};
59+
60+
\begin{scope}[node distance=2mm]
61+
\node[CIRCLE, thin, minimum size=5mm, draw=none, text=white, fill=color0!80] (x_0) at (graph) [xshift=-15mm] {\(X\)};
62+
\node[CIRCLE, thin, minimum size=5mm, draw=none, text=white, fill=color1!80] (x_1) [right=of x_0] {\(X\)};
63+
\node[CIRCLE, thin, minimum size=5mm, draw=none, text=white, fill=color2!80] (x_2) [right=of x_1] {\(X\)};
64+
65+
\node (dots) [right=of x_2] {\(\cdots\)};
66+
\node[CIRCLE, thin, minimum size=5mm, draw=none, text=white, fill=black] (x_3) [right=of dots] {\(X\)};
67+
\end{scope}
68+
69+
% Marg Factor
70+
\node[FAC] (marg) [below=7mm of x_2] {};
71+
\draw (x_0) -- (marg);
72+
\draw (x_1) -- (marg);
73+
\draw (x_2) -- (marg);
74+
\draw (dots) -- (marg);
75+
76+
% Random connections
77+
\path[OLD] (x_0) edge[bend left = 55] node[FAC, OLD, pos=0.5] {} (x_1);
78+
\path[OLD] (x_0) edge[bend left = 55] node[FAC, OLD, pos=0.5] {} (x_2);
79+
\path[OLD] (x_0) edge[bend left = 55] node[FAC, OLD, pos=0.5] {} (dots);
80+
\path[OLD] (x_1) edge[bend left = 55] node[FAC, OLD, pos=0.5] {} (x_2);
81+
\path[OLD] (x_1) edge[bend left = 55] node[FAC, OLD, pos=0.5] {} (dots);
82+
\path[OLD] (x_2) edge[bend left = 55] node[FAC, OLD, pos=0.5] {} (dots);
83+
84+
% Most recent connections
85+
\path (x_0) edge[bend left=65] node[FAC, pos=0.35] {} (x_3);
86+
\path (x_1) edge[bend left=65] node[FAC, pos=0.35] {} (x_3);
87+
\path (x_2) edge[bend left=65] node[FAC, pos=0.35] {} (x_3);
88+
\path (dots) edge[bend left=65] node[FAC, pos=0.35] {} (x_3);
89+
90+
91+
% arrow between circles
92+
\path[->, -{Latex[length=3mm]}, ultra thick] (zoom_big.south east) edge[bend right=-45] (graph.north east);
93+
94+
\end{tikzpicture}
95+
\end{document}

0 commit comments

Comments
 (0)