def main():
parsed = parseArgs()
data = load_time_trans_quat(parsed.infile)
if parsed.transform_json:
transform = utility_functions.read_pose_from_json(
parsed.transform_json)
transform_4x4 = tf_helpers.transformtransformation(transform)
transformed_data = tf_helpers.left_multiply_transform(
data, transform_4x4)
else:
transformed_data = data
if parsed.plus_time_file:
with open(parsed.plus_time_file) as stream:
for index, line in enumerate(stream):
time_offset = float(line.rstrip("\n"))
if index == 0:
break
for index, _ in enumerate(transformed_data):
transformed_data[index][0] -= rospy.Duration.from_sec(time_offset)
transformed_data = downsample_if_needed(transformed_data)
bag_tf_poses(transformed_data, parsed.outfile, parsed.child_frame,
parsed.red, parsed.green, parsed.blue)
def get_odometry_covariance(self, timed_pose_list):
"""
covariance over many steps for the odometry
W and V must have (+/-)z axis along the gravity
:param timed_pose_list: each row T_WV in [rostime tx ty tz qx qy qz qw]
:return:
"""
cov = np.zeros([3, 3])
x_list = []
u_list = []
prev_time = 0.0
prev_pose = np.identity(4)
for index, timed_pose_vec in enumerate(timed_pose_list):
curr_pose = tf_helpers.transformtransformation(timed_pose_vec[1:])
if index > 0:
delta_pose = np.dot(prev_pose, curr_pose)
theta = tf_helpers.to_yaw_angle(delta_pose)
xytheta = [delta_pose[0, 3], delta_pose[1, 3], theta]
duration = timed_pose_vec[0] - prev_time
u_list.append(Control(xytheta, duration.to_sec()))
prev_time = timed_pose_vec[0]
prev_pose = np.linalg.pinv(curr_pose)
xytheta = timed_pose_vec[1:3] + \
[tf_helpers.to_yaw_angle_quat(timed_pose_vec[4:])]
x_list.append(xytheta)
for index, one_u in enumerate(u_list):
f_mat, w_mat = self.compute_jacobians(x_list[index], one_u)
noise_cov = self.get_process_noise_covariance(one_u.rel_pose)
cov = np.dot(f_mat, np.dot(cov, f_mat.transpose())) + \
np.dot(w_mat, np.dot(noise_cov, w_mat.transpose()))
return cov
def compute_loc_metrics_hokuyo(T_GI_txt, ref_T_Wl_L, T_LI_json): # pylint: disable=invalid-name
"""
:param T_GI_txt: map_align_reloccheck_debug.txt
:param ref_T_Wl_L: hokuyo lidar tf_offline_processed.txt
:param T_LI_json: calibration_optimized.json for hokuyo lidar,
tied to ref_T_Wl_L
:return:
"""
output_dir = os.path.dirname(T_GI_txt)
mat_T_IC = np.array([
[0.999908, -0.0130974, -0.00351362, 0.00681938], # pylint: disable=invalid-name
[0.0130978, 0.999914, 7.85796e-05, 0.00237952],
[0.00351229, -0.000124593, 0.999994, 2.37739e-05],
[0, 0, 0, 1]
])
# localization result with the camera frame as the reference body frame
# If T_IC is very close to Identity, this step may be extraneous
timed_marker_poses = load_and_convert_poses(T_GI_txt,
skip_rows=1,
left_multiplier=None,
right_multiplier=mat_T_IC)
T_GC_txt = os.path.join(output_dir, "temp_T_GC.txt") # pylint: disable=invalid-name
vlf.save_time_trans_quat_txt(timed_marker_poses,
T_GC_txt,
line_format=0,
delimiter=" ")
delay, right_multi_vec = read_time_pose_from_json(T_LI_json)
right_multi_mat = tf_helpers.transformtransformation(right_multi_vec)
right_multi_mat = np.dot(right_multi_mat, mat_T_IC)
timed_traj_poses = load_and_convert_poses(ref_T_Wl_L,
skip_rows=1,
left_multiplier=None,
right_multiplier=right_multi_mat,
delay_time=delay)
ref_T_WC_txt = os.path.join(output_dir, "temp_ref_T_WC.txt") # pylint: disable=invalid-name
vlf.save_time_trans_quat_txt(timed_traj_poses, ref_T_WC_txt, 0)
lmc = vlf.LocalizationMetricCalculator(T_GC_txt, ref_T_WC_txt, 5.0)
lmc.calculate()
msg = lmc.print_msg()
# os.remove(T_GC_txt)
# os.remove(ref_T_WC_txt)
return msg
def test_transformtransformation():
"""
test func
:return:
"""
mat = np.array(
[[1.00000000e+00, -1.07565694e-15, -1.55732588e-15, 1.11022302e-15],
[-6.03453836e-16, 1.00000000e+00, -3.48313117e-16, 5.55111512e-16],
[-2.66448070e-16, 3.85094971e-16, 1.00000000e+00, 1.38777878e-17],
[-3.53332323e-16, 3.11761750e-16, - 3.59784738e-16, 1.00000000e+00]]
)
vec = tf_helpers.transformtransformation(mat)
expected_vec = [0.0] * 7
expected_vec[6] = 1.0
assert np.allclose(expected_vec, vec)
dist, angle = tf_helpers.measure_difference(vec)
assert np.allclose(dist, 0.0) and np.allclose(angle, 0.0)
def generate_frustum_marker(t_q, scale=1.0, rostime=rospy.Time(0, 0),
red=1.0, green=1.0, blue=0.0):
"""
:param t_q: txyz, qxyzw
:param scale:
:return:
"""
marker = Marker()
marker.header.frame_id = "/world"
marker.header.stamp = rostime
marker.ns = "namespace"
marker.id = 0
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = red
marker.color.g = green
marker.color.b = blue
marker.color.a = 1.0
marker.id += 1
T4X4 = tf_helpers.transformtransformation(t_q)
for point_index in APEX_ORDER_NUM:
pinC = ABCDE[point_index, :]
pinC_scaled = pinC * scale
pinC_scaled[3] = 1.0
T_WC = np.dot(T4X4, T_BC)
pinW = np.dot(T_WC, pinC_scaled)
p = Point()
p.x = pinW[0]
p.y = pinW[1]
p.z = pinW[2]
marker.points.append(p)
return marker
def detect_loc_outliers(
loctimes,
loc_T_GCp,
ref_T_WCp, # pylint: disable=invalid-name
ref_timed_T_WCp_full, # pylint: disable=invalid-name
mahal_tol,
lc_gravity_deviation_tol):
"""
detect outliers in localizations relative to a reference trajectory
:param loctimes: rospy epochs of localizations
:param loc_T_GCp: poses of localizations at every epoch with the Cp
frame being the reference body frame.
The reference body frame should have its z axis roughly along
the gravity direction
Each pose [tx ty tz qx qy qz qw]
:param ref_T_WCp: poses of reference trajectory at every epoch have the
same length and the same reference body frame as loc_T_GCp
Each pose [tx ty tz qx qy qz qw]
ref_T_WCp may have None entries as some poses may not be interpolatable
:return: estimated number of outliers,
and the probabilities of a loc being an outlier
if the prob is 0.5, it means that the loc has been adjacent to an outlier
if the prob is 1, it means that the loc is probably an outlier,
"""
outliercount = 0
outlierprob = [0.0] * len(loc_T_GCp)
if len(loc_T_GCp) < 2:
return outliercount, outlierprob
if len(loctimes) != len(loc_T_GCp):
raise ValueError("Should provide the same number of epochs and "
"localizations, which are {} and {}, resp.".format(
len(loctimes), len(loc_T_GCp)))
if len(loc_T_GCp) != len(ref_T_WCp):
raise ValueError(
"Should provide localizations and reference poses at "
"the same epochs. Current #loc. {} #ref_poses {}".format(
len(loc_T_GCp), len(ref_T_WCp)))
reftimeandpose = None # the first of a pair of localizations
for index, temp_T_GCp in enumerate(loc_T_GCp): # pylint: disable=invalid-name
if not ref_T_WCp[index]:
print("Reference pose not available at {} sec".format(
loctimes[index].to_sec()))
continue
# gravity consistency check
T_GCp1 = tf_helpers.transformtransformation(temp_T_GCp) # pylint: disable=invalid-name
T_WCp1 = tf_helpers.transformtransformation(ref_T_WCp[index]) # pylint: disable=invalid-name
T_WG = np.dot(T_WCp1, np.linalg.pinv(T_GCp1)) # pylint: disable=invalid-name
gravity_consistent = math.acos(T_WG[2, 2]) < lc_gravity_deviation_tol
if not gravity_consistent:
print("failed gravity check at {} sec".format(
loctimes[index].to_sec()))
outlierprob[index] += 1.0
outliercount += 2
continue
if not reftimeandpose: # first entry
reftimeandpose = [loctimes[index], index]
continue
T_GCp0 = tf_helpers.transformtransformation(
loc_T_GCp[reftimeandpose[1]]) # pylint: disable=invalid-name
T_WCp0 = tf_helpers.transformtransformation(
ref_T_WCp[reftimeandpose[1]]) # pylint: disable=invalid-name
T_Cp0W = np.linalg.pinv(T_WCp0) # pylint: disable=invalid-name
T_Cp0Cp1 = np.dot(T_Cp0W, T_WCp1) # pylint: disable=invalid-name
# T_GCp0 may have a very large translation,
# causing its pinv(3,3) close to zero
T_Cp0Cp1_loc = np.dot(np.linalg.pinv(T_GCp0), T_GCp1) # pylint: disable=invalid-name
deltaT = np.dot(T_Cp0Cp1_loc, np.linalg.pinv(T_Cp0Cp1)) # pylint: disable=invalid-name
delta_vec = tf_helpers.to_xy_theta(deltaT)
delta_arr = np.array(delta_vec)
ddm = differential_drive_model.DifferentialDriveModel(
0.03, 0.01, 0.01, 0.0076)
try:
timed_pose_list = crop_timed_pose_list(
[reftimeandpose[0]] + ref_T_WCp[reftimeandpose[1]],
[loctimes[index]] + ref_T_WCp[index], ref_timed_T_WCp_full)
except ValueError as err:
print("crop begin {} data begin {} crop end {} data end {}".format(
reftimeandpose[0].to_sec(),
ref_timed_T_WCp_full[0][0].to_sec(), loctimes[index].to_sec(),
ref_timed_T_WCp_full[-1][0].to_sec()))
raise err
delta_cov = ddm.get_delta_T_covariance(timed_pose_list)
dist = math.sqrt(
np.dot(delta_arr,
np.dot(np.linalg.pinv(delta_cov), delta_arr.transpose())))
if dist > mahal_tol:
outlierprob[reftimeandpose[1]] += 0.5
outlierprob[index] += 0.5
outliercount += 1
print("dist {} mahal_tol {} for {} and {}".format(
dist, mahal_tol, reftimeandpose[0].to_sec(),
loctimes[index].to_sec()))
# print("T_Cp0Cp1\n{}\ndeltaT\n{}\ndelta_cov\n{}\ndist {} mahal {}".format(
# T_Cp0Cp1, deltaT, delta_cov,
# dist, mahal_tol))
reftimeandpose[0] = loctimes[index]
reftimeandpose[1] = index
# unfortunately, an inlier sandwiched by two outliers will have a prob 1.0
# the below trick tries to alleviate the issue
for index, val in enumerate(outlierprob):
if index > 1:
if outlierprob[index - 2] == 1.0 and outlierprob[index - 1] == 1.0 \
and val == 1.0:
outlierprob[index - 1] = 0.0
# sanity check
count = 0
for val in outlierprob:
if val >= 1.0:
count += 1
outliercount = outliercount / 2.0
tolerance = 2.0
if abs(outliercount - count) >= tolerance:
warnings.warn(
"#outlier computed from outlier probabilities {} deviates"
" more than {} from the estimated #outlier {}".format(
count, tolerance, outliercount))
print("outlier count {} and prob {}".format(outliercount, outlierprob))
return outliercount, outlierprob
def draw_traj_on_map_one_dir(map_folder, pose_result_dir, calib_dir):
"""
:param map_folder: containing map.pgm and yaml
:param pose_result_dir: dir containing candidate traj pose txt
:param calib_dir: dir containing calibration json files
:return: a handle to fig
"""
map_pgm = os.path.join(map_folder, "map.pgm")
map_yaml = os.path.join(map_folder, "map.yaml")
skip_rows = 0
left_multi_mat = None
right_multi_mat = None
trt = TrajResultType.UNKNOWN
candidates = ["amcl_pose_result.txt", "map_align_pose_debug.txt"]
if os.path.isfile(os.path.join(pose_result_dir, candidates[0])):
trt = TrajResultType.AMCL_LOC
elif os.path.isfile(os.path.join(pose_result_dir, candidates[1])):
trt = TrajResultType.APR_LOC
else:
raise NotImplementedError(
"Missing known pose files under {}".format(pose_result_dir))
calib_json = ["lidarworld_T_vioworld.json", "calibration_optimized.json"]
loc_basename = "map_align_reloccheck_debug.txt"
B_T_I_json = None # pylint: disable=invalid-name
if trt == TrajResultType.APR_LOC:
pose_result_file = os.path.join(pose_result_dir, candidates[1])
skip_rows = 1
Wl_T_M_json = os.path.join(calib_dir, calib_json[0]) # pylint: disable=invalid-name
B_T_I_json = os.path.join(calib_dir, calib_json[1]) # pylint: disable=invalid-name
if os.path.isfile(Wl_T_M_json):
_, left_multi_vec = read_time_pose_from_json(Wl_T_M_json)
left_multi_mat = tf_helpers.transformtransformation(left_multi_vec)
if os.path.isdir(B_T_I_json):
_, right_multi_vec = read_time_pose_from_json(B_T_I_json)
right_multi_mat = tf_helpers.transformtransformation(
right_multi_vec)
right_multi_mat = np.linalg.pinv(right_multi_mat)
elif trt == TrajResultType.AMCL_LOC:
pose_result_file = os.path.join(pose_result_dir, candidates[0])
skip_rows = 0
timed_traj_poses = load_and_convert_poses(pose_result_file,
skip_rows=skip_rows,
left_multiplier=left_multi_mat,
right_multiplier=right_multi_mat)
timed_marker_poses = None
msg = ""
loc_time_to_dist = None
if trt == TrajResultType.APR_LOC:
loc_result_file = os.path.join(pose_result_dir, loc_basename)
if os.path.isfile(loc_result_file):
timed_marker_poses = load_and_convert_poses(
loc_result_file,
skip_rows=1,
left_multiplier=left_multi_mat,
right_multiplier=right_multi_mat)
info_debug_txt = os.path.join(pose_result_dir, "info_debug.txt")
test_data_dir = ""
with open(info_debug_txt, "r") as stream:
for index, line in enumerate(stream):
if index == 0:
str_list = line.split(":")
test_data_dir = str_list[1].strip()
break
print("test data dir {} vs pose_result_file path {}".format(
test_data_dir,
os.path.basename(os.path.dirname(pose_result_file))))
assert os.path.basename(test_data_dir) in pose_result_file
# hack begin
fake_test_data_dir = "/persist/data/huyue_lidar_dataset/B6-B1/lidar"
if "B2" in pose_result_file:
fake_test_data_dir = "/persist/data/huyue_lidar_dataset/B2-F1/lidar"
test_data_dir = os.path.join(fake_test_data_dir,
os.path.basename(test_data_dir))
# hack end
ref_T_W_Cp_txt = folder_helper.find_file_under_folder( # pylint: disable=invalid-name
test_data_dir, "pose_fisheye_tf.txt")
msg, std_prf, loc_time_to_dist, dist_interval = \
compute_loc_metrics_odom(loc_result_file, ref_T_W_Cp_txt)
test_data_basename = os.path.basename(os.path.dirname(pose_result_file))
# hack begin
# test_data_basename = test_data_basename.split("_2019-05")[0]
# hack end
title_str = test_data_basename + "\n" + msg
figs = []
if timed_traj_poses:
figs.append(
plot_traj_on_raster_map(map_pgm, map_yaml, timed_traj_poses,
timed_marker_poses, title_str,
pose_result_dir))
if loc_time_to_dist is not None:
figs.append(
draw_loc_histogram(std_prf, loc_time_to_dist, dist_interval,
pose_result_dir))
return figs, title_str