275 lines
11 KiB
Python
Executable File
275 lines
11 KiB
Python
Executable File
#!/usr/bin/env python
|
|
#
|
|
# Software License Agreement (BSD License)
|
|
#
|
|
# Copyright (c) 2009, Willow Garage, Inc.
|
|
# All rights reserved.
|
|
#
|
|
# Redistribution and use in source and binary forms, with or without
|
|
# modification, are permitted provided that the following conditions
|
|
# are met:
|
|
#
|
|
# * Redistributions of source code must retain the above copyright
|
|
# notice, this list of conditions and the following disclaimer.
|
|
# * Redistributions in binary form must reproduce the above
|
|
# copyright notice, this list of conditions and the following
|
|
# disclaimer in the documentation and/or other materials provided
|
|
# with the distribution.
|
|
# * Neither the name of the Willow Garage nor the names of its
|
|
# contributors may be used to endorse or promote products derived
|
|
# from this software without specific prior written permission.
|
|
#
|
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
# POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
import cv2
|
|
|
|
import collections
|
|
import copy
|
|
import numpy
|
|
import os
|
|
import requests
|
|
import tarfile
|
|
import unittest
|
|
|
|
from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, \
|
|
Patterns, CalibrationException, ChessboardInfo, image_from_archive
|
|
|
|
board = ChessboardInfo()
|
|
board.n_cols = 8
|
|
board.n_rows = 6
|
|
board.dim = 0.108
|
|
|
|
class TestDirected(unittest.TestCase):
|
|
def setUp(self):
|
|
if not os.path.isfile('/tmp/camera_calibration.tar.gz'):
|
|
url = 'http://download.ros.org/data/camera_calibration/camera_calibration.tar.gz'
|
|
r = requests.get(url, allow_redirects=True)
|
|
with open('/tmp/camera_calibration.tar.gz', 'wb') as cf:
|
|
cf.write(r.content)
|
|
|
|
tar_path = '/tmp/camera_calibration.tar.gz'
|
|
self.tar = tarfile.open(tar_path, 'r')
|
|
self.limages = [image_from_archive(self.tar, "wide/left%04d.pgm" % i) for i in range(3, 15)]
|
|
self.rimages = [image_from_archive(self.tar, "wide/right%04d.pgm" % i) for i in range(3, 15)]
|
|
self.l = {}
|
|
self.r = {}
|
|
self.sizes = [(320,240), (640,480), (800,600), (1024,768)]
|
|
for dim in self.sizes:
|
|
self.l[dim] = []
|
|
self.r[dim] = []
|
|
for li,ri in zip(self.limages, self.rimages):
|
|
rli = cv2.resize(li, (dim[0], dim[1]))
|
|
rri = cv2.resize(ri, (dim[0], dim[1]))
|
|
self.l[dim].append(rli)
|
|
self.r[dim].append(rri)
|
|
|
|
def assert_good_mono(self, c, dim, max_err):
|
|
self.assertTrue(len(c.ost()) > 0)
|
|
lin_err = 0
|
|
n = 0
|
|
for img in self.l[dim]:
|
|
lin_err_local = c.linear_error_from_image(img)
|
|
if lin_err_local:
|
|
lin_err += lin_err_local
|
|
n += 1
|
|
if n > 0:
|
|
lin_err /= n
|
|
self.assertTrue(0.0 < lin_err, 'lin_err is %f' % lin_err)
|
|
self.assertTrue(lin_err < max_err, 'lin_err is %f' % lin_err)
|
|
|
|
flat = c.remap(img)
|
|
self.assertEqual(img.shape, flat.shape)
|
|
|
|
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_stereo(self):
|
|
epierrors = [0.1, 0.2, 0.45, 1.0]
|
|
for i, dim in enumerate(self.sizes):
|
|
print("Dim =", dim)
|
|
sc = StereoCalibrator([board], cv2.CALIB_FIX_K3)
|
|
sc.cal(self.l[dim], self.r[dim])
|
|
|
|
sc.report()
|
|
#print sc.ost()
|
|
|
|
# NOTE: epipolar error currently increases with resolution.
|
|
# At highest res expect error ~0.75
|
|
epierror = 0
|
|
n = 0
|
|
for l_img, r_img in zip(self.l[dim], self.r[dim]):
|
|
epierror_local = sc.epipolar_error_from_images(l_img, r_img)
|
|
if epierror_local:
|
|
epierror += epierror_local
|
|
n += 1
|
|
epierror /= n
|
|
self.assertTrue(epierror < epierrors[i],
|
|
'Epipolar error is %f for resolution i = %d' % (epierror, i))
|
|
|
|
self.assertAlmostEqual(sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]), .108, 2)
|
|
|
|
#print sc.as_message()
|
|
|
|
img = self.l[dim][0]
|
|
flat = sc.l.remap(img)
|
|
self.assertEqual(img.shape, flat.shape)
|
|
flat = sc.r.remap(img)
|
|
self.assertEqual(img.shape, flat.shape)
|
|
|
|
sc2 = StereoCalibrator([board])
|
|
sc2.from_message(sc.as_message())
|
|
# sc2.set_alpha(1.0)
|
|
#sc2.report()
|
|
self.assertTrue(len(sc2.ost()) > 0)
|
|
|
|
def test_nochecker(self):
|
|
# Run with same images, but looking for an incorrect chessboard size (8, 7).
|
|
# Should raise an exception because of lack of input points.
|
|
new_board = copy.deepcopy(board)
|
|
new_board.n_cols = 8
|
|
new_board.n_rows = 7
|
|
|
|
sc = StereoCalibrator([new_board])
|
|
self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages))
|
|
mc = MonoCalibrator([new_board])
|
|
self.assertRaises(CalibrationException, lambda: mc.cal(self.limages))
|
|
|
|
|
|
|
|
class TestArtificial(unittest.TestCase):
|
|
Setup = collections.namedtuple('Setup', ['pattern', 'cols', 'rows', 'lin_err', 'K_err'])
|
|
|
|
def setUp(self):
|
|
# Define some image transforms that will simulate a camera position
|
|
M = []
|
|
self.k = numpy.array([[500, 0, 250], [0, 500, 250], [0, 0, 1]], numpy.float32)
|
|
self.d = numpy.array([])
|
|
# physical size of the board
|
|
self.board_width_dim = 1
|
|
|
|
# Generate data for different grid types. For each grid type, define the different sizes of
|
|
# grid that are recognized (n row, n col)
|
|
# Patterns.Circles, Patterns.ACircles
|
|
self.setups = [ self.Setup(pattern=Patterns.Chessboard, cols=7, rows=8, lin_err=0.2, K_err=8.2),
|
|
self.Setup(pattern=Patterns.Circles, cols=7, rows=8, lin_err=0.1, K_err=4),
|
|
self.Setup(pattern=Patterns.ACircles, cols=3, rows=5, lin_err=0.1, K_err=8) ]
|
|
self.limages = []
|
|
self.rimages = []
|
|
for setup in self.setups:
|
|
self.limages.append([])
|
|
self.rimages.append([])
|
|
|
|
# Create the pattern
|
|
if setup.pattern == Patterns.Chessboard:
|
|
pattern = numpy.zeros((50*(setup.rows+3), 50*(setup.cols+3), 1), numpy.uint8)
|
|
pattern.fill(255)
|
|
for j in range(1, setup.rows+2):
|
|
for i in range(1+(j%2), setup.cols+2, 2):
|
|
pattern[50*j:50*(j+1), 50*i:50*(i+1)].fill(0)
|
|
elif setup.pattern == Patterns.Circles:
|
|
pattern = numpy.zeros((50*(setup.rows+2), 50*(setup.cols+2), 1), numpy.uint8)
|
|
pattern.fill(255)
|
|
for j in range(1, setup.rows+1):
|
|
for i in range(1, setup.cols+1):
|
|
cv2.circle(pattern, (50*i + 25, 50*j + 25), 15, (0,0,0), -1 )
|
|
elif setup.pattern == Patterns.ACircles:
|
|
x = 60
|
|
pattern = numpy.zeros((x*(setup.rows+2), x*(setup.cols+5), 1), numpy.uint8)
|
|
pattern.fill(255)
|
|
for j in range(1, setup.rows+1):
|
|
for i in range(0, setup.cols):
|
|
cv2.circle(pattern, (int(x*(1 + 2*i + (j%2)) + x/2), int(x*j + x/2)), int(x/3), (0,0,0), -1)
|
|
|
|
rows, cols, _ = pattern.shape
|
|
object_points_2d = numpy.array([[0, 0], [0, cols-1], [rows-1, cols-1], [rows-1, 0]], numpy.float32)
|
|
object_points_3d = numpy.array([[0, 0, 0], [0, cols-1, 0], [rows-1, cols-1, 0], [rows-1, 0, 0]], numpy.float32)
|
|
object_points_3d *= self.board_width_dim/float(cols)
|
|
|
|
# create the artificial view points
|
|
rvec = [ [0, 0, 0], [0, 0, 0.4], [0, 0.4, 0], [0.4, 0, 0], [0.4, 0.4, 0], [0.4, 0, 0.4], [0, 0.4, 0.4], [0.4, 0.4, 0.4] ]
|
|
tvec = [ [-0.5, -0.5, 3], [-0.5, -0.5, 3], [-0.5, -0.1, 3], [-0.1, -0.5, 3], [-0.1, -0.1, 3], [-0.1, -0.5, 3], [-0.5, -0.1, 3], [-0.1, 0.1, 3] ]
|
|
|
|
dsize = (480, 640)
|
|
for i in range(len(rvec)):
|
|
R = numpy.array(rvec[i], numpy.float32)
|
|
T = numpy.array(tvec[i], numpy.float32)
|
|
|
|
image_points, _ = cv2.projectPoints(object_points_3d, R, T, self.k, self.d)
|
|
|
|
# deduce the perspective transform
|
|
M.append(cv2.getPerspectiveTransform(object_points_2d, image_points))
|
|
|
|
# project the pattern according to the different cameras
|
|
pattern_warped = cv2.warpPerspective(pattern, M[i], dsize)
|
|
self.limages[-1].append(pattern_warped)
|
|
|
|
def assert_good_mono(self, c, images, max_err):
|
|
#c.report()
|
|
self.assertTrue(len(c.ost()) > 0)
|
|
lin_err = 0
|
|
n = 0
|
|
for img in images:
|
|
lin_err_local = c.linear_error_from_image(img)
|
|
if lin_err_local:
|
|
lin_err += lin_err_local
|
|
n += 1
|
|
if n > 0:
|
|
lin_err /= n
|
|
print("linear error is %f" % lin_err)
|
|
self.assertTrue(0.0 < lin_err, 'lin_err is %f' % lin_err)
|
|
self.assertTrue(lin_err < max_err, 'lin_err is %f' % lin_err)
|
|
|
|
flat = c.remap(img)
|
|
self.assertEqual(img.shape, flat.shape)
|
|
|
|
def test_monocular(self):
|
|
# Run the calibrator, produce a calibration, check it
|
|
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_FIX_K3, pattern=setup.pattern)
|
|
|
|
if 0:
|
|
# display the patterns viewed by the camera
|
|
for pattern_warped in self.limages[i]:
|
|
cv2.imshow("toto", pattern_warped)
|
|
cv2.waitKey(0)
|
|
|
|
mc.cal(self.limages[i])
|
|
self.assert_good_mono(mc, self.limages[i], setup.lin_err)
|
|
|
|
# Make sure the intrinsics are similar
|
|
err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.k, ord=numpy.inf)
|
|
self.assertTrue(err_intrinsics < setup.K_err,
|
|
'intrinsics error is %f for resolution i = %d' % (err_intrinsics, i))
|
|
print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.k, ord=numpy.inf))
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main(verbosity=2)
|