def check_ros_pipeline(use_distortion):
pm = ROSPipelineMimic(use_distortion=use_distortion)
pm.generate_camera()
pm.generate_images()
#pm.save_tarball('/tmp/pipeline-mimic.tar.gz') # optional
cals = pm.run_ros_calibrator()
print(cals)
err1 = pm.calc_mean_reproj_error(cals['perfect'])
err2 = pm.calc_mean_reproj_error(cals['good'])
print(err1,err2)
if DRAW:
from mpl_toolkits.mplot3d import Axes3D
from pymvg.plot_utils import plot_camera
f = plt.figure()
ax = f.add_subplot(111,projection='3d')
for imd in pm.db:
wcs3d = imd['wc']
ax.plot(wcs3d[:,0],wcs3d[:,1],wcs3d[:,2], 'o-')
plot_camera( ax, pm.cam )#, scale=10, axes_size=5.0 )
if DRAW:
print('using perfect point data, mean reprojection error is %s'%err1)
print('mean reprojection error is %s'%err2)
plt.show()
assert err1 < 1.0
# FIXME: why is the error we get so large? Logically, it must be
# from detect checkerboard corners code, so it's not hugely
# important. Nevertheless, this is an annoyingly large error.
assert err2 < 30.0
def test_simple_projection():
# get some 3D points
pts_3d = _build_points_3d()
if DRAW:
fig = plt.figure(figsize=(8, 12))
ax1 = fig.add_subplot(3, 1, 1, projection='3d')
ax1.scatter(pts_3d[:, 0], pts_3d[:, 1], pts_3d[:, 2])
ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_zlabel('Z')
# build a camera calibration matrix
focal_length = 1200
width, height = 640, 480
R = np.eye(3) # look at +Z
c = np.array((9.99, 19.99, 20))
M = make_M(focal_length, width, height, R, c)['M']
# now, project these 3D points into our image plane
pts_3d_H = np.vstack((pts_3d.T, np.ones((1, len(pts_3d))))) # make homog.
undist_rst_simple = np.dot(M, pts_3d_H) # multiply
undist_simple = undist_rst_simple[:2, :] / undist_rst_simple[
2, :] # project
if DRAW:
ax2 = fig.add_subplot(3, 1, 2)
ax2.plot(undist_simple[0, :], undist_simple[1, :], 'b.')
ax2.set_xlim(0, width)
ax2.set_ylim(height, 0)
ax2.set_title('matrix multiply')
# build a camera model from our M and project onto image plane
cam = CameraModel.load_camera_from_M(M, width=width, height=height)
undist_full = cam.project_3d_to_pixel(pts_3d).T
if DRAW:
plot_camera(ax1, cam, scale=10, axes_size=5.0)
sz = 20
x = 5
y = 8
z = 19
ax1.auto_scale_xyz([x, x + sz], [y, y + sz], [z, z + sz])
ax3 = fig.add_subplot(3, 1, 3)
ax3.plot(undist_full[0, :], undist_full[1, :], 'b.')
ax3.set_xlim(0, width)
ax3.set_ylim(height, 0)
ax3.set_title('pymvg')
if DRAW:
plt.show()
assert np.allclose(undist_full, undist_simple)
def test_simple_projection():
# get some 3D points
pts_3d = _build_points_3d()
if DRAW:
fig = plt.figure(figsize=(8,12))
ax1 = fig.add_subplot(3,1,1, projection='3d')
ax1.scatter( pts_3d[:,0], pts_3d[:,1], pts_3d[:,2])
ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_zlabel('Z')
# build a camera calibration matrix
focal_length = 1200
width, height = 640,480
R = np.eye(3) # look at +Z
c = np.array( (9.99, 19.99, 20) )
M = make_M( focal_length, width, height, R, c)['M']
# now, project these 3D points into our image plane
pts_3d_H = np.vstack( (pts_3d.T, np.ones( (1,len(pts_3d))))) # make homog.
undist_rst_simple = np.dot(M, pts_3d_H) # multiply
undist_simple = undist_rst_simple[:2,:]/undist_rst_simple[2,:] # project
if DRAW:
ax2 = fig.add_subplot(3,1,2)
ax2.plot( undist_simple[0,:], undist_simple[1,:], 'b.')
ax2.set_xlim(0,width)
ax2.set_ylim(height,0)
ax2.set_title('matrix multiply')
# build a camera model from our M and project onto image plane
cam = CameraModel.load_camera_from_M( M, width=width, height=height )
undist_full = cam.project_3d_to_pixel(pts_3d).T
if DRAW:
plot_camera( ax1, cam, scale=10, axes_size=5.0 )
sz = 20
x = 5
y = 8
z = 19
ax1.auto_scale_xyz( [x,x+sz], [y,y+sz], [z,z+sz] )
ax3 = fig.add_subplot(3,1,3)
ax3.plot( undist_full[0,:], undist_full[1,:], 'b.')
ax3.set_xlim(0,width)
ax3.set_ylim(height,0)
ax3.set_title('pymvg')
if DRAW:
plt.show()
assert np.allclose( undist_full, undist_simple )
from pymvg.plot_utils import plot_camera
from pymvg.multi_camera_system import build_example_system
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
n=6
z=5.0
system = build_example_system(n=n,z=z)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for name in system.get_names():
plot_camera( ax, system.get_camera(name), scale = z/5.0 )
if 1:
# put some points to force mpl's view dimensions
pts = np.array([[0,0,0],
[2*n, 2*z, 2*z]])
ax.plot( pts[:,0], pts[:,1], pts[:,2], 'k.')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
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)
from pymvg.plot_utils import plot_camera
from pymvg.multi_camera_system import build_example_system
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
n = 6
z = 5.0
system = build_example_system(n=n, z=z)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for name in system.get_names():
plot_camera(ax, system.get_camera(name), scale=z / 5.0)
if 1:
# put some points to force mpl's view dimensions
pts = np.array([[0, 0, 0], [2 * n, 2 * z, 2 * z]])
ax.plot(pts[:, 0], pts[:, 1], pts[:, 2], 'k.')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
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)
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()
