Repository navigation
Expand file tree
/
Copy pathevaluate.py
More file actions
176 lines (144 loc) · 7.97 KB
/
Copy pathevaluate.py
File metadata and controls
176 lines (144 loc) · 7.97 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
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
# output the trajctory in the world frame for visualization and evaluation
import os, sys
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), os.pardir)))
import os
import glob
import json
import argparse
import numpy as np
import pypose as pp
import torch
import torch.utils.data as Data
from pyhocon import ConfigFactory
from datasets import imu_seq_collate, SeqDataset
from utils import CPU_Unpickler, interp_xyz
from integrate import integrate
from velocity_integrator import Velocity_Integrator, integrate_pos
def calculate_rte(outstate,duration, step_size):
poses, poses_gt = outstate['poses'],outstate['poses_gt'][1:,:]
dp = poses[:, duration-1:] - poses[:, :-duration+1]
dp_gt = poses_gt[duration-1:] - poses_gt[:-duration+1]
rte = (dp - dp_gt).norm(dim=-1)
return rte
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("--device", type=str, default="cuda:1", help="cuda or cpu")
parser.add_argument("--exp", type=str, default="experiments/nymeria", help="Path for AirIO netoutput")
parser.add_argument("--seqlen", type=int, default="1000", help="the length of the segment")
parser.add_argument("--dataconf", type=str, default="configs/nymeria.conf", help="the configuration of the dataset")
parser.add_argument("--savedir",type=str,default = "./result/loss_result",help = "Directory where the results wiil be saved")
parser.add_argument("--usegtrot", action="store_true", help="Use ground truth rotation for gravity compensation")
args = parser.parse_args();
print(("\n"*3) + str(args) + ("\n"*3))
config = ConfigFactory.parse_file(args.dataconf)
dataset_conf = config.eval
print(dataset_conf.keys())
if args.exp is not None:
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()
else:
raise Exception(f"Unable to load the network result: {net_result_path}")
folder = args.savedir
os.makedirs(folder, exist_ok=True)
AllResults = []
net_out_result = {}
for data_conf in dataset_conf.data_list:
print(data_conf)
if isinstance(data_conf.data_drive, list) and len(data_conf.data_drive) == 0:
data_drive = sorted(
os.path.splitext(os.path.basename(p))[0]
for p in glob.glob(os.path.join(data_conf["data_root"], "*.pkl"))
)
else:
data_drive = list(data_conf.data_drive)
for data_name in data_drive:
print(data_conf.data_root, data_name)
print("data_conf.dataroot", data_conf.data_root)
print("data_name", data_name)
print("data_conf.name", data_conf.name)
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()
gravity = dataset.get_gravity()
integrator_outstate = pp.module.IMUPreintegrator(
init['pos'], init['rot'], init['vel'],gravity=gravity,
reset=False
).to(args.device)
integrator_reset = pp.module.IMUPreintegrator(
init['pos'], init['rot'], init['vel'],gravity = gravity,
reset=True
).to(args.device)
outstate = integrate(
integrator_outstate, loader, init,
device=args.device, gtinit=False, save_full_traj=True,
use_gt_rot=args.usegtrot
)
relative_outstate = integrate(
integrator_reset, loader, init,
device=args.device, gtinit=True,
use_gt_rot=args.usegtrot
)
if args.exp is not None:
motion_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)
motion_loader = Data.DataLoader(dataset=motion_dataset, batch_size=1, collate_fn=imu_seq_collate, shuffle=False, drop_last=False)
inference_state = inference_state_load[data_name]
gt_ts = motion_dataset.data['time']
vel_ts = inference_state['ts']
indices = torch.cat([torch.where(gt_ts == item)[0] for item in vel_ts[:,0]]).to(torch.int32)
if "coordinate" in dataset_conf.keys():
print("*************",dataset_conf["coordinate"],"*************")
if dataset_conf["coordinate"] == "body_coord":
rotation = motion_dataset.data['gt_orientation'] # Set data['gt_orientation'] using AirIMU or gt-truth in the dataconf
vel_dist = rotation[indices,:] * inference_state['net_vel'] - motion_dataset.data['velocity'][indices,:]
net_vel = interp_xyz(gt_ts, vel_ts[:,0], inference_state['net_vel']).to(rotation.dtype)
net_vel = rotation * net_vel
if dataset_conf["coordinate"] == "glob_coord":
vel_dist = inference_state['net_vel'] - motion_dataset.data['velocity'][indices,:]
net_vel = interp_xyz(gt_ts, vel_ts[:,0], inference_state['net_vel']).to(motion_dataset.data['velocity'].dtype)
if data_conf.name == "BlackBird":
save_prefix = os.path.dirname(data_name).split('/')[1]
else:
save_prefix = data_name
dt = gt_ts[1:] - gt_ts[:-1]
data_inte = {"vel":net_vel,'dt':dt}
integrator_vel =Velocity_Integrator(
init['pos']).to(args.device)
inf_outstate =integrate_pos(
integrator_vel, data_inte, init, motion_dataset,
device=args.device
)
inf_rte = calculate_rte(inf_outstate, args.seqlen,args.seqlen)
pred_traj = inf_outstate['poses'][0]
gt_traj = inf_outstate['poses_gt'][1:]
err = pred_traj - gt_traj
err_v = err[..., 1].abs()
err_h = err[..., [0, 2]].norm(dim=-1)
ate_v = torch.sqrt((err_v ** 2).mean()).item()
ate_h = torch.sqrt((err_h ** 2).mean()).item()
final_err = err[-1].norm().item()
total_dist = (gt_traj[1:] - gt_traj[:-1]).norm(dim=-1).sum().item()
drift_rate = 100.0 * final_err / total_dist if total_dist > 0 else float('nan')
#save loss result
result_dic = {
'name': data_name,
'ATE':torch.sqrt((inf_outstate['pos_dist']**2).mean()).item(),
'ATE_V': ate_v,
'ATE_H': ate_h,
'AVE':inf_outstate['vel_dist'].mean().item(),
'RP_RMSE': np.sqrt((inf_rte**2).mean()).numpy().item(),
'drift_rate': drift_rate,
}
AllResults.append(result_dic)
print("==============Integration==============")
print("outstate:")
print("pos_err: ", outstate['pos_dist'].mean())
print("rte",relative_outstate['vel_dist'].mean())
print("==============AirIO==============")
print("infstate:")
print("pos_err: ", inf_outstate['pos_dist'].mean())
print("rte",inf_rte.mean())
file_path = os.path.join(folder, "result.json")
with open(file_path, 'w') as f:
json.dump(AllResults, f, indent=4)