def handle_monocular(self, msg):
if self.c == None:
if self._camera_name:
self.c = MonoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
name=self._camera_name,
detection=self._detection,
output=self._output,
checkerboard_flags=self._checkerboard_flags,
min_good_enough=self._min_good_enough)
else:
self.c = MonoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
detection=self._detection,
output=self._output,
checkerboard_flags=self.checkerboard_flags,
min_good_enough=self._min_good_enough)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.shape[1]
self.redraw_monocular(drawable)
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))
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("[WARN] Couldn't open image " + fname + "!", file=sys.stderr)
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 __init__(self, chess_size, dim, approximate=0):
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
image_topic = rospy.resolve_name("monocular") + "/image_rect"
camera_topic = rospy.resolve_name("monocular") + "/camera_info"
tosync_mono = [
(image_topic, sensor_msgs.msg.Image),
(camera_topic, sensor_msgs.msg.CameraInfo),
]
if approximate <= 0:
sync = message_filters.TimeSynchronizer
else:
sync = functools.partial(ApproximateTimeSynchronizer,
slop=approximate)
tsm = sync([
message_filters.Subscriber(topic, type)
for (topic, type) in tosync_mono
], 10)
tsm.registerCallback(self.queue_monocular)
left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
right_camera_topic = rospy.resolve_name(
"stereo") + "/right/camera_info"
tosync_stereo = [(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo)]
tss = sync([
message_filters.Subscriber(topic, type)
for (topic, type) in tosync_stereo
], 10)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue()
self.q_stereo = Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
self.sc = StereoCalibrator([self.board])
def loadImages(self,
cal_img_path,
name,
n_corners,
square_length,
n_disp_img=1e5,
display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
cols, rows = n_corners
self.boards.append(ChessboardInfo(cols, rows, float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file,
0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(
img, 'mono8') # Convert to ROS Image msg
# Extract chessboard corners using ROS camera_calibration package
drawable = self.c.handle_msg(img_msg)
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Corner Extraction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
# Length of a chessboard square
self.d_square = square_length
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
# Number of examined images
self.n_chessboards = len(self.c.good_corners)
# Dimensions of extracted corner grid
self.n_corners_y, self.n_corners_x = n_corners
self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def handle_monocular(self, msg):
if self.c == None:
if self._camera_name:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.shape[1]
self.redraw_monocular(drawable)
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))
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 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 handle_monocular(self, msg):
if self.c == None:
if self._camera_name:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name,
checkerboard_flags=self._checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
checkerboard_flags=self.checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.shape[1]
self.redraw_monocular(drawable)
def handle_monocular(self, msg):
if self.c == None:
if self._camera_name:
self.c = MonoCalibrator(self._boards,
self._calib_flags,
self._pattern,
name=self._camera_name)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags,
self._pattern)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.cols
self.redraw_monocular(drawable)
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)
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 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 cal_from_tarfile(boards, tarname, mono=False):
if mono:
calibrator = MonoCalibrator(boards)
else:
calibrator = StereoCalibrator(boards)
calibrator.do_tarfile_calibration(tarname)
print calibrator.ost()
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 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.assert_(err_intrinsics < setup.K_err, 'intrinsics error is %f' % err_intrinsics)
print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf))
def cal_from_tarfile(boards, tarname, mono = False):
if mono:
calibrator = MonoCalibrator(boards)
else:
calibrator = StereoCalibrator(boards)
calibrator.do_tarfile_calibration(tarname)
print calibrator.ost()
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 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 __init__(self, chess_size, dim):
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
image_topic = rospy.resolve_name("monocular") + "/image_rect"
camera_topic = rospy.resolve_name("monocular") + "/camera_info"
tosync_mono = [
(image_topic, sensor_msgs.msg.Image),
(camera_topic, sensor_msgs.msg.CameraInfo),
]
tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
tsm.registerCallback(self.queue_monocular)
left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"
tosync_stereo = [
(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo)
]
tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue()
self.q_stereo = Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
self.sc = StereoCalibrator([self.board])
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()
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self,
cal_img_path,
name,
n_corners,
square_length,
n_disp_img=1e5,
display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(
ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file,
0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(
img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(
img_msg
) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Corner Extraction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(
self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def genCornerCoordinates(self, u_meas, v_meas):
'''
Inputs:
u_meas: a list of arrays where each array are the u values for each board.
v_meas: a list of arrays where each array are the v values for each board.
Output:
corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board.
HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end
HINT: our solution does not use the u_meas and v_meas values
HINT: it does not matter where your frame it, as long as you are consistent!
'''
########## Code starts here ##########
# using the left bottom corner as orgin. x-axis pointing right and y-axis pointing upward
row_idx = np.linspace(0, self.d_square * (self.n_corners_x - 1),
self.n_corners_x)
col_idx = np.linspace(0, self.d_square * (self.n_corners_y - 1),
self.n_corners_y).reshape(self.n_corners_y, 1)
col_idx = col_idx[::-1]
X_world = np.broadcast_to(
row_idx, (self.n_corners_y, self.n_corners_x)).reshape(-1)
Y_world = np.broadcast_to(
col_idx, (self.n_corners_y, self.n_corners_x)).reshape(-1)
Xg = [X_world] * self.n_chessboards
Yg = [Y_world] * self.n_chessboards
corner_coordinates = (Xg, Yg)
########## Code ends here ##########
return corner_coordinates
def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A
'''
Inputs:
u_meas: an array of the u values for a board.
v_meas: an array of the v values for a board.
X: an array of the X values for a board. (from genCornerCoordinates)
Y: an array of the Y values for a board. (from genCornerCoordinates)
Output:
H: the homography matrix. its size is 3x3
HINT: What is the size of the matrix L?
HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigen value live?
HINT: np.stack and/or np.hstack may come in handy here.
'''
########## Code starts here ##########
num_points = u_meas.size # 63 points
# size of matrix L (2n x 9)
L = np.zeros((2 * num_points, 9))
# note, write the world coordinates in homogeneous form
for i in range(num_points):
L[2 * i] = [
X[i], Y[i], 1, 0, 0, 0, -u_meas[i] * X[i], -u_meas[i] * Y[i],
-u_meas[i]
]
L[2 * i + 1] = [
0, 0, 0, X[i], Y[i], 1, -v_meas[i] * X[i], -v_meas[i] * Y[i],
-v_meas[i]
]
# x is the right singular vector of L, which is the third output of svd
P, D, x = np.linalg.svd(L)
# associated with the smallest singular value is the last one , descending order
H = np.reshape(x[-1, :], (3, 3))
########## Code ends here ##########
return H
def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B
'''
Input:
H: a list of homography matrices for each board
Output:
A: the camera intrinsic matrix
HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT
HINT: What is the definition of h_ij?
HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function)
HINT: What is the size of V?
'''
########## Code starts here ##########
# get number of images
n_images = len(H)
# Solve Vb = 0
V = np.zeros((2 * n_images, 6))
# get hi and v_ij first
for n in range(n_images):
h = H[n].T
v_ij = lambda i, j: np.array([
h[i - 1, 0] * h[j - 1, 0], h[i - 1, 0] * h[j - 1, 1] +
h[i - 1, 1] * h[j - 1, 0], h[i - 1, 1] * h[j - 1, 1], h[
i - 1, 2] * h[j - 1, 0] + h[i - 1, 0] * h[j - 1, 2], h[
i - 1, 2] * h[j - 1, 1] + h[i - 1, 1] * h[j - 1, 2], h[
i - 1, 2] * h[j - 1, 2]
])
# note, v_ij is not transpose, so don't need transpose in V
V[2 * n] = v_ij(1, 2)
V[2 * n + 1] = v_ij(1, 1) - v_ij(2, 2)
P, Q, solution = np.linalg.svd(V)
# b is associated with the smallest singular value
b = solution[-1, :]
B11, B12, B22, B13, B23, B33 = b
v0 = (B12 * B13 - B11 * B23) / (B11 * B22 - B12 * B12)
lam = B33 - (B13**2 + v0 * (B12 * B13 - B11 * B23)) / B11
alpha = np.sqrt(lam / B11)
beta = np.sqrt(lam * B11 / (B11 * B22 - B12**2))
gamma = -B12 * alpha**2 * beta / lam
u0 = gamma * v0 / beta - B13 * alpha**2 / lam
A = np.array([[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]])
########## Code ends here ##########
return A
def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C
'''
Inputs:
H: a single homography matrix
A: the camera intrinsic matrix
Outputs:
R: the rotation matrix
t: the translation vector
'''
########## Code starts here ##########
h1 = H[:, 0]
h2 = H[:, 1]
h3 = H[:, 2]
lam = 1 / np.linalg.norm(np.linalg.inv(A).dot(h1))
r1 = lam * np.linalg.inv(A).dot(h1)
r2 = lam * np.linalg.inv(A).dot(h2)
r3 = np.cross(r1, r2)
t = lam * np.linalg.inv(A).dot(h3)
Q = np.hstack((r1.reshape(np.size(r1), 1), r2.reshape(np.size(r2), 1),
r3.reshape(np.size(r3), 1)))
U, S, V_T = np.linalg.svd(Q)
R = np.matmul(U, V_T)
########## Code ends here ##########
return R, t
def transformWorld2NormImageUndist(self, X, Y, Z, R,
t): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
x, y: the coordinates in the ideal normalized image plane
'''
########## Code starts here ##########
num_points = np.size(X) # 63 points
# get points in world frame in homogeneous form
P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1)
for i in range(1, num_points):
P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1)))
Rt = np.hstack((R, t.reshape(3, 1))) # 3x4
p = np.matmul(Rt, P) # 3x63
p = np.divide(p, p[-1, :]) # transform to homogenous form
x = p[0, :]
y = p[1, :]
########## Code ends here ##########
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t,
A): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
A: the camera intrinsic parameters
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
u, v: the coordinates in the ideal pixel image plane
'''
########## Code starts here ##########
num_points = np.size(X) # 63 points
# get points in world frame in homogeneous form
P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1)
for i in range(1, num_points):
P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1)))
Rt = np.hstack((R, t.reshape(3, 1))) # 3x4
M = np.matmul(A, Rt) # 3x63
p = np.matmul(M, P)
p = np.divide(p, p[-1, :]) # transform to homogeneous form
u = p[0, :]
v = p[1, :]
########## Code ends here ##########
return u, v
def plotBoardPixImages(self,
u_meas,
v_meas,
X,
Y,
R,
t,
A,
n_disp_img=1e5,
k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame',
figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
x, y = self.transformWorld2NormImageUndist(X[p], Y[p],
np.zeros(X[p].size),
R[p], t[p])
u, v = self.transformWorld2PixImageUndist(X[p], Y[p],
np.zeros(X[p].size),
R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([
box.x0, box.y0 + box.height * 0.15, box.width,
box.height * 0.85
])
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p + 1))
ax.legend(loc='lower center',
bbox_to_anchor=(0.5, -0.3),
fontsize='medium',
fancybox=True,
shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [
0,
self.n_corners_x - 1,
self.n_corners_x * self.n_corners_y - 1,
self.n_corners_x * (self.n_corners_y - 1),
]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([
zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])
])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(
np.array(
[X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1],
verts[p][0][0][2], '{0}'.format(p + 1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2 * view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title(
'Estimated Board Locations (Chessboard {0})'.format(i + 1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape(
(1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]
) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(img_msg) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
fig.canvas.set_window_title('Corner Extraction (Chessboard {0})'.format(i+1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0]*n_corners[1]
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack([k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6*n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
fig.canvas.set_window_title('Image Correction (Chessboard {0})'.format(i+1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([box.x0, box.y0 + box.height * 0.15, box.width, box.height*0.85])
if k[0] != 0:
u_br, v_br = self.transformWorld2PixImageDist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k)
ax.plot(u_br, v_br, 'g+', label='Radial Distortion Calibration')
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p+1))
ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [0, self.n_corners_x-1, self.n_corners_x*self.n_corners_y-1, self.n_corners_x*(self.n_corners_y-1), ]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(np.array([X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p+1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2*view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title('Estimated Board Locations (Chessboard {0})'.format(i+1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape((1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
def genCornerCoordinates(self, u_meas, v_meas):
# TODO - part (i)
raise NotImplementedError
return X, Y
def estimateHomography(self, u_meas, v_meas, X, Y):
# TODO - part (ii)
raise NotImplementedError
return H
def getCameraIntrinsics(self, H):
# TODO - part (iii)
raise NotImplementedError
return A
def getExtrinsics(self, H, A):
# TODO - part (iv)
raise NotImplementedError
return R, t
def transformWorld2NormImageUndist(self, X, Y, Z, R, t):
"""
Note: The transformation functions should only process one chessboard at a time!
This means X, Y, Z, R, t should be individual arrays
"""
# TODO - part (v)
raise NotImplementedError
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A):
# TODO - part (v)
raise NotImplementedError
return u, v
def transformWorld2NormImageDist(self, X, Y, R, t, k): # TODO: test
# TODO - part (vi)
raise NotImplementedError
return x_br, y_br
def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k):
# TODO - part (vi)
raise NotImplementedError
return u_br, v_br
class CameraCheckerNode:
def __init__(self, chess_size, dim):
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
image_topic = rospy.resolve_name("monocular") + "/image_rect"
camera_topic = rospy.resolve_name("monocular") + "/camera_info"
tosync_mono = [
(image_topic, sensor_msgs.msg.Image),
(camera_topic, sensor_msgs.msg.CameraInfo),
]
tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
tsm.registerCallback(self.queue_monocular)
left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"
tosync_stereo = [
(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo)
]
tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue()
self.q_stereo = Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
self.sc = StereoCalibrator([self.board])
def queue_monocular(self, msg, cmsg):
self.q_mono.put((msg, cmsg))
def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
def mkgray(self, msg):
return self.mc.mkgray(msg)
def image_corners(self, im):
(ok, corners, b) = self.mc.get_corners(im)
if ok:
return corners
else:
return None
def handle_monocular(self, msg):
(image, camera) = msg
gray = self.mkgray(image)
C = self.image_corners(gray)
if C is not None:
linearity_rms = self.mc.linear_error(C, self.board)
# Add in reprojection check
image_points = C
object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0]
dist_coeffs = numpy.zeros((4, 1))
camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ],
[ camera.P[4], camera.P[5], camera.P[6] ],
[ camera.P[8], camera.P[9], camera.P[10] ] ] )
ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3, _ = cv2.Rodrigues(rot)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans)
reprojected_h = camera_matrix * object_points_world
reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
reprojection_errors = image_points.squeeze().T - reprojected
reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))
# Print the results
print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
else:
print('no chessboard')
def handle_stereo(self, msg):
(lmsg, lcmsg, rmsg, rcmsg) = msg
lgray = self.mkgray(lmsg)
rgray = self.mkgray(rmsg)
L = self.image_corners(lgray)
R = self.image_corners(rgray)
if L and R:
epipolar = self.sc.epipolar_error(L, R)
dimension = self.sc.chessboard_size(L, R, [self.board], msg=(lcmsg, rcmsg))
print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension))
else:
print("no chessboard")
class CameraCheckerNode:
def __init__(self, chess_size, dim):
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
image_topic = rospy.resolve_name("monocular") + "/image_rect"
camera_topic = rospy.resolve_name("monocular") + "/camera_info"
tosync_mono = [
(image_topic, sensor_msgs.msg.Image),
(camera_topic, sensor_msgs.msg.CameraInfo),
]
tsm = message_filters.TimeSynchronizer([
message_filters.Subscriber(topic, type)
for (topic, type) in tosync_mono
], 10)
tsm.registerCallback(self.queue_monocular)
left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
right_camera_topic = rospy.resolve_name(
"stereo") + "/right/camera_info"
tosync_stereo = [(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo)]
tss = message_filters.TimeSynchronizer([
message_filters.Subscriber(topic, type)
for (topic, type) in tosync_stereo
], 10)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue.Queue()
self.q_stereo = Queue.Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
def queue_monocular(self, msg, cmsg):
self.q_mono.put((msg, cmsg))
def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
def mkgray(self, msg):
return self.br.imgmsg_to_cv(msg, "bgr8")
def image_corners(self, im):
(ok, corners, b) = self.mc.get_corners(im)
if ok:
return list(
cvmat_iterator(
cv.Reshape(self.mc.mk_image_points([(corners, b)]), 2)))
else:
return None
def handle_monocular(self, msg):
def pt2line(x0, y0, x1, y1, x2, y2):
""" point is (x0, y0), line is (x1, y1, x2, y2) """
return abs((x2 - x1) * (y1 - y0) - (x1 - x0) *
(y2 - y1)) / math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
(image, camera) = msg
rgb = self.mkgray(image)
C = self.image_corners(rgb)
if C:
cc = self.board.n_cols
cr = self.board.n_rows
errors = []
for r in range(cr):
(x1, y1) = C[(cc * r) + 0]
(x2, y2) = C[(cc * r) + cc - 1]
for i in range(1, cc - 1):
(x0, y0) = C[(cc * r) + i]
errors.append(pt2line(x0, y0, x1, y1, x2, y2))
linearity_rms = math.sqrt(
sum([e**2 for e in errors]) / len(errors))
# Add in reprojection check
A = cv.CreateMat(3, 3, 0)
image_points = cv.fromarray(numpy.array(C))
object_points = cv.fromarray(numpy.zeros([cc * cr, 3]))
for i in range(cr):
for j in range(cc):
object_points[i * cc + j, 0] = j * self.board.dim
object_points[i * cc + j, 1] = i * self.board.dim
object_points[i * cc + j, 2] = 0.0
dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]]))
camera_matrix = numpy.array(
[[camera.P[0], camera.P[1], camera.P[2]],
[camera.P[4], camera.P[5], camera.P[6]],
[camera.P[8], camera.P[9], camera.P[10]]])
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(object_points, image_points,
cv.fromarray(camera_matrix),
dist_coeffs, rot, trans)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot3x3)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * (
numpy.asmatrix(object_points).T) + numpy.asmatrix(trans)
reprojected_h = camera_matrix * object_points_world
reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
reprojection_errors = image_points - reprojected.T
reprojection_rms = numpy.sqrt(
numpy.sum(numpy.array(reprojection_errors)**2) /
numpy.product(reprojection_errors.shape))
# Print the results
print "Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (
linearity_rms, reprojection_rms)
else:
print 'no chessboard'
def handle_stereo(self, msg):
(lmsg, lcmsg, rmsg, rcmsg) = msg
lrgb = self.mkgray(lmsg)
rrgb = self.mkgray(rmsg)
sc = StereoCalibrator([self.board])
L = self.image_corners(lrgb)
R = self.image_corners(rrgb)
scm = image_geometry.StereoCameraModel()
scm.fromCameraInfo(lcmsg, rcmsg)
if L and R:
d = [(y0 - y1) for ((_, y0), (_, y1)) in zip(L, R)]
epipolar = math.sqrt(sum([i**2 for i in d]) / len(d))
disparities = [(x0 - x1) for ((x0, y0), (x1, y1)) in zip(L, R)]
pt3d = [
scm.projectPixelTo3d((x, y), d)
for ((x, y), d) in zip(L, disparities)
]
def l2(p0, p1):
return math.sqrt(
sum([(c0 - c1)**2 for (c0, c1) in zip(p0, p1)]))
# Compute the length from each horizontal and vertical lines, and return the mean
cc = self.board.n_cols
cr = self.board.n_rows
lengths = ([
l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1)
for r in range(cr)
] + [
l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1)
for c in range(cc)
])
dimension = sum(lengths) / len(lengths)
print "epipolar error: %f pixels dimension: %f m" % (epipolar,
dimension)
else:
print "no chessboard"
def __init__(self, name, chess_size, dim, approximate=0):
super().__init__(name)
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
# make sure n_cols is not smaller than n_rows, otherwise error computation will be off
if self.board.n_cols < self.board.n_rows:
self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols
image_topic = "monocular/image_rect"
camera_topic = "monocular/camera_info"
tosync_mono = [
(image_topic, sensor_msgs.msg.Image),
(camera_topic, sensor_msgs.msg.CameraInfo),
]
if approximate <= 0:
sync = message_filters.TimeSynchronizer
else:
sync = functools.partial(ApproximateTimeSynchronizer,
slop=approximate)
tsm = sync([
message_filters.Subscriber(self, type, topic)
for (topic, type) in tosync_mono
], 10)
tsm.registerCallback(self.queue_monocular)
left_topic = "stereo/left/image_rect"
left_camera_topic = "stereo/left/camera_info"
right_topic = "stereo/right/image_rect"
right_camera_topic = "stereo/right/camera_info"
tosync_stereo = [(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo)]
tss = sync([
message_filters.Subscriber(self, type, topic)
for (topic, type) in tosync_stereo
], 10)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue()
self.q_stereo = Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
self.sc = StereoCalibrator([self.board])
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(img_msg) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
fig.canvas.set_window_title('Corner Extraction (Chessboard {0})'.format(i+1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0]*n_corners[1]
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack([k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6*n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
fig.canvas.set_window_title('Image Correction (Chessboard {0})'.format(i+1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([box.x0, box.y0 + box.height * 0.15, box.width, box.height*0.85])
if k[0] != 0:
u_br, v_br = self.transformWorld2PixImageDist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k)
ax.plot(u_br, v_br, 'g+', label='Radial Distortion Calibration')
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p+1))
ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [0, self.n_corners_x-1, self.n_corners_x*self.n_corners_y-1, self.n_corners_x*(self.n_corners_y-1), ]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(np.array([X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p+1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2*view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title('Estimated Board Locations (Chessboard {0})'.format(i+1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape((1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
def genCornerCoordinates(self, u_meas, v_meas):
# part (i) - World coordinates for each corner
x_line = self.d_square*np.array(range(0, self.n_corners_x, 1))
y_line = self.d_square*np.array(range(self.n_corners_y-1, -1, -1)) # Flip Y-axis to traditional direction
# Form 2-D array of xy-coordinates
x_coord = np.tile(x_line, (self.n_corners_y, 1))
y_coord = np.tile(y_line, (self.n_corners_x, 1)).T
# Flatten and replicate for all chessboards
X = self.n_chessboards*[x_coord.flatten()]
Y = self.n_chessboards*[y_coord.flatten()]
return X, Y
def estimateHomography(self, u_meas, v_meas, X, Y):
# part (ii) - Homography of single chessboard
n = self.n_corners_per_chessboard
M_tld_T = np.vstack((X, Y, np.ones(n))).T
uM_tld_T = np.multiply(np.tile(u_meas, (3,1)).T, M_tld_T)
vM_tld_T = np.multiply(np.tile(v_meas, (3,1)).T, M_tld_T)
# Homography equation: L*x = 0
L = np.empty((2*n,9))
L[::2,:] = np.hstack((M_tld_T, np.zeros((n,3)), -uM_tld_T))
L[1::2,:] = np.hstack((np.zeros((n,3)), M_tld_T, -vM_tld_T))
# Solve for x and reshape into H
U, s, V_T = np.linalg.svd(L, full_matrices=True, compute_uv=True)
x = V_T[np.argmin(s),:] # x = [h1, h2, h3] (stacked columns of H)
H = np.reshape(x, (3,3))
return H
def getCameraIntrinsics(self, H):
# part (iii) - Linear intrinsic parameters of camera
def v_row(i, j, H_obs):
hi = H_obs[:,i] # i-th column vector of H
hj = H_obs[:,j] # j-th column vector of H
return np.array([hi[0]*hj[0], hi[0]*hj[1] + hi[1]*hj[0], hi[1]*hj[1],
hi[2]*hj[0] + hi[0]*hj[2], hi[2]*hj[1] + hi[1]*hj[2], hi[2]*hj[2]])
# Homography constraints: V_hom*b = 0
V_hom = []
for H_obs in H:
V_hom.append(v_row(0, 1, H_obs))
V_hom.append(v_row(0, 0, H_obs) - v_row(1, 1, H_obs))
if len(H) == 2: # Impose skewness constraint: gamma = 0
V_hom.append([0, 1, 0, 0, 0, 0])
if len(H) == 1:
raise NotImplementedError # Need to know (u0, v0) and set gamma = 0
V_hom = np.asarray(V_hom)
U, s, V_T = np.linalg.svd(V_hom, full_matrices=True, compute_uv=True)
b = V_T[np.argmin(s),:] # b = [B11, B12, B22, B13, B23, B33]
# Extract intrinsic parameters from B
v0 = (b[1]*b[2] - b[0]*b[4])/(b[0]*b[2] - b[1]**2)
lamb = b[5] - (b[3]**2 + v0*(b[1]*b[3] - b[0]*b[4]))/b[0]
alpha = np.sqrt(lamb/b[0])
beta = np.sqrt(lamb*b[0]/(b[0]*b[2] - b[1]**2))
gamma = -b[1]*(alpha**2)*beta/lamb
u0 = gamma*v0/beta - b[3]*alpha**2/lamb
A = np.array([[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]])
return A
def getExtrinsics(self, H, A):
# part (iv) - Rotation and translation of single chessboard
A_inv = np.linalg.inv(A)
lamb = 1.0/np.linalg.norm(A_inv.dot(H[:,0]))
r1 = lamb*A_inv.dot(H[:,0])
r2 = lamb*A_inv.dot(H[:,1])
r3 = np.cross(r1, r2)
t = lamb*A_inv.dot(H[:,2])
# Approximate rotation matrix
Q = np.vstack((r1, r2, r3)).T
U, s, V_T = np.linalg.svd(Q, full_matrices=False, compute_uv=True)
R = U.dot(V_T)
return R, t
def transformWorld2NormImageUndist(self, X, Y, Z, R, t):
"""
Note: The transformation functions should only process one chessboard at a time!
This means X, Y, Z, R, t should be individual arrays
"""
# part (v) - World coordinate (X,Y,Z) to normalized image coordinate (x,y) in undistorted frame
n = self.n_corners_per_chessboard
P_hW = np.vstack((X, Y, Z, np.ones(n)))
Rt = np.column_stack((R, t))
P_C = Rt.dot(P_hW) # [X_C, Y_C, Z_C]
# Camera to image coordinate (assume f = 1, origin = bottom left corner)
x = P_C[0,:]/P_C[2,:] # X_C/Z_C
y = P_C[1,:]/P_C[2,:] # Y_C/Z_C
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A):
# part (v) - World coordinate (X,Y,Z) to pixel image coordinate (u,v) in undistorted frame
n = self.n_corners_per_chessboard
P_hW = np.vstack((X, Y, Z, np.ones(n)))
Rt = np.column_stack((R, t))
p_h = A.dot(Rt).dot(P_hW) # [u, v, w]
# Transform to inhomogeneous coordinates
u = p_h[0,:]/p_h[2,:]
v = p_h[1,:]/p_h[2,:]
return u, v
def transformWorld2NormImageDist(self, X, Y, Z, R, t, k):
# part (vi) - World coordinate (X,Y,Z) to normalized image coordinate (x,y) in distorted frame
x, y = self.transformWorld2NormImageUndist(X, Y, Z, R, t)
x_br = x + x*(k[0]*(x**2 + y**2) + k[1]*(x**2 + y**2)**2)
y_br = y + y*(k[0]*(x**2 + y**2) + k[1]*(x**2 + y**2)**2)
return x_br, y_br
def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k):
# part (vi) - World coordinate (X,Y,Z) to pixel image coordinate (u,v) in distorted frame
n = self.n_corners_per_chessboard
x_br, y_br = self.transformWorld2NormImageDist(X, Y, Z, R, t, k)
p_br = np.vstack((x_br, y_br, np.ones(n)))
u_br = A[0,:].dot(p_br)
v_br = A[1,:].dot(p_br)
return u_br, v_br
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
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self, cal_img_path, name, n_corners, square_length, n_disp_img=1e5, display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(ChessboardInfo(n_corners[0], n_corners[1], float(square_length))) # n_rows, n_cols, dims
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file, 0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(img_msg) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
fig.canvas.set_window_title('Corner Extraction (Chessboard {0})'.format(i+1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions (480 x 640)
self.n_chessboards = len(self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0]*n_corners[1]
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack([k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(A, np.hstack([k, 0, 0]), None, Anew_w_k, (self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6*n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None, Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
fig.canvas.set_window_title('Image Correction (Chessboard {0})'.format(i+1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, n_disp_img=1e5, k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame', figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([box.x0, box.y0 + box.height * 0.15, box.width, box.height*0.85])
if k[0] != 0:
u_br, v_br = self.transformWorld2PixImageDist(X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k)
ax.plot(u_br, v_br, 'g+', label='Radial Distortion Calibration')
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p+1))
ax.legend(loc='lower center', bbox_to_anchor=(0.5, -0.3), fontsize='medium', fancybox=True, shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [0, self.n_corners_x-1, self.n_corners_x*self.n_corners_y-1, self.n_corners_x*(self.n_corners_y-1), ]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(np.array([X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1], verts[p][0][0][2], '{0}'.format(p+1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2*view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title('Estimated Board Locations (Chessboard {0})'.format(i+1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape((1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
# Flip Y-axis to traditional direction (aka NOT CV coordinate system)
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1])
return u_meas, v_meas # Lists of arrays (one per chessboard)
def genCornerCoordinates(self, u_meas, v_meas):
# part (i)
'''
u_meas and v_meas are not necessarily needed in this function since the world frame is
NOT relative to pixel/camera frame. Here, we define the top left corner as the origin.
'''
X = []
Y = []
# set the bottom left corner as the origin, x-axis pointing right, y-axis pointing up
row_coords = np.linspace(0., self.d_square*(self.n_corners_x-1), num=self.n_corners_x)
col_coords = np.linspace(0., self.d_square*(self.n_corners_y-1), num=self.n_corners_y).reshape(self.n_corners_y, 1)
col_coords = col_coords[-1, :] - col_coords
corner_world_x = np.broadcast_to(row_coords, (self.n_corners_y, self.n_corners_x)).reshape(-1)
corner_world_y = np.broadcast_to(col_coords, (self.n_corners_y, self.n_corners_x)).reshape(-1)
X = [corner_world_x] * self.n_chessboards
Y = [corner_world_y] * self.n_chessboards
return X, Y # Lists of arrays (length: 23)
def estimateHomography(self, u_meas, v_meas, X, Y):
# part (ii)
# u_meas/v_meas: (63,)
# X/Y: (63,)
assert u_meas.shape == X.shape
num_points = u_meas.shape[0] # 63 points
# each point: 2 constraints (9 items) --> P: 2nx9 matrix
# minimize Ph = 0 subject to 2-norm(h) = 1
P = np.zeros((2*num_points, 9))
for i in range(num_points):
P[2*i] = [X[i], Y[i], 1, 0, 0, 0, -u_meas[i]*X[i], -u_meas[i]*Y[i], -u_meas[i]]
P[2*i+1] = [0, 0, 0, X[i], Y[i], 1, -v_meas[i]*X[i], -v_meas[i]*Y[i], -v_meas[i]]
_, _, V_T = np.linalg.svd(P)
h = V_T[-1, :]
H = np.reshape(h, (3, 3))
return H
def getCameraIntrinsics(self, H):
# part (iii): n images of the model plane, construct (2n, 6) V matrix
num_images = len(H)
# solve Vb = 0
V = np.zeros((2*num_images, 6))
for n in range(num_images):
_H = H[n].T # follow the notation of literature
v_ij = lambda i, j: np.array([_H[i-1, 0]*_H[j-1, 0],
_H[i-1, 0]*_H[j-1, 1]+_H[i-1, 1]*_H[j-1, 0],
_H[i-1, 1]*_H[j-1, 1],
_H[i-1, 2]*_H[j-1, 0]+_H[i-1, 0]*_H[j-1, 2],
_H[i-1, 2]*_H[j-1, 1]+_H[i-1, 1]*_H[j-1, 2],
_H[i-1, 2]*_H[j-1, 2]])
V[2*n] = v_ij(1, 2)
V[2*n+1] = v_ij(1, 1) - v_ij(2, 2)
_, _, V_T = np.linalg.svd(V)
b = V_T[-1, :]
B11, B12, B22, B13, B23, B33 = b
# extract intrinsic parameters
v0 = (B12*B13 - B11*B23) / (B11*B22 - B12**2)
_lambda = B33 - (B13**2 + v0*(B12*B13 - B11*B23)) / B11
alpha = np.sqrt(_lambda / B11)
beta = np.sqrt(_lambda*B11 / (B11*B22 - B12**2))
gamma = -B12*(alpha**2)*beta / _lambda
u0 = gamma*v0 / beta - B13*(alpha**2) / _lambda
# sanity check
# print("u0: {}".format(u0/self.w_pixels))
# print("v0: {}".format(v0/self.h_pixels))
# print("gamma << alpha: {}".format(abs(gamma)/alpha))
# construct intrinsic matrix
A = np.array([[alpha, gamma, u0],
[0, beta, v0],
[0, 0, 1]])
return A
def getExtrinsics(self, H, A):
# part (iv)
h1, h2, h3 = H[:, 0], H[:, 1], H[:, 2]
lambda1 = 1. / np.linalg.norm(np.linalg.inv(A).dot(h1))
lambda2 = 1. / np.linalg.norm(np.linalg.inv(A).dot(h2))
# sanity check (lambda1 = lambda2 theoretically)
_lambda = 0.5 * (lambda1 + lambda2) if abs(lambda1 - lambda2) > 1e-5 else lambda1
r1 = _lambda * np.linalg.inv(A).dot(h1)
r2 = _lambda * np.linalg.inv(A).dot(h2)
r3 = np.cross(r1, r2)
t = _lambda * np.linalg.inv(A).dot(h3)
# initial R
Q = np.hstack((r1.reshape(-1, 1), r2.reshape(-1, 1), r3.reshape(-1, 1)))
U, _, V_T = np.linalg.svd(Q)
R = np.matmul(U, V_T)
return R, t
def transformWorld2NormImageUndist(self, X, Y, Z, R, t):
"""
Note: The transformation functions should only process one chessboard at a time!
This means X, Y, Z, R, t should be individual arrays
"""
# part (v)
num_points = X.shape[0]
P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1)
for i in range(1, num_points):
P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1)))
assert P.shape == (4, 63)
Rt = np.hstack((R, t.reshape(-1, 1))) # 3x4
p = np.matmul(Rt, P) # 3x63
p = np.divide(p, p[-1, :])
x = p[0, :].tolist()
y = p[1, :].tolist()
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A):
# part (v)
num_points = X.shape[0]
P = np.array([X[0], Y[0], Z[0], 1.]).reshape(4, 1)
for i in range(1, num_points):
P = np.hstack((P, np.array([X[i], Y[i], Z[i], 1.]).reshape(4, 1)))
T = np.matmul(A, np.hstack((R, t.reshape(-1, 1)))) # 3x4
p = np.matmul(T, P) # 3x63
p = np.divide(p, p[-1, :])
u = p[0, :].tolist()
v = p[1, :].tolist()
return u, v
def transformWorld2NormImageDist(self, X, Y, R, t, k):
# part (vi)
x, y = self.transformWorld2NormImageUndist(X, Y, np.zeros_like(X), R, t)
x, y = np.array(x), np.array(y)
x_br = x + x * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2))
y_br = y + y * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2))
return x_br, y_br
def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k):
# part (vi)
u, v = self.transformWorld2PixImageUndist(X, Y, np.zeros_like(Z), R, t, A)
u_0, v_0 = A[0, 2], A[1, 2]
x, y = self.transformWorld2NormImageUndist(X, Y, np.zeros_like(Z), R, t)
u, v = np.array(u), np.array(v)
x, y = np.array(x), np.array(y)
u_0, v_0 = np.array([u_0]*u.shape[0]), np.array([v_0]*v.shape[0])
u_br = u + (u - u_0) * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2))
v_br = v + (v - v_0) * (k[0]*(np.power(x, 2) + np.power(y, 2)) + k[1]*np.power((np.power(x, 2) + np.power(y, 2)), 2))
return u_br, v_br
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)
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self, cal_img_path, name, n_corners, square_length,
display_flag):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(
ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(os.listdir(self.cal_img_path)):
img = cv2.imread(self.cal_img_path + '/' + file,
0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(
img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(
img_msg
) # Extract chessboard corners using ROS camera_calibration package
if display_flag:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Corner Extraction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(
self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def undistortImages(self, A, k=np.zeros(2), scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(os.listdir(self.cal_img_path)):
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A, np.hstack([k, 0, 0]),
None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self, u_meas, v_meas, X, Y, R, t, A, k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame',
figsize=(8, 6))
plt.clf()
for p in range(self.n_chessboards):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p],
np.zeros(X[p].size),
R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([
box.x0, box.y0 + box.height * 0.15, box.width,
box.height * 0.85
])
if k[0] != 0:
u_br, v_br = self.transformWorld2PixImageDist(
X[p], Y[p], np.zeros(X[p].size), R[p], t[p], A, k)
ax.plot(u_br,
v_br,
'g+',
label='Radial Distortion Calibration')
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p + 1))
ax.legend(loc='lower center',
bbox_to_anchor=(0.5, -0.3),
fontsize='medium',
fancybox=True,
shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [
0,
self.n_corners_x - 1,
self.n_corners_x * self.n_corners_y - 1,
self.n_corners_x * (self.n_corners_y - 1),
]
s_cam = 0.02
d_cam = 0.1
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([
zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])
])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(1, 2, 1)
ax3d = fig.add_subplot(1, 2, 2, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(
np.array(
[X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
M_tld *= np.sign(M_tld[2])
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(os.listdir(self.cal_img_path)):
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_color('green')
cam.set_alpha(0.2)
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_color('blue')
boards[p].set_alpha(1.0)
else:
boards[p].set_color('red')
boards[p].set_alpha(0.1)
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1],
verts[p][0][0][2], '{0}'.format(p + 1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-5 * view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
plt.tight_layout()
fig.canvas.set_window_title(
'Estimated Board Locations (Chessboard {0})'.format(i + 1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = A
self.c.distortion = np.hstack((k, np.zeros(3))).reshape((5, 1))
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]
) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
def genCornerCoordinates(self, u_meas, v_meas):
X = []
Y = []
# DEFINED FROM BOTTOM LEFT
# for i in range(self.n_corners_y):
# line = np.multiply(self.d_square,np.arange(self.n_corners_x))
# reversedLine = line[::-1]
# X.append(reversedLine)
# for i in range(self.n_corners_y):
# Y.append(np.multiply(i*self.d_square,np.ones(self.n_corners_x)))
# DEFINED FROM TOP LEFT
for i in range(self.n_corners_y):
line = np.multiply(self.d_square, np.arange(self.n_corners_x))
X.append(line)
for i in range(self.n_corners_y):
Y.append(np.multiply(i * self.d_square, np.ones(self.n_corners_x)))
X = np.concatenate(X)
Y = np.concatenate(Y)
return X, Y
def estimateHomography(self, u_meas, v_meas, X, Y):
# Generate L Matrix
L = np.zeros((2 * len(u_meas), 9))
iter = np.arange(len(u_meas))
M_i_T = np.column_stack((X, Y, np.ones((len(X)))))
u_meas_mat = np.column_stack((u_meas, u_meas, u_meas))
v_meas_mat = np.column_stack((v_meas, v_meas, v_meas))
FirstStack = np.column_stack((M_i_T, np.zeros(
(M_i_T.shape[0], 3)), -np.multiply(u_meas_mat, M_i_T)))
SecondStack = np.column_stack((np.zeros(
(M_i_T.shape[0], 3)), M_i_T, -np.multiply(v_meas_mat, M_i_T)))
L = np.vstack((FirstStack, SecondStack))
# Use SVD to find lowest singular vector - solution to min norm (Lx)
U, s, V = np.linalg.svd(L, full_matrices=False)
# Extract solution vector
smallestV = V[-1]
# Reconstruct H
H = np.zeros((3, 3))
H[0] = smallestV[0:3]
H[1] = smallestV[3:6]
H[2] = smallestV[6:9]
return H
def getCameraIntrinsics(self, H):
V_LENGTH = 6 # Length of the V concatenated from every image
V_mat = np.empty(
(0, V_LENGTH)) # Initialize to empty matrix for concatenation
# Loop over all images
for i in range(self.n_chessboards):
# Get the current H
H_cur = H[i]
# Generate V Matrix
V_11 = self.getVij(H_cur, 1, 1)
V_12 = self.getVij(H_cur, 1, 2)
V_22 = self.getVij(H_cur, 2, 2)
# Concatenate results onto V_mat
V_mat = np.vstack((V_mat, V_12, (V_11 - V_22)))
# Use SVD to find lowest singular vector - solution to min norm (Vx)
U, s, V = np.linalg.svd(V_mat, full_matrices=False)
# Extract solution vector
b = V[-1]
# NOTE: b = (B11;B12;B22;B13;B23;B33)^T :
A = self.solveForIntrinsics(b)
return A
# Helper function for getCameraIntrinsics()
# Gets the Vij term of each homography matrix. i and j are in form of equation, NOT for indexes
# Usage: V_11 = self.getVij(H_cur,1,1)
def getVij(self, H, i, j):
# Adjust i and j to account for indexing
i -= 1
j -= 1
H = H.T
V_ij = np.array([H[i,0]*H[j,0] , \
H[i,0]*H[j,1] + H[i,1]*H[j,0], \
H[i,1]*H[j,1], \
H[i,2]*H[j,0] + H[i,0]*H[j,2], \
H[i,2]*H[j,1] + H[i,1]*H[j,2], \
H[i,2]*H[j,2]])
return V_ij
# Helper function for getCameraIntrinsics()
def solveForIntrinsics(self, b_sol):
# NOTE: b = (B11;B12;B22;B13;B23;B33)^T :
B = np.zeros((4, 4))
B[1, 1] = b_sol[0]
B[1, 2] = b_sol[1]
B[2, 2] = b_sol[2]
B[1, 3] = b_sol[3]
B[2, 3] = b_sol[4]
B[3, 3] = b_sol[5]
A = np.zeros((3, 3))
# Solve for intrinsic parameters
v0 = (B[1, 2] * B[1, 3] - B[1, 1] * B[2, 3]) / (B[1, 1] * B[2, 2] -
B[1, 2]**2)
lam = B[3, 3] - (B[1, 3]**2 + v0 *
(B[1, 2] * B[1, 3] - B[1, 1] * B[2, 3])) / B[1, 1]
Beta = np.sqrt(lam * B[1, 1] / (B[1, 1] * B[2, 2] - B[1, 2]**2))
alpha = np.sqrt(lam / B[1, 1])
gamma = -B[1, 2] * alpha**2 * Beta / lam
u0 = gamma * v0 / alpha - B[1, 3] * alpha**2 / lam
A[0, 0] = alpha
A[0, 1] = gamma
A[0, 2] = u0 #u01
A[1, 1] = Beta
A[1, 2] = v0 #
A[2, 2] = 1
return A
def getExtrinsics(self, H, A):
lam = 1 / np.linalg.norm(np.dot(np.linalg.inv(A), H[:, 0]))
r1 = lam * np.dot(np.linalg.inv(A), H[:, 0])
r2 = lam * np.dot(np.linalg.inv(A), H[:, 1])
r3 = np.cross(r1, r2)
t = lam * np.dot(np.linalg.inv(A), H[:, 2])
R = np.column_stack((r1, r2, r3))
# Use SVD to find lowest singular vector - solution to min norm (Vx)
U, s, V = np.linalg.svd(R, full_matrices=False)
R_best = np.dot(U, V)
return R_best, t
def transformWorld2NormImageUndist(self, X, Y, Z, R, t):
"""
Note: The transformation functions should only process one chessboard at a time!
This means X, Y, Z, R, t should be individual arrays
"""
worldCoords = np.row_stack((X, Y, Z, np.ones((len(X)))))
cameraCoords = np.dot(np.column_stack((R, t)), worldCoords)
X_C = cameraCoords[0]
Y_C = cameraCoords[1]
Z_C = cameraCoords[2]
x = X_C / Z_C
y = Y_C / Z_C
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t, A):
worldCoords = np.row_stack((X, Y, Z, np.ones((len(X)))))
pixCoords = np.dot(np.dot(A, np.column_stack((R, t))), worldCoords)
# Convert back to homogenous coordinates
u = pixCoords[0] / pixCoords[2]
v = pixCoords[1] / pixCoords[2]
return u, v
def transformWorld2NormImageDist(self, X, Y, Z, R, t, k):
x, y = self.transformWorld2NormImageUndist(X, Y, Z, R, t)
# Apply radial distortion correction
x_br = x + x * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2)
y_br = y + y * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2)
return x_br, y_br
def transformWorld2PixImageDist(self, X, Y, Z, R, t, A, k):
u, v = self.transformWorld2PixImageUndist(X, Y, Z, R, t, A)
x, y = self.transformWorld2NormImageUndist(X, Y, Z, R, t)
u0 = A[0, 2]
v0 = A[1, 2]
# Apply radial distortion correction
u_br = u + (u - u0) * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2)
v_br = v + (v - v0) * (k[0] * (x**2 + y**2) + k[1] * (x**2 + y**2)**2)
return u_br, v_br
def estimateLensDistortion(self, u_meas, v_meas, X, Y, Z, R, t, A):
u0 = A[0, 2]
v0 = A[1, 2]
u = []
v = []
x = []
y = []
# loop over all images and predict u,v for each (assumed undistorted)
for i in range(self.n_chessboards):
u_cur, v_cur = self.transformWorld2PixImageUndist(
X[i], Y[i], Z[i], R[i], t[i], A)
u.append(u_cur)
v.append(v_cur)
x_cur, y_cur = self.transformWorld2NormImageUndist(
X[i], Y[i], Z[i], R[i], t[i])
x.append(x_cur)
y.append(y_cur)
u = np.concatenate(u)
v = np.concatenate(v)
x = np.concatenate(x)
y = np.concatenate(y)
u_breve = np.concatenate(u_meas)
v_breve = np.concatenate(v_meas)
# Form D-matrix
d11 = (u - u0) * (x**2 + y**2)
d12 = (u - u0) * ((x**2 + y**2)**2)
d21 = (v - v0) * (x**2 + y**2)
d22 = (v - v0) * ((x**2 + y**2)**2)
D = np.row_stack((np.column_stack(
(d11, d12)), np.column_stack((d21, d22))))
d = np.concatenate((u_breve - u, v_breve - v))
k = np.dot(np.linalg.pinv(D), d)
return k
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)
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self,
cal_img_path,
name,
n_corners,
square_length,
n_disp_img=1e5,
display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(
ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file,
0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(
img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(
img_msg
) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Corner Extraction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(
self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def genCornerCoordinates(self, u_meas, v_meas):
'''
Inputs:
u_meas: a list of arrays where each array are the u values for each board.
v_meas: a list of arrays where each array are the v values for each board.
Output:
corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where
each array are the x/y values for each board.
HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end
HINT: our solution does not use the u_meas and v_meas values
HINT: it does not matter where your frame it, as long as you are consistent!
'''
########## Code starts here ##########
# u_meas, v_meas returns the corners in pixel coordinates, where
# bottom-left is (0,0). Points are ordered from blue to pink.
# Slanted lines show where the start of the next row is.
# This is all good, but
# the Pset wants World coordinates (X,Y) attached to the
# checkerboards, so we just generate these accordingly
# based on what we know about the checkerboards.
# In world coordinates (X,Y) in meters. The world reference frame
# is centered on our arbitrary object. The way we're generating it,
# X-axis goes from left-> right
# Y-axis goes from top -> down
# because that's how the points in u/v_meas are ordered in the array.
N = self.n_chessboards
nx, ny = self.n_corners_x, self.n_corners_y
d = self.d_square
# u_meas, v_meas have shapes (N, nx*ny)
X_list = []
Y_list = []
# We don't assume nx, ny to be constant, so we need a loop
for _ in range(N):
Xs = np.multiply(d, np.arange(nx))
Xs = np.tile(Xs, ny) # Shape (nx*ny,) 1,2,3,4,1,2,3,4,1,2,3,4,...
Ys = np.multiply(d, np.arange(ny))
Ys = np.repeat(Ys,
nx) # Shape (nx*ny,) 1,1,1,2,2,2,3,3,3,4,4,4,...
X_list.append(Xs)
Y_list.append(Ys)
corner_coordinates = (X_list, Y_list)
########## Code ends here ##########
return corner_coordinates
def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A
'''
Inputs:
u_meas: an array of the u values for a board.
v_meas: an array of the v values for a board.
X: an array of the X values for a board. (from genCornerCoordinates)
Y: an array of the Y values for a board. (from genCornerCoordinates)
Output:
H: the homography matrix. its size is 3x3
HINT: What is the size of the matrix L?
HINT: What are the outputs of the np.linalg.svd function? Based on this,
where does the eigenvector corresponding to the smallest eigen value live?
HINT: np.stack and/or np.hstack may come in handy here.
'''
########## Code starts here ##########
# Zhang 1998. See also lecture 9 notes, especially section 9.2.1
# Lecture 9 (2019) 54 minute mark onwards (Step 1)
# Equation number (x) refer to lecture 9 notes.
# We follow the notation in the lecture notes.
# We need to reconstruct the homography matrix M mapping (homogeneous) world
# coordinates to pixel coordinates.
nx, ny = self.n_corners_x, self.n_corners_y
u, v = u_meas, v_meas # shapes (nx*ny, )
# X, Y as given, Z = 0 as we know the points are co-planar.
P_W = np.array([X, Y, np.ones(nx * ny)]) # shape (k, nx*ny) = (3, 63)
k = P_W.shape[0]
# P_tilde has shape (2*nx*ny, 3k)
# Each 'block' is [-1 0 ui ] * P_{W,i}^T
# [ 0 -1 vi ]
# and there are N of these blocks as we have N points per image.
# P_{W,i} is a col vector of length 3.
# This is not strictly matrix P_tilde as written in eq (18).
# We blocked all rows with u and all the rows with v together.
# Since each row is just a different constraint, this works just fine for SVD.
P_tilde = np.block([[-1 * P_W.T, 0 * P_W.T, (u * P_W).T],
[0 * P_W.T, -1 * P_W.T, (v * P_W).T]])
U, s, VT = np.linalg.svd(P_tilde,
full_matrices=False) # We don't need padding.
# Eigenvalues s sorted from largest to smallest.
# We want the eigenvectors of P.T dot P, which corrresponds to V transpose.
# Grab the smallest one in the last row, and reshape to M, which is 3x3.
M = np.reshape(VT[-1, :], (k, k))
H = M # Convert back to Zhang's terminology.
########## Code ends here ##########
return H
def compute_Vij(self, H_T, i, j):
"""
Helper for getCameraIntrinsics.
Expects H to already be transposed.
arguments i and j should be cardinal (1-indexed)
"""
i, j = i - 1, j - 1 # Translate to ordinal array index
Hi, Hj = H_T[i], H_T[j]
V_ij = np.array([
Hi[0] * Hj[0], Hi[0] * Hj[1] + Hi[1] * Hj[0], Hi[1] * Hj[1],
Hi[2] * Hj[0] + Hi[0] * Hj[2], Hi[2] * Hj[1] + Hi[1] * Hj[2],
Hi[2] * Hj[2]
])
return V_ij
def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B
'''
Input:
H: a list of homography matrices for each board
Output:
A: the camera intrinsic matrix
HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT
HINT: What is the definition of h_ij?
HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function)
HINT: What is the size of V?
'''
########## Code starts here ##########
# Equation numbers (x) refer to lecture 9 notes.
H_list = H # Rebind variable name
N = self.n_chessboards
k = 3
# Likewise, we will not replicate the matrix V exactly as in eq (27)
# since it's just rows of SVD constraints.
# First let's get rid of the list. (why use a list ?)
# Each H is a 3x3, so let's turn the list into a tensor of shape (3, 3, N),
# such that H[i, j] has shape (N, ).
H_T = np.zeros((k, k, N))
for n in range(N):
H_T[:, :, n] = H_list[n].T # Note the transpose just under eq (26)
V_11 = self.compute_Vij(H_T, 1, 1) # shape (2k, N) = (6, 23)
V_12 = self.compute_Vij(H_T, 1, 2)
V_22 = self.compute_Vij(H_T, 2, 2)
V = np.vstack((V_12.T, (V_11 - V_22).T)) # shape (2N, 2k) = (46, 6)
U, s, VT_ = np.linalg.svd(V, full_matrices=False)
b = VT_[-1, :]
# We don't actually end up using the matrix B.
# B = np.array([ # This is symmetric. We don't need the lower triangle.
# [b[0], b[1], b[3]],
# [b[1], b[2], b[4]],
# [b[3], b[4], b[5]]
# ])
# Then just plug in eq (30)
(B11, B12, B22, B13, B23, B33) = b
t1 = B12 * B13 - B11 * B23
t2 = B11 * B22 - B12 * B12
v0 = t1 / t2
lmb = B33 - (B13 * B13 + v0 * t1) / B11
alp = np.sqrt(lmb / B11)
bet = np.sqrt(lmb * B11 / t2)
gam = -B12 * alp * alp * bet / lmb
u0 = gam * v0 / bet - B13 * alp * alp / lmb
A = np.array([[alp, gam, u0], [0, bet, v0], [0, 0, 1]])
########## Code ends here ##########
return A
def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C
'''
Inputs:
H: a single homography matrix
A: the camera intrinsic matrix
Outputs:
R: the rotation matrix
t: the translation vector
'''
########## Code starts here ##########
# Just follow equation (31) of lecture 9 notes
A_inv = np.linalg.inv(A)
c1, c2, c3 = H[:, 0], H[:, 1], H[:, 2] # The cs are columns of H
q = 1 / np.linalg.norm(np.dot(A_inv, c1)) # common recurring term
r1 = q * np.dot(A_inv, c1)
r2 = q * np.dot(A_inv, c2)
r3 = np.cross(r1, r2)
t = q * np.dot(A_inv, c3)
R = np.column_stack((r1, r2, r3)) # The rs are columns of R.
U, s, VT = np.linalg.svd(R, full_matrices=False)
R_sol = np.dot(U, VT)
R = R_sol
########## Code ends here ##########
return R, t
def transformWorld2NormImageUndist(self, X, Y, Z, R,
t): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
x, y: the coordinates in the ideal normalized image plane
'''
########## Code starts here ##########
d = X.shape[0]
P_Wh = np.array([X, Y, Z, np.ones(d)
]) # shape (4, 63) # Homogeneous world coords
Rt = np.column_stack((R, t)) # shape (3, 4)
P_Ch = np.dot(Rt, P_Wh) # shape (3, 63) # Homogeneous camera coords
# Transform back to nonhomogeneous coords
P_Ch = P_Ch / P_Ch[2]
x, y, _ = P_Ch
########## Code ends here ##########
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t,
A): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
A: the camera intrinsic parameters
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
u, v: the coordinates in the ideal pixel image plane
'''
########## Code starts here ##########
d = X.shape[0]
P_Wh = np.array([X, Y, Z, np.ones(d)
]) # shape (4, 63) # Homogeneous world coords
Rt = np.column_stack((R, t)) # shape (3, 4)
P_Ch = np.matmul(Rt, P_Wh) # shape (3, 63) # Homogeneous camera coords
ph = np.matmul(A, P_Ch) # shape (3, 63) # Homogeneous pixel coords
# Transform to nonhomogeneous coords
ph = ph / ph[2]
u, v, _ = ph
########## Code ends here ##########
return u, v
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A,
np.hstack([k, 0,
0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self,
u_meas,
v_meas,
X,
Y,
R,
t,
A,
n_disp_img=1e5,
k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame',
figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p],
np.zeros(X[p].size),
R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([
box.x0, box.y0 + box.height * 0.15, box.width,
box.height * 0.85
])
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p + 1))
ax.legend(loc='lower center',
bbox_to_anchor=(0.5, -0.3),
fontsize='medium',
fancybox=True,
shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [
0,
self.n_corners_x - 1,
self.n_corners_x * self.n_corners_y - 1,
self.n_corners_x * (self.n_corners_y - 1),
]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([
zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])
])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(
np.array(
[X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1],
verts[p][0][0][2], '{0}'.format(p + 1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2 * view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title(
'Estimated Board Locations (Chessboard {0})'.format(i + 1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A,
np.hstack([k, 0,
0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape(
(1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
# v_meas.append(chessboards[0][:, 0][:, 1]) # This keeps Y's original direction instead.
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]
) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCheckerNode:
def __init__(self, chess_size, dim):
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
image_topic = rospy.resolve_name("monocular") + "/image_rect"
camera_topic = rospy.resolve_name("monocular") + "/camera_info"
tosync_mono = [
(image_topic, sensor_msgs.msg.Image),
(camera_topic, sensor_msgs.msg.CameraInfo),
]
tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
tsm.registerCallback(self.queue_monocular)
left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"
tosync_stereo = [
(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo)
]
tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue()
self.q_stereo = Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
self.sc = StereoCalibrator([self.board])
def queue_monocular(self, msg, cmsg):
self.q_mono.put((msg, cmsg))
def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
def mkgray(self, msg):
return self.mc.mkgray(msg)
def image_corners(self, im):
(ok, corners, b) = self.mc.get_corners(im)
if ok:
return corners
else:
return None
def handle_monocular(self, msg):
(image, camera) = msg
gray = self.mkgray(image)
C = self.image_corners(gray)
if C:
linearity_rms = self.mc.linear_error(C, self.board)
# Add in reprojection check
image_points = C
object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0]
dist_coeffs = numpy.zeros((4, 1))
camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ],
[ camera.P[4], camera.P[5], camera.P[6] ],
[ camera.P[8], camera.P[9], camera.P[10] ] ] )
ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3, _ = cv2.Rodrigues(rot)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * (numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans))
reprojected_h = camera_matrix * object_points_world
reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
reprojection_errors = image_points.squeeze().T - reprojected.T
reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))
# Print the results
print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
else:
print('no chessboard')
def handle_stereo(self, msg):
(lmsg, lcmsg, rmsg, rcmsg) = msg
lgray = self.mkgray(lmsg)
rgray = self.mkgray(rmsg)
L = self.image_corners(lgray)
R = self.image_corners(rgray)
if L and R:
epipolar = self.sc.epipolar_error(L, R)
dimension = self.sc.chessboard_size(L, R, [self.board], msg=(lcmsg, rcmsg))
print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension))
else:
print("no chessboard")
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self,
cal_img_path,
name,
n_corners,
square_length,
n_disp_img=1e5,
display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(
ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file,
0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(
img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(
img_msg
) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Corner Extraction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(
self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def genCornerCoordinates(self, u_meas, v_meas):
'''
Inputs:
u_meas: a list of arrays where each array are the u values for each board.
v_meas: a list of arrays where each array are the v values for each board.
Output:
corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board.
HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end
HINT: our solution does not use the u_meas and v_meas values
HINT: it does not matter where your frame it, as long as you are consistent!
'''
########## Code starts here ##########
Xg = []
Yg = []
for i in range(len(u_meas)):
Xg_single_board = np.zeros(self.n_corners_x * self.n_corners_y)
Yg_single_board = np.zeros(self.n_corners_x * self.n_corners_y)
X_origin = 0
Y_origin = 6 * self.d_square
for rows in range(self.n_corners_y):
for cols in range(self.n_corners_x):
Xg_single_board[rows * self.n_corners_x +
cols] = X_origin + cols * self.d_square
Yg_single_board[rows * self.n_corners_x + cols] = Y_origin
Y_origin -= self.d_square
Xg.append(Xg_single_board)
Yg.append(Yg_single_board)
corner_coordinates = (Xg, Yg)
########## Code ends here ##########
return corner_coordinates
def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A
'''
Inputs:
u_meas: an array of the u values for a board.
v_meas: an array of the v values for a board.
X: an array of the X values for a board. (from genCornerCoordinates)
Y: an array of the Y values for a board. (from genCornerCoordinates)
Output:
H: the homography matrix. its size is 3x3
HINT: What is the size of the matrix L?
HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigenvalue live?
HINT: np.stack and/or np.hstack may come in handy here.
'''
########## Code starts here ##########
for n in range(self.n_corners_per_chessboard):
M_h_T = np.array([[X[n], Y[n], 1]])
Line_1 = np.hstack((M_h_T, np.zeros(
(1, 3)), (-1) * u_meas[n] * M_h_T))
Line_2 = np.hstack((np.zeros(
(1, 3)), M_h_T, (-1) * v_meas[n] * M_h_T))
if n == 0:
L = Line_1
L = np.vstack((L, Line_2))
else:
L = np.vstack((L, Line_1, Line_2))
u, s, vh = np.linalg.svd(L, full_matrices=True)
H = vh[-1].reshape(3, 3)
########## Code ends here ##########
return H
def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B
'''
Input:
H: a list of homography matrices for each board
Output:
A: the camera intrinsic matrix
HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT
HINT: What is the definition of h_ij?
HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function)
HINT: What is the size of V?
'''
########## Code starts here ##########
for h in H:
h = np.transpose(h)
v12_T = np.array([[
h[0, 0] * h[1, 0], h[0, 0] * h[1, 1] + h[0, 1] * h[1, 0],
h[0, 1] * h[1, 1], h[0, 2] * h[1, 0] + h[0, 0] * h[1, 2],
h[0, 2] * h[1, 1] + h[0, 1] * h[1, 2], h[0, 2] * h[1, 2]
]])
v11_T = np.array([[
h[0, 0] * h[0, 0], h[0, 0] * h[0, 1] + h[0, 1] * h[0, 0],
h[0, 1] * h[0, 1], h[0, 2] * h[0, 0] + h[0, 0] * h[0, 2],
h[0, 2] * h[0, 1] + h[0, 1] * h[0, 2], h[0, 2] * h[0, 2]
]])
v22_T = np.array([[
h[1, 0] * h[1, 0], h[1, 0] * h[1, 1] + h[1, 1] * h[1, 0],
h[1, 1] * h[1, 1], h[1, 2] * h[1, 0] + h[1, 0] * h[1, 2],
h[1, 2] * h[1, 1] + h[1, 1] * h[1, 2], h[1, 2] * h[1, 2]
]])
if (h == np.transpose(H[0])).all():
V = v12_T
V = np.vstack((V, v11_T - v22_T))
else:
V = np.vstack((V, v12_T, v11_T - v22_T))
u, s, vh = np.linalg.svd(V, full_matrices=True)
B = vh[-1]
A = np.zeros((3, 3))
v0 = (B[1] * B[3] - B[0] * B[4]) / (B[0] * B[2] - B[1] * B[1])
lamda = B[5] - (B[3]**2 + v0 * (B[1] * B[3] - B[0] * B[4])) / B[0]
alpha = (lamda / B[0])**0.5
beta = (lamda * B[0] / (B[0] * B[2] - B[1]**2))**0.5
gamma = (-1) * B[1] * alpha**2 * beta / lamda
u0 = gamma * v0 / beta - B[3] * alpha**2 / lamda
A[0, 0], A[0,
1], A[0,
2], A[1,
1], A[1,
2], A[2,
2] = alpha, gamma, u0, beta, v0, 1
########## Code ends here ##########
return A
def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C
'''
Inputs:
H: a single homography matrix
A: the camera intrinsic matrix
Outputs:
R: the rotation matrix
t: the translation vector
'''
########## Code starts here ##########
A_inv = np.linalg.inv(A)
lamda = 1 / np.linalg.norm(A_inv.dot(H[:, 0]))
r1_r2_t = A_inv.dot(H)
t = lamda * r1_r2_t[:, -1]
r1_est = lamda * r1_r2_t[:, 0]
r2_est = lamda * r1_r2_t[:, 1]
r3_est = np.cross(r1_est, r2_est)
R_est = np.c_[r1_est, r2_est, r3_est]
u, s, vh = np.linalg.svd(R_est, full_matrices=True)
R = np.matmul(u, vh)
########## Code ends here ##########
return R, t
def transformWorld2NormImageUndist(self, X, Y, Z, R,
t): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
x, y: the coordinates in the ideal normalized image plane
'''
########## Code starts here ##########
x = np.zeros(63)
y = np.zeros(63)
Transform_Matrix = np.vstack((np.c_[R, t], np.array([0, 0, 0, 1])))
for i in range(len(x)):
Pw_h = np.array([X[i], Y[i], Z[i], 1])
Pc_h = Transform_Matrix.dot(Pw_h)
x[i] = Pc_h[0] / Pc_h[2]
y[i] = Pc_h[1] / Pc_h[2]
########## Code ends here ##########
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t,
A): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
A: the camera intrinsic parameters
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
u, v: the coordinates in the ideal pixel image plane
'''
########## Code starts here ##########
u = np.zeros(63)
v = np.zeros(63)
T_M_WtoPix = A.dot(np.c_[R, t])
for i in range(len(u)):
Pw_h = np.array([X[i], Y[i], Z[i], 1])
Pp_h = T_M_WtoPix.dot(Pw_h)
u[i] = Pp_h[0] / Pp_h[2]
v[i] = Pp_h[1] / Pp_h[2]
########## Code ends here ##########
return u, v
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A,
np.hstack([k, 0,
0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self,
u_meas,
v_meas,
X,
Y,
R,
t,
A,
n_disp_img=1e5,
k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame',
figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p],
np.zeros(X[p].size),
R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([
box.x0, box.y0 + box.height * 0.15, box.width,
box.height * 0.85
])
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p + 1))
ax.legend(loc='lower center',
bbox_to_anchor=(0.5, -0.3),
fontsize='medium',
fancybox=True,
shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [
0,
self.n_corners_x - 1,
self.n_corners_x * self.n_corners_y - 1,
self.n_corners_x * (self.n_corners_y - 1),
]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([
zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])
])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(
np.array(
[X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1],
verts[p][0][0][2], '{0}'.format(p + 1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2 * view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title(
'Estimated Board Locations (Chessboard {0})'.format(i + 1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape(
(1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]
) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
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 CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self,
cal_img_path,
name,
n_corners,
square_length,
n_disp_img=1e5,
display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(
ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file,
0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(
img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(
img_msg
) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Corner Extraction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(
self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def genCornerCoordinates(self, u_meas, v_meas):
'''
Inputs:
u_meas: a list of arrays where each array are the u values for each board.
v_meas: a list of arrays where each array are the v values for each board.
Output:
corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board.
HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end
HINT: our solution does not use the u_meas and v_meas values
HINT: it does not matter where your frame it, as long as you are consistent!
'''
########## Code starts here ##########
x_coor, y_coor = [], []
# make the range of real world squares in the x and y directions
for i in range(self.n_corners_x):
x_coor.append(i * self.d_square)
for j in range(self.n_corners_y):
y_coor.append(j * self.d_square)
corner_coordinates = ([], [])
# for each chessboard add each x (n_corners_x*n_corners_y,) and y to the lists
for _ in range(self.n_chessboards):
corner_coordinates[0].append(np.array(x_coor * self.n_corners_y))
corner_coordinates[1].append((np.array(y_coor) * np.ones(
(self.n_corners_x, self.n_corners_y))).T.reshape(
(self.n_corners_x * self.n_corners_y, )))
########## Code ends here ##########
return corner_coordinates
def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A
'''
Inputs:
u_meas: an array of the u values for a board.
v_meas: an array of the v values for a board.
X: an array of the X values for a board. (from genCornerCoordinates)
Y: an array of the Y values for a board. (from genCornerCoordinates)
Output:
H: the homography matrix. its size is 3x3
HINT: What is the size of the matrix L?
HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigen value live?
HINT: np.stack and/or np.hstack may come in handy here.
'''
########## Code starts here ##########
# reshapes to be able to multiply
X = X.reshape(X.shape[0], 1)
Y = Y.reshape(Y.shape[0], 1)
u_meas = u_meas.reshape(u_meas.shape[0], 1)
v_meas = v_meas.reshape(v_meas.shape[0], 1)
# stacking up the values to be able to get L
M_tilde = np.hstack((X, Y, np.ones(X.shape)))
a = np.hstack((M_tilde, np.zeros(M_tilde.shape), -u_meas * M_tilde))
b = np.hstack((np.zeros(M_tilde.shape), M_tilde, -v_meas * M_tilde))
L = np.vstack((a, b))
# calculate the svd and reshape the singular values into the H matrix
svd = np.linalg.svd(L)
x = svd[2][-1]
H = np.array([[x[0], x[1], x[2]], [x[3], x[4], x[5]],
[x[6], x[7], x[8]]])
########## Code ends here ##########
return H
def getvij(self, i, j, H):
a = H[0][i] * H[0][j]
b = H[0][i] * H[1][j] + H[1][i] * H[0][j]
c = H[1][i] * H[1][j]
d = H[2][i] * H[0][j] + H[0][i] * H[2][j]
e = H[2][i] * H[1][j] + H[1][i] * H[2][j]
f = H[2][i] * H[2][j]
v_ij = np.array([a, b, c, d, e, f]).reshape((6, 1))
return v_ij
def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B
'''
Input:
H: a list of homography matrices for each board
Output:
A: the camera intrinsic matrix
HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT
HINT: What is the definition of h_ij?
HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function)
HINT: What is the size of V?
'''
########## Code starts here ##########
V_12 = self.getvij(0, 1, H[0])
V_11 = self.getvij(0, 0, H[0])
V_22 = self.getvij(1, 1, H[0])
V = np.vstack((V_12.T, (V_11 - V_22).T))
for x in range(1, len(H)):
V_12 = self.getvij(0, 1, H[x])
V_11 = self.getvij(0, 0, H[x])
V_22 = self.getvij(1, 1, H[x])
V_cur = np.vstack((V_12.T, (V_11 - V_22).T))
V = np.vstack((V, V_cur))
u, s, vh = np.linalg.svd(V)
b = vh[-1]
A = np.zeros((3, 3))
# b = {B11, B12, B22, B13, B23, B33}
v0 = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1]**2)
lamb = b[5] - ((b[3]**2) + v0 * (b[1] * b[3] - b[0] * b[4])) / b[0]
alpha = np.sqrt(lamb / b[0])
beta = np.sqrt((lamb * b[0]) / (b[0] * b[2] - b[1]**2))
gamma = -b[1] * (alpha**2) * beta / lamb
u0 = gamma * v0 / beta - b[3] * (alpha**2) / lamb
A[0, 0] = alpha
A[0, 1] = gamma
A[0, 2] = u0
A[1, 1] = beta
A[1, 2] = v0
A[2, 2] = 1
########## Code ends here ##########
return A
def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C
'''
Inputs:
H: a single homography matrix
A: the camera intrinsic matrix
Outputs:
R: the rotation matrix
t: the translation vector
'''
########## Code starts here ##########
h1 = H[:, 0].reshape((3, 1))
h2 = H[:, 1].reshape((3, 1))
h3 = H[:, 2].reshape((3, 1))
A_inv = np.linalg.inv(A)
lamb = 1 / np.linalg.norm(np.dot(A_inv, h1))
r1 = lamb * np.dot(A_inv, h1)
r2 = lamb * np.dot(A_inv, h2)
r3 = np.cross(r1.T, r2.T).T
R = np.hstack((r1, r2, r3))
t = lamb * np.dot(A_inv, h3).reshape((3, ))
svd = np.linalg.svd(R)
R_new = np.dot(svd[0], svd[2])
########## Code ends here ##########
return R_new, t
def transformWorld2NormImageUndist(self, X, Y, Z, R,
t): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
x, y: the coordinates in the ideal normalized image plane
'''
########## Code starts here ##########
t = t.reshape((3, 1))
X = X.reshape(len(X), 1)
Y = Y.reshape(len(Y), 1)
Z = Z.reshape(len(Z), 1)
M_tilde = np.hstack((X, Y, Z, np.ones(X.shape))).T
transform = np.hstack((R, t))
rotated = np.dot(transform, M_tilde)
return rotated[0] / rotated[2], rotated[1] / rotated[2]
def transformWorld2PixImageUndist(self, X, Y, Z, R, t,
A): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
A: the camera intrinsic parameters
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
u, v: the coordinates in the ideal pixel image plane
'''
########## Code starts here ##########
t = t.reshape((3, 1))
X = X.reshape(len(X), 1)
Y = Y.reshape(len(Y), 1)
Z = Z.reshape(len(Z), 1)
M_tilde = np.hstack((X, Y, Z, np.ones(X.shape))).T
transform = np.hstack((R, t))
m = np.dot(A, np.dot(transform, M_tilde))
u = m[0] / m[2]
v = m[1] / m[2]
########## Code ends here ##########
return u, v
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A,
np.hstack([k, 0,
0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self,
u_meas,
v_meas,
X,
Y,
R,
t,
A,
n_disp_img=1e5,
k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame',
figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p],
np.zeros(X[p].size),
R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([
box.x0, box.y0 + box.height * 0.15, box.width,
box.height * 0.85
])
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p + 1))
ax.legend(loc='lower center',
bbox_to_anchor=(0.5, -0.3),
fontsize='medium',
fancybox=True,
shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [
0,
self.n_corners_x - 1,
self.n_corners_x * self.n_corners_y - 1,
self.n_corners_x * (self.n_corners_y - 1),
]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([
zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])
])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(
np.array(
[X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1],
verts[p][0][0][2], '{0}'.format(p + 1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2 * view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title(
'Estimated Board Locations (Chessboard {0})'.format(i + 1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A,
np.hstack([k, 0,
0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape(
(1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]
) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCalibrator:
def __init__(self):
self.calib_flags = 0
self.pattern = Patterns.Chessboard
def loadImages(self,
cal_img_path,
name,
n_corners,
square_length,
n_disp_img=1e5,
display_flag=True):
self.name = name
self.cal_img_path = cal_img_path
self.boards = []
self.boards.append(
ChessboardInfo(n_corners[0], n_corners[1], float(square_length)))
self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)
if display_flag:
fig = plt.figure('Corner Extraction', figsize=(12, 5))
gs = gridspec.GridSpec(1, 2)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
img = cv2.imread(self.cal_img_path + '/' + file,
0) # Load the image
img_msg = self.c.br.cv2_to_imgmsg(
img, 'mono8') # Convert to ROS Image msg
drawable = self.c.handle_msg(
img_msg
) # Extract chessboard corners using ROS camera_calibration package
if display_flag and i < n_disp_img:
ax = plt.subplot(gs[0, 0])
plt.imshow(img, cmap='gray')
plt.axis('off')
ax = plt.subplot(gs[0, 1])
plt.imshow(drawable.scrib)
plt.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Corner Extraction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
# Useful parameters
self.d_square = square_length # Length of a chessboard square
self.h_pixels, self.w_pixels = img.shape # Image pixel dimensions
self.n_chessboards = len(
self.c.good_corners) # Number of examined images
self.n_corners_y, self.n_corners_x = n_corners # Dimensions of extracted corner grid
self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
def genCornerCoordinates(self, u_meas, v_meas):
'''
Inputs:
u_meas: a list of arrays where each array are the u values for each board.
v_meas: a list of arrays where each array are the v values for each board.
Output:
corner_coordinates: a tuple (Xg, Yg) where Xg/Yg is a list of arrays where each array are the x/y values for each board.
HINT: u_meas, v_meas starts at the blue end, and finishes with the pink end
HINT: our solution does not use the u_meas and v_meas values
HINT: it does not matter where your frame it, as long as you are consistent!
'''
# 3D points from world corner
X = []
Y = []
# Defining the world 2D coordinates
wcoordinates = np.zeros((self.n_corners_y * self.n_corners_x, 3),
np.float32)
wcoordinates[:, :2] = (np.mgrid[0:self.n_corners_x,
0:self.n_corners_y].T.reshape(
-1, 2)) * self.d_square
for ii in range(len(os.listdir(self.cal_img_path))):
X.append(wcoordinates[:, 0])
Y.append(wcoordinates[:, 1])
corner_coordinates = (X, Y)
return corner_coordinates
def estimateHomography(self, u_meas, v_meas, X, Y): # Zhang Appendix A
'''
Inputs:
u_meas: an array of the u values for a board.
v_meas: an array of the v values for a board.
X: an array of the X values for a board. (from genCornerCoordinates)
Y: an array of the Y values for a board. (from genCornerCoordinates)
Output:
H: the homography matrix. its size is 3x3
HINT: What is the size of the matrix L?
HINT: What are the outputs of the np.linalg.svd function? Based on this, where does the eigenvector corresponding to the smallest eigen value live?
HINT: np.stack and/or np.hstack may come in handy here.
'''
########## Code starts here ##########
n_points = self.n_corners_x * self.n_corners_y
z_T = np.array([0, 0, 0]).reshape(1, 3)
L = np.zeros((0, 9))
for i in range(n_points):
M_tilde_T = np.stack([X[i], Y[i], 1]).T.reshape(1, 3)
row1 = np.hstack([M_tilde_T, z_T, -u_meas[i] * M_tilde_T])
row2 = np.hstack([z_T, M_tilde_T, -v_meas[i] * M_tilde_T])
L_i = np.vstack((row1, row2))
L = np.vstack((L, L_i))
U, S, vh = np.linalg.svd(L)
H = vh.T[:, 8].reshape(3, 3)
return H
def getCameraIntrinsics(self, H): # Zhang 3.1, Appendix B
'''
Input:
H: a list of homography matrices for each board
Output:
A: the camera intrinsic matrix
HINT: MAKE SURE YOU READ SECTION 3.1 THOROUGHLY!!! V. IMPORTANT
HINT: What is the definition of h_ij?
HINT: It might be cleaner to write an inner function (a function inside the getCameraIntrinsics function)
HINT: What is the size of V?
'''
def v(i, j, H):
#note all indices are subtracted by 1 because matrices index at 1
i = i - 1
j = j - 1
v1 = h(i, H)[0] * h(j, H)[0]
v2 = h(i, H)[0] * h(j, H)[1] + h(i, H)[1] * h(j, H)[0]
v3 = h(i, H)[1] * h(j, H)[1]
v4 = h(i, H)[2] * h(j, H)[0] + h(i, H)[0] * h(j, H)[2]
v5 = h(i, H)[2] * h(j, H)[1] + h(i, H)[1] * h(j, H)[2]
v6 = h(i, H)[2] * h(j, H)[2]
v = np.array([v1, v2, v3, v4, v5, v6]).reshape(6, 1)
return v
def h(i, H):
return H[:, i]
V = np.zeros((0, 6))
for i in range(len(H)):
V_i = np.vstack((v(1, 2,
H[i]).T, (v(1, 1, H[i]) - v(2, 2, H[i])).T))
V = np.vstack((V, V_i))
U, S, vh = np.linalg.svd(V)
print('V shape:', V.shape)
print('vh shape:', vh.shape)
b = vh.T[:, 5]
B11 = b[0]
B12 = b[1]
B22 = b[2]
B13 = b[3]
B23 = b[4]
B33 = b[5]
v0 = (B12 * B13 - B11 * B23) / (B11 * B22 - B12**2)
lam = B33 - (B13**2 + v0 * (B12 * B13 - B11 * B23)) / B11
alpha = np.sqrt(lam / B11)
beta = np.sqrt(lam * B11 / (B11 * B22 - B12**2))
gamma = -B12 * alpha**2 * beta / lam
u0 = gamma * v0 / beta - B13 * alpha**2 / lam
A = np.array([[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]])
return A
def getExtrinsics(self, H, A): # Zhang 3.1, Appendix C
'''
Inputs:
H: a single homography matrix
A: the camera intrinsic matrix
Outputs:
R: the rotation matrix
t: the translation vector
'''
########## Code starts here ##########
A_inv = np.linalg.inv(A)
lam = 1 / np.linalg.norm(np.matmul(A_inv, H[:, 0]))
r1 = lam * np.matmul(A_inv, H[:, 0]).reshape(3, 1)
r2 = lam * np.matmul(A_inv, H[:, 1]).reshape(3, 1)
r3 = np.cross(r1, r2, axis=0)
t = lam * np.matmul(A_inv, H[:, 2])
Q = np.hstack((np.hstack((r1, r2)), r3))
U, S, vh = np.linalg.svd(Q)
R = np.matmul(U, vh)
########## Code ends here ##########
return R, t
def transformWorld2NormImageUndist(self, X, Y, Z, R,
t): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
x, y: the coordinates in the ideal normalized image plane
'''
########## Code starts here ##########
M_tilde = np.vstack((np.vstack((np.vstack(
(X, Y)), Z)), np.ones(len(X))))
Rt = np.hstack((R, t.reshape(3, 1)))
m_tilde = np.matmul(Rt, M_tilde)
x = m_tilde[0, :] / m_tilde[2, :]
y = m_tilde[1, :] / m_tilde[2, :]
########## Code ends here ##########
return x, y
def transformWorld2PixImageUndist(self, X, Y, Z, R, t,
A): # Zhang 2.1, Eq. (1)
'''
Inputs:
X, Y, Z: the world coordinates of the points for a given board. This is an array of 63 elements
X, Y come from genCornerCoordinates. Since the board is planar, we assume Z is an array of zeros.
A: the camera intrinsic parameters
R, t: the camera extrinsic parameters (rotation matrix and translation vector) for a given board.
Outputs:
u, v: the coordinates in the ideal pixel image plane
'''
########## Code starts here ##########
M_tilde = np.vstack((np.vstack((np.vstack(
(X, Y)), Z)), np.ones(len(X))))
Rt = np.hstack((R, t.reshape(3, 1)))
m_tilde = np.matmul(A, np.matmul(Rt, M_tilde))
u = m_tilde[0, :] / m_tilde[2, :]
v = m_tilde[1, :] / m_tilde[2, :]
########## Code ends here ##########
return u, v
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A,
np.hstack([k, 0,
0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardPixImages(self,
u_meas,
v_meas,
X,
Y,
R,
t,
A,
n_disp_img=1e5,
k=np.zeros(2)):
# Expects X, Y, R, t to be lists of arrays, just like u_meas, v_meas
fig = plt.figure('Chessboard Projection to Pixel Image Frame',
figsize=(8, 6))
plt.clf()
for p in range(min(self.n_chessboards, n_disp_img)):
plt.clf()
ax = plt.subplot(111)
ax.plot(u_meas[p], v_meas[p], 'r+', label='Original')
u, v = self.transformWorld2PixImageUndist(X[p], Y[p],
np.zeros(X[p].size),
R[p], t[p], A)
ax.plot(u, v, 'b+', label='Linear Intrinsic Calibration')
box = ax.get_position()
ax.set_position([
box.x0, box.y0 + box.height * 0.15, box.width,
box.height * 0.85
])
ax.axis([0, self.w_pixels, 0, self.h_pixels])
plt.gca().set_aspect('equal', adjustable='box')
plt.title('Chessboard {0}'.format(p + 1))
ax.legend(loc='lower center',
bbox_to_anchor=(0.5, -0.3),
fontsize='medium',
fancybox=True,
shadow=True)
plt.show(block=False)
plt.waitforbuttonpress()
def plotBoardLocations(self, X, Y, R, t, n_disp_img=1e5):
# Expects X, U, R, t to be lists of arrays, just like u_meas, v_meas
ind_corners = [
0,
self.n_corners_x - 1,
self.n_corners_x * self.n_corners_y - 1,
self.n_corners_x * (self.n_corners_y - 1),
]
s_cam = 0.02
d_cam = 0.05
xyz_cam = [[0, -s_cam, s_cam, s_cam, -s_cam],
[0, -s_cam, -s_cam, s_cam, s_cam],
[0, -d_cam, -d_cam, -d_cam, -d_cam]]
ind_cam = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1]]
verts_cam = []
for i in range(len(ind_cam)):
verts_cam.append([
zip([xyz_cam[0][j] for j in ind_cam[i]],
[xyz_cam[1][j] for j in ind_cam[i]],
[xyz_cam[2][j] for j in ind_cam[i]])
])
fig = plt.figure('Estimated Chessboard Locations', figsize=(12, 5))
axim = fig.add_subplot(121)
ax3d = fig.add_subplot(122, projection='3d')
boards = []
verts = []
for p in range(self.n_chessboards):
M = []
W = np.column_stack((R[p], t[p]))
for i in range(4):
M_tld = W.dot(
np.array(
[X[p][ind_corners[i]], Y[p][ind_corners[i]], 0, 1]))
if np.sign(M_tld[2]) == 1:
Rz = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
M_tld = Rz.dot(M_tld)
M_tld[2] *= -1
M.append(M_tld[0:3])
M = (np.array(M).T).tolist()
verts.append([zip(M[0], M[1], M[2])])
boards.append(Poly3DCollection(verts[p]))
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img = cv2.imread(self.cal_img_path + '/' + file, 0)
axim.imshow(img, cmap='gray')
axim.axis('off')
ax3d.clear()
for j in range(len(ind_cam)):
cam = Poly3DCollection(verts_cam[j])
cam.set_alpha(0.2)
cam.set_color('green')
ax3d.add_collection3d(cam)
for p in range(self.n_chessboards):
if p == i:
boards[p].set_alpha(1.0)
boards[p].set_color('blue')
else:
boards[p].set_alpha(0.1)
boards[p].set_color('red')
ax3d.add_collection3d(boards[p])
ax3d.text(verts[p][0][0][0], verts[p][0][0][1],
verts[p][0][0][2], '{0}'.format(p + 1))
plt.show(block=False)
view_max = 0.2
ax3d.set_xlim(-view_max, view_max)
ax3d.set_ylim(-view_max, view_max)
ax3d.set_zlim(-2 * view_max, 0)
ax3d.set_xlabel('X axis')
ax3d.set_ylabel('Y axis')
ax3d.set_zlabel('Z axis')
if i == 0:
ax3d.view_init(azim=90, elev=120)
plt.tight_layout()
fig.canvas.set_window_title(
'Estimated Board Locations (Chessboard {0})'.format(i + 1))
plt.show(block=False)
raw_input('<Hit Enter To Continue>')
def undistortImages(self, A, k=np.zeros(2), n_disp_img=1e5, scale=0):
Anew_no_k, roi = cv2.getOptimalNewCameraMatrix(
A, np.zeros(4), (self.w_pixels, self.h_pixels), scale)
mapx_no_k, mapy_no_k = cv2.initUndistortRectifyMap(
A, np.zeros(4), None, Anew_no_k, (self.w_pixels, self.h_pixels),
cv2.CV_16SC2)
Anew_w_k, roi = cv2.getOptimalNewCameraMatrix(A, np.hstack(
[k, 0, 0]), (self.w_pixels, self.h_pixels), scale)
mapx_w_k, mapy_w_k = cv2.initUndistortRectifyMap(
A, np.hstack([k, 0, 0]), None, Anew_w_k,
(self.w_pixels, self.h_pixels), cv2.CV_16SC2)
if k[0] != 0:
n_plots = 3
else:
n_plots = 2
fig = plt.figure('Image Correction', figsize=(6 * n_plots, 5))
gs = gridspec.GridSpec(1, n_plots)
gs.update(wspace=0.025, hspace=0.05)
for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
if i < n_disp_img:
img_dist = cv2.imread(self.cal_img_path + '/' + file, 0)
img_undist_no_k = cv2.undistort(img_dist, A, np.zeros(4), None,
Anew_no_k)
img_undist_w_k = cv2.undistort(img_dist, A,
np.hstack([k, 0,
0]), None, Anew_w_k)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_dist, cmap='gray')
ax.axis('off')
ax = plt.subplot(gs[0, 1])
ax.imshow(img_undist_no_k, cmap='gray')
ax.axis('off')
if k[0] != 0:
ax = plt.subplot(gs[0, 2])
ax.imshow(img_undist_w_k, cmap='gray')
ax.axis('off')
plt.subplots_adjust(left=0.02,
right=0.98,
top=0.98,
bottom=0.02)
fig.canvas.set_window_title(
'Image Correction (Chessboard {0})'.format(i + 1))
plt.show(block=False)
plt.waitforbuttonpress()
def writeCalibrationYaml(self, A, k):
self.c.intrinsics = np.array(A)
self.c.distortion = np.hstack(([k[0], k[1]], np.zeros(3))).reshape(
(1, 5))
#self.c.distortion = np.zeros(5)
self.c.name = self.name
self.c.R = np.eye(3)
self.c.P = np.column_stack((np.eye(3), np.zeros(3)))
self.c.size = [self.w_pixels, self.h_pixels]
filename = self.name + '_calibration.yaml'
with open(filename, 'w') as f:
f.write(self.c.yaml())
print('Calibration exported successfully to ' + filename)
def getMeasuredPixImageCoord(self):
u_meas = []
v_meas = []
for chessboards in self.c.good_corners:
u_meas.append(chessboards[0][:, 0][:, 0])
v_meas.append(self.h_pixels - chessboards[0][:, 0][:, 1]
) # Flip Y-axis to traditional direction
return u_meas, v_meas # Lists of arrays (one per chessboard)
class CameraCheckerNode:
def __init__(self, chess_size, dim):
self.board = ChessboardInfo()
self.board.n_cols = chess_size[0]
self.board.n_rows = chess_size[1]
self.board.dim = dim
image_topic = rospy.resolve_name("monocular") + "/image_rect"
camera_topic = rospy.resolve_name("monocular") + "/camera_info"
tosync_mono = [(image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo)]
tsm = message_filters.TimeSynchronizer(
[message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10
)
tsm.registerCallback(self.queue_monocular)
left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"
tosync_stereo = [
(left_topic, sensor_msgs.msg.Image),
(left_camera_topic, sensor_msgs.msg.CameraInfo),
(right_topic, sensor_msgs.msg.Image),
(right_camera_topic, sensor_msgs.msg.CameraInfo),
]
tss = message_filters.TimeSynchronizer(
[message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10
)
tss.registerCallback(self.queue_stereo)
self.br = cv_bridge.CvBridge()
self.q_mono = Queue.Queue()
self.q_stereo = Queue.Queue()
mth = ConsumerThread(self.q_mono, self.handle_monocular)
mth.setDaemon(True)
mth.start()
sth = ConsumerThread(self.q_stereo, self.handle_stereo)
sth.setDaemon(True)
sth.start()
self.mc = MonoCalibrator([self.board])
def queue_monocular(self, msg, cmsg):
self.q_mono.put((msg, cmsg))
def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
def mkgray(self, msg):
return self.br.imgmsg_to_cv(msg, "bgr8")
def image_corners(self, im):
(ok, corners, b) = self.mc.get_corners(im)
if ok:
return list(cvmat_iterator(cv.Reshape(self.mc.mk_image_points([(corners, b)]), 2)))
else:
return None
def handle_monocular(self, msg):
def pt2line(x0, y0, x1, y1, x2, y2):
""" point is (x0, y0), line is (x1, y1, x2, y2) """
return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
(image, camera) = msg
rgb = self.mkgray(image)
C = self.image_corners(rgb)
if C:
cc = self.board.n_cols
cr = self.board.n_rows
errors = []
for r in range(cr):
(x1, y1) = C[(cc * r) + 0]
(x2, y2) = C[(cc * r) + cc - 1]
for i in range(1, cc - 1):
(x0, y0) = C[(cc * r) + i]
errors.append(pt2line(x0, y0, x1, y1, x2, y2))
linearity_rms = math.sqrt(sum([e ** 2 for e in errors]) / len(errors))
# Add in reprojection check
A = cv.CreateMat(3, 3, 0)
image_points = cv.fromarray(numpy.array(C))
object_points = cv.fromarray(numpy.zeros([cc * cr, 3]))
for i in range(cr):
for j in range(cc):
object_points[i * cc + j, 0] = j * self.board.dim
object_points[i * cc + j, 1] = i * self.board.dim
object_points[i * cc + j, 2] = 0.0
dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]]))
camera_matrix = numpy.array(
[
[camera.P[0], camera.P[1], camera.P[2]],
[camera.P[4], camera.P[5], camera.P[6]],
[camera.P[8], camera.P[9], camera.P[10]],
]
)
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(
object_points, image_points, cv.fromarray(camera_matrix), dist_coeffs, rot, trans
)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot3x3)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * (numpy.asmatrix(object_points).T) + numpy.asmatrix(trans)
reprojected_h = camera_matrix * object_points_world
reprojected = reprojected_h[0:2, :] / reprojected_h[2, :]
reprojection_errors = image_points - reprojected.T
reprojection_rms = numpy.sqrt(
numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape)
)
# Print the results
print "Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (
linearity_rms,
reprojection_rms,
)
else:
print "no chessboard"
def handle_stereo(self, msg):
(lmsg, lcmsg, rmsg, rcmsg) = msg
lrgb = self.mkgray(lmsg)
rrgb = self.mkgray(rmsg)
sc = StereoCalibrator([self.board])
L = self.image_corners(lrgb)
R = self.image_corners(rrgb)
scm = image_geometry.StereoCameraModel()
scm.fromCameraInfo(lcmsg, rcmsg)
if L and R:
d = [(y0 - y1) for ((_, y0), (_, y1)) in zip(L, R)]
epipolar = math.sqrt(sum([i ** 2 for i in d]) / len(d))
disparities = [(x0 - x1) for ((x0, y0), (x1, y1)) in zip(L, R)]
pt3d = [scm.projectPixelTo3d((x, y), d) for ((x, y), d) in zip(L, disparities)]
def l2(p0, p1):
return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)]))
# Compute the length from each horizontal and vertical lines, and return the mean
cc = self.board.n_cols
cr = self.board.n_rows
lengths = [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] + [
l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)
]
dimension = sum(lengths) / len(lengths)
print "epipolar error: %f pixels dimension: %f m" % (epipolar, dimension)
else:
print "no chessboard"
