def find_loop_closures(traj, cum_dist):
num_loop_closures = 0
filtered_loop_closures = 0
idx_list = []
for i in range(
0, traj.shape[0], 8
): #check for loop closure points (compare current frame with all future points, and do this for every 20th frame)
current_pose = traj[i]
current_trans = current_pose[0:3, 3]
current_rot = SO3.from_matrix(current_pose[0:3, 0:3],
normalize=True).to_rpy()
current_yaw = current_rot[2]
current_cum_dist = cum_dist[i]
loop_closure_idx = np.linalg.norm(
np.abs(current_trans[0:3] - traj[i + 1:, 0:3, 3]), axis=1) <= 7
dist_idx = (cum_dist[i + 1:] - current_cum_dist) >= 10
loop_closure_idx = loop_closure_idx & dist_idx
idx = np.where(loop_closure_idx == 1)
if idx != np.array([]):
for pose_idx in idx[0]:
T = traj[i + 1:][pose_idx]
yaw = SE3.from_matrix(T, normalize=True).rot.to_rpy()[2]
yaw_diff = np.abs(np.abs(current_yaw) - np.abs(yaw))
in_range = ((yaw_diff <= 0.15)) or (np.abs(
(np.pi - yaw_diff) <= 0.15))
filtered_loop_closures += in_range
if in_range:
idx_list.append(pose_idx + i)
num_loop_closures += np.sum(loop_closure_idx)
return num_loop_closures, filtered_loop_closures, idx_list
def add_noise(pose_list, trans_noise, rot_noise):
''' Add Noise to the pose measurements as a percentage of the translation and rotation magnitude'''
#Create the translation noise vectors
trans_noise_sigma = trans_noise * np.linalg.norm(
np.array([pose.trans for pose in pose_list]), axis=1)
trans_noise_vec = [
np.random.randn((3)) * trans_noise_sigma[i]
for i in range(len(pose_list))
]
#Create the rotation noise vectors
rot_noise_sigma = rot_noise * np.linalg.norm(
np.array([pose.rot.log() for pose in pose_list]), axis=1)
rot_noise_unit_vecs = np.random.rand(3, len(pose_list))
rot_noise_unit_vecs = rot_noise_unit_vecs / np.linalg.norm(
rot_noise_unit_vecs, axis=0)
rot_noise_vec = [
rot_noise_sigma[i] * np.random.rand() * rot_noise_unit_vecs[:, i]
for i in range(len(pose_list))
]
#Add the noise to the poses
noisy_poses = [
SE3(rot=pose.rot.dot(SO3.exp(rot_noise_vec[i])),
trans=pose.trans + trans_noise_vec[i])
for i, pose in enumerate(pose_list)
]
return noisy_poses
def do_tracking(basepath,
seq,
track_cond,
vo,
frames=None,
outfile=None,
rgb_dir='rgb'):
track_data = tum_rgbd.LocalizationDataset(basepath,
seq,
track_cond,
frames=frames,
rgb_dir=rgb_dir)
# Ground truth
T_w_c_gt = [
SE3(SO3.from_quaternion(p[0], ordering='xyzw'), p[1])
for p in track_data.poses
]
T_0_w = T_w_c_gt[0].inv()
print('Tracking using {}/{}'.format(seq, track_cond))
vo.set_mode('track')
start = time.perf_counter()
for c_idx in range(len(track_data)):
image = track_data.get_gray(c_idx)
depth = track_data.get_depth(c_idx)
try:
vo.track(image, depth)
# vo.track(image, depth, guess=T_w_c_gt[c_idx].inv()) # debug
end = time.perf_counter()
print('Image {}/{} ({:.2f} %) | {:.3f} s'.format(
c_idx, len(track_data), 100. * c_idx / len(track_data),
end - start),
end='\r')
except Exception as e:
print('Error on {}/{}'.format(seq, track_cond))
print(e)
print('Image {}/{} ({:.2f} %) | {:.3f} s'.format(
c_idx, len(track_data), 100. * c_idx / len(track_data),
end - start))
break
start = end
# Compute errors
T_w_c_est = [T.inv() for T in vo.T_c_w]
tm = TrajectoryMetrics(T_w_c_gt, T_w_c_est)
# Save to file
if outfile is not None:
print('Saving to {}'.format(outfile))
tm.savemat(outfile)
return tm, vo
def do_vo_mapping(basepath,
seq,
ref_cond,
frames=None,
outfile=None,
rgb_dir='rgb'):
ref_data = tum_rgbd.LocalizationDataset(basepath,
seq,
ref_cond,
frames=frames,
rgb_dir=rgb_dir)
test_im = ref_data.get_gray(0)
camera = get_camera(seq, test_im)
# Ground truth
T_w_c_gt = [
SE3(SO3.from_quaternion(p[0], ordering='xyzw'), p[1])
for p in ref_data.poses
]
T_0_w = T_w_c_gt[0].inv()
vo = DenseRGBDPipeline(camera, first_pose=T_0_w)
vo.keyframe_trans_thresh = 0.2 # meters
vo.keyframe_rot_thresh = 15. * np.pi / 180. # rad
vo.depth_stiffness = 1. / 0.01 # 1/meters
vo.intensity_stiffness = 1. / 0.1 # 1/ (intensity is in [0,1] )
vo.use_motion_model_guess = True
print('Mapping using {}/{}'.format(seq, ref_cond))
vo.set_mode('map')
start = time.perf_counter()
for c_idx in range(len(ref_data)):
image = ref_data.get_gray(c_idx)
depth = ref_data.get_depth(c_idx)
vo.track(image, depth)
# vo.track(image, depth, guess=T_w_c_gt[c_idx].inv()) # debug
end = time.perf_counter()
print('Image {}/{} ({:.2f} %) | {:.3f} s'.format(
c_idx, len(ref_data), 100. * c_idx / len(ref_data), end - start),
end='\r')
start = end
# Compute errors
T_w_c_est = [T.inv() for T in vo.T_c_w]
tm = TrajectoryMetrics(T_w_c_gt, T_w_c_est)
# Save to file
if outfile is not None:
print('Saving to {}'.format(outfile))
tm.savemat(outfile)
return tm, vo
def evaluate_results_with_andreff(my_solver, T_v_rel, T_c_rel, scale, cons):
# Solve with the Andreff method
fail = 1000.
# Extrinsic calibration inverse for evaluating error
ext_cal_trans = SE3.from_matrix(my_solver.extcal).trans
ext_cal_rot_inv = SE3.from_matrix(my_solver.extcal).rot.inv()
# Set the pose data
my_solver.set_T_v_rel(T_v_rel)
T_c_rel_scaled = my_solver.set_T_c_rel(T_c_rel, scale)
my_solver.andreff_solver.set_vals(T_v_rel, T_c_rel_scaled, None)
X_andreff, scale_andreff, runtime_andreff = my_solver.andreff_solver.solve(
schur=True)
# Solve for the extrinsic calibration
dual_time, dual_gt_value, dual_primal, dual_gap, dual_opt, dual_solution, dual_flag = my_solver.dual_solve(
cons)
rel_time, rel_gt_value, rel_primal, rel_gap, relax_opt, relax_solution, rel_flag = my_solver.relax_solve(
cons)
if dual_flag: #Solution was found
# Store the estimation errors
dual_trans_error = np.linalg.norm(ext_cal_trans - dual_solution[:3, 3])
dual_rot_error = np.linalg.norm((ext_cal_rot_inv.dot(
SO3.from_matrix(dual_solution[:3, :3], normalize=True))).log())
dual_alpha_error = np.abs(scale - 1 / dual_opt[3])
dual_results_dict = {
'dual_time': dual_time,
'dual_gt_value': np.asscalar(dual_gt_value),
'dual_primal': np.asscalar(dual_primal),
'dual_gap': np.asscalar(dual_gap),
'dual_trans_error': dual_trans_error,
'dual_rot_error': dual_rot_error,
'dual_alpha_error': np.asscalar(dual_alpha_error)
}
print("DUAL SCALE: {:}".format(1 / dual_opt[3]))
else: #Solution was not found
dual_results_dict = {
'dual_time': fail,
'dual_gt_value': fail,
'dual_primal': fail,
'dual_gap': fail,
'dual_trans_error': fail,
'dual_rot_error': fail,
'dual_alpha_error': fail
}
if rel_flag:
rel_trans_error = np.linalg.norm(ext_cal_trans - relax_solution[:3, 3])
rel_rot_error = np.linalg.norm((ext_cal_rot_inv.dot(
SO3.from_matrix(relax_solution[:3, :3], normalize=True))).log())
rel_alpha_error = np.abs(scale - 1 / relax_opt[3])
rel_results_dict = {
'rel_time': rel_time,
'rel_gt_value': np.asscalar(rel_gt_value),
'rel_primal': np.asscalar(rel_primal),
'rel_gap': np.asscalar(rel_gap),
'rel_trans_error': rel_trans_error,
'rel_rot_error': rel_rot_error,
'rel_alpha_error': np.asscalar(rel_alpha_error)
}
else: #Solution was not found
rel_results_dict = {
'rel_time': fail,
'rel_gt_value': fail,
'rel_primal': fail,
'rel_gap': fail,
'rel_trans_error': fail,
'rel_rot_error': fail,
'rel_alpha_error': fail
}
rel_results_dict.update(dual_results_dict)
# Store Andreff estimation errors
andreff_trans_error = np.linalg.norm(ext_cal_trans - X_andreff[0:3, 3])
andreff_rot_error = np.linalg.norm(
(ext_cal_rot_inv.dot(SO3.from_matrix(X_andreff[:3, :3],
normalize=True))).log())
andreff_alpha_error = np.abs(scale - 1 / scale_andreff)
print("ANDREFF SCALE: {:}".format(scale_andreff))
# Store data
and_dict = {
"andreff_trans_error": andreff_trans_error,
"andreff_rot_error": andreff_rot_error,
"andreff_alpha_error": andreff_alpha_error,
'andreff_time': runtime_andreff
}
rel_results_dict.update(and_dict)
return rel_results_dict
# import copy
import numpy as np
from liegroups import SE3, SO3
from pyslam.residuals import PoseResidual, PoseToPoseResidual
from pyslam.problem import Options, Problem
from pyslam.utils import invsqrt
T_1_0_true = SE3.identity()
T_2_0_true = SE3(SO3.identity(), -np.array([0.5, 0, 0]))
T_3_0_true = SE3(SO3.identity(), -np.array([1, 0, 0]))
T_4_0_true = SE3(SO3.rotz(np.pi / 2),
-(SO3.rotz(np.pi / 2).dot(np.array([1, 0.5, 0]))))
T_5_0_true = SE3(SO3.rotz(np.pi),
-(SO3.rotz(np.pi).dot(np.array([0.5, 0.5, 0]))))
T_6_0_true = SE3(SO3.rotz(-np.pi / 2),
-(SO3.rotz(-np.pi / 2).dot(np.array([0.5, 0, 0]))))
# T_6_0_true = copy.deepcopy(T_2_0_true)
# Odometry
T_1_0_obs = SE3.identity()
T_2_1_obs = T_2_0_true.dot(T_1_0_true.inv())
T_3_2_obs = T_3_0_true.dot(T_2_0_true.inv())
T_4_3_obs = T_4_0_true.dot(T_3_0_true.inv())
T_5_4_obs = T_5_0_true.dot(T_4_0_true.inv())
T_6_5_obs = T_6_0_true.dot(T_5_0_true.inv())
# Loop closure
T_6_2_obs = T_6_0_true.dot(T_2_0_true.inv())
def evaluate_results(my_solver, T_v_rel, T_c_rel, scale, cons):
#Number to know that the solution failed
fail = 1000
#Extrinsic calibration inverse for evaluating error
ext_cal_trans = SE3.from_matrix(my_solver.extcal).trans
ext_cal_rot_inv = SE3.from_matrix(my_solver.extcal).rot.inv()
# Set the pose data
my_solver.set_T_v_rel(T_v_rel)
my_solver.set_T_c_rel(T_c_rel, scale)
# Solve for the extrinsic calibration
dual_time, dual_gt_value, dual_primal, dual_gap, dual_opt, dual_solution, dualflag = my_solver.dual_solve(
cons)
rel_time, rel_gt_value, rel_primal, rel_gap, relax_opt, relax_solution, relflag = my_solver.relax_solve(
cons)
#and_extcal, and_scale, and_runtime = my_solver.andreff_solver.solve()
if dualflag: #Solution was found
# Store the estimation errors
dual_trans_error = np.linalg.norm(ext_cal_trans - dual_solution[:3, 3])
dual_rot_error = np.linalg.norm((ext_cal_rot_inv.dot(
SO3.from_matrix(dual_solution[:3, :3], normalize=True))).log())
dual_alpha_error = np.abs(scale - 1 / dual_opt[3])
dual_results_dict = {
'dual_time': dual_time,
'dual_gt_value': np.asscalar(dual_gt_value),
'dual_primal': np.asscalar(dual_primal),
'dual_gap': np.asscalar(dual_gap),
'dual_trans_error': dual_trans_error,
'dual_rot_error': dual_rot_error,
'dual_alpha_error': np.asscalar(dual_alpha_error)
}
else: #Solution was not found
dual_results_dict = {
'dual_time': fail,
'dual_gt_value': fail,
'dual_primal': fail,
'dual_gap': fail,
'dual_trans_error': fail,
'dual_rot_error': fail,
'dual_alpha_error': fail
}
if relflag:
rel_trans_error = np.linalg.norm(ext_cal_trans - relax_solution[:3, 3])
rel_rot_error = np.linalg.norm((ext_cal_rot_inv.dot(
SO3.from_matrix(relax_solution[:3, :3], normalize=True))).log())
rel_alpha_error = np.abs(scale - 1 / relax_opt[3])
rel_results_dict = {
'rel_time': rel_time,
'rel_gt_value': np.asscalar(rel_gt_value),
'rel_primal': np.asscalar(rel_primal),
'rel_gap': np.asscalar(rel_gap),
'rel_trans_error': rel_trans_error,
'rel_rot_error': rel_rot_error,
'rel_alpha_error': np.asscalar(rel_alpha_error)
}
else: #Solution was not found
rel_results_dict = {
'rel_time': fail,
'rel_gt_value': fail,
'rel_primal': fail,
'rel_gap': fail,
'rel_trans_error': fail,
'rel_rot_error': fail,
'rel_alpha_error': fail
}
rel_results_dict.update(dual_results_dict)
return rel_results_dict