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
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 plot_poses(config, scale_factor=1):
'''Creates a plot showing the location and orientation of all cameras.
Creates a plot showing the location and orientation of all cameras given based on translation
and rotation vectors. If your cameras are very close together or far apart you can change the
scaling factor as necessary.
Arguments:
pose_estimation_config {dict} -- see help(ncams.camera_tools). Should have following keys:
serials {list of numbers} -- list of camera serials.
world_locations {list of np.arrays} -- world locations of each camera.
world_orientations {list of np.arrays} -- world orientation of each camera.
'''
from utils.calibration_utils import load_intrinsics, load_extrinsics
path = config['calibration']['calib_video_path']
# intrinscis_dict = load_intrinsics(path, ['1', '2', '3', '4'])
extrinsics_dict = load_extrinsics(path)
num_cameras = 4
world_orientations = []
world_locations = []
for i in range(num_cameras):
extrinsics = np.array(extrinsics_dict[str(i+1)])
world_orientations.append(extrinsics[:3, :3])
world_locations.append(extrinsics[:3, -1])
labels = ['cam_'+str(i) for i in range(num_cameras)]
# world_locations = pose_estimation_config['world_locations']
# world_orientations = pose_estimation_config['world_orientations']
# serials = pose_estimation_config['serials']
# num_cameras = len(serials)
# Only accepts list format so check if this is true only when a single camera is present
# if num_cameras == 1: # AS: Not sure if needed anymore
# if isinstance(world_locations, np.ndarray):
# world_locations = [world_locations]
# if isinstance(world_orientations, np.ndarray):
# world_orientations = [world_orientations]
# Create a figure with axes
fig = plt.figure()
ax = fig.gca(projection='3d')
plt.setp(ax.get_xticklabels(), visible=False)
plt.setp(ax.get_yticklabels(), visible=False)
plt.setp(ax.get_zticklabels(), visible=False)
# Keep the verts for setting the axes later
cam_verts = [[] for _ in range(num_cameras)]
for icam in range(num_cameras):
# Get the vertices to plot appropriate to the translation and rotation
cam_verts[icam], cam_center = create_camera(
scale_factor=scale_factor,
rotation_vector=world_orientations[icam],
translation_vector=world_locations[icam])
# Plot it and change the color according to it's number
ax.add_collection3d(Poly3DCollection(
cam_verts[icam], facecolors='C'+str(icam), linewidths=1, edgecolors='k', alpha=1))
# Give each camera a label
ax.text(np.asscalar(cam_center[0]), np.asscalar(cam_center[1]), np.asscalar(cam_center[2]),
'cam_' + str(icam))
# mpl is weird about maintaining aspect ratios so this has to be done
ax_min = np.min(np.hstack(cam_verts))
ax_max = np.max(np.hstack(cam_verts))
# Set the axes and viewing angle
# Note that this is reversed so that the cameras are looking towards us
ax.set_xlim([ax_max, ax_min])
ax.set_ylim([ax_min, ax_max])
ax.set_zlim([ax_min, ax_max])
ax.view_init(elev=105, azim=-90)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')