def se3_exp(se3):
w = se3[:3]
t = se3[3:].reshape([3,1])
SE3_mat = np.identity(4)
SE3_mat[:3, :3] = utils.so3_exp(w)
# I hate numpy
SE3_mat[:3, 3] = (V_operator(w) @ t).flatten()
return SE3_mat
def right_add(self, so3_local_and_translation):
assert (so3_local_and_translation.size == 6)
w_so3_local = so3_local_and_translation[:3]
translation = so3_local_and_translation[3:].reshape([3, 1])
ret = SO3AndTranslation()
ret.R = self.R @ utils.so3_exp(w_so3_local)
ret.translation = self.translation + translation
assert (ret.translation.size == 3)
return ret
def numerical_jacobi_wrt_so3(self):
"""
r_i = p_i - first_k_cols(R) * w
"""
DELTA = 1e-8
jacobian = np.zeros([self.num_residuals, 3])
w_so3_local = np.array([0, 0, 0.])
curret_params = w_so3_local.copy()
for p_idx in range(3):
params_plus = curret_params.copy()
params_plus[p_idx] += DELTA
residual_plus = self.residaul(self.var_SO3 @ utils.so3_exp(params_plus), self.var_projection)
params_minus = curret_params.copy()
params_minus[p_idx] -= DELTA
residual_minus = self.residaul(self.var_SO3 @ utils.so3_exp(params_minus), self.var_projection)
dr_dpidx = (residual_plus - residual_minus) / (2. * DELTA)
jacobian[:, p_idx] = dr_dpidx
return jacobian
def icp_local_so3_numirical(point_src, point_target):
w_so3_local = np.array([0, 0, 0.])
R_current = np.identity(3)
for iter in range(10):
# Jocobi on so3
jacobi, b = compute_local_so3_jacobian_numurical(
point_src, point_target, R_current)
delta = np.linalg.solve(jacobi.transpose() @ jacobi,
-jacobi.transpose() @ b)
# Update on SO3
R_current = R_current @ so3_exp(delta)
#print('jocobian:', jacobi)
#print('b: ', b)
print('iter: ', iter, ' cost:', b.transpose() @ b)
def solve_normal_equation_and_update_wrt_so3(self):
"""
"""
jacobi = self.jacobi_wrt_so3()
# jacobi = self.numerical_jacobi_wrt_so3()
# print('jacobian', jacobi)
r = self.residaul(self.var_SO3, self.var_projection)
# rhs is invertable when rank == 1
regulization = 1e-6
rhs = jacobi.transpose() @ jacobi + regulization * np.identity(3)
lhs = - jacobi.transpose() @ r
# print('rhs:', rhs)
# print('lhs:', lhs)
delta_so3 = np.linalg.solve(rhs, lhs)
print('delta_so3:', delta_so3)
self.var_SO3 = self.var_SO3 @ utils.so3_exp(delta_so3)
def icp_residual_so3(point_src, point_target, w_so3):
R = so3_exp(w_so3)
residual = R @ point_src - point_target
# [p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, ...]
residual = residual.flatten('F')
return residual
def icp_residual_local_so3(point_src, point_target, R_current, w_so3_local):
R = R_current @ so3_exp(w_so3_local)
residual = R @ point_src - point_target
# [p1_x, p1_y, p1_z, p2_x, p2_y, p2_z, ...]
residual = residual.flatten('F')
return residual