def calculatejacobian(pose_num, first_pose, second_pose, relative_pose,
first_pose_ind, second_pose_ind, resid):
jacobian = np.zeros([6, 6 * pose_num])
for j in range(6):
epsVec = np.zeros(6)
eps = 1e-6
epsVec[j] = eps
# multiply pose increment from left onto the absolute poses
first_pose_eps = se3.se3Exp(epsVec) @ first_pose
second_pose_eps = se3.se3Exp(epsVec) @ second_pose
# calculate new pose difference after eps pose increment and new relative pose residual
# first key frame, fix second_pose
pose_diff_eps = np.linalg.inv(second_pose) @ first_pose_eps
resid_first_eps = se3.se3Log(
np.linalg.inv(relative_pose) @ pose_diff_eps)
# second key frame, fix first pose
pose_diff_eps = np.linalg.inv(second_pose_eps) @ first_pose
resid_second_eps = se3.se3Log(
np.linalg.inv(relative_pose) @ pose_diff_eps)
# calculate jacobian at first and second pose position
jacobian[:, 6 * first_pose_ind + j] = (resid_first_eps - resid) / eps
jacobian[:, 6 * second_pose_ind + j] = (resid_second_eps - resid) / eps
return jacobian
def do_alignment(IRef, DRef, K, I, D, xi, norm_param, use_hubernorm):
errLast = 1e10
hessian_m = np.identity(6)
hessian_m_new = hessian_m.copy()
xi_new = xi.copy()
for j in range(20):
Jac, residual, weights = deriveJacobianNumeric(IRef, DRef, I, D,
xi_new, K, norm_param,
use_hubernorm)
notValid = np.isnan(np.sum(Jac, 1) + residual.ravel())
residual[notValid] = 0
Jac[notValid] = 0
weights[notValid] = 0
hessian_m = Jac.T @ (np.tile(weights, (1, 6)) * Jac)
upd = -np.linalg.inv(hessian_m) @ Jac.T @ (weights * residual)
xi = se3.se3Log(se3.se3Exp(upd) @ se3.se3Exp(xi_new))
err = np.mean(residual**2)
# print('step:', j, 'err:', err, 't', xi)
if err > errLast:
break
errLast = err
xi_new = xi.copy()
hessian_m_new = hessian_m.copy()
return xi_new, hessian_m_new
def calculate_residual(absolute_poses, relative_poses, loop_pairs, loop_num):
pose_num = len(absolute_poses)
loop_ind = 0
residual = np.zeros([pose_num + loop_num, 6])
for ind in range(pose_num - 1 + loop_num):
if ind >= pose_num - 1:
# add loop closure manually at the end of the trajectory by observing overlap
loop_pair = loop_pairs[loop_ind]
loop_ind += 1
first_pose_ind = loop_pair[0]
second_pose_ind = loop_pair[1]
first_pose = absolute_poses[first_pose_ind]
second_pose = absolute_poses[second_pose_ind]
else:
first_pose_ind = ind
second_pose_ind = ind + 1
first_pose = absolute_poses[first_pose_ind]
second_pose = absolute_poses[second_pose_ind]
# calculate the pose difference between keyframes pair
pose_diff = np.linalg.inv(second_pose) @ first_pose
relative_pose = se3.se3Exp(relative_poses[ind])
residual[ind + 1] = se3.se3Log(
np.linalg.inv(relative_pose) @ pose_diff)
residual[0] = se3.se3Log(absolute_poses[0])
residual = residual.reshape(-1)
return residual
def deriveJacobianNumeric(IRef, DRef, I, D, xi, K, norm_param, use_hubernorm):
eps = 1e-6
Jac = np.zeros([I.shape[0] * I.shape[1], 6])
residuals, weights = calcResiduals(IRef, DRef, I, D, xi, K, norm_param,
use_hubernorm)
for j in range(6):
epsVec = np.zeros([6, 1])
epsVec[j] = eps
# multiply epsilon from left onto the current estimate
xiPerm = se3.se3Log(se3.se3Exp(epsVec) @ se3.se3Exp(xi))
residual_xiPerm, _ = calcResiduals(IRef, DRef, I, D, xiPerm, K,
norm_param, use_hubernorm)
Jac[:, j] = (residual_xiPerm - residuals).ravel() / eps
return Jac, residuals, weights
def calculate_jaco_single_frame(pose_num, absolute_pose, resid):
jacobian = np.zeros([6, 6 * pose_num])
for j in range(6):
epsVec = np.zeros(6)
eps = 1e-6
epsVec[j] = eps
# multiply pose increment from left onto the absolute poses
first_pose_eps = se3.se3Exp(epsVec) @ absolute_pose
resid_eps = se3.se3Log(first_pose_eps)
# calculate jacobian at first and second pose position
jacobian[:, j] = (resid_eps - resid) / eps
return jacobian
def calcResiduals(IRef, DRef, I, D, xi, K, norm_param, use_hubernorm):
T = se3.se3Exp(xi)
R = T[0:3, 0:3]
t = T[0:3, 3]
KInv = np.linalg.inv(K)
xImg = np.zeros_like(IRef) - 10
yImg = np.zeros_like(IRef) - 10
Depth1 = np.zeros_like(DRef)
for x in range(IRef.shape[0]):
for y in range(IRef.shape[1]):
p = DRef[x, y] * np.dot(KInv, np.array([x, y, 1]))
pTrans = np.dot(K, np.dot(R, p) + t)
Depth1[x, y] = pTrans[2]
if (pTrans[2] > 0) & (DRef[x, y] > 0):
xImg[x, y] = pTrans[0] / pTrans[2]
yImg[x, y] = pTrans[1] / pTrans[2]
# interpolation method 1
# xx, yy = np.mgrid[:I.shape[0], :I.shape[1]]
# interp_I = interpolate.griddata(np.stack([xx.ravel(), yy.ravel()]).T, I.ravel(), (xImg.real, yImg.real))
# interpolation method 2
xx = np.arange(0, I.shape[0])
yy = np.arange(0, I.shape[1])
f = interpolate.RegularGridInterpolator((xx, yy), I, bounds_error=False)
# g = interpolate.RegularGridInterpolator((xx, yy), D, bounds_error=False)
interp_I = f((xImg.real.ravel(), yImg.real.ravel())).reshape(I.shape)
# interp_D = g((xImg.real.ravel(), yImg.real.ravel())).reshape(D.shape)
# residuals = IRef - interp_I + Depth1 - interp_D
residuals = IRef - interp_I
weights = 0 * residuals + 1
if use_hubernorm:
idx = np.where(abs(residuals) > norm_param)
weights[idx] = norm_param / abs(residuals[idx])
else:
weights = 2 / (1 + residuals**2 / norm_param**2)**2
residuals = residuals.reshape(I.shape[0] * I.shape[1], -1, order='F')
weights = weights.reshape(I.shape[0] * I.shape[1], -1, order='F')
return residuals, weights
def calculate_residual_with_auto_lc(absolute_poses, relative_poses, loop_pairs,
kf_index):
pair_num = len(loop_pairs)
residual = np.zeros([pair_num + 1, 6]) # residual vector
for i in np.arange(pair_num):
ref_frame = loop_pairs[i][0]
cur_frame = loop_pairs[i][1]
first_pose_ind = np.where(kf_index == ref_frame)[0][0]
second_pose_ind = np.where(kf_index == cur_frame)[0][0]
first_pose = absolute_poses[first_pose_ind]
second_pose = absolute_poses[second_pose_ind]
# calculate the pose difference between keyframes pair
pose_diff = np.linalg.inv(second_pose) @ first_pose
relative_pose = se3.se3Exp(relative_poses[i])
residual[i + 1] = se3.se3Log(np.linalg.inv(relative_pose) @ pose_diff)
residual[0] = se3.se3Log(absolute_poses[0])
residual = residual.reshape(-1)
return residual
def camera_tracking_with_entropy(pair_file_path=None,
level=4,
entropy_ratio_threshold=0.95):
# Implement the relative entropy measure from lecture 9 to decide when to create new keyframes.
# read first frame as original frame
image, depth, _ = rp.read_pair(pair_file_path, 0)
# camera calibration matrix
fx = 520.9 # focal length x
fy = 521.0 # focal length y
cx = 325.1 # optical center x
cy = 249.7 # optical center y
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
# perform level times down_sampling and display the downscaled image
Id = image
Dd = depth
Kd = K
for i in range(level):
Id, Dd, Kd = ds.downscale(Id, Dd, Kd)
# set the first key frame
Iref = Id
Dref = Dd
Kref = Kd
use_hubernorm = 1
norm_param = 0.2
frames_length = len(open(pair_file_path).readlines())
def entropy(relative_pose_corvariance):
entropy = 0.5 * math.log(
np.linalg.det(2 * np.pi * np.e * relative_pose_corvariance))
return entropy
entropy_next_to_keyframe = np.identity(6)
add_new_kf = True
keyfr_to_wfr = np.identity(4)
xi = np.zeros(6)
for i in np.arange(1, frames_length):
Id, Dd, timestamp = rp.read_pair(pair_file_path, i)
Kd = K
# downscale the original image
for j in range(level):
Id, Dd, Kd = ds.downscale(Id, Dd, Kd)
# do direct image alignment between the current frame and the last key frame
xi, hessian_m = lk.do_alignment(Iref, Dref, Kd, Id, Dd, xi, norm_param,
use_hubernorm)
relative_pose_corvariance_i = np.linalg.inv(hessian_m)
if add_new_kf:
entropy_next_to_keyframe = entropy(relative_pose_corvariance_i)
add_new_kf = False
else:
entropy_cur_frame = entropy(relative_pose_corvariance_i)
ratio = entropy_cur_frame / entropy_next_to_keyframe
if ratio < entropy_ratio_threshold:
Iref = Id
Dref = Dd
add_new_kf = True
print("add new key frame")
keyfr_to_wfr = keyfr_to_wfr @ se3.se3Exp(-xi)
xi = np.zeros(6)
trans = keyfr_to_wfr @ se3.se3Exp(-xi)
# convert rotation matrix to unit quaternion [qx,qy,qz,qw]
r = R.from_dcm(trans[0:3, 0:3])
quater = r.as_quat()
# create the output pose [tx,ty,tz,qx,qy,qz,qw]
pose = np.round(
np.array([
trans[0, 3], trans[1, 3], trans[2, 3], quater[0], quater[1],
quater[2], quater[3]
]), 4)
# write the current pose into 'poses.txt' to get the trajectory
with open('poses_entropy_level{}.txt'.format(level),
'a') as file_handle:
file_handle.write(timestamp)
for ii in range(len(pose)):
file_handle.write(' ' + str(pose[ii]))
file_handle.write('\n')
np.set_printoptions(suppress=True)
print("timestamp:", i, pose)
def camera_tracking_with_rot_and_trans_thresh(pair_file_path=None,
level=4,
rot_thresh=0.1,
trans_thresh=0.1):
# read first frame as original frame
image, depth, timestamp = rp.read_pair(pair_file_path, 0)
# save initial pose as the first (key)frame pose into '.txt'
pose = np.array([0, 0, 0, 0, 0, 0, 1])
with open('kf_pose_level{}.txt'.format(level), 'a') as file_handle:
file_handle.write(timestamp)
for ii in range(len(pose)):
file_handle.write(' ' + str(pose[ii]))
file_handle.write('\n')
with open('kf_index_level{}.txt'.format(level), 'a') as file_handle:
file_handle.write(timestamp)
file_handle.write(' ' + str(0))
file_handle.write('\n')
with open('poses_rot_trans_level{}.txt'.format(level), 'a') as file_handle:
file_handle.write(timestamp)
for ii in range(len(pose)):
file_handle.write(' ' + str(pose[ii]))
file_handle.write('\n')
# camera calibration matrix
fx = 520.9 # focal length x
fy = 521.0 # focal length y
cx = 325.1 # optical center x
cy = 249.7 # optical center y
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
cv2.imshow('original_image', image)
cv2.imshow('original_depth', depth)
# perform level times down_sampling and display the downscaled image
Id = image
Dd = depth
Kd = K
for i in range(level):
Id, Dd, Kd = ds.downscale(Id, Dd, Kd)
cv2.imshow('Id', Id)
cv2.imshow('Dd', Dd)
cv2.waitKey(0)
cv2.destroyAllWindows()
# set the first key frame
Iref = Id
Dref = Dd
use_hubernorm = 1
norm_param = 0.2
frames_length = len(open(pair_file_path).readlines())
# camera tracking with key-frame based method
keyfr_to_wfr = np.identity(
4
) # the transform from the latest key frame to the original(world) frame
xi = np.zeros(
6) # the transform from the latest key frame to the current frame
for i in np.arange(1, frames_length):
Id, Dd, timestamp = rp.read_pair(pair_file_path, i)
Kd = K
# downscale the original image
for j in range(level):
Id, Dd, Kd = ds.downscale(Id, Dd, Kd)
# do direct image alignment between the current frame and the latest key frame
xi, _ = lk.do_alignment(Iref, Dref, Kd, Id, Dd, xi, norm_param,
use_hubernorm)
# choose new key frame wrt. rotational and translational threshold
add_new_keyframe = False
if np.linalg.norm(xi[:3]) > trans_thresh or np.linalg.norm(
xi[3:]) > rot_thresh:
# set the current frame as new key frame
Iref = Id
Dref = Dd
# update the transform from the new key frame to the world frame
keyfr_to_wfr = keyfr_to_wfr @ se3.se3Exp(-xi)
# reset the transform from the latest key frame to the current frame to identity
xi = np.zeros(6)
print("add new key frame")
add_new_keyframe = True
trans = keyfr_to_wfr @ se3.se3Exp(-xi)
# convert rotation matrix to unit quaternion [qx,qy,qz,qw]
r = R.from_dcm(trans[0:3, 0:3])
quater = r.as_quat()
# create the output pose [tx,ty,tz,qx,qy,qz,qw]
pose = np.round(
np.array([
trans[0, 3], trans[1, 3], trans[2, 3], quater[0], quater[1],
quater[2], quater[3]
]), 4)
# save key frames trajectory for pose graph optimization
if add_new_keyframe:
with open('kf_pose_level{}.txt'.format(level), 'a') as file_handle:
file_handle.write(timestamp)
for ii in range(len(pose)):
file_handle.write(' ' + str(pose[ii]))
file_handle.write('\n')
with open('kf_index_level{}.txt'.format(level),
'a') as file_handle:
file_handle.write(timestamp)
file_handle.write(' ' + str(i))
file_handle.write('\n')
# write the current pose into '.txt' to get the trajectory
with open('poses_rot_trans_level{}.txt'.format(level),
'a') as file_handle:
file_handle.write(timestamp)
for ii in range(len(pose)):
file_handle.write(' ' + str(pose[ii]))
file_handle.write('\n')
np.set_printoptions(suppress=True)
print("timestamp:", i, pose)
def pgo_with_auto_loop_closure(pair_path=None,
absolute_poses=None,
kf_index=None,
loop_pairs=None,
level=4,
r_p_list=[],
s_m_list=[],
lambd=0.1):
pose_num = len(absolute_poses)
pair_num = len(loop_pairs)
b_k = np.zeros(6 * pose_num) # b_k vector
h_k = np.zeros([6 * pose_num, 6 * pose_num]) # H_k matrix
residual = np.zeros([pair_num + 1, 6]) # residual vector
relative_pose_list = r_p_list
sigma_matrix_list = s_m_list
for i in np.arange(pair_num):
ref_frame = loop_pairs[i][0]
cur_frame = loop_pairs[i][1]
first_pose_ind = np.where(kf_index == ref_frame)[0][0]
second_pose_ind = np.where(kf_index == cur_frame)[0][0]
first_pose = absolute_poses[first_pose_ind]
second_pose = absolute_poses[second_pose_ind]
image1, depth1, _ = rp.read_pair(pair_path, ref_frame)
image2, depth2, _ = rp.read_pair(pair_path, cur_frame)
# calculate the pose difference between keyframes pair
pose_diff = np.linalg.inv(second_pose) @ first_pose
# create relative pose constraint between keyframes pair through direct image alignment in the first pgo iter
if len(relative_pose_list) < i + 1:
image1, depth1, _ = rp.read_pair(pair_path, ref_frame)
image2, depth2, _ = rp.read_pair(pair_path, cur_frame)
relative_pose, sigma_matrix = create_relative_pose_constraint(
image1, depth1, image2, depth2, level, se3.se3Log(pose_diff))
relative_pose_list.append(relative_pose)
sigma_matrix_list.append(sigma_matrix)
else:
relative_pose = relative_pose_list[i]
sigma_matrix = sigma_matrix_list[i]
# convert twist coordinate to 4*4 transformation matrix
relative_pose = se3.se3Exp(relative_pose)
# calculate relative pose residual between this key-frame pair
resid = se3.se3Log(np.linalg.inv(relative_pose) @ pose_diff)
# stack residual vector
residual[i + 1] = resid
# calculate jacobian matrix
jacobian = calculatejacobian(pose_num, first_pose, second_pose,
relative_pose, first_pose_ind,
second_pose_ind, resid)
# accumulate b_k and h_k for gauss newton step
b_k += jacobian.T @ sigma_matrix @ resid
h_k += jacobian.T @ sigma_matrix @ jacobian
# print('keyframes pair:{}'.format(i + 1))
# add another term for first pose in b_k and h_k
resid_0 = se3.se3Log(absolute_poses[0])
residual[0] = resid_0
jaco_0 = calculate_jaco_single_frame(pose_num, absolute_poses[0], resid_0)
sigma_matrix = np.identity(6) * 1e6
b_k += jaco_0.T @ sigma_matrix @ resid_0
h_k += jaco_0.T @ sigma_matrix @ jaco_0
# update all key frame poses
# upd = - np.linalg.inv(h_k) @ b_k # use gauss-newton
# use cholesky factorization to solve the linear system H x = b
c = cholesky(h_k +
lambd * np.diag(np.diag(h_k))) # use levenberg marquardt
upd = -cho_solve((c, False), b_k)
upd = upd.reshape(-1, 6)
for jj in range(pose_num):
absolute_poses[jj] = se3.se3Exp(upd[jj]) @ absolute_poses[jj]
residual = residual.reshape(-1)
residual_after_update = calculate_residual_with_auto_lc(
absolute_poses, relative_pose_list, loop_pairs, kf_index)
return absolute_poses, residual, relative_pose_list, sigma_matrix_list, residual_after_update
def pgo(pair_path=None,
absolute_poses=None,
kf_index=None,
loop_closure=False,
loop_pairs=None,
level=4,
r_p_list=[],
s_m_list=[],
lambd=0.1):
pose_num = len(absolute_poses)
if loop_closure:
loop_num = len(loop_pairs)
else:
loop_num = 0
loop_ind = 0
b_k = np.zeros(6 * pose_num) # b_k vector
h_k = np.zeros([6 * pose_num, 6 * pose_num]) # H_k matrix
residual = np.zeros([pose_num + loop_num, 6]) # residual vector
relative_pose_list = r_p_list
sigma_matrix_list = s_m_list
for ind in np.arange(pose_num - 1 + loop_num):
if ind >= pose_num - 1:
# add loop closure manually at the end of the trajectory by observing overlap
loop_pair = loop_pairs[loop_ind]
loop_ind += 1
first_pose_ind = loop_pair[0]
second_pose_ind = loop_pair[1]
ref_frame = kf_index[first_pose_ind]
cur_frame = kf_index[second_pose_ind]
first_pose = absolute_poses[first_pose_ind]
second_pose = absolute_poses[second_pose_ind]
else:
first_pose_ind = ind
second_pose_ind = ind + 1
ref_frame = kf_index[first_pose_ind]
cur_frame = kf_index[second_pose_ind]
first_pose = absolute_poses[first_pose_ind]
second_pose = absolute_poses[second_pose_ind]
# calculate the pose difference between keyframes pair
pose_diff = np.linalg.inv(second_pose) @ first_pose
# create relative pose constraint between keyframes pair through direct image alignment in the first pgo iter
if len(relative_pose_list) < ind + 1:
image1, depth1, _ = rp.read_pair(pair_path, ref_frame)
image2, depth2, _ = rp.read_pair(pair_path, cur_frame)
relative_pose, sigma_matrix = create_relative_pose_constraint(
image1, depth1, image2, depth2, level, se3.se3Log(pose_diff))
relative_pose_list.append(relative_pose)
sigma_matrix_list.append(sigma_matrix)
else:
relative_pose = relative_pose_list[ind]
sigma_matrix = sigma_matrix_list[ind]
# convert twist coordinate to 4*4 transformation matrix
relative_pose = se3.se3Exp(relative_pose)
# calculate relative pose residual between this key-frame pair
resid = se3.se3Log(np.linalg.inv(relative_pose) @ pose_diff)
# print(resid)
# stack residual vector
residual[ind + 1] = resid
# calculate jacobian matrix
jacobian = calculatejacobian(pose_num, first_pose, second_pose,
relative_pose, first_pose_ind,
second_pose_ind, resid)
# accumulate b_k and h_k for gauss newton step
b_k += jacobian.T @ sigma_matrix @ resid
h_k += jacobian.T @ sigma_matrix @ jacobian
# print('keyframes pair:{}'.format(ind + 1))
# add another term for first pose in b_k and h_k
resid_0 = se3.se3Log(absolute_poses[0])
residual[0] = resid_0
jaco_0 = calculate_jaco_single_frame(pose_num, absolute_poses[0], resid_0)
sigma_matrix = np.identity(6) * 1e6
b_k += jaco_0.T @ sigma_matrix @ resid_0
h_k += jaco_0.T @ sigma_matrix @ jaco_0
residual = residual.reshape(-1)
# update all key frame poses with Levenberg-Marquardt method
# upd = - np.linalg.inv(h_k + lambd * np.diag(np.diag(h_k))) @ b_k
# with gauss newton method
# upd = - np.linalg.inv(h_k) @ b_k
# use cholesky factorization to solve the linear system -h_k * upd = b_k
c = cholesky(h_k)
upd = -cho_solve((c, False), b_k)
upd = upd.reshape(-1, 6)
for jj in range(pose_num):
absolute_poses[jj] = se3.se3Exp(upd[jj]) @ absolute_poses[jj]
residual_after_update = calculate_residual(absolute_poses,
relative_pose_list, loop_pairs,
loop_num)
return absolute_poses, residual, relative_pose_list, sigma_matrix_list, residual_after_update