def test_equals():
system = make_default_system()
assert system != 1234
system2 = MultiCameraSystem(
[CameraModel.load_camera_simple(name='cam%d' % i) for i in range(2)])
system3 = MultiCameraSystem(
[CameraModel.load_camera_simple(name='cam%d' % i) for i in range(3)])
assert system2 != system3
system4 = make_default_system()
d = system4.to_dict()
d['camera_system'][0]['width'] += 1
system5 = MultiCameraSystem.from_dict(d)
assert system4 != system5
def transform_system(old, s, R, T):
new_cams = []
for name in old.get_names():
orig_cam = old.get_camera(name)
new_cam = orig_cam.get_aligned_camera(s, R, T)
new_cams.append(new_cam)
return MultiCameraSystem(new_cams)
def test_distortion():
base = CameraModel.load_camera_default()
lookat = np.array( (0.0, 0.0, 0.0) )
up = np.array( (0.0, 0.0, 1.0) )
cams = []
cams.append( base.get_view_camera(eye=np.array((1.0,0.0,1.0)),lookat=lookat,up=up) )
distortion1 = np.array( [0.2, 0.3, 0.1, 0.1, 0.1] )
cam_wide = CameraModel.load_camera_simple(name='cam_wide',
fov_x_degrees=90,
eye=np.array((-1.0,-1.0,0.7)),
lookat=lookat,
distortion_coefficients=distortion1,
)
cams.append(cam_wide)
cam_ids = []
for i in range(len(cams)):
cams[i].name = 'cam%02d'%i
cam_ids.append(cams[i].name)
cam_system = MultiCameraSystem(cams)
R = reconstruct.Reconstructor.from_pymvg(cam_system)
for cam_id in cam_ids:
nl_params = R.get_intrinsic_nonlinear(cam_id)
mvg_cam = cam_system.get_camera_dict()[cam_id]
assert np.allclose(mvg_cam.distortion, nl_params)
def test_align():
system1 = make_default_system()
system2 = system1.get_aligned_copy(system1) # This should be a no-op.
assert system1 == system2
system3 = MultiCameraSystem(
[CameraModel.load_camera_simple(name='cam%d' % i) for i in range(2)])
nose.tools.assert_raises(ValueError, system3.get_aligned_copy, system1)
def _get_cams(with_distortion):
base = CameraModel.load_camera_default()
lookat = np.array((0.0, 0.0, 0.0))
up = np.array((0.0, 0.0, 1.0))
cams = []
cams.append(
base.get_view_camera(eye=np.array((1.0, 0.0, 1.0)),
lookat=lookat,
up=up))
cams.append(
base.get_view_camera(eye=np.array((1.2, 3.4, 5.6)),
lookat=lookat,
up=up))
cams.append(
base.get_view_camera(eye=np.array((0, 0.3, 1.0)), lookat=lookat,
up=up))
if with_distortion:
distortion1 = np.array([0.2, 0.3, 0.1, 0.1, 0.1])
else:
distortion1 = np.zeros((5, ))
cam_wide = CameraModel.load_camera_simple(
name='cam_wide',
fov_x_degrees=90,
eye=np.array((-1.0, -1.0, 0.7)),
lookat=lookat,
distortion_coefficients=distortion1,
)
cams.append(cam_wide)
for i in range(len(cams)):
cams[i].name = 'cam%02d' % i
cam_system = MultiCameraSystem(cams)
reconstructor = Reconstructor.from_pymvg(cam_system)
result = dict(
cams=cams,
cam_system=cam_system,
reconstructor=reconstructor,
)
return result
def make_default_system():
'''helper function to generate an instance of MultiCameraSystem'''
lookat = np.array((0.0, 0.0, 0.0))
center1 = np.array((0.0, 0.0, 5.0))
distortion1 = np.array([0.2, 0.3, 0.1, 0.1, 0.1])
cam1 = CameraModel.load_camera_simple(
name='cam1',
fov_x_degrees=90,
eye=center1,
lookat=lookat,
distortion_coefficients=distortion1,
)
center2 = np.array((0.5, 0.0, 0.0))
cam2 = CameraModel.load_camera_simple(
name='cam2',
fov_x_degrees=90,
eye=center2,
lookat=lookat,
)
center3 = np.array((0.5, 0.5, 0.0))
cam3 = CameraModel.load_camera_simple(
name='cam3',
fov_x_degrees=90,
eye=center3,
lookat=lookat,
)
center4 = np.array((0.5, 0.0, 0.5))
cam4 = CameraModel.load_camera_simple(
name='cam4',
fov_x_degrees=90,
eye=center4,
lookat=lookat,
)
cameras = [cam1, cam2, cam3, cam4]
system = MultiCameraSystem(cameras)
return system
def make_default_system(with_separate_distorions=False):
'''helper function to generate an instance of MultiCameraSystem'''
if with_separate_distorions:
raise NotImplementedError
camxs = np.linspace(-2, 2, 3)
camzs = np.linspace(-2, 2, 3).tolist()
camzs.pop(1)
cameras = []
lookat = np.array((0.0, 0, 0))
up = np.array((0.0, 0, 1))
for enum, camx in enumerate(camxs):
center = np.array((camx, -5, 0))
name = 'camx_%d' % (enum + 1, )
cam = CameraModel.load_camera_simple(
name=name,
fov_x_degrees=45,
eye=center,
lookat=lookat,
up=up,
)
cameras.append(cam)
for enum, camz in enumerate(camzs):
center = np.array((0, -5, camz))
name = 'camz_%d' % (enum + 1, )
cam = CameraModel.load_camera_simple(
name=name,
fov_x_degrees=45,
eye=center,
lookat=lookat,
up=up,
)
cameras.append(cam)
system = MultiCameraSystem(cameras)
return system
def build_multi_camera_system(cameras, no_distortion=False):
"""
Build a multi-camera system with pymvg package for triangulation
Args:
cameras: list of camera parameters
Returns:
cams_system: a multi-cameras system
"""
pymvg_cameras = []
for (name, camera) in cameras:
R, T, f, c, k, p = unfold_camera_param(camera, avg_f=False)
camera_matrix = np.array(
[[f[0], 0, c[0]], [0, f[1], c[1]], [0, 0, 1]], dtype=float)
proj_matrix = np.zeros((3, 4))
proj_matrix[:3, :3] = camera_matrix
distortion = np.array([k[0], k[1], p[0], p[1], k[2]])
distortion.shape = (5,)
T = -np.matmul(R, T)
M = camera_matrix.dot(np.concatenate((R, T), axis=1))
camera = CameraModel.load_camera_from_M(
M, name=name, distortion_coefficients=None if no_distortion else distortion)
pymvg_cameras.append(camera)
return MultiCameraSystem(pymvg_cameras)
def triangulate_points(cameras,
filtered_applied,
stereo_triangulation,
min_likelihood=0.7):
if len(cameras) < 2:
raise Exception('Triangulation process needs at least two cameras')
number_of_frames = len(cameras[0].frames)
number_of_markers = len(cameras[0].frames[0].markers)
triangulated_frames = []
stereo_pair = None
if stereo_triangulation:
stereo_pair = get_best_pair(cameras)
# set up filter values
dt = 1.0 / 24
transition_matrix = np.eye(9, dtype=np.float32)
transition_matrix[0][3] = dt
transition_matrix[0][6] = 0.5 * dt * dt
transition_matrix[1][4] = dt
transition_matrix[1][7] = 0.5 * dt * dt
transition_matrix[2][5] = dt
transition_matrix[2][8] = 0.5 * dt * dt
measurement_matrix = np.array([(1, 0, 0, 0, 0, 0, 0, 0, 0),
(0, 1, 0, 0, 0, 0, 0, 0, 0),
(0, 0, 1, 0, 0, 0, 0, 0, 0)],
dtype=np.float32)
# init filters for all markers tracked
filters = []
for i in range(number_of_markers):
kalman_filter = cv2.KalmanFilter(9, 3, 0)
kalman_filter.transitionMatrix = transition_matrix
kalman_filter.measurementMatrix = measurement_matrix
filters.append(Filter(kalman_filter))
for i in range(number_of_frames):
triangulated_markers = []
for j in range(number_of_markers):
visible_counter = 0
for camera in cameras:
if camera.frames[i].markers[j].likelihood > 0 and \
((stereo_triangulation and (camera in stereo_pair[0] or camera in stereo_pair[1]))
or not stereo_triangulation):
visible_counter += 1
if visible_counter < 2:
continue
# check if old stereo triangulation method is used
if stereo_triangulation:
best_cameras = get_front_back_cameras_for_marker(
stereo_pair, i, j, 0.5)
triangulated = triangulate_point(best_cameras, i, j,
best_cameras[0].image_size)
else:
# use n view triangulation method
best_cameras = get_best_cameras(cameras, i, j, len(cameras),
min_likelihood)
system = MultiCameraSystem([cam.model for cam in best_cameras])
points = [
(cam.model.name,
[cam.frames[i].markers[j].x, cam.frames[i].markers[j].y])
for cam in best_cameras
]
triangulated = system.find3d(points)
average_likelihood = np.mean(
[cam.frames[i].markers[j].likelihood for cam in best_cameras])
point_triangulated = triangulated is not None and average_likelihood > min_likelihood
marker_key = best_cameras[0].frames[i].markers[j].marker_key
if point_triangulated:
# check if kalman filter is necessary
if filtered_applied:
triangulated_ec_world_frame_formatted = np.array(
([triangulated]), np.float32).T
# compensate for the initial state set to 0,0,0 in opencv kalman filter
if filters[j].first:
for l in range(100):
filters[j].filter.predict()
filters[j].filter.correct(
triangulated_ec_world_frame_formatted)
filters[j].first = False
filters[j].filter.predict()
estimated = filters[j].filter.correct(
triangulated_ec_world_frame_formatted)
# append triangulated point
triangulated_markers.append({
'point':
np.array([
estimated[0][0], estimated[1][0], estimated[2][0]
]),
'marker':
marker_key,
'cameras':
"".join([str(cam.number) for cam in best_cameras]),
'likelihood':
str(average_likelihood)
})
else:
# append triangulated point
triangulated_markers.append({
'point':
triangulated,
'marker':
marker_key,
'cameras':
"".join([str(cam.number) for cam in best_cameras]),
'likelihood':
str(average_likelihood)
})
triangulated_frames.append(triangulated_markers)
return triangulated_frames
print "~~~~~~~~~~~~~~~~~~~~~~~~~~"
print "Importing {0}".format(calib_id[idx])
pmat = calib[idx][0]
distortion = calib[idx][1]
name = calib_id[idx]
width = cam_settings[name]["f7"]["width"]
height = cam_settings[name]["f7"]["height"]
print pmat
print distortion
camera = CameraModel.load_camera_from_M(pmat, width=width, height=height, name=name,
distortion_coefficients=distortion)
cameras.append(camera)
system = MultiCameraSystem(cameras)
system.save_to_pymvg_file(join(CALIB, "camera_system.json"))
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# plot_system(ax, system)
for name in system.get_names():
plot_camera(ax, system.get_camera(name))
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
def test_pymvg_roundtrip():
from pymvg.camera_model import CameraModel
from pymvg.multi_camera_system import MultiCameraSystem
from flydra_core.reconstruct import Reconstructor
# ----------- with no distortion ------------------------
center1 = np.array( (0, 0.0, 5) )
center2 = np.array( (1, 0.0, 5) )
lookat = np.array( (0,1,0))
cam1 = CameraModel.load_camera_simple(fov_x_degrees=90,
name='cam1',
eye=center1,
lookat=lookat)
cam2 = CameraModel.load_camera_simple(fov_x_degrees=90,
name='cam2',
eye=center2,
lookat=lookat)
mvg = MultiCameraSystem( cameras=[cam1, cam2] )
R = Reconstructor.from_pymvg(mvg)
mvg2 = R.convert_to_pymvg()
cam_ids = ['cam1','cam2']
for distorted in [True,False]:
for cam_id in cam_ids:
v1 = mvg.find2d( cam_id, lookat, distorted=distorted )
v2 = R.find2d( cam_id, lookat, distorted=distorted )
v3 = mvg2.find2d( cam_id, lookat, distorted=distorted )
assert np.allclose(v1,v2)
assert np.allclose(v1,v3)
# ----------- with distortion ------------------------
cam1dd = cam1.to_dict()
cam1dd['D'] = (0.1, 0.2, 0.3, 0.4, 0.0)
cam1d = CameraModel.from_dict(cam1dd)
cam2dd = cam2.to_dict()
cam2dd['D'] = (0.11, 0.21, 0.31, 0.41, 0.0)
cam2d = CameraModel.from_dict(cam2dd)
mvgd = MultiCameraSystem( cameras=[cam1d, cam2d] )
Rd = Reconstructor.from_pymvg(mvgd)
mvg2d = Rd.convert_to_pymvg()
cam_ids = ['cam1','cam2']
for distorted in [True,False]:
for cam_id in cam_ids:
v1 = mvgd.find2d( cam_id, lookat, distorted=distorted )
v2 = Rd.find2d( cam_id, lookat, distorted=distorted )
v3 = mvg2d.find2d( cam_id, lookat, distorted=distorted )
assert np.allclose(v1,v2)
assert np.allclose(v1,v3)
# ------------ with distortion at different focal length ------
mydir = os.path.dirname(__file__)
caldir = os.path.join(mydir,'sample_calibration')
print mydir
print caldir
R3 = Reconstructor(caldir)
mvg3 = R3.convert_to_pymvg()
#R4 = Reconstructor.from_pymvg(mvg3)
mvg3b = MultiCameraSystem.from_mcsc( caldir )
for distorted in [True,False]:
for cam_id in R3.cam_ids:
v1 = R3.find2d( cam_id, lookat, distorted=distorted )
v2 = mvg3.find2d( cam_id, lookat, distorted=distorted )
#v3 = R4.find2d( cam_id, lookat, distorted=distorted )
v4 = mvg3b.find2d( cam_id, lookat, distorted=distorted )
assert np.allclose(v1,v2)
#assert np.allclose(v1,v3)
assert np.allclose(v1,v4)
