class TestGraphMethods(TestCase):
def setUp(self):
self.__graph = Graph()
def test_structure(self):
self.assertIsInstance(self.__graph, Graph)
def test_add_node(self):
self.assertEqual(len(self.__graph.get_all_node()), 0)
self.assertNotEqual(len(self.__graph.get_all_node()), 1)
self.__graph.add_node(node=1)
self.assertNotEqual(len(self.__graph.get_all_node()), 0)
self.assertEqual(len(self.__graph.get_all_node()), 1)
def test_add_edges(self):
# Adding nodes
self.__graph.add_node(node='B')
self.__graph.add_node(node='A')
# Connecting two nodes
self.__graph.add_edge(edge=('A', 'B'))
# Adding nodes
self.__graph.add_node(node='C')
self.__graph.add_node(node='D')
self.__graph.add_edge(edge=('A', 'D'))
# Assertions
self.assertTrue(self.__graph.is_connected('A', 'B'))
self.assertTrue(self.__graph.is_connected('B', 'A'))
self.assertFalse(self.__graph.is_connected('C', 'D'))
self.assertFalse(self.__graph.is_connected('B', 'D'))
with self.assertRaises(TypeError):
self.__graph.add_edge(edge=('Z', 'F'))
def test_if_two_nodes_was_on_the_same_network(self):
# Adding nodes
self.__graph.add_node(node='C')
self.__graph.add_node(node='D')
self.__graph.add_node(node='A')
self.__graph.add_node(node='B')
self.__graph.add_edge(edge=('A', 'D'))
# Assertions
self.assertFalse(self.__graph.same_network(('A', 'B')))
self.assertFalse(self.__graph.same_network(('B', 'A')))
self.assertFalse(self.__graph.same_network(('C', 'D')))
self.assertFalse(self.__graph.same_network(('B', 'D')))
self.assertTrue(self.__graph.same_network(('A', 'D')))
self.assertTrue(self.__graph.same_network(('D', 'A')))
def estimate_extrinsics_pnp(tagpose_estimator,
cam_intrinsic,
cam_dist,
point2d_coord,
point2d_cid,
point2d_fid,
point2d_pid,
point2d_mid,
verbose=0):
""" Estimates extrinsic parameters for each camera from the given 2D point correspondences alone.
It estimates the essential matrix for camera pairs along the observation graph.
Input:
tagpose_estimator: custom object, Estimates the pose between a camera and the calibration objects.
cam_intrinsic: list of 3x3 np.array, Intrinsic calibration of each camera.
cam_dist: list of 1x5 np.array, Distortion coefficients following the OpenCV pinhole camera model.
point2d_coord: Nx2 np.array, Array containing 2D coordinates of N points.
point2d_cid: Nx1 np.array, Array containing the camera id for each of the N points.
point2d_fid: Nx1 np.array, Array containing the frame id for each of the N points.
point2d_pid: Nx1 np.array, Array containing a unique point id for each of the N points.
point2d_mid: Nx1 np.array, Array containing a marker-unique id for each of the N points.
Returns:
cam_extrinsic: list of 4x4 np.array, Intrinsic calibration of each camera.
calib_object_points3d: Mx3 np.array, 3D Points of the calibration object in a object based frame.
"""
assert len(cam_intrinsic) >= 2, "Too little cameras."
assert len(cam_intrinsic) == len(cam_dist), "Shape mismatch."
assert len(point2d_cid.shape) == 1, "Shape mismatch."
assert len(point2d_fid.shape) == 1, "Shape mismatch."
assert len(point2d_pid.shape) == 1, "Shape mismatch."
assert len(point2d_mid.shape) == 1, "Shape mismatch."
assert point2d_coord.shape[0] == point2d_cid.shape[0], "Shape mismatch."
assert point2d_coord.shape[0] == point2d_fid.shape[0], "Shape mismatch."
assert point2d_coord.shape[0] == point2d_pid.shape[0], "Shape mismatch."
assert point2d_coord.shape[0] == point2d_mid.shape[0], "Shape mismatch."
assert len(cam_intrinsic) == len(
np.unique(point2d_cid).flatten().tolist()), "Shape mismatch."
if verbose > 0:
print('\n\n------------')
print('- Estimating extrinsic parameters by solving PNP problems')
num_cams = len(cam_intrinsic)
num_frames = np.max(point2d_fid) + 1
# get model shape
calib_object_points3d = tagpose_estimator.object_points.copy()
# 1. Iterate cams and estimate relative pose to the calibration object for each frame
scores_object, T_obj2cam = estimate_and_score_object_poses(
tagpose_estimator, point2d_coord, point2d_cid, point2d_fid,
point2d_mid, cam_intrinsic, cam_dist)
def _calc_score(T1, T2):
""" Estimates how closely T1 * T2 = eye() holds. """
R = np.matmul(T1, T2) - np.eye(T1.shape[0])
return np.sum(np.abs(R)) # frobenius norm
# try to find the pair of frames which worked best --> estimate relative camera pose from there
scores_rel_calib = dict(
) # store how good this guess seems to be for calibrating a cam pair
for fid1 in range(num_frames): # for each pair of frames
for fid2 in range(fid1, num_frames):
scores_rel_calib[fid1, fid2] = dict()
for cid1 in range(num_cams): # check each pair of cams
for cid2 in range(cid1 + 1, num_cams):
# check if its valid
if (T_obj2cam[fid1][cid2] is None) or (T_obj2cam[fid1][cid1] is None) or \
(T_obj2cam[fid2][cid2] is None) or (T_obj2cam[fid2][cid1] is None):
s_rel = float('inf')
else:
# calculate the transformation cam1 -> cams2 using fid1
T12_fid1 = np.matmul(
T_obj2cam[fid1][cid2],
np.linalg.inv(T_obj2cam[fid1][cid1]))
# calculate the transformation cam2 -> cams1 using fid2
T21_fid2 = np.matmul(
T_obj2cam[fid2][cid1],
np.linalg.inv(T_obj2cam[fid2][cid2]))
# for perfect estimations the two mappings should be the inverse of each others
s_rel = _calc_score(T12_fid1, T21_fid2)
scores_rel_calib[fid1, fid2][cid1, cid2] = s_rel
# 3. Find out which frames are optimal for a given cam pair
cam_pair_best_fid = dict()
for cid1 in range(num_cams):
for cid2 in range(cid1 + 1, num_cams):
min_fid = None
min_v = float('inf')
for fid_pair, score_dict in scores_rel_calib.items():
# get an initial value
if min_fid is None:
min_fid = fid_pair
min_v = score_dict[cid1, cid2]
continue
# if current best is worse than current item replace
if min_v > score_dict[cid1, cid2]:
min_fid = fid_pair
min_v = score_dict[cid1, cid2]
cam_pair_best_fid[cid1, cid2] = min_fid
# 3. Build observation graph and use djikstra to estimate relative camera poses
observation_graph = Graph()
for cid in range(num_cams):
observation_graph.add_node(cid)
# populate with edges
score_accumulated = [0 for _ in range(num_cams)
] # accumulate score for each cam
for cid1 in range(num_cams):
for cid2 in range(cid1 + 1, num_cams):
fid_pair = cam_pair_best_fid[cid1, cid2]
s = scores_rel_calib[fid_pair][cid1, cid2]
observation_graph.add_edge(cid1, cid2, s)
observation_graph.add_edge(cid2, cid1, s)
score_accumulated[cid1] += s
score_accumulated[cid2] += s
# root cam (the one that has the lowest overall score)
root_cam_id = np.argmin(np.array(score_accumulated))
if verbose > 1:
print('- Accumulated score (lower is better): ', score_accumulated)
print('- Choosing root cam', root_cam_id)
# 4. Determine which relative poses to estimate
# use Dijkstra to find "cheapest" path (i.e. the one with most observations) from the starting cam to all others
cam_path = dict(
) # contains how to get from the starting cam to another one cam_path[target_cam] = [path]
for cid in range(num_cams):
if cid == root_cam_id:
cam_path[cid] = [cid]
continue
cost, camchain = shortest_path(observation_graph, root_cam_id, cid)
cam_path[cid] = camchain
if verbose > 1:
for k, v in cam_path.items():
print('- Camchain to %d: ' % k, v)
# 5. Put together the relative camera poses
relative_pose = dict() # contains the trafo from start -> target
relative_pose_pair = dict(
) # contains already estimated poses between cameras;
# is the trafo from j -> i; xi = relative_pose_pair[i, j] * xj
for target_camid in range(num_cams):
if target_camid == root_cam_id:
relative_pose[target_camid] = np.eye(4)
continue
M = np.eye(4) # this is the trafo from start -> X
for i in range(len(cam_path[target_camid]) - 1): # traverse cam path
# get current cam_pair on the cam_path
inter_camid1 = cam_path[target_camid][
i] # this is where we currently are
inter_camid2 = cam_path[target_camid][
i + 1] # this is where we want to transform to
swapped = False
if inter_camid2 < inter_camid1:
t = inter_camid2
inter_camid2 = inter_camid1
inter_camid1 = t
swapped = True
if verbose > 1:
print(
'- Attempting to estimate the relative pose from cam %d --> %d'
% (inter_camid1, inter_camid2))
# calculate only when not calculated yet
if (inter_camid1, inter_camid2) not in relative_pose_pair.keys():
fid1, fid2 = cam_pair_best_fid[inter_camid1, inter_camid2]
msg = "Calibration impossible! There is no way feasible way to calibrate cam%d and cam%d." % \
(inter_camid1, inter_camid2)
assert T_obj2cam[fid1][inter_camid1] is not None, msg
assert T_obj2cam[fid1][inter_camid2] is not None, msg
# calculate the transformation cam1 -> cams2 using the optimal fids
T12 = np.matmul(T_obj2cam[fid1][inter_camid1],
np.linalg.inv(T_obj2cam[fid1][inter_camid2]))
relative_pose_pair[inter_camid1, inter_camid2] = T12
delta = relative_pose_pair[inter_camid1, inter_camid2]
if swapped:
delta = np.linalg.inv(delta)
# accumulate trafos
M = np.matmul(delta, M)
relative_pose[target_camid] = M
if verbose > 0:
print('- Extrinsics estimated')
if verbose > 2:
for cid in range(num_cams):
print('\n- Trafo Root (%d) --> %d' % (root_cam_id, cid))
print(relative_pose[cid])
print('')
cam_extrinsic = list()
for cid in range(num_cams):
cam_extrinsic.append(relative_pose[cid])
# 6. Figure out the object poses (if there is no observation its impossible)
object_poses = greedy_pick_object_pose(scores_object, T_obj2cam,
relative_pose, verbose)
cam_extrinsic, object_poses = _center_extrinsics(
cam_extrinsic, object_poses) # ensure camera 0 is the world center
point3d_coord, pid2d_to_pid3d = calc_3d_object_points(
calib_object_points3d, object_poses, point2d_fid, point2d_cid,
point2d_mid)
return cam_extrinsic, point3d_coord, pid2d_to_pid3d, object_poses