def project_on_last(cam_list, p):
p = [p_.reshape(1, 2) for p_ in p]
point3d = triangulate_linear(cam_list[:-1], p)
point2d = cam_list[-1].project(point3d)
point2d = np.squeeze(point2d)
return point2d
def energy_drosoph(
cam_list,
img_id,
j_id,
points2d,
points3d=None,
bone_length=None,
image_shape=config["image_shape"],
hm_shape=config["heatmap_shape"],
):
"""
calculate energy from 2d observations
points2d: 2x3 array, observations from three cameras
points3d: 15x3 array, used only to calculate the bone probability
"""
points2d_list = [p_.reshape(1, 2) for p_ in points2d * image_shape]
p3d = triangulate_linear(cam_list, points2d_list)
err_proj = error_reprojection(cam_list, (points2d * image_shape).astype(int))
err_proj = np.mean(np.abs(err_proj))
prob_heatm = probability_heatmap(
cam_list, img_id, j_id, (points2d * hm_shape).astype(int)
)
# not the root of the chain
prob_bone = None
return p3d, err_proj, prob_heatm, prob_bone
def triangulate(self, cam_id_list=None):
assert self.cam_list
if cam_id_list is None:
cam_id_list = list(range(self.num_cameras))
points2d_shape = self.cam_list[0].points2d.shape
self.points3d_m = np.zeros(shape=(points2d_shape[0], points2d_shape[1],
3),
dtype=np.float)
data_shape = self.cam_list[0].points2d.shape
for img_id in range(data_shape[0]):
for j_id in range(data_shape[1]):
cam_list_iter = list()
points2d_iter = list()
for cam in [self.cam_list[cam_idx] for cam_idx in cam_id_list]:
if np.any(cam[img_id, j_id, :] == 0):
continue
if not config["skeleton"].camera_see_joint(
cam.cam_id, j_id):
continue
cam_list_iter.append(cam)
points2d_iter.append(cam[img_id, j_id, :])
if len(cam_list_iter) >= 2:
self.points3d_m[img_id, j_id, :] = triangulate_linear(
cam_list_iter, points2d_iter)
def error_reprojection(cam_list, points2d):
"""
points2d: nx2 array containing projections
"""
points2d_list = [p.reshape(1, 2) for p in points2d]
point3d = triangulate_linear(cam_list, points2d_list)
err = list()
for cam, p in zip(cam_list, points2d):
err.append(cam.project(point3d) - p)
return np.array(err)
def triangulate(self):
assert self.cam_list
s = self.cam_list[0].points2d.shape
self.points3d = np.zeros(shape=(s[0], s[1], 3), dtype=np.float)
for img_id, j_id in product(range(s[0]), range(s[1])):
cam_list_iter = list()
points2d_iter = list()
for cam in self.cam_list:
if not (
np.any(cam[img_id, j_id, :] == 0)
or not config["skeleton"].camera_see_joint(cam.cam_id, j_id)
):
cam_list_iter.append(cam)
points2d_iter.append(cam[img_id, j_id, :])
if len(cam_list_iter) >= 2:
self.points3d[img_id, j_id, :] = triangulate_linear(
cam_list_iter, points2d_iter
)