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 calibrate_intrinsic(config):
path, videos, vid_indices = get_video_path(config)
board = get_calibration_board(config)
for vid, vid_idx in zip(videos, vid_indices):
output_filename = 'intrinsics_{}.toml'.format(vid_idx)
output_path = os.path.join(path, output_filename)
if os.path.exists(output_path):
print('\n{} already exists.'.format(output_filename))
continue
else:
calib = calibrate_camera(vid, board)
with open(output_path, 'w') as f:
toml.dump(calib, f)
def draw_dropout_histogram(config):
data_paths = config['paths_to_2d_data']
bp_interested = config['labeling']['bodyparts_interested']
path, videos, vid_indices = get_video_path(config)
from matplotlib import pyplot as plt
data = load_2d_data(config, vid_indices, bp_interested)
l = len(data['scores'])
fig = plt.figure()
# print(data['scores'].shape)
for i in range(4):
ax = fig.add_subplot(2,2,i+1)
ax.hist(data['scores'][:,i,:])
ax.set_title('Cam '+str(i))
ax.set_xlabel('Likelihood')
ax.set_ylabel('Percentage')
ax.set_ylim([0, l])
ax.set_yticks([0, l/5, 2*l/5, 3*l/5, 4*l/5, l])
ax.set_yticklabels(['0', '20', '40', '60', '80', '100'])
ax.legend(config['labeling']['bodyparts_interested'],loc='upper center',prop={'size': 6})
plt.show()
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 generate_three_dim_pictures(config,csv_idx, **kwargs):
path, videos, vid_indices = get_video_path(config)
if config.get('output_video_path') is None:
output_path = kwargs.get('output_path', '')
else:
output_path = config['output_video_path']
try:
connections = config['labeling']['scheme']
except KeyError:
connections = []
bp_interested = config['labeling']['bodyparts_interested']
bp_dict = dict(zip(bp_interested, range(len(bp_interested))))
if config['triangulation'].get('reconstruction_output_path') is None:
reconstruction_output_path = kwargs.get('output_path', '')
else:
reconstruction_output_path = config['triangulation']['reconstruction_output_path']
reconstruction_filename = 'output_3d_data_' + str(csv_idx) + '.csv'
data = pd.read_csv(os.path.join(reconstruction_output_path, reconstruction_filename))
all_points = np.array([np.array(data.loc[:, (bp+'_x', bp+'_y', bp+'_z')])
for bp in bp_interested])
all_errors = np.array([np.array(data.loc[:, bp+'_error'])
for bp in bp_interested])
all_errors[np.isnan(all_errors)] = 10000
good = (all_errors < 150)
all_points[~good] = np.nan
all_points_flat = all_points.reshape(-1, 3)
check = ~np.isnan(all_points_flat[:, 0])
low, high = np.percentile(all_points_flat[check], [5, 95], axis=0)
fps = config['video']['fps']
resolution = config['video']['resolution']
resolution = (640, 480)
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
three_dim_video_filename = 'output_3d_video_' + str(csv_idx) + '.mp4'
# cmap = plt.get_cmap('tab10')
all_x_points = all_points[:,:,0]
all_y_points = all_points[:,:,1]
all_z_points = all_points[:,:,2]
x_low, x_high = np.percentile(all_x_points[np.isfinite(all_x_points)], [3, 97])
y_low, y_high = np.percentile(all_y_points[np.isfinite(all_y_points)], [3, 97])
z_low, z_high = np.percentile(all_z_points[np.isfinite(all_z_points)], [3, 97])
fig = plt.figure()
ax = Axes3D(fig)
all_points = np.transpose(all_points, (1,0,2))
ax.view_init(azim=-48, elev=28)
def draw_lines(points):
ax.set_xlim([x_low-100, x_high+100])
ax.set_ylim([y_low-100, y_high+100])
ax.set_zlim([z_low-100, z_high+100])
ax.set_xlabel('x (mm)')
ax.set_ylabel('y (mm)')
ax.set_zlabel('z (mm)')
ax.scatter(points[:, 0], points[:, 1], points[:, 2], 'bo', alpha=0.5)
for connection in connections:
xs = [points[bp_dict[connection[0]]][0], points[bp_dict[connection[1]]][0]]
ys = [points[bp_dict[connection[0]]][1], points[bp_dict[connection[1]]][1]]
zs = [points[bp_dict[connection[0]]][2], points[bp_dict[connection[1]]][2]]
ax.plot(xs, ys, zs, 'g-')
points = all_points[1000]
draw_lines(points)
def generate_three_dim_video(config,csv_idx, **kwargs):
#def generate_three_dim_video(config, azimuth, elevation **kwargs):
path, videos, vid_indices = get_video_path(config)
if config.get('output_video_path') is None:
output_path = kwargs.get('output_path', '')
else:
output_path = config['output_video_path']
try:
connections = config['labeling']['scheme']
except KeyError:
connections = []
bp_interested = config['labeling']['bodyparts_interested']
bp_dict = dict(zip(bp_interested, range(len(bp_interested))))
if config['triangulation'].get('reconstruction_output_path') is None:
reconstruction_output_path = kwargs.get('output_path', '')
else:
reconstruction_output_path = config['triangulation']['reconstruction_output_path']
reconstruction_filename = 'output_3d_data_' + str(csv_idx)+ '.csv'
data = pd.read_csv(os.path.join(reconstruction_output_path, reconstruction_filename))
all_points = np.array([np.array(data.loc[:, (bp+'_x', bp+'_y', bp+'_z')])
for bp in bp_interested])
all_errors = np.array([np.array(data.loc[:, bp+'_error'])
for bp in bp_interested])
all_errors[np.isnan(all_errors)] = 10000
good = (all_errors < 150)
all_points[~good] = np.nan
all_points_flat = all_points.reshape(-1, 3)
check = ~np.isnan(all_points_flat[:, 0])
low, high = np.percentile(all_points_flat[check], [5, 95], axis=0)
fps = config['video']['fps']
resolution = config['video']['resolution']
resolution = (640, 480)
fourcc = cv2.VideoWriter_fourcc(*'DIVX')
three_dim_video_filename = 'output_3d_video_' + str(csv_idx) + '.avi'
writer = cv2.VideoWriter(os.path.join(output_path, three_dim_video_filename),
fourcc, fps, resolution)
# cmap = plt.get_cmap('tab10')
all_x_points = all_points[:,:,0]
all_y_points = all_points[:,:,1]
all_z_points = all_points[:,:,2]
x_low, x_high = np.percentile(all_x_points[np.isfinite(all_x_points)], [3, 97])
y_low, y_high = np.percentile(all_y_points[np.isfinite(all_y_points)], [3, 97])
z_low, z_high = np.percentile(all_z_points[np.isfinite(all_z_points)], [3, 97])
fig = plt.figure()
ax = Axes3D(fig)
all_points = np.transpose(all_points, (1,0,2))
ax.view_init(azim=-48, elev=28)
def draw_lines(points):
ax.set_xlim([x_low-100, x_high+100])
ax.set_ylim([y_low-100, y_high+100])
ax.set_zlim([z_low-100, z_high+100])
ax.set_xlabel('x (mm)')
ax.set_ylabel('y (mm)')
ax.set_zlabel('z (mm)')
ax.scatter(points[:, 0], points[:, 1], points[:, 2], 'bo', alpha=0.5)
for connection in connections:
xs = [points[bp_dict[connection[0]]][0], points[bp_dict[connection[1]]][0]]
ys = [points[bp_dict[connection[0]]][1], points[bp_dict[connection[1]]][1]]
zs = [points[bp_dict[connection[0]]][2], points[bp_dict[connection[1]]][2]]
ax.plot(xs, ys, zs, 'g-')
# points = all_points[0]
# draw_lines(points)
# print('ax.azim {}'.format(ax.azim))
# print('ax.elev {}'.format(ax.elev))
# # For testing
# all_points = all_points[:600]
canvas = FigureCanvas(fig)
for i in trange((len(all_points)), ncols=70):
ax.cla()
points = all_points[i]
draw_lines(points)
canvas.draw()
img = np.array(canvas.renderer._renderer)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cv2.resize(img, resolution)
writer.write(img)
plt.close()
writer.release()
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
