Skip to content

Commit

Permalink
Added stereo calibration using charuco board (#976)
Browse files Browse the repository at this point in the history
From #972 
Doing this first for rolling.

This was a TODO in the repository, opening this PR to add this feature.
- The main issue why this wasn't possible imo is the way `mk_obj_points`
works. I'm using the inbuilt opencv function to get the points there.
- The other is a condition when aruco markers are detected they are
added as good points, This is fine in case of mono but in stereo these
have to be the same number as the object points to find matches although
this should be possible with aruco.
  • Loading branch information
MRo47 authored Jun 3, 2024
1 parent 847920b commit efb9005
Showing 1 changed file with 41 additions and 25 deletions.
66 changes: 41 additions & 25 deletions camera_calibration/src/camera_calibration/calibrator.py
Original file line number Diff line number Diff line change
Expand Up @@ -491,8 +491,11 @@ def compute_goodenough(self):
return list(zip(self._param_names, min_params, max_params, progress))

def mk_object_points(self, boards, use_board_size = False):
if self.pattern == Patterns.ChArUco:
opts = [board.charuco_board.chessboardCorners for board in boards]
return opts
opts = []
for i, b in enumerate(boards):
for b in boards:
num_pts = b.n_cols * b.n_rows
opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32)
for j in range(num_pts):
Expand Down Expand Up @@ -1151,29 +1154,28 @@ def cal_fromcorners(self, good):
self.T = numpy.zeros((3, 1), dtype=numpy.float64)
self.R = numpy.eye(3, dtype=numpy.float64)

if self.pattern == Patterns.ChArUco:
# TODO: implement stereo ChArUco calibration
raise NotImplemented("Stereo calibration not implemented for ChArUco boards")

if self.camera_model == CAMERA_MODEL.PINHOLE:
print("stereo pinhole calibration...")
if VersionInfo.parse(cv2.__version__).major < 3:
cv2.stereoCalibrate(opts, lipts, ripts, self.size,
self.l.intrinsics, self.l.distortion,
self.r.intrinsics, self.r.distortion,
self.R, # R
self.T, # T
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags = flags)
ret_values = cv2.stereoCalibrate(opts, lipts, ripts, self.size,
self.l.intrinsics, self.l.distortion,
self.r.intrinsics, self.r.distortion,
self.R, # R
self.T, # T
criteria=(cv2.TERM_CRITERIA_EPS + \
cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags=flags)
else:
cv2.stereoCalibrate(opts, lipts, ripts,
self.l.intrinsics, self.l.distortion,
self.r.intrinsics, self.r.distortion,
self.size,
self.R, # R
self.T, # T
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags = flags)
ret_values = cv2.stereoCalibrate(opts, lipts, ripts,
self.l.intrinsics, self.l.distortion,
self.r.intrinsics, self.r.distortion,
self.size,
self.R, # R
self.T, # T
criteria=(cv2.TERM_CRITERIA_EPS + \
cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags=flags)
print(f"Stereo RMS re-projection error: {ret_values[0]}")
elif self.camera_model == CAMERA_MODEL.FISHEYE:
print("stereo fisheye calibration...")
if VersionInfo.parse(cv2.__version__).major < 3:
Expand Down Expand Up @@ -1343,6 +1345,19 @@ def l2(p0, p1):
[l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)])
return sum(lengths) / len(lengths)

def update_db(self, lgray, rgray, lcorners, rcorners, lids, rids, lboard):
"""
update database with images and good corners if good samples are detected
"""
params = self.get_parameters(
lcorners, lids, lboard, (lgray.shape[1], lgray.shape[0]))
if self.is_good_sample(params, lcorners, lids, self.last_frame_corners, self.last_frame_ids):
self.db.append((params, lgray, rgray))
self.good_corners.append(
(lcorners, rcorners, lids, rids, lboard))
print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" %
tuple([len(self.db)] + params)))

def handle_msg(self, msg):
# TODO Various asserts that images have same dimension, same board detected...
(lmsg, rmsg) = msg
Expand Down Expand Up @@ -1399,11 +1414,12 @@ def handle_msg(self, msg):

# Add sample to database only if it's sufficiently different from any previous sample
if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners):
params = self.get_parameters(lcorners, lids, lboard, (lgray.shape[1], lgray.shape[0]))
if self.is_good_sample(params, lcorners, lids, self.last_frame_corners, self.last_frame_ids):
self.db.append( (params, lgray, rgray) )
self.good_corners.append( (lcorners, rcorners, lids, rids, lboard) )
print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params)))
# Add samples only with entire board in view if charuco
if self.pattern == Patterns.ChArUco:
if len(lcorners) == lboard.charuco_board.chessboardCorners.shape[0]:
self.update_db(lgray, rgray, lcorners, rcorners, lids, rids, lboard)
else:
self.update_db(lgray, rgray, lcorners, rcorners, lids, rids, lboard)

self.last_frame_corners = lcorners
self.last_frame_ids = lids
Expand Down

0 comments on commit efb9005

Please sign in to comment.