def _show_marker_det_img_folder(marker_path, img_data_path, det_file_name,
block):
# load detections
det_file = os.path.join(img_data_path, det_file_name)
print('\tDetection file: %s' % det_file)
assert os.path.exists(det_file), 'Could not find detection file.'
det = json_load(det_file)
# check for image files
img_list = find_images(img_data_path)
print('Found %s images for marker detection.' % len(img_list))
# sanity check
assert len(det['p2d']) == len(
img_list), 'Number of detections and number of images differs.'
assert len(det['pid']) == len(
img_list), 'Number of detections and number of images differs.'
# set up detector
detector = BoardDetector(marker_path)
# show
for idx, img_p in enumerate(img_list):
img_file = os.path.basename(img_p)
if img_file not in det['files']:
print('No detection available for: %s' % img_file)
continue
img = cv2.imread(img_p)
detector.draw_board(img,
np.array(det['p2d'][idx]),
np.array(det['pid'][idx]),
block=block)
def _detect_marker_video(marker_path, vid_data_path):
# set up detector
detector = BoardDetector(marker_path)
# detect board in images
points2d, point_ids = detector.process_video(vid_data_path)
# image shape
cap = cv2.VideoCapture(vid_data_path)
w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
img_shape = (h, w)
return points2d, point_ids, img_shape, vid_data_path, os.path.dirname(
vid_data_path)
def _detect_marker_img_folder(marker_path, img_data_path, verbose):
# check for image files
img_list = find_images(img_data_path)
if verbose > 1:
print('Found %s images for marker detection.' % len(img_list))
# set up detector
detector = BoardDetector(marker_path)
# detect board in images
points2d, point_ids = detector.process_image_batch(img_list)
# image shape
img_shape = cv2.imread(img_list[0]).shape[:2]
files = [os.path.basename(x) for x in img_list]
return points2d, point_ids, img_shape, files, img_data_path
def _show_marker_det_video(marker_path, video_path, det_file_name, block):
# load detections
data_path = os.path.dirname(video_path)
det_file = os.path.join(data_path, det_file_name)
print('\tDetection file: %s' % det_file)
assert os.path.exists(det_file), 'Could not find detection file.'
det = json_load(det_file)
# check video path
video = cv2.VideoCapture(video_path)
num_frames = video.get(cv2.CAP_PROP_FRAME_COUNT)
print('Found video with %d frames.' % num_frames)
# sanity check
assert len(
det['p2d']
) == num_frames, 'Number of detections and number of frames differs.'
assert len(
det['pid']
) == num_frames, 'Number of detections and number of frames differs.'
# set up detector
detector = BoardDetector(marker_path)
# show
idx = 0
while True:
if not video.isOpened():
break
ret, img = video.read()
if not ret:
break
detector.draw_board(img,
np.array(det['p2d'][idx]),
np.array(det['pid'][idx]),
block=block)
idx += 1
def test_board_pose_estimator(show=False):
""" Test detecting Boards in images. """
import cv2
from utils.general_util import json_load
from core.BoardDetector import BoardDetector
img_list = ['./data/calib_test_data/real/april_board_tags_sample.JPG']
assert os.path.exists(img_list[0]), 'Image file not found.'
detector = BoardDetector(
'./data/calib_test_data/marker_32h11b2_4x4x_7cm.json')
point_coords_frames, point_ids_frames = detector.process_image_batch(
img_list)
gt1 = json_load('data/calib_test_data/real/gt_det1.json')
_same(point_coords_frames[0], gt1['c'])
_same(point_ids_frames[0], gt1['i'])
if show:
for img_path, points, point_ids in zip(img_list, point_coords_frames,
point_ids_frames):
image = cv2.imread(img_path)
detector.draw_board(image, points, point_ids, linewidth=2)
detector = BoardDetector(
'./data/calib_test_data/marker_16h5b1_4x4x_15cm.json')
point_coords_frames, point_ids_frames = detector.process_image_batch(
img_list)
gt2 = json_load('data/calib_test_data/real/gt_det2.json')
_same(point_coords_frames[0], gt2['c'])
_same(point_ids_frames[0], gt2['i'])
if show:
for img_path, points, point_ids in zip(img_list, point_coords_frames,
point_ids_frames):
image = cv2.imread(img_path)
detector.draw_board(image, points, point_ids, linewidth=2)
print('SUCCESS: test_board_pose_estimator')
def calc_intrinsics(marker_path,
data_path,
det_file_name,
output_file=None,
estimate_dist=True,
dist_complexity=5,
cache=False,
verbose=0):
if os.path.isdir(data_path):
base_dir = data_path
else:
base_dir = os.path.dirname(data_path)
# try to load precomputed
if output_file is not None:
calib_file = os.path.join(base_dir, output_file)
if cache and os.path.exists(calib_file):
if verbose > 0:
print('Loading intrinsic calibration from: %s' % calib_file)
calib = json_load(calib_file)
return np.array(calib['K']), np.array(calib['dist'])
if verbose > 0:
print('Calculating intrinsic calibration for:')
print('\tData path: %s' % data_path)
print('\tMarker file: %s' % marker_path)
# set up detector and estimator
detector = BoardDetector(marker_path)
if os.path.isdir(data_path):
if verbose > 0:
print('\tAssuming: Folder of images.')
base_dir = data_path
else:
if verbose > 0:
print('\tAssuming: Video file.')
base_dir = os.path.dirname(data_path)
# check for detections
if det_file_name is None:
det = detect_marker(marker_path,
data_path,
cache=cache,
verbose=verbose - 1)
else:
detections_file = os.path.join(base_dir, det_file_name)
if not os.path.exists(detections_file):
if verbose > 1:
print(
'Could not locate marker detections. Running detector now and saving them to folder.'
)
det = detect_marker(marker_path,
data_path,
det_file_name,
verbose=verbose - 1)
else:
det = json_load(detections_file)
# give points unique ids
max_num_pts = len(detector.object_points)
p2d, pid, p3dm, fid = enumerate_points(detector, det['p2d'], det['pid'],
max_num_pts)
if verbose > 0:
print('Found %d unique points to estimate intrinsics from.' %
pid.shape[0])
# estimate intrinsics
K, dist = estimate_intrinsics(p2d,
fid,
p3dm,
det['img_shape'],
estimate_dist=estimate_dist,
dist_complexity=dist_complexity,
verbose=verbose)
# save intrinsics
if output_file is not None:
calib = {'K': K, 'dist': dist}
json_dump(calib_file, calib, verbose=verbose > 0)
return K, dist
def check_extrinsics(marker_path, data_path, cam_pat, run_pat, K, dist, M,
verbose):
# find input data
base_path, img_shapes, data, cam_ids = find_data(data_path, cam_pat,
run_pat)
# get detections
det = list()
for x in data:
det.append(
detect_marker(marker_path,
x,
output_file=None,
cache=False,
verbose=False))
# uniquely number detections
detector = BoardDetector(marker_path)
tagpose = TagPoseEstimator(detector.object_points)
p2d, pid, p3d, fid, cid, mid = enumerate_points(det,
detector.object_points)
# calculate object poses and pick greedily
scores_object, T_obj2cam = estimate_and_score_object_poses(
tagpose, p2d, cid, fid, mid, K, dist)
object_poses = greedy_pick_object_pose(scores_object, T_obj2cam, M,
verbose)
point3d_coord, pid2d_to_pid3d = calc_3d_object_points(
tagpose.object_points, object_poses, fid, cid, mid)
# find reprojection error
err_mean, error_per_cam = calculate_reprojection_error(
p2d,
point3d_coord,
pid2d_to_pid3d,
K,
dist,
M,
cid,
return_cam_wise=True)
# give some output of the results
hist_per_cam = dict()
print('Error per cam:')
for cid, error_cam in error_per_cam.items():
y, x = np.histogram(error_cam)
x = 0.5 * (x[:-1] + x[1:])
hist_per_cam[cid] = (x, y)
print('\t> Cam%d: %.3f (px)' % (cid, np.mean(error_cam)))
print('Mean error: %.2f pixels' % err_mean)
fig = plt.figure()
ax = fig.add_subplot(111)
for cid, (x, y) in hist_per_cam.items():
ax.plot(x, y, label='cam%d' % cid)
plt.legend()
plt.title('Reprojection error distribution')
plt.xlabel('Pixels')
plt.xlabel('Occurences')
img = fig2data(fig)
cv2.imshow('stats', img[:, :, ::-1])
cv2.waitKey()
def show_intrinsic_calib(marker_path, data_path, det_file_name,
calib_file_name, show_size):
print('Showing marker detections for:')
print('\tData path: %s' % data_path)
print('\tMarker file: %s' % marker_path)
if os.path.isdir(data_path):
print('\tAssuming: Folder of images.')
base_dir = data_path
else:
print('\tAssuming: Video file.')
base_dir = os.path.dirname(data_path)
det_file = os.path.join(base_dir, det_file_name)
calib_file = os.path.join(base_dir, calib_file_name)
print('\tDetection file: %s' % det_file)
print('\tCalib file: %s' % calib_file)
# load detections
assert os.path.exists(det_file), 'Could not find detection file.'
det = json_load(det_file)
# load calibration
assert os.path.exists(calib_file), 'Could not find detection file.'
calib = json_load(calib_file)
K, dist = np.array(calib['K']), np.array(calib['dist'])
img = _read_first_frame(data_path, det)
# set up detector
detector = BoardDetector(marker_path)
# calculate statistics
err, angle, depths = list(), list(), list()
for p2d, pid in tqdm(zip(det['p2d'], det['pid']),
total=len(det['p2d']),
desc='Calculating stats'):
if len(pid) == 0:
continue
p3d_m = detector.object_points[pid]
a, d, e = _calc_board_stats(np.array(p2d), p3d_m, K, dist)
angle.append(a)
depths.extend(d)
err.extend(e)
# Print reprojection error
err = np.array(err)
print('Reprojection error: min=%.2f, mean=%.2f, max=%.2f (px)' %
(err.min(), err.mean(), err.max()))
# show error and depth distribution
angle = np.array(angle)
angle = angle[~np.isnan(angle)]
depths = np.array(depths)
fig = plt.figure()
ax1 = fig.add_subplot(121)
ax2 = fig.add_subplot(122)
ax1.hist(depths), ax1.set_title('distance to camera'), ax1.set_xlim(
[0, depths.max()])
ax2.hist(angle), ax2.set_title('angle wrt camera'), ax2.set_xlim([0, 180])
stats_img = fig2data(fig)
cv2.imshow('stats', stats_img[:, :, ::-1])
cv2.waitKey(100)
# show image space coverage
pts_all = np.concatenate([d for d in det['p2d'] if len(d) > 0], 0)
print('Showing %d detected points' % pts_all.shape[0])
_show_coverage(img, pts_all, show_size)
return det