def find_intrinsics(visualize=False):
fatal = False
good = []
xys = []
if 1:
with open('setup.json', mode='r') as fd:
setup = json.loads(fd.read())
globber = '*-corners*.data'
files = glob.glob(globber)
files.sort()
print 'all data files matching "%s"' % (globber, )
print files
for f in files:
this_fatal = False
parts = f.split('-')
board = None
for p in parts:
if p.startswith('board'):
boardname = p
with open(boardname + '.json', mode='r') as fd:
buf = fd.read()
boardd = json.loads(buf)
board = ChessboardInfo()
board.n_cols = boardd['n_cols']
board.n_rows = boardd['n_rows']
board.dim = boardd['dim']
assert board is not None
xy = np.loadtxt(f)
xys.append(xy)
if len(xy) != board.n_cols * board.n_rows:
rospy.logfatal('Error: %d points in %s. Expected %d.' %
(len(xy), f, board.n_cols * board.n_rows))
this_fatal = True
fatal = True
if visualize:
if 0:
plt.figure()
fmt = 'o-'
label = f
if this_fatal:
fmt = 'kx:'
label = 'BAD: ' + f
plt.plot(xy[:, 0], xy[:, 1], fmt, mfc='none', label=label)
if this_fatal:
for i in range(len(xy)):
plt.text(xy[i, 0], xy[i, 1], str(i))
corners = [(x, y) for (x, y) in xy]
good.append((corners, board))
if visualize:
plt.legend()
plt.show()
if fatal:
sys.exit(1)
mc = MonoCalibrator(
[board]
) #, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST )
mc.size = tuple(setup['size'])
mc.cal_fromcorners(good)
msg = mc.as_message()
cam = pymvg.CameraModel.from_ros_like(intrinsics=msg)
cam_mirror = cam.get_mirror_camera()
if 1:
# The intrinsics are valid for the whole physical display and
# each virtual display.
names = setup['names']
for name in names:
name = str(
name) # remove unicode -- ROS bag fails on unicode topic name
if 'mirror' in name:
c = cam_mirror
else:
c = cam
msg = c.get_intrinsics_as_msg()
fname = 'display-intrinsic-cal-%s.bag' % (name.replace('/', '-'), )
bagout = rosbag.Bag(fname, 'w')
topic = '/%s/camera_info' % name
bagout.write(topic, msg)
bagout.close()
print 'saved to', fname
print 'You can play this calibration with:\n'
print 'rosbag play %s -l' % (fname, )
print
def find_intrinsics(visualize=False):
fatal = False
good = []
xys = []
if 1:
with open('setup.json',mode='r') as fd:
setup = json.loads(fd.read())
globber = '*-corners*.data'
files = glob.glob(globber)
files.sort()
print 'all data files matching "%s"'%(globber,)
print files
for f in files:
this_fatal = False
parts = f.split('-')
board = None
for p in parts:
if p.startswith('board'):
boardname = p
with open(boardname+'.json',mode='r') as fd:
buf = fd.read()
boardd = json.loads(buf)
board = ChessboardInfo()
board.n_cols = boardd['n_cols']
board.n_rows = boardd['n_rows']
board.dim = boardd['dim']
assert board is not None
xy = np.loadtxt(f)
xys.append(xy)
if len(xy) != board.n_cols * board.n_rows:
rospy.logfatal('Error: %d points in %s. Expected %d.'%(
len(xy), f, board.n_cols * board.n_rows))
this_fatal = True
fatal = True
if visualize:
if 0:
plt.figure()
fmt = 'o-'
label = f
if this_fatal:
fmt = 'kx:'
label = 'BAD: '+ f
plt.plot( xy[:,0], xy[:,1], fmt, mfc='none', label=label )
if this_fatal:
for i in range(len(xy)):
plt.text( xy[i,0],
xy[i,1],
str(i) )
corners = [ (x,y) for (x,y) in xy ]
good.append( (corners, board) )
if visualize:
plt.legend()
plt.show()
if fatal:
sys.exit(1)
mc = MonoCalibrator([ board ])#, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST )
mc.size = tuple(setup['size'])
mc.cal_fromcorners(good)
msg = mc.as_message()
cam = pymvg.CameraModel.from_ros_like( intrinsics=msg )
cam_mirror = cam.get_mirror_camera()
if 1:
# The intrinsics are valid for the whole physical display and
# each virtual display.
names = setup['names']
for name in names:
name = str(name) # remove unicode -- ROS bag fails on unicode topic name
if 'mirror' in name:
c = cam_mirror
else:
c = cam
msg = c.get_intrinsics_as_msg()
fname = 'display-intrinsic-cal-%s.bag'%( name.replace('/','-'), )
bagout = rosbag.Bag(fname, 'w')
topic = '/%s/camera_info'%name
bagout.write(topic, msg)
bagout.close()
print 'saved to',fname
print 'You can play this calibration with:\n'
print 'rosbag play %s -l'%(fname,)
print
