Repository navigation
Expand file tree
/
Copy pathintegrate.py
More file actions
91 lines (82 loc) · 3.2 KB
/
Copy pathintegrate.py
File metadata and controls
91 lines (82 loc) · 3.2 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
import torch
import tqdm
from utils import move_to
def integrate(
integrator,
loader,
init,
device="cpu",
gtinit=False,
save_full_traj=False,
use_gt_rot=True,
):
"""
gtinit:
If gtinit is True, use the ground truth initial state to integrate the small segments.
This is used for the evaluation of the local pattern of the trajectory.
If gtinit is False, use the predicted initial state to integrate the segments.
Which is equivalent to integrate through the entire trajectory.
save_full_traj:
If save_full_traj is True, save the full trajectory.
If save_full_traj is False, save the last frame of each segment.
"""
# states to ouput
integrator.eval()
out_state = dict()
poses, poses_gt = [init["pos"][None, :]], [init["pos"][None, :]]
orientations, orientations_gt = [init["rot"][None, :]], [init["rot"][None, :]]
vel, vel_gt = [init["vel"][None, :]], [init["vel"][None, :]]
covs = [torch.zeros(9, 9)]
for idx, data in tqdm.tqdm(enumerate(loader)):
data = move_to(data, device)
if gtinit:
init_state = {
"pos": data["init_pos"][:, :1, :],
"vel": data["init_vel"][:, :1, :],
"rot": data["init_rot"][:, :1, :],
}
else:
init_state = None
init_rot = data["init_rot"] if use_gt_rot else None
state = integrator(
init_state=init_state,
dt=data["dt"],
gyro=data["gyro"],
acc=data["acc"],
rot=init_rot,
)
if save_full_traj:
vel.append(state["vel"][..., :, :].cpu())
vel_gt.append(data["gt_vel"][..., :, :].cpu())
orientations.append(state["rot"][..., :, :].cpu())
orientations_gt.append(data["gt_rot"][..., :, :].cpu())
poses_gt.append(data["gt_pos"][..., :, :].cpu())
poses.append(state["pos"][..., :, :].cpu())
else:
vel.append(state["vel"][..., -1:, :].cpu())
vel_gt.append(data["gt_vel"][..., -1:, :].cpu())
orientations.append(state["rot"][..., -1:, :].cpu())
orientations_gt.append(data["gt_rot"][..., -1:, :].cpu())
poses_gt.append(data["gt_pos"][..., -1:, :].cpu())
poses.append(state["pos"][..., -1:, :].cpu())
covs.append(state["cov"][..., -1, :, :].cpu())
out_state["vel"] = torch.cat(vel, dim=-2)
out_state["vel_gt"] = torch.cat(vel_gt, dim=-2)
out_state["orientations"] = torch.cat(orientations, dim=-2)
out_state["orientations_gt"] = torch.cat(orientations_gt, dim=-2)
out_state["poses"] = torch.cat(poses, dim=-2)
out_state["poses_gt"] = torch.cat(poses_gt, dim=-2)
out_state["covs"] = torch.stack(covs, dim=0)
out_state["pos_dist"] = (
out_state["poses"][:, 1:, :] - out_state["poses_gt"][:, 1:, :]
).norm(dim=-1)
out_state["vel_dist"] = (
out_state["vel"][:, 1:, :] - out_state["vel_gt"][:, 1:, :]
).norm(dim=-1)
out_state["rot_dist"] = (
(
out_state["orientations_gt"][:, 1:, :].Inv()
@ out_state["orientations"][:, 1:, :]
).Log()
).norm(dim=-1)
return out_state