Basic PointFusion¶
*NOTE:* Make sure to have ran the prerequisits section before running this section.
This section demonstrates the basic use of PointFusion.
[2]:
device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")
slam = PointFusion(device=device)
pointclouds, recovered_poses = slam(rgbdimages)
pointclouds.plotly(0, max_num_points=15000).update_layout(autosize=False, width=600).show()
Step by step PointFusion¶
*NOTE:* Make sure to have ran the prerequisits section before running this section.
This section demonstrates building the pointcloud map from one frame at a time by calling the SLAM object’s .step() method.
[3]:
# load dataset
dataset = ICL(icl_path, seqlen=4, height=240, width=320)
loader = DataLoader(dataset=dataset, batch_size=1)
colors, depths, intrinsics, poses, *_ = next(iter(loader))
# create rgbdimages object
rgbdimages = RGBDImages(colors, depths, intrinsics)
# step by step SLAM
device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")
slam = PointFusion(device=device)
pointclouds = Pointclouds(device=device)
batch_size, seq_len = rgbdimages.shape[:2]
initial_poses = torch.eye(4, device=device).view(1, 1, 4, 4).repeat(batch_size, 1, 1, 1)
prev_frame = None
for s in range(seq_len):
live_frame = rgbdimages[:, s].to(device)
if s == 0 and live_frame.poses is None:
live_frame.poses = initial_poses
pointclouds, live_frame.poses = slam.step(pointclouds, live_frame, prev_frame)
prev_frame = live_frame
pointclouds.plotly(0, max_num_points=15000).update_layout(autosize=False, width=600).show()
Advanced visualization¶
*NOTE:* Make sure to have ran the prerequisits section before running this section.
This section demonstrates visualization of the pointcloud map as it gets updated from new rgbd frames. It also visualizes the poses with frustums in the 3d map. We use ground truth poses here (odom=gt) as the data sequences were fetched with a large dilation value (i.e. small fps) and ICP/gradICP won’t work well in this case.
[4]:
import numpy as np
import plotly.graph_objects as go
def plotly_map_update_visualization(intermediate_pcs, poses, K, max_points_per_pc=50000, ms_per_frame=50):
"""
Args:
- intermediate_pcs (List[gradslam.Pointclouds]): list of gradslam.Pointclouds objects, each of batch size 1
- poses (torch.Tensor): poses for drawing frustums
- K (torch.Tensor): Intrinsics matrix
- max_points_per_pc (int): maximum number of points to plot for each pointcloud
- ms_per_frame (int): miliseconds per frame for the animation
Shape:
- poses: :math:`(L, 4, 4)`
- K: :math:`(4, 4)`
"""
def plotly_poses(poses, K):
"""
Args:
poses (np.ndarray):
K (np.ndarray):
Shapes:
- poses: :math:`(L, 4, 4)`
- K: :math:`(4, 4)`
"""
fx = abs(K[0, 0])
fy = abs(K[1, 1])
f = (fx + fy) / 2
cx = K[0, 2]
cy = K[1, 2]
cx = cx / f
cy = cy / f
f = 1.
pos_0 = np.array([0., 0., 0.])
fustum_0 = np.array(
[
[-cx, -cy, f],
[cx, -cy, f],
list(pos_0),
[-cx, -cy, f],
[-cx, cy, f],
list(pos_0),
[cx, cy, f],
[-cx, cy, f],
[cx, cy, f],
[cx, -cy, f],
]
)
traj = []
traj_frustums = []
for pose in poses:
rot = pose[:3, :3]
tvec = pose[:3, 3]
fustum_i = fustum_0 @ rot.T
fustum_i = fustum_i + tvec
pos_i = pos_0 + tvec
pos_i = np.round(pos_i, decimals=2)
fustum_i = np.round(fustum_i, decimals=2)
traj.append(pos_i)
traj_array = np.array(traj)
traj_frustum = [
go.Scatter3d(
x=fustum_i[:, 0], y=fustum_i[:, 1], z=fustum_i[:, 2],
marker=dict(
size=0.1,
),
line=dict(
color='purple',
width=4,
)
),
go.Scatter3d(
x=pos_i[None, 0], y=pos_i[None, 1], z=pos_i[None, 2],
marker=dict(
size=6.,
color='purple',
)
),
go.Scatter3d(
x=traj_array[:, 0], y=traj_array[:, 1], z=traj_array[:, 2],
marker=dict(
size=0.1,
),
line=dict(
color='purple',
width=2,
)
),
]
traj_frustums.append(traj_frustum)
return traj_frustums
def frame_args(duration):
return {
"frame": {"duration": duration, "redraw": True},
"mode": "immediate",
"fromcurrent": True,
"transition": {"duration": duration, "easing": "linear"},
}
# visualization
scatter3d_list = [pc.plotly(0, as_figure=False, max_num_points=max_points_per_pc) for pc in intermediate_pcs]
traj_frustums = plotly_poses(poses.cpu().numpy(), K.cpu().numpy())
data = [[*frustum, scatter3d] for frustum, scatter3d in zip(traj_frustums, scatter3d_list)]
steps = [
{"args": [[i], frame_args(0)], "label": i, "method": "animate"}
for i in range(seq_len)
]
sliders = [
{
"active": 0,
"yanchor": "top",
"xanchor": "left",
"currentvalue": {"prefix": "Frame: "},
"pad": {"b": 10, "t": 60},
"len": 0.9,
"x": 0.1,
"y": 0,
"steps": steps,
}
]
updatemenus = [
{
"buttons": [
{
"args": [None, frame_args(ms_per_frame)],
"label": "▶",
"method": "animate",
},
{
"args": [[None], frame_args(0)],
"label": "◼",
"method": "animate",
},
],
"direction": "left",
"pad": {"r": 10, "t": 70},
"showactive": False,
"type": "buttons",
"x": 0.1,
"xanchor": "right",
"y": 0,
"yanchor": "top",
}
]
fig = go.Figure()
frames = [{"data": frame, "name": i} for i, frame in enumerate(data)]
fig.add_traces(frames[0]["data"])
fig.update(frames=frames)
fig.update_layout(
updatemenus=updatemenus,
sliders=sliders,
showlegend=False,
scene=dict(
xaxis=dict(showticklabels=False, showgrid=False, zeroline=False, visible=False,),
yaxis=dict(showticklabels=False, showgrid=False, zeroline=False, visible=False,),
zaxis=dict(showticklabels=False, showgrid=False, zeroline=False, visible=False,),
)
)
fig.show()
return fig
[5]:
dataset = ICL(icl_path, seqlen=8, dilation=19, height=60, width=80)
loader = DataLoader(dataset=dataset, batch_size=1)
colors, depths, intrinsics, poses, *_ = next(iter(loader))
# create rgbdimages object
rgbdimages = RGBDImages(colors, depths, intrinsics, poses)
# step by step SLAM and store intermediate maps
device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")
slam = PointFusion(odom='gt', device=device) # use gt poses because large dilation (small fps) makes ICP difficult
pointclouds = Pointclouds(device=device)
batch_size, seq_len = rgbdimages.shape[:2]
initial_poses = torch.eye(4, device=device).view(1, 1, 4, 4).repeat(batch_size, 1, 1, 1)
prev_frame = None
intermediate_pcs = []
for s in range(seq_len):
live_frame = rgbdimages[:, s].to(device)
if s == 0 and live_frame.poses is None:
live_frame.poses = initial_poses
pointclouds, live_frame.poses = slam.step(pointclouds, live_frame, prev_frame)
prev_frame = live_frame if slam.odom != 'gt' else None
intermediate_pcs.append(pointclouds[0])
# visualize
rgbdimages.plotly(0).update_layout(autosize=False, height=600, width=400).show()
fig = plotly_map_update_visualization(intermediate_pcs, poses[0], intrinsics[0, 0], 10000)