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_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 test_pymvg_file_in_docs():
pymvg_src_path = pymvg.__file__
pymvg_base = os.path.split(pymvg_src_path)[0]
pymvg_src_dir = os.path.join(pymvg_base, '..')
fname = os.path.join(pymvg_src_dir, 'docs', 'source',
'pymvg_camsystem_example.json')
system = MultiCameraSystem.from_pymvg_file(fname)
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_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 main():
out_fname = sys.argv[1]
system1 = make_default_system()
system1.save_to_pymvg_file( out_fname )
# just make sure we can actually read it!
system2 = MultiCameraSystem.from_pymvg_file( out_fname )
assert system1==system2
def test_roundtrip_to_pymvg_file():
system1 = make_default_system()
fname = tempfile.mktemp(suffix='.json')
system1.save_to_pymvg_file( fname )
try:
system2 = MultiCameraSystem.from_pymvg_file( fname )
assert system1==system2
finally:
os.unlink( fname )
def test_roundtrip_to_pymvg_file():
system1 = make_default_system()
fname = tempfile.mktemp(suffix='.json')
system1.save_to_pymvg_file(fname)
try:
system2 = MultiCameraSystem.from_pymvg_file(fname)
assert system1 == system2
finally:
os.unlink(fname)
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 _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 main(args):
if not path.exists(args.data):
raise ValueError("Data path does not exist!")
# Load calibration:
cam_path = path.join(args.data, "camera_system_aligned.json")
camera_system = MultiCameraSystem.from_pymvg_file(cam_path)
# Load data:
data = load_data(args.data, camera_system)
# Filter by area:
data = data[data.area > args.area_filter]
# Select a range:
if args.frame_range is None:
a, b = 0, data.index.levels[1].max()
else:
a, b = args.frame_range
data = data.loc[pd.IndexSlice[:, a:b], :]
data = data.reset_index().set_index(["camera_id",
"frame_number"]).sort_index()
# Set up reconstruction:
rec = FastSeqH(camera_system, minimum_tracks=args.minimum_tracks)
# Run:
output = multi_reconstruct(data, rec, args).reset_index().set_index(
["frame_number", "point_id"])
# Save:
output_directory = path.join(args.data, "reconstruction")
if not path.exists(output_directory):
os.mkdir(output_directory)
output_path = path.join(output_directory, "points.csv")
output.to_csv(output_path)
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 test_pymvg_file_in_docs():
pymvg_src_path = pymvg.__file__
pymvg_base = os.path.split(pymvg_src_path)[0]
pymvg_src_dir = os.path.join(pymvg_base,'..')
fname = os.path.join( pymvg_src_dir, 'docs', 'source', 'pymvg_camsystem_example.json')
system = MultiCameraSystem.from_pymvg_file( fname )
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_roundtrip_to_dict():
system1 = make_default_system()
system2 = MultiCameraSystem.from_dict( system1.to_dict() )
assert system1==system2
def test_roundtrip_to_str():
system1 = make_default_system()
buf = system1.get_pymvg_str()
system2 = MultiCameraSystem.from_pymvg_str( buf )
assert system1==system2
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
def check_mcsc_roundtrip(with_rad_files=False,align_existing=False):
np.random.seed(3)
mcscdir = os.path.join( os.path.dirname(mcsce.__file__),
'..', '..', 'MultiCamSelfCal' )
mcscdir = os.path.abspath(mcscdir)
out_dirname = tempfile.mkdtemp()
try:
mcsc = mcsce.MultiCamSelfCal(out_dirname=out_dirname,mcscdir=mcscdir)
if with_rad_files:
orig_cams = make_default_system(with_separate_distorions=True)
else:
orig_cams = make_default_system()
cam_resolutions = dict( (n['name'], (n['width'],n['height']))
for n in orig_cams.to_dict()['camera_system'])
pts_3d = get_default_points()
if 0:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from pymvg.plot_utils import plot_camera
np.set_printoptions(precision=3, suppress=True)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for name,cam in orig_cams.get_camera_dict().iteritems():
print()
print(name,'-'*80)
print(' center:',cam.get_camcenter())
print(' lookat:',cam.get_lookat())
print(' up:',cam.get_up())
print(' P')
print(cam.P)
print()
plot_camera( ax, cam)#, scale = dim/5.0 )
ax.plot( pts_3d[:,0], pts_3d[:,1], pts_3d[:,2], 'k.')
plt.show()
cam_points=defaultdict(list)
nan_tup = (np.nan, np.nan)
for pt_3d in pts_3d:
valid = True
for name in orig_cams.get_names():
w,h = cam_resolutions[name]
x,y = orig_cams.find2d( name, pt_3d )
if 1:
# add noise to the 2D images
xn, yn = 0.01*np.random.randn( 2 )
x += xn
y += yn
if (0 <= x) and (x < w) and (0 <= y) and (y < h):
cam_points[name].append((x,y))
else:
cam_points[name].append(nan_tup)
valid = False
if not valid:
for name in orig_cams.get_names():
cam_points[name].pop()
cam_ids=orig_cams.get_names()
if with_rad_files:
cam_calibrations = {} # dictionary of .yaml filenames with ROS yaml format
raise NotImplementedError
else:
cam_calibrations = {}
if align_existing:
cam_centers = np.array([ orig_cams.get_cam(n).get_camcenter() for n in orig_cams.get_names() ]).T
else:
cam_centers = []
mcsc.create_from_cams(cam_ids=cam_ids,
cam_resolutions=cam_resolutions,
cam_points=cam_points,
cam_calibrations=cam_calibrations,
cam_centers=cam_centers,
)
result = mcsc.execute(silent=True)
raw_cams = MultiCameraSystem.from_mcsc( result )
if align_existing:
aligned_cams = raw_cams
else:
aligned_cams = raw_cams.get_aligned_copy( orig_cams )
assert is_close(orig_cams,aligned_cams,pts_3d)
finally:
shutil.rmtree(out_dirname)
def _build_test_camera(**kwargs):
o = Bunch(**kwargs)
if not kwargs.get('at_origin',False):
translation = geometry_msgs.msg.Point()
translation.x = 0.273485679077
translation.y = 0.0707310128808
translation.z = 0.0877802104531
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.309377331102
rotation.y = 0.600893485738
rotation.z = 0.644637681813
rotation.w = 0.357288321925
else:
translation = geometry_msgs.msg.Point()
translation.x = 0.0
translation.y = 0.0
translation.z = 0.0
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.0
rotation.y = 0.0
rotation.z = 0.0
rotation.w = 1.0
if 'from_file' in kwargs:
if kwargs['type']=='ros':
cam = CameraModel.load_camera_from_file( fname=kwargs['filename'],
extrinsics_required=False )
i = cam.get_intrinsics_as_bunch()
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
elif kwargs['type']=='pymvg':
del translation
del rotation
system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename'])
names = system.get_names()
names.sort()
name = names[0] # get first camera from system
cam = system._cameras[name]
rotation = cam.get_extrinsics_as_bunch().rotation
translation = cam.get_extrinsics_as_bunch().translation
else:
raise ValueError('unknown file type')
else:
if o.ROS_test_data:
i = sensor_msgs.msg.CameraInfo()
# these are from image_geometry ROS package in the utest.cpp file
i.height = 480
i.width = 640
i.distortion_model = 'plumb_bob'
i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
i.K = [430.15433020105519, 0.0, 311.71339830549732,
0.0, 430.60920415473657, 221.06824942698509,
0.0, 0.0, 1.0]
i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
-0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
-0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0,
0.0, 295.53402059708782, 223.29617881774902, 0.0,
0.0, 0.0, 1.0, 0.0]
else:
i = sensor_msgs.msg.CameraInfo()
i.height = 494
i.width = 659
i.distortion_model = 'plumb_bob'
i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243]
i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0]
i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0]
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
if kwargs.get('flipped',False):
cam = cam.get_flipped_camera()
if kwargs.get('get_input_data',False):
return dict(cam=cam,
translation=translation,
rotation=rotation,
)
return cam
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)
def check_mcsc_roundtrip(with_rad_files=False, align_existing=False):
np.random.seed(3)
mcscdir = os.path.join(os.path.dirname(mcsce.__file__), '..', '..',
'MultiCamSelfCal')
mcscdir = os.path.abspath(mcscdir)
out_dirname = tempfile.mkdtemp()
try:
mcsc = mcsce.MultiCamSelfCal(out_dirname=out_dirname, mcscdir=mcscdir)
if with_rad_files:
orig_cams = make_default_system(with_separate_distorions=True)
else:
orig_cams = make_default_system()
cam_resolutions = dict((n['name'], (n['width'], n['height']))
for n in orig_cams.to_dict()['camera_system'])
pts_3d = get_default_points()
if 0:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from pymvg.plot_utils import plot_camera
np.set_printoptions(precision=3, suppress=True)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for name, cam in orig_cams.get_camera_dict().iteritems():
print()
print(name, '-' * 80)
print(' center:', cam.get_camcenter())
print(' lookat:', cam.get_lookat())
print(' up:', cam.get_up())
print(' P')
print(cam.P)
print()
plot_camera(ax, cam) #, scale = dim/5.0 )
ax.plot(pts_3d[:, 0], pts_3d[:, 1], pts_3d[:, 2], 'k.')
plt.show()
cam_points = defaultdict(list)
nan_tup = (np.nan, np.nan)
for pt_3d in pts_3d:
valid = True
for name in orig_cams.get_names():
w, h = cam_resolutions[name]
x, y = orig_cams.find2d(name, pt_3d)
if 1:
# add noise to the 2D images
xn, yn = 0.01 * np.random.randn(2)
x += xn
y += yn
if (0 <= x) and (x < w) and (0 <= y) and (y < h):
cam_points[name].append((x, y))
else:
cam_points[name].append(nan_tup)
valid = False
if not valid:
for name in orig_cams.get_names():
cam_points[name].pop()
cam_ids = orig_cams.get_names()
if with_rad_files:
cam_calibrations = {
} # dictionary of .yaml filenames with ROS yaml format
raise NotImplementedError
else:
cam_calibrations = {}
if align_existing:
cam_centers = np.array([
orig_cams.get_cam(n).get_camcenter()
for n in orig_cams.get_names()
]).T
else:
cam_centers = []
mcsc.create_from_cams(
cam_ids=cam_ids,
cam_resolutions=cam_resolutions,
cam_points=cam_points,
cam_calibrations=cam_calibrations,
cam_centers=cam_centers,
)
result = mcsc.execute(silent=True)
raw_cams = MultiCameraSystem.from_mcsc(result)
if align_existing:
aligned_cams = raw_cams
else:
aligned_cams = raw_cams.get_aligned_copy(orig_cams)
assert is_close(orig_cams, aligned_cams, pts_3d)
finally:
shutil.rmtree(out_dirname)
def test_roundtrip_to_str():
system1 = make_default_system()
buf = system1.get_pymvg_str()
system2 = MultiCameraSystem.from_pymvg_str(buf)
assert system1 == system2
def _build_test_camera(**kwargs):
o = Bunch(**kwargs)
if not kwargs.get('at_origin',False):
translation = geometry_msgs.msg.Point()
translation.x = 0.273485679077
translation.y = 0.0707310128808
translation.z = 0.0877802104531
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.309377331102
rotation.y = 0.600893485738
rotation.z = 0.644637681813
rotation.w = 0.357288321925
else:
translation = geometry_msgs.msg.Point()
translation.x = 0.0
translation.y = 0.0
translation.z = 0.0
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.0
rotation.y = 0.0
rotation.z = 0.0
rotation.w = 1.0
if 'from_file' in kwargs:
if kwargs['type']=='ros':
cam = CameraModel.load_camera_from_file( fname=kwargs['filename'],
extrinsics_required=False )
i = cam.get_intrinsics_as_bunch()
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
elif kwargs['type']=='pymvg':
del translation
del rotation
system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename'])
names = system.get_names()
names.sort()
name = names[0] # get first camera from system
cam = system._cameras[name]
rotation = cam.get_extrinsics_as_bunch().rotation
translation = cam.get_extrinsics_as_bunch().translation
else:
raise ValueError('unknown file type')
else:
if o.ROS_test_data:
i = sensor_msgs.msg.CameraInfo()
# these are from image_geometry ROS package in the utest.cpp file
i.height = 480
i.width = 640
i.distortion_model = 'plumb_bob'
i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
i.K = [430.15433020105519, 0.0, 311.71339830549732,
0.0, 430.60920415473657, 221.06824942698509,
0.0, 0.0, 1.0]
i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
-0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
-0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0,
0.0, 295.53402059708782, 223.29617881774902, 0.0,
0.0, 0.0, 1.0, 0.0]
else:
i = sensor_msgs.msg.CameraInfo()
i.height = 494
i.width = 659
i.distortion_model = 'plumb_bob'
i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243]
i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0]
i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0]
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
if kwargs.get('flipped',False):
cam = cam.get_flipped_camera()
if kwargs.get('get_input_data',False):
return dict(cam=cam,
translation=translation,
rotation=rotation,
)
return cam
print cam.get_camera_info()
cam.set_property_values(cam_settings[sn])
cam.set_format_configuration(**cam_settings[sn]["f7"])
cam.set_trigger(True)
cam.start_capture()
time.sleep(0.5)
return cam
if __name__ == "__main__":
# Load Camera System:
camsys = MultiCameraSystem.from_pymvg_file(BASE_CALIB)
# Set up sync:
sync = FrameSync()
sync.set_framerate(30, duty_cycle=0.5)
sync.start()
time.sleep(1)
cams = [set_up_camera(sn) for sn in cam_settings.keys()]
camid = [cam.get_camera_info()["serial_number"] for cam in cams]
cv2.namedWindow("viewer", cv2.WINDOW_AUTOSIZE)
starttime = time.time()
update_rate = 1.0
def test_load_mcsc():
mydir = os.path.dirname(__file__)
mcsc_dirname = os.path.join(mydir,'mcsc_output_20130726')
cam_system = MultiCameraSystem.from_mcsc( mcsc_dirname )
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)
if __name__ == "__main__":
images, camids = take_images()
points = run_point_select(camids, images)
# np.save("points", points)
camera_system = MultiCameraSystem.from_pymvg_file(path.join(BASE_CALIBRATION, "camera_system.json"))
# points = np.load("points.npy")
final_points = triangulate_points(camera_system, points)
sorted_points = identify_points(final_points)
plot_reprojection(camera_system, images, sorted_points)
s, R, T = align.estsimt(sorted_points.T, CALIBRATED_COORDINATES.T)
new_system = transform_system(camera_system, s, R, T)
new_system.save_to_pymvg_file(path.join(BASE_CALIBRATION, "camera_system_aligned.json"))
# DEBUGGING:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
def test_load_mcsc():
mydir = os.path.dirname(__file__)
mcsc_dir = os.path.join(mydir, 'external', 'mcsc')
mcsc_dirname = os.path.join(mcsc_dir, 'mcsc_output_20130726')
cam_system = MultiCameraSystem.from_mcsc(mcsc_dirname)
def test_roundtrip_to_dict():
system1 = make_default_system()
system2 = MultiCameraSystem.from_dict(system1.to_dict())
assert system1 == system2