-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathplot_RO_vs_groundtruth.py
156 lines (126 loc) · 6.2 KB
/
plot_RO_vs_groundtruth.py
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
# python plot_RO_vs_groundtruth.py --input_path
# /workspace/data/landmark-distortion/ro_state_pb_developing/2019-01-10-14-50-05-radar-oxford-10k-RO-output/
import numpy as np
import matplotlib.pyplot as plt
import sys
from pathlib import Path
import shutil
from argparse import ArgumentParser
import settings
from pose_tools.pose_utils import *
from unpack_ro_protobuf import get_ro_state_from_pb, get_matrix_from_pb
# Include paths - need these for interfacing with custom protobufs
sys.path.insert(-1, "/workspace/code/corelibs/src/tools-python")
sys.path.insert(-1, "/workspace/code/corelibs/build/datatypes")
sys.path.insert(-1, "/workspace/code/radar-navigation/build/radarnavigation_datatypes_python")
from mrg.logging.indexed_monolithic import IndexedMonolithic
from mrg.adaptors.pointcloud import PbSerialisedPointCloudToPython
from mrg.pointclouds.classes import PointCloud
def plot_odometries(params, radar_state_mono):
figure_path = params.input_path + "figs_odometry/"
output_path = Path(figure_path)
if output_path.exists() and output_path.is_dir():
shutil.rmtree(output_path)
output_path.mkdir(parents=True)
se3s, timestamps = get_ground_truth_poses_from_csv(
"/workspace/data/RadarDataLogs/2019-01-10-14-50-05-radar-oxford-10k/gt/radar_odometry.csv")
# print(se3s[0])
k_start_index_from_odometry = 0
ro_se3s = []
ro_timestamps = []
for i in range(params.num_samples):
pb_state, name_scan, _ = radar_state_mono[i]
ro_state = get_ro_state_from_pb(pb_state)
relative_pose_index = i + k_start_index_from_odometry + 1
relative_pose_timestamp = timestamps[relative_pose_index]
# ensure timestamps are within a reasonable limit of each other (microseconds)
assert (ro_state.timestamp - relative_pose_timestamp) < 500
# Get motion estimate that was calculated from RO
ro_se3, ro_timestamp = get_poses_from_serialised_transform(ro_state.g_motion_estimate)
ro_se3s.append(ro_se3)
ro_timestamps.append(ro_timestamp)
start_time_offset = ro_timestamps[0]
ro_time_seconds = [(x - start_time_offset) / 1e6 for x in ro_timestamps[1:]]
ro_x, ro_y, ro_th = get_x_y_th_velocities_from_poses(ro_se3s, ro_timestamps)
gt_start_time_offset = timestamps[0]
gt_time_seconds = [(x - gt_start_time_offset) / 1e6 for x in timestamps[1:]]
gt_x, gt_y, gt_th = get_x_y_th_velocities_from_poses(se3s, timestamps)
gt_x = gt_x[k_start_index_from_odometry:]
plt.figure(figsize=(15, 5))
dim = params.num_samples
# plt.xlim(0, dim)
plt.grid()
# plt.gca().set_aspect('equal', adjustable='box')
plt.plot(ro_time_seconds, ro_x, '.-', label="ro_x")
plt.plot(ro_time_seconds, ro_y, '.-', label="ro_y")
plt.plot(ro_time_seconds, ro_th, '.-', label="ro_th")
plt.plot(gt_time_seconds[:dim], gt_x[:dim], '.-', label="gt_x")
plt.plot(gt_time_seconds[:dim], gt_y[:dim], '.-', label="gt_y")
plt.plot(gt_time_seconds[:dim], gt_th[:dim], '.-', label="gt_th")
plt.title("Pose estimates: RO vs ground-truth")
plt.xlabel("Time (s)")
plt.ylabel("units/s")
plt.legend()
plt.savefig("%s%s" % (output_path, "/odometry_comparison.pdf"))
plt.close()
def plot_basic_readings(params, radar_state_mono):
# Used to sanity check the ground truth and RO data which should be in the same reference frame when plotted here
figure_path = params.input_path + "figs_odometry/"
output_path = Path(figure_path)
if output_path.exists() and output_path.is_dir():
shutil.rmtree(output_path)
output_path.mkdir(parents=True)
gt_se3s, gt_timestamps = get_ground_truth_poses_from_csv(
"/workspace/data/RadarDataLogs/2019-01-10-14-50-05-radar-oxford-10k/gt/radar_odometry.csv")
k_start_index_from_odometry = 0
ro_se3s = []
ro_timestamps = []
for i in range(params.num_samples):
pb_state, name_scan, _ = radar_state_mono[i]
ro_state = get_ro_state_from_pb(pb_state)
relative_pose_index = i + k_start_index_from_odometry + 1
relative_pose_timestamp = gt_timestamps[relative_pose_index]
# ensure timestamps are within a reasonable limit of each other (microseconds)
assert (ro_state.timestamp - relative_pose_timestamp) < 500
# Get motion estimate that was calculated from RO
ro_se3, ro_timestamp = get_poses_from_serialised_transform(ro_state.g_motion_estimate)
ro_se3s.append(ro_se3)
ro_timestamps.append(ro_timestamp)
ro_x, ro_y, ro_th = get_x_y_th_from_se3s(ro_se3s)
gt_x, gt_y, gt_th = get_x_y_th_from_se3s(gt_se3s)
gt_x, gt_y, gt_th = gt_x[k_start_index_from_odometry:], \
gt_y[k_start_index_from_odometry:], \
gt_th[k_start_index_from_odometry:]
plt.figure(figsize=(15, 10))
dim = params.num_samples
# plt.xlim(0, dim)
plt.grid()
plt.plot(ro_x, '.-', label="ro_x")
plt.plot(ro_y, '.-', label="ro_y")
plt.plot(ro_th, '.-', label="ro_th")
plt.plot(gt_x[:dim], '.-', label="gt_x")
plt.plot(gt_y[:dim], '.-', label="gt_y")
plt.plot(gt_th[:dim], '.-', label="gt_th")
plt.title("Pose estimates: RO vs ground-truth")
plt.xlabel("Time (s)")
plt.ylabel("units/s")
plt.legend()
plt.savefig("%s%s" % (output_path, "/odometry_comparison.png"))
plt.close()
def main():
parser = ArgumentParser(add_help=False)
parser.add_argument('--input_path', type=str, default="",
help='Path to folder containing required inputs')
parser.add_argument('--num_samples', type=int, default=settings.TOTAL_SAMPLES,
help='Number of samples to process')
params = parser.parse_args()
print("Running plotting...")
# You need to run this: ~/code/corelibs/build/tools-cpp/bin/MonolithicIndexBuilder
# -i /Users/roberto/Desktop/ro_state.monolithic -o /Users/roberto/Desktop/ro_state.monolithic.index
radar_state_mono = IndexedMonolithic(params.input_path + "ro_state.monolithic")
print("Number of indices in this radar odometry state monolithic:", len(radar_state_mono))
# get a landmark set in and plot it
# plot_odometries(params, radar_state_mono)
plot_basic_readings(params, radar_state_mono)
if __name__ == "__main__":
main()