def generate_rotation_group(axis_number=3,
right_angle_rotation_number=3,
negative_reflection=False,
include_identity=True):
'''
:param axis_number:
:param right_angle_rotation_number:
:param negative_reflection: if True, right_angle_array = [right_angle_array, -1*right_angle_array]
:param include_identity: if False, remove identity from right_angle_array
:return: numpy array of Nx3x3
'''
right_angle_array = np.zeros(
(axis_number * right_angle_rotation_number + 1, 3))
for axis in range(axis_number):
for angle in range(right_angle_rotation_number):
right_angle_array[axis * right_angle_rotation_number + angle,
axis] = 90 * (angle + 1)
if False == include_identity:
right_angle_array = right_angle_array[0:right_angle_array.shape[0] -
1, :]
if negative_reflection:
right_angle_array = np.append(right_angle_array,
-1 * right_angle_array,
axis=0)
# print('right_angle_array:')
# print(right_angle_array)
# print(right_angle_array.shape)
# degree to radian
right_angle_array = right_angle_array * np.pi / 180.0
rotation_group = np.array([[[1, 0, 0], [0, 1, 0], [0, 0, 1]]])
for i in range(right_angle_array.shape[0]):
for j in range(right_angle_array.shape[0]):
rot_i = augmentation.angles2rotation_matrix(right_angle_array[i])
rot_j = augmentation.angles2rotation_matrix(right_angle_array[j])
rot_ij = np.dot(rot_i, rot_j)
# check whether rot_ij exits in rotation_group
is_exist = False
for r in range(rotation_group.shape[0]):
if np.allclose(rot_ij, rotation_group[r]):
is_exist = True
break
if False == is_exist:
# rot_ij is not in the group yet, add it into the group
rotation_group = np.append(rotation_group,
np.expand_dims(rot_ij, axis=0),
axis=0)
# print('rotation_group:')
# print(rotation_group)
# print(rotation_group.shape)
return rotation_group
def generate_uniform_random_transform(P_tx_amplitude, P_ty_amplitude,
P_tz_amplitude, P_Rx_amplitude,
P_Ry_amplitude, P_Rz_amplitude):
"""
:param pc_np: pc in NWU coordinate
:return:
"""
t = [
random.uniform(-P_tx_amplitude, P_tx_amplitude),
random.uniform(-P_ty_amplitude, P_ty_amplitude),
random.uniform(-P_tz_amplitude, P_tz_amplitude)
]
angles = [
random.uniform(-P_Rx_amplitude, P_Rx_amplitude),
random.uniform(-P_Ry_amplitude, P_Ry_amplitude),
random.uniform(-P_Rz_amplitude, P_Rz_amplitude)
]
rotation_mat = augmentation.angles2rotation_matrix(angles)
P_random = np.identity(4, dtype=np.float)
P_random[0:3, 0:3] = rotation_mat
P_random[0:3, 3] = t
return P_random
def get_initial_guess(pc_np, coarse_predictions_np):
"""
:param pc_np: 3xN
:param coarse_predictions_np: N
:return:
"""
pc_np_masked = pc_np[:, coarse_predictions_np == 1]
pc_np_masked_mean = np.mean(pc_np_masked, axis=1) # 3
src_mean_point_angle_y = math.atan2(pc_np_masked_mean[2],
pc_np_masked_mean[0])
dst_mean_point_angle_y = math.pi / 2
init_y_angle = wrap_in_pi(src_mean_point_angle_y - dst_mean_point_angle_y)
R1 = angles2rotation_matrix([0, init_y_angle, 0])
R1_pc_np = np.dot(R1, pc_np)
R1_pc_np_min = np.min(R1_pc_np[:, coarse_predictions_np == 1], axis=1) # 3
front_mask = R1_pc_np[2, :] > R1_pc_np_min[2] - 10
pc_np_front = pc_np[:, front_mask]
coarse_predictions_np_front = coarse_predictions_np[front_mask]
P_init = np.identity(4)
P_init[0:3, 0:3] = R1
return P_init, init_y_angle, pc_np_front, coarse_predictions_np_front
def generate_gaussian_random_transform(tx_sigma, ty_sigma, tz_sigma, rx_sigma,
ry_sigma, rz_sigma):
t = [
random.gauss(0, tx_sigma),
random.gauss(0, ty_sigma),
random.gauss(0, tz_sigma)
]
angles = [
random.gauss(0, rx_sigma),
random.gauss(0, ry_sigma),
random.gauss(0, rz_sigma)
]
rotation_mat = augmentation.angles2rotation_matrix(angles)
P_random = np.identity(4, dtype=np.float)
P_random[0:3, 0:3] = rotation_mat
P_random[0:3, 3] = t
return P_random