def calibrate_extrinsic(config):
path, videos, vid_indices = get_video_path(config)
output_filename = 'extrinsics.toml'
output_path = os.path.join(path, output_filename)
if os.path.exists(output_path):
print('\n{} already exists.'.format(output_filename))
return
try:
intrinsics = load_intrinsics(path, vid_indices)
except:
print("\nIntrinsic calibration output does not exist.")
return
board = get_calibration_board(config)
cam_align = config['triangulation']['cam_align']
extrinsics, error = get_extrinsics(vid_indices, videos, intrinsics,
cam_align, board)
extrinsics_out = {}
for k, v in extrinsics.items():
extrinsics_out[k] = v.tolist()
extrinsics_out['error'] = float(error)
with open(output_path, 'w') as f:
toml.dump(extrinsics_out, f)
def validate_3d(config, **kwargs):
path, videos, vid_indices = get_video_path(config)
bp_interested = config['labeling']['bodyparts_interested']
reconstruction_threshold = config['triangulation'][
'reconstruction_threshold']
if config['triangulation'].get('reconstruction_output_path') is None:
output_path = kwargs.get('output_path', '')
else:
output_path = config['triangulation']['reconstruction_output_path']
try:
intrinsics = load_intrinsics(path, vid_indices)
except:
print("Intrinsic calibration output does not exist.")
return
try:
extrinsics = load_extrinsics(path)
except:
print("Extrinsic calibration output does not exist.")
return
cam_mats = []
cam_mats_dist = []
for vid_idxs in vid_indices:
mat = arr(extrinsics[vid_idxs])
left = arr(intrinsics[vid_idxs]['camera_mat'])
cam_mats.append(mat)
cam_mats_dist.append(left)
cam_mats = arr(cam_mats)
cam_mats_dist = arr(cam_mats_dist)
out = load_labeled_2d_data(config, vid_indices, bp_interested)
all_points_raw = out['points']
all_points_und = undistort_points(all_points_raw, vid_indices, intrinsics)
length = all_points_raw.shape[0]
shape = all_points_raw.shape
all_points_3d = np.zeros((shape[0], shape[2], 3))
all_points_3d.fill(np.nan)
errors = np.zeros((shape[0], shape[2]))
errors.fill(np.nan)
scores_3d = np.zeros((shape[0], shape[2]))
scores_3d.fill(np.nan)
num_cams = np.zeros((shape[0], shape[2]))
num_cams.fill(np.nan)
for i in trange(all_points_und.shape[0], ncols=70):
for j in range(all_points_und.shape[2]):
pts = all_points_und[i, :, j, :]
good = ~np.isnan(pts[:, 0])
if np.sum(good) >= 2:
# TODO: make triangulation type configurable
# p3d = triangulate_optim(pts[good], cam_mats[good])
p3d = triangulate_simple(pts[good], cam_mats[good])
all_points_3d[i, j] = p3d[:3]
errors[i, j] = reprojection_error_und(p3d, pts[good],
cam_mats[good],
cam_mats_dist[good])
num_cams[i, j] = np.sum(good)
if 'reference_point' in config['triangulation'] and 'axes' in config[
'triangulation']:
all_points_3d_adj = correct_coordinate_frame(config, all_points_3d,
bp_interested)
else:
all_points_3d_adj = all_points_3d
dout = pd.DataFrame()
for bp_num, bp in enumerate(bp_interested):
for ax_num, axis in enumerate(['x', 'y', 'z']):
dout[bp + '_' + axis] = all_points_3d_adj[:, bp_num, ax_num]
dout[bp + '_error'] = errors[:, bp_num]
dout[bp + '_ncams'] = num_cams[:, bp_num]
dout['fnum'] = np.arange(length)
dout.to_csv(os.path.join(output_path, 'validate_3d_data.csv'), index=False)
def reconstruction_validation(config, img_paths):
images = [cv2.imread(img_path) for img_path in img_paths]
board = get_calibration_board(config)
(w,h) = board.getChessboardSize()
num_corners = (w-1)*(h-1)
path, videos, vid_indices = get_video_path(config)
num_cams = len(vid_indices)
intrinsics = load_intrinsics(path, vid_indices)
all_detectedCorners = np.zeros((num_corners, num_cams, 2))
all_detectedCorners.fill(np.nan)
all_detectedIds = []
images_with_corners = []
for i, (image, vid_idx) in enumerate(zip(images, vid_indices)):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
intrinsic = intrinsics[vid_idx]
detectedCorners, detectedIds = detect_aruco(gray, intrinsic, board)
images_with_corners.append(cv2.aruco.drawDetectedCornersCharuco(image,
detectedCorners,
detectedIds))
all_detectedIds.append(detectedIds)
for coord, j in zip(detectedCorners, detectedIds):
all_detectedCorners[j[0]][i] = coord
extrinsics = load_extrinsics(path)
cam_mats = np.array([extrinsics[vid_idx] for vid_idx in vid_indices])
undistorted_corners = undistort_points(all_detectedCorners, vid_indices, intrinsics)
points_3d, _ = triangulate_points(undistorted_corners, cam_mats)
points_3d = points_3d[:, :3]
points_3d_transform = transform_corners(points_3d, w, h)
square_length = config['calibration']['board_square_side_length']
x = np.linspace((-w/2+1)*square_length, (w/2-1)*square_length, w-1)
y = np.linspace((-h/2+1)*square_length, (h/2-1)*square_length, h-1)
xv, yv = np.meshgrid(x, y)
pseudo_real_corners = np.zeros((num_corners, 3))
pseudo_real_corners[:, 0] = np.reshape(xv, (-1))
pseudo_real_corners[:, 1] = np.reshape(yv, (-1))
subplot_w = int(num_cams // np.floor(np.sqrt(num_cams)))
subplot_h = num_cams / subplot_w
fig = plt.figure()
for i in range(num_cams):
ax = fig.add_subplot(subplot_w, subplot_h, i+1)
ax.imshow(images_with_corners[i])
ax.axis('off')
fig = plt.figure()
ax = fig.gca(projection='3d')
labels = ['id_'+str(i) for i in range(num_corners)]
for i, (pred, real) in enumerate(zip(points_3d_transform, pseudo_real_corners)):
ax.scatter(pred[0], pred[1], pred[2], c='r', alpha=0.5)
ax.scatter(real[0], real[1], real[2], c='b', alpha=0.5)
annotate3D(ax, s=labels[i], xyz=pred, fontsize=8, xytext=(-3,3),
textcoords='offset points', ha='center',va='bottom')
ax_min, ax_max = -np.floor(max(w, h)*square_length)//2, np.floor(max(w, h)*square_length//2)
ax.set_xlim([ax_min, ax_max])
ax.set_ylim([ax_min, ax_max])
ax.set_zlim([ax_min, ax_max])
ax.view_init(elev=50, azim=-70)
ax.set_xlabel('x (mm)')
ax.set_ylabel('y (mm)')
ax.set_zlabel('z (mm)')
return points_3d, points_3d_transform