def main(args):
from optparse import OptionParser
parser = OptionParser()
parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]")
parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]")
options, args = parser.parse_args()
size = tuple([int(c) for c in options.size.split('x')])
dim = float(options.square)
images = []
for fname in args:
if os.path.isfile(fname):
img = cv.LoadImage(fname)
if img is None:
print >> sys.stderr, "[WARN] Couldn't open image " + fname + "!"
sys.exit(1)
else:
print "[INFO] Loaded " + fname + " (" + str(img.width) + "x" + str(img.height) + ")"
images.append(img)
cboard = ChessboardInfo()
cboard.dim = dim
cboard.n_cols = size[0]
cboard.n_rows = size[1]
mc = MonoCalibrator([cboard])
mc.cal(images)
print mc.as_message()
def test_rational_polynomial_model(self):
"""Test that the distortion coefficients returned for a rational_polynomial model are not empty."""
for i, setup in enumerate(self.setups):
board = ChessboardInfo()
board.n_cols = setup.cols
board.n_rows = setup.rows
board.dim = self.board_width_dim
mc = MonoCalibrator([board],
flags=cv2.CALIB_RATIONAL_MODEL,
pattern=setup.pattern)
mc.cal(self.limages[i])
self.assertEqual(
len(mc.distortion.flat), 8,
'length of distortion coefficients is %d' %
len(mc.distortion.flat))
self.assertTrue(
all(mc.distortion.flat != 0),
'some distortion coefficients are zero: %s' %
str(mc.distortion.flatten()))
self.assertEqual(mc.as_message().distortion_model,
'rational_polynomial')
self.assert_good_mono(mc, self.limages[i], setup.lin_err)
yaml = mc.yaml()
# Issue #278
self.assertIn('cols: 8', yaml)
def cal_from_tarfile(boards, tarname, mono = False, upload = False):
if mono:
calibrator = MonoCalibrator(boards)
else:
calibrator = StereoCalibrator(boards)
calibrator.do_tarfile_calibration(tarname)
print calibrator.ost()
if upload:
info = calibrator.as_message()
if mono:
set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)
response = set_camera_info_service(info)
if not response.success:
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
else:
set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)
response1 = set_left_camera_info_service(info[0])
response2 = set_right_camera_info_service(info[1])
if not (response1.success and response2.success):
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
def cal_from_tarfile(boards, tarname, mono = False, upload = False):
if mono:
calibrator = MonoCalibrator(boards)
else:
calibrator = StereoCalibrator(boards)
calibrator.do_tarfile_calibration(tarname)
print calibrator.ost()
if upload:
info = calibrator.as_message()
if mono:
set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)
response = set_camera_info_service(info)
if not response.success:
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
else:
set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)
response1 = set_camera_info_service(info[0])
response2 = set_camera_info_service(info[1])
if not (response1.success and response2.success):
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
def test_monocular(self):
# Run the calibrator, produce a calibration, check it
mc = MonoCalibrator([board], flags=cv2.CALIB_FIX_K3)
for dim in self.sizes:
mc.cal(self.l[dim])
self.assert_good_mono(mc, dim)
# Make another calibration, import previous calibration as a message,
# and assert that the new one is good.
mc2 = MonoCalibrator([board])
mc2.from_message(mc.as_message())
self.assert_good_mono(mc2, dim)
def test_monocular(self):
# Run the calibrator, produce a calibration, check it
mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3)
for dim in self.sizes:
mc.cal(self.l[dim])
self.assert_good_mono(mc, dim)
# Make another calibration, import previous calibration as a message,
# and assert that the new one is good.
mc2 = MonoCalibrator([board])
mc2.from_message(mc.as_message())
self.assert_good_mono(mc2, dim)
def main(args):
from optparse import OptionParser
parser = OptionParser()
parser.add_option(
"-s",
"--size",
default="8x6",
help="specify chessboard size as nxm [default: %default]")
parser.add_option(
"-q",
"--square",
default=".108",
help="specify chessboard square size in meters [default: %default]")
options, args = parser.parse_args()
size = tuple([int(c) for c in options.size.split('x')])
dim = float(options.square)
images = []
for fname in args:
if os.path.isfile(fname):
img = cv.LoadImage(fname)
if img is None:
print >> sys.stderr, "[WARN] Couldn't open image " + fname + "!"
sys.exit(1)
else:
print "[INFO] Loaded " + fname + " (" + str(
img.width) + "x" + str(img.height) + ")"
images.append(img)
cboard = ChessboardInfo()
cboard.dim = dim
cboard.n_cols = size[0]
cboard.n_rows = size[1]
mc = MonoCalibrator([cboard])
mc.cal(images)
print mc.as_message()
def test_monocular(self):
# Run the calibrator, produce a calibration, check it
mc = MonoCalibrator([board], cv2.CALIB_FIX_K3)
max_errs = [0.1, 0.2, 0.4, 0.7]
for i, dim in enumerate(self.sizes):
mc.cal(self.l[dim])
self.assert_good_mono(mc, dim, max_errs[i])
# Make another calibration, import previous calibration as a message,
# and assert that the new one is good.
mc2 = MonoCalibrator([board])
mc2.from_message(mc.as_message())
self.assert_good_mono(mc2, dim, max_errs[i])
def test_monocular(self):
# Run the calibrator, produce a calibration, check it
mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3)
max_errs = [0.1, 0.2, 0.4, 0.7]
for i, dim in enumerate(self.sizes):
mc.cal(self.l[dim])
self.assert_good_mono(mc, dim, max_errs[i])
# Make another calibration, import previous calibration as a message,
# and assert that the new one is good.
mc2 = MonoCalibrator([board])
mc2.from_message(mc.as_message())
self.assert_good_mono(mc2, dim, max_errs[i])
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 cal_from_tarfile(boards,
tarname,
mono=False,
upload=False,
calib_flags=0,
visualize=False,
alpha=1.0):
if mono:
calibrator = MonoCalibrator(boards, calib_flags)
else:
calibrator = StereoCalibrator(boards, calib_flags)
calibrator.do_tarfile_calibration(tarname)
print(calibrator.ost())
if upload:
info = calibrator.as_message()
if mono:
set_camera_info_service = rospy.ServiceProxy(
"%s/set_camera_info" % rospy.remap_name("camera"),
sensor_msgs.srv.SetCameraInfo)
response = set_camera_info_service(info)
if not response.success:
raise RuntimeError(
"connected to set_camera_info service, but failed setting camera_info"
)
else:
set_left_camera_info_service = rospy.ServiceProxy(
"%s/set_camera_info" % rospy.remap_name("left_camera"),
sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rospy.ServiceProxy(
"%s/set_camera_info" % rospy.remap_name("right_camera"),
sensor_msgs.srv.SetCameraInfo)
response1 = set_left_camera_info_service(info[0])
response2 = set_right_camera_info_service(info[1])
if not (response1.success and response2.success):
raise RuntimeError(
"connected to set_camera_info service, but failed setting camera_info"
)
if visualize:
#Show rectified images
calibrator.set_alpha(alpha)
archive = tarfile.open(tarname, 'r')
if mono:
for f in archive.getnames():
if f.startswith('left') and (f.endswith('.pgm')
or f.endswith('png')):
filedata = archive.extractfile(f).read()
file_bytes = numpy.asarray(bytearray(filedata),
dtype=numpy.uint8)
im = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
bridge = cv_bridge.CvBridge()
try:
msg = bridge.cv2_to_imgmsg(im, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
#handle msg returns the recitifed image with corner detection once camera is calibrated.
drawable = calibrator.handle_msg(msg)
vis = numpy.asarray(drawable.scrib[:, :])
#Display. Name of window:f
display(f, vis)
else:
limages = [
f for f in archive.getnames() if (f.startswith('left') and (
f.endswith('pgm') or f.endswith('png')))
]
limages.sort()
rimages = [
f for f in archive.getnames() if (f.startswith('right') and (
f.endswith('pgm') or f.endswith('png')))
]
rimages.sort()
if not len(limages) == len(rimages):
raise RuntimeError(
"Left, right images don't match. %d left images, %d right"
% (len(limages), len(rimages)))
for i in range(len(limages)):
l = limages[i]
r = rimages[i]
if l.startswith('left') and (
l.endswith('.pgm')
or l.endswith('png')) and r.startswith('right') and (
r.endswith('.pgm') or r.endswith('png')):
# LEFT IMAGE
filedata = archive.extractfile(l).read()
file_bytes = numpy.asarray(bytearray(filedata),
dtype=numpy.uint8)
im_left = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
bridge = cv_bridge.CvBridge()
try:
msg_left = bridge.cv2_to_imgmsg(im_left, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
#RIGHT IMAGE
filedata = archive.extractfile(r).read()
file_bytes = numpy.asarray(bytearray(filedata),
dtype=numpy.uint8)
im_right = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
try:
msg_right = bridge.cv2_to_imgmsg(im_right, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
drawable = calibrator.handle_msg([msg_left, msg_right])
h, w = numpy.asarray(drawable.lscrib[:, :]).shape[:2]
vis = numpy.zeros((h, w * 2, 3), numpy.uint8)
vis[:h, :w, :] = numpy.asarray(drawable.lscrib[:, :])
vis[:h, w:w * 2, :] = numpy.asarray(drawable.rscrib[:, :])
display(l + " " + r, vis)
def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0):
if mono:
calibrator = MonoCalibrator(boards, calib_flags)
else:
calibrator = StereoCalibrator(boards, calib_flags)
calibrator.do_tarfile_calibration(tarname)
print(calibrator.ost())
if upload:
info = calibrator.as_message()
if mono:
set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)
response = set_camera_info_service(info)
if not response.success:
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
else:
set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)
response1 = set_left_camera_info_service(info[0])
response2 = set_right_camera_info_service(info[1])
if not (response1.success and response2.success):
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
if visualize:
#Show rectified images
calibrator.set_alpha(alpha)
archive = tarfile.open(tarname, 'r')
if mono:
for f in archive.getnames():
if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')):
filedata = archive.extractfile(f).read()
file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
im=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
bridge = cv_bridge.CvBridge()
try:
msg=bridge.cv2_to_imgmsg(im, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
#handle msg returns the recitifed image with corner detection once camera is calibrated.
drawable=calibrator.handle_msg(msg)
vis=numpy.asarray( drawable.scrib[:,:])
#Display. Name of window:f
display(f, vis)
else:
limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ]
limages.sort()
rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ]
rimages.sort()
if not len(limages) == len(rimages):
raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages)))
for i in range(len(limages)):
l=limages[i]
r=rimages[i]
if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')):
# LEFT IMAGE
filedata = archive.extractfile(l).read()
file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
im_left=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
bridge = cv_bridge.CvBridge()
try:
msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
#RIGHT IMAGE
filedata = archive.extractfile(r).read()
file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
im_right=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
try:
msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
drawable=calibrator.handle_msg([ msg_left,msg_right] )
h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2]
vis = numpy.zeros((h, w*2, 3), numpy.uint8)
vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:])
vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:])
display(l+" "+r,vis)
import cv2
from camera_calibration.calibrator import MonoCalibrator, ChessboardInfo
numImages = 30
images = [
cv2.imread('../Images/frame{:04d}.jpg'.format(i)) for i in range(numImages)
]
board = ChessboardInfo()
board.n_cols = 7
board.n_rows = 5
board.dim = 0.050
mc = MonoCalibrator([board], cv2.CALIB_FIX_K3)
mc.cal(images)
print(mc.as_message())
mc.do_save()
class CalibrationNode:
def __init__(self,
boards,
service_check=True,
synchronizer=message_filters.TimeSynchronizer,
flags=0):
if service_check:
# assume any non-default service names have been set. Wait for the service to become ready
for svcname in ["camera", "left_camera", "right_camera"]:
remapped = rospy.remap_name(svcname)
if remapped != svcname:
fullservicename = "%s/set_camera_info" % remapped
print("Waiting for service", fullservicename, "...")
try:
rospy.wait_for_service(fullservicename, 5)
print("OK")
except (rospy.ROSException):
print("Service not found")
rospy.signal_shutdown('Quit')
self._boards = boards
self._calib_flags = flags
msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
msub.registerCallback(self.queue_monocular)
self.set_camera_info_service = rospy.ServiceProxy(
"%s/set_camera_info" % rospy.remap_name("camera"),
sensor_msgs.srv.SetCameraInfo)
self.q_mono = Queue.Queue()
self.c = None
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
def redraw_monocular(self, *args):
pass
def queue_monocular(self, msg):
self.q_mono.put(msg)
def handle_monocular(self, msg):
if self.c == None:
self.c = MonoCalibrator(self._boards, self._calib_flags)
#print 'BBBBBBBBOOOOOOOOOOOOOOARRRRRRDS',self._boards.gridsize
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
#print drawable.scrib.shape[1]
self.displaywidth = drawable.scrib.shape[1]
print(drawable)
print(drawable.scrib)
self.redraw_monocular(drawable)
def check_set_camera_info(self, response):
if response.success:
return True
for i in range(10):
print("!" * 80)
print("Attempt to set camera info failed: " + response.status_message)
for i in range(10):
print("!" * 80)
rospy.logerr(
'Unable to set camera info for calibration. Failure message: %s' %
response.status_message)
return False
def do_upload(self):
self.c.report()
print(self.c.ost())
info = self.c.as_message()
rv = True
if self.c.is_mono:
response = self.set_camera_info_service(info)
rv = self.check_set_camera_info(response)
else:
response = self.set_left_camera_info_service(info[0])
rv = rv and self.check_set_camera_info(response)
response = self.set_right_camera_info_service(info[1])
rv = rv and self.check_set_camera_info(response)
return rv
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
