Repository navigation
Expand file tree
/
Copy pathvelocity_integrator.py
More file actions
140 lines (109 loc) · 4.87 KB
/
Copy pathvelocity_integrator.py
File metadata and controls
140 lines (109 loc) · 4.87 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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
import torch
from torch import nn
from utils import move_to
class Velocity_Integrator(nn.Module):
def __init__(self, pos = torch.zeros(3)):
super().__init__()
self.register_buffer('pos',self._check(pos).clone(), persistent=False)
def _check(self, obj):
if obj is not None:
if len(obj.shape) == 2:
obj = obj[None, ...]
elif len(obj.shape) == 1:
obj = obj[None, None, ...]
return obj
def forward(self, dt, vel, init_state=None):
dt = self._check(dt)
B = dt.shape[0]
if init_state is None:
init_state = {'pos': self.pos}
predict = self.integrate(dt,vel,init_state)
self.pos = predict['pos'][...,-1,:]
return {**predict}
def integrate(self, dt, vel,init_state):
B, F = dt.shape[:2]
dp = torch.zeros(B,1,3,dtype = dt.dtype, device = dt.device)
# dp = torch.cat([dp, 0.5 * (vel[:,:F]+vel[:,1:])*dt ],dim=1)
dp = torch.cat([dp, vel[:,:F]*dt],dim=1)
incre_p = torch.cumsum(dp,dim=1)
return {'pos': init_state['pos'] + incre_p[:,1:,:]}
# #################TEST#############
def integrate_pos(integrator, datainte, init,dataset, device="cpu"):
out_state = dict()
vel_gt, poses_gt = [init['vel'][None,:]],[init['pos'][None,:]]
state = integrator(
dt=datainte['dt'][...,None],vel=datainte["vel"][None,...]
)
out_state['poses'] = state["pos"]
out_state['net_vel'] = datainte["vel"][None,...]
out_state['vel_gt'] = dataset.data['velocity']
out_state['poses_gt'] = dataset.data['gt_translation']
out_state['pos_dist'] = (out_state['poses'] -out_state['poses_gt'][1:,:]).norm(dim=-1)
out_state['vel_dist'] = (datainte["vel"] - out_state['vel_gt']).norm(dim=-1)
out_state['vel_mag_dist'] = torch.abs(datainte["vel"].norm(dim=-1) - out_state['vel_gt'].norm(dim=-1))
out_state['vel_error'] = (datainte["vel"] - out_state['vel_gt'])
return out_state
if __name__ == "__main__":
import argparse
import os
import torch
import torch.utils.data as Data
import tqdm
from pyhocon import ConfigFactory
from datasets import SeqDataset, imu_seq_collate
from utils import CPU_Unpickler, move_to
parser = argparse.ArgumentParser()
parser.add_argument(
"--device", type=str, default="cpu", help="cuda or cpu, Default is cuda:0"
)
parser.add_argument("--exp", type=str, default=None, help="experiment name")
parser.add_argument(
"--seqlen", type=int, default="200", help="the length of the segment"
)
parser.add_argument(
"--dataconf",
type=str,
default="configs/datasets/SubTDataset/SubT_UGV1_final_half.conf",
help="the configuration of the dataset",
)
args = parser.parse_args()
print(("\n" * 3) + str(args) + ("\n" * 3))
config = ConfigFactory.parse_file(args.dataconf)
dataset_conf = config.inference
net_result_path = os.path.join(args.exp, "net_output.pickle")
if os.path.isfile(net_result_path):
with open(net_result_path, "rb") as handle:
inference_state_load = CPU_Unpickler(handle).load()
for data_conf in dataset_conf.data_list:
for data_name in data_conf.data_drive:
dataset = SeqDataset(
data_conf.data_root,
data_name,
args.device,
name=data_conf.name,
duration=args.seqlen,
step_size=args.seqlen,
drop_last=False,
conf=dataset_conf,
)
loader = Data.DataLoader(
dataset=dataset,
batch_size=1,
collate_fn=imu_seq_collate,
shuffle=False,
drop_last=False,
)
init = dataset.get_init_value()
inference_state = inference_state_load[data_name]
integrator = Velocity_Integrator(
init['pos']).to(args.device).double()
outstate =integrate_pos(
integrator, data_inte, init, loader,
device=args.device)
relative_outstate = calculate_rte(outstate, args.seqlen,args.seqlen)
print("==============Integration==============")
print("outstate:")
print("pos_err: ", outstate['pos_dist'].mean())
print("vel_err: ", outstate['vel_dist'].mean())
print("relative_state:")
print("pos_err: ", relative_outstate['pos_dist'].mean())