def add_gaussian_noise(seq, rot_noise_deg=10.0, loc_displace_factor=0.1):
"""
Add gaussian noise for the pose of keyframes
:param seq: keyframe sequences, dim: (M, 3, 4), M is the number of keyframes
:param rot_noise_deg: noise in rotation (unit: deg)
:param loc_displace_factor: displacement factor in translation, the unit 1 is the avg. baseline among all cameras.
:return: noise sequences with dim (M, 3, 4), displacement std.
"""
n_frames = seq.shape[0]
avg_frame_dist = 0
R, t = cam_opt.Rt(seq[0])
pre_frame_center = cam_opt.camera_center_from_Tcw(R, t)
for frame_idx in range(1, n_frames):
R, t = cam_opt.Rt(seq[frame_idx])
frame_center = cam_opt.camera_center_from_Tcw(R, t)
dist = np.linalg.norm(frame_center - pre_frame_center)
avg_frame_dist += dist
avg_frame_dist /= n_frames
# Set the translation noise
loc_disp_noise_sigma = loc_displace_factor * avg_frame_dist # std. for random displacement
disp_noise = np.random.normal(0, loc_disp_noise_sigma, size=(n_frames, 3))
# Set the rotation noise
rot_noise_factor = np.deg2rad(rot_noise_deg)
rot_noise = np.random.normal(0, rot_noise_factor, size=n_frames)
new_seq = seq.copy()
for frame_idx in range(1, n_frames):
T = seq[frame_idx]
R, t = cam_opt.Rt(T)
# Add random noise to translation
C = cam_opt.camera_center_from_Tcw(R, t)
rand_C = C + disp_noise[frame_idx]
# Add random noise to rotation
temp_T = np.eye(4)
temp_T[:3, :3] = R
angle, axis, _ = trans.rotation_from_matrix(temp_T)
new_angle = angle + rot_noise[frame_idx]
new_axis = axis + np.random.normal(0, 0.1, size=3)
new_R = trans.rotation_matrix(new_angle, new_axis)[:3, :3]
new_t = cam_opt.translation_from_center(new_R, rand_C)
new_seq[frame_idx][:3, :3] = new_R[:3, :3]
new_seq[frame_idx][:3, 3] = new_t
return new_seq, loc_disp_noise_sigma
def plot_frames_seq_2d(frames: FrameSeqData,
plt_axes=None,
color=None,
legend=None,
point_style='-',
index_range=(-1, -1),
show_view_direction=False):
"""
Plot frame sequences in 2D figure
Note: use plt.show() in the end
:param frames: frame sequences
:param plt_axes: matplotlib axis handle
:param color: color tuple, e.g. (1.0, 1.0, 1.0)
:param legend: legend name for the curve
"""
if plt_axes is not None:
ax = plt_axes
else:
plt.figure()
ax = plt.gca()
if color is None:
color = np.random.uniform(0, 1, size=3).tolist()
n_frames = len(frames)
first_frame = frames.frames[0]
avg_frame_dist = 0
R, t = cam_opt.Rt(first_frame['extrinsic_Tcw'])
pre_frame_center = cam_opt.camera_center_from_Tcw(R, t)
for frame_idx in range(1, n_frames):
R, t = cam_opt.Rt(frames.frames[frame_idx]['extrinsic_Tcw'])
frame_center = cam_opt.camera_center_from_Tcw(R, t)
dist = np.linalg.norm(frame_center - pre_frame_center)
avg_frame_dist += dist
avg_frame_dist /= n_frames
start_idx = 0 if index_range[0] == -1 else index_range[0]
end_idx = n_frames if index_range[1] == -1 else min(
index_range[1], n_frames)
frame_centers = []
view_directions = []
for frame in frames.frames[start_idx:end_idx]:
cur_Tcw = frame['extrinsic_Tcw']
cur_center = cam_opt.camera_center_from_Tcw(cur_Tcw[:3, :3],
cur_Tcw[:3, 3])
frame_centers.append(cur_center)
view_direction = np.dot(R.T, np.asarray([0, 0, 1]))
view_directions.append((view_direction[0], view_direction[2]))
view_direct_len = 0.1 * avg_frame_dist
view_directions = view_direct_len * np.asarray(view_directions,
dtype=np.float32)
frame_centers = np.asarray(frame_centers, dtype=np.float32)
ax.plot(frame_centers[0, 0], frame_centers[0, 2], '*',
color=color) # First frame
ax.plot(frame_centers[:, 0],
frame_centers[:, 2],
point_style,
color=color,
label=legend)
if show_view_direction:
for frame_idx in range(0, len(frame_centers)):
ax.arrow(frame_centers[frame_idx, 0],
frame_centers[frame_idx, 2],
view_directions[frame_idx, 0],
view_directions[frame_idx, 1],
head_width=0.3 * view_direct_len,
head_length=0.2 * view_direct_len,
fc='k',
color=color)
if legend is not None:
ax.legend()
ax.set_aspect('equal', adjustable='box')
def plot_array_seq_2d(seq_array,
plt_axes=None,
color=None,
legend=None,
point_style='-',
index_range=(-1, -1),
show_view_direction=False,
arrow_color=None):
"""
Plot frame sequences in 2D figure
Note: use plt.show() in the end
:param frames: frame poses with numpy array, dim: (N, 3, 4)
:param plt_axes: matplotlib axis handle
:param color: color tuple, e.g. (1.0, 1.0, 1.0)
:param legend: legend name for the curve
:param show_view_direction: show view direction in 2d
"""
if plt_axes is not None:
ax = plt_axes
else:
plt.figure()
ax = plt.gca()
if color is None:
color = np.random.uniform(0, 1, size=3).tolist()
n_frames = seq_array.shape[0]
cam_centers = []
view_directions = []
avg_frame_dist = 0
R, t = cam_opt.Rt(seq_array[0])
pre_frame_center = cam_opt.camera_center_from_Tcw(R, t)
for frame_idx in range(1, n_frames):
R, t = cam_opt.Rt(seq_array[frame_idx])
frame_center = cam_opt.camera_center_from_Tcw(R, t)
dist = np.linalg.norm(frame_center - pre_frame_center)
avg_frame_dist += dist
avg_frame_dist /= n_frames
start_idx = 0 if index_range[0] == -1 else index_range[0]
end_idx = n_frames if index_range[1] == -1 else min(
index_range[1], n_frames)
for frame_idx in range(start_idx, end_idx):
Tcw = seq_array[frame_idx]
R, t = cam_opt.Rt(Tcw)
C = cam_opt.camera_center_from_Tcw(R, t)
view_direction = np.dot(R.T, np.asarray([0, 0, 1]))
view_directions.append((view_direction[0], view_direction[2]))
cam_centers.append(C)
cam_centers = np.asarray(cam_centers)
view_direct_len = 0.1 * avg_frame_dist
view_directions = view_direct_len * np.asarray(view_directions)
ax.plot(cam_centers[0, 0], cam_centers[0, 2], '*',
color=color) # First frame
ax.plot(cam_centers[:, 0],
cam_centers[:, 2],
point_style,
color=color,
label=legend)
if show_view_direction:
for frame_idx in range(0, len(cam_centers)):
ax.arrow(cam_centers[frame_idx, 0],
cam_centers[frame_idx, 2],
view_directions[frame_idx, 0],
view_directions[frame_idx, 1],
width=0.01,
head_width=0.3 * view_direct_len,
head_length=0.2 * view_direct_len,
fc='k',
color=arrow_color[frame_idx]
if arrow_color is not None else color)
if legend is not None:
ax.legend()
ax.set_aspect('equal', adjustable='box')
K = K_from_frame(cur_frame)
# Read image
cur_img = cv2.imread(os.path.join(base_dir, cur_name)).astype(
np.float32) / 255.0
next_img = cv2.imread(os.path.join(base_dir, next_name)).astype(
np.float32) / 255.0
cur_depth = load_depth_from_png(os.path.join(base_dir, cur_depth_name))
h, w, c = cur_img.shape
rel_T = cam_opt.relateive_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3],
next_Tcw[:3, :3], next_Tcw[:3, 3])
# Translation
Cb = cam_opt.camera_center_from_Tcw(rel_T[:3, :3], rel_T[:3, 3])
baseline = np.linalg.norm(Cb)
# View angle
q = trans.quaternion_from_matrix(rel_T)
R = trans.quaternion_matrix(q)
rel_rad, rel_axis, _ = trans.rotation_from_matrix(R)
rel_deg = np.rad2deg(rel_rad)
next2cur, _ = cam_opt.wrapping(cur_img, next_img, cur_depth, K,
rel_T[:3, :3], rel_T[:3, 3])
show_multiple_img([{
'img': cur_img,
'title': 'a'
}, {
'img': next2cur,
displacement_dist_std=0.05)
# print('Displacement:', disp_sigmaq)
T_noise = torch.from_numpy(T_noise)
torch.set_default_tensor_type('torch.cuda.FloatTensor')
fig = plt.figure()
ax = plt.gca()
Tcw_n = np.load(os.path.join(in_dir, "Tcw_%05d.npy") % sample_idx)
T_noise_n = np.load(os.path.join(in_dir, "Tcw_n%05d.npy") % sample_idx)
T_pred_n = np.load(os.path.join(in_dir, "Tcw_p%05d.npy") % sample_idx)
# T_noise_n = T_noise.cpu().numpy().reshape(L, 3, 4)
for f_idx in range(0, L):
T_item = Tcw_n[f_idx, :, :]
T_n_item = T_noise_n[f_idx, :, :]
c1 = cam_opt.camera_center_from_Tcw(T_item[:3, :3], T_item[:3, 3])
c2 = cam_opt.camera_center_from_Tcw(T_n_item[:3, :3], T_n_item[:3, 3])
print('Baseline: ', np.linalg.norm(c1 - c2))
plot_array_seq_2d(Tcw_n,
plt_axes=ax,
color=(0, 0, 1),
show_view_direction=True,
legend='GT')
plot_array_seq_2d(T_noise_n,
plt_axes=ax,
color=(1, 0, 0),
show_view_direction=True,
legend='Noise')
plot_array_seq_2d(T_pred_n,
plt_axes=ax,
if n1_name not in bundle_image2idx or n2_name not in bundle_image2idx:
continue
if len(match['n1_feat_id']) == 0 or len(match['n2_feat_id']) == 0:
continue
assert n1 == pair[1] and n2 == pair[2]
K1 = bundle_recon_cams[n1].K_matrix()
K2 = bundle_recon_cams[n2].K_matrix()
covis = max(int(covis_map[n1, n2]), int(covis_map[n2, n1]))
n1_Rt = bundle_recon_cams[n1].recon_cam_Es()
n2_Rt = bundle_recon_cams[n2].recon_cam_Es()
n1_C = cam_opt.camera_center_from_Tcw(n1_Rt[:3, :3], n1_Rt[:3, 3])
n2_C = cam_opt.camera_center_from_Tcw(n2_Rt[:3, :3], n2_Rt[:3, 3])
if n1_Rt[2, 2] == 0 or n2_Rt[2, 2] == 0:
continue
rel_Rt_gt = cam_opt.relateive_pose(n1_Rt[:, :3], n1_Rt[:, 3],
n2_Rt[:, :3], n2_Rt[:, 3])
Rij = rel_Rt_gt[:, :3]
tij = rel_Rt_gt[:, 3]
tij = tij / np.sqrt(np.sum(np.square(tij)))
rel_R = np.matmul(rel_Rt[:3, :3], Rij.T)
R_err = np.rad2deg(np.arccos((np.trace(rel_R) - 1) / 2))
t_err = np.rad2deg(rel_t_err(tij, rel_Rt[:3, 3]))
def add_drift_noise(seq, rot_noise_deg=10.0, displacement_dist_std=0.1):
"""
Add gaussian noise for the pose of keyframes, here we update the noise-track with random noise to
simulate drift error.
:param seq: keyframe sequences, dim: (M, 3, 4), M is the number of keyframes
:param rot_noise_deg: noise in rotation (unit: deg)
:param displacement_dist_std: displacement factor in translation, the unit 1 is the avg. baseline among all cameras.
:return: noise sequences with dim (M, 3, 4), displacement std.
"""
n_frames = seq.shape[0]
avg_frame_dist = 0
R, t = cam_opt.Rt(seq[0])
pre_frame_center = cam_opt.camera_center_from_Tcw(R, t)
for frame_idx in range(1, n_frames):
R, t = cam_opt.Rt(seq[frame_idx])
frame_center = cam_opt.camera_center_from_Tcw(R, t)
dist = np.linalg.norm(frame_center - pre_frame_center)
pre_frame_center = frame_center
avg_frame_dist += dist
avg_frame_dist /= n_frames
# Set the translation noise
loc_disp_noise_sigma = displacement_dist_std # std. for random displacement
disp_noise = np.random.normal(0, loc_disp_noise_sigma, size=(n_frames, 3))
# Set the rotation noise
rot_noise_factor = np.deg2rad(rot_noise_deg)
rot_noise = np.random.normal(0, rot_noise_factor, size=n_frames)
# Add random noise, here we have two track: 'ground-truth-track' and 'noise-track'
# the 'ground-truth-track' providing ground-truth relative pose, we then add noise
# onto relative pose, and added to the 'noise-track' in the end.
new_seq = seq.copy()
pre_frame = seq[0] # used for ground-truth track
pre_noise_frame = np.eye(4) # noise-track, init the same with first ground-truth pose
pre_noise_frame[:3, :] = pre_frame
for frame_idx in range(1, n_frames):
# Ground-truth-track
# current frame:
T = seq[frame_idx]
R, t = cam_opt.Rt(T)
# previous frame:
pre_R, pre_t = cam_opt.Rt(pre_frame)
pre_T = np.eye(4, dtype=np.float32)
pre_T[:3, :3] = pre_R
pre_T[:3, 3] = pre_t
# inv_T = cam_opt.camera_pose_inv(R, t)
# r_angle, r_axis, _ = trans.rotation_from_matrix(inv_T)
# print('Old Rotation:', r_angle)
# Compute ground-truth relative pose, and add random noise to translation
rel_T = cam_opt.relateive_pose(pre_R, pre_t, R, t)
rel_R, rel_t = cam_opt.Rt(rel_T)
rel_C = cam_opt.camera_center_from_Tcw(rel_R, rel_t)
rand_C = rel_C + disp_noise[frame_idx]
# Add random noise to rotation
temp_T = np.eye(4, dtype=np.float32)
temp_T[:3, :3] = rel_R
angle, axis, _ = trans.rotation_from_matrix(temp_T)
new_angle = rot_noise[frame_idx]
new_axis = np.random.normal(0, 1.0, size=3)
noise_R = trans.rotation_matrix(new_angle, new_axis)[:3, :3]
# print('New', np.rad2deg(new_angle))
# Build new relative transformation matrix
new_R = np.dot(noise_R, rel_R)
new_t = cam_opt.translation_from_center(new_R, rand_C)
temp_T[:3, :3] = new_R
temp_T[:3, 3] = new_t
# Add the noise relative transformation onto noise-track
new_T = np.dot(temp_T, pre_noise_frame)
new_seq[frame_idx][:3, :] = new_T[:3, :]
# Update both ground-truth-track as well as noise-track
pre_frame = T
pre_noise_frame = new_T
# inv_new_T = cam_opt.camera_pose_inv(new_T[:3, :3], new_T[:3, 3])
# r_angle, r_axis, _ = trans.rotation_from_matrix(inv_new_T)
# print('New Rotation:', r_angle)
return new_seq, loc_disp_noise_sigma
# noise_T, rand_std_radius = add_gaussian_noise(seq_T, rot_noise_deg=8.0, loc_displace_factor=0.1)
noise_T, rand_std_radius = add_drift_noise(seq_T, rot_noise_deg=8.0, displacement_dist_std=0.08)
# Show 2D Seq
if show_2d_path:
import matplotlib.pyplot as plt
from seq_data.plot_seq_2d import plot_array_seq_2d
plt.figure()
ax = plt.gca()
plot_array_seq_2d(noise_T, plt_axes=ax, show_view_direction=True, legend='noise', color='r')
plot_array_seq_2d(seq_T, plt_axes=ax, show_view_direction=True, legend='ori', color='b')
# Plot displacement radius
for frame_idx in range(1, noise_T.shape[0]):
T = seq_T[frame_idx]
C = cam_opt.camera_center_from_Tcw(T[:3, :3], T[:3, 3])
circle = plt.Circle((C[0], C[2]), 3 * rand_std_radius, color=(1.0, 0.0, 0.0, 0.1))
ax.add_patch(circle)
ax.set_aspect('equal', adjustable='box')
plt.show()
else:
from visualizer.visualizer_3d import Visualizer
# Show 3D case
vis = Visualizer()
count = 0
def keyPressEvent(obj, event):
global seq_T
global noise_T
def sel_triple_sun3d(base_dir, scene_frames, max_triple_num,
num_sample_per_triple, trans_thres, overlap_thres):
"""
Select triples (anchor, positive, negative) from a sun3d sequence
:param base_dir: dataset base directory
:param scene_frames: scene frames to extract triples
:param max_triple_num: maximum number of triples
:param num_sample_per_triple: number of positive/negative samples per triple
:param trans_thres: translation threshold for positive samples, based on the center of different frames
:param overlap_thres: overlap threshold for positive samples, (low, high)
:return: [{'anchor': frame_dict, 'positive': FrameSeqData, 'negative': FrameSeqData}, {...}, ...]
"""
dim = scene_frames.get_frame_dim(scene_frames.frames[0])
K = scene_frames.get_K_mat(scene_frames.frames[0])
pre_cache_x2d = cam_opt.x_2d_coords(dim[0], dim[1])
camera_centers = np.empty((len(scene_frames), 3), dtype=np.float32)
for i, frame in enumerate(scene_frames.frames):
Tcw = scene_frames.get_Tcw(frame)
center = cam_opt.camera_center_from_Tcw(Tcw[:3, :3], Tcw[:3, 3])
camera_centers[i, :] = center
kdtree = KDTree(camera_centers)
triple_list = []
anchor_idces = np.random.choice(len(scene_frames),
max_triple_num,
replace=False)
for anchor_idx in anchor_idces:
anchor_frame = scene_frames.frames[anchor_idx]
anchor_Tcw = scene_frames.get_Tcw(anchor_frame)
anchor_depth_path = scene_frames.get_depth_name(anchor_frame)
anchor_depth = read_sun3d_depth(
os.path.join(base_dir, anchor_depth_path))
anchor_depth[anchor_depth < 1e-5] = 1e-5
potential_pos_idces = kdtree.query_ball_point(
camera_centers[anchor_idx], trans_thres)
pos_idces = []
for potential_pos_idx in potential_pos_idces:
potential_pos_frame = scene_frames.frames[potential_pos_idx]
potential_pos_Tcw = scene_frames.get_Tcw(potential_pos_frame)
overlap = cam_opt.photometric_overlap(anchor_depth,
K,
Ta=anchor_Tcw,
Tb=potential_pos_Tcw,
pre_cache_x2d=pre_cache_x2d)
if overlap_thres[0] < overlap < overlap_thres[1]:
pos_idces.append(potential_pos_idx)
if len(pos_idces) < num_sample_per_triple:
continue
else:
sel_pos_idces = np.random.choice(pos_idces,
num_sample_per_triple,
replace=False)
neg_idces = list(set(range(len(scene_frames))) - set(pos_idces))
sel_neg_idces = np.random.choice(neg_idces,
num_sample_per_triple,
replace=False)
triple_list.append({
'anchor':
copy.deepcopy(anchor_frame),
'positive': [
copy.deepcopy(scene_frames.frames[idx])
for idx in sorted(sel_pos_idces)
],
'negative': [
copy.deepcopy(scene_frames.frames[idx])
for idx in sorted(sel_neg_idces)
],
})
# print(camera_centers[anchor_idx])
# print(camera_centers[pos_idces])
# print(camera_centers[neg_idces])
# print('----------------------------------------------------------')
return triple_list
