def run_stereo_calibration(self):
'''
Runs the calibration
'''
# set up Checkerboard, CheckerboardDetector and MonoCalibrator
board = Checkerboard(self.pattern_size, self.square_size)
detector = CheckerboardDetector(board)
calibrator = StereoCalibrator(board, detector, self.folder, self.image_prefix_l, self.image_prefix_r)
# run calibration
((rms_l, rms_r, rms_stereo), camera_matrix_l, dist_coeffs_l, rectification_matrix_l, projection_matrix_l,
camera_matrix_r, dist_coeffs_r, rectification_matrix_r, projection_matrix_r,
(h, w), R, T) = calibrator.calibrate_stereo_camera(self.alpha)
print "==> successfully calibrated, stereo reprojection RMS (in pixels):", rms_stereo
# create CalibrationData object with results
camera_info_l = CalibrationData(self.camera_name_l, self.frame_id_l, w, h)
camera_info_l.camera_matrix = camera_matrix_l
camera_info_l.rectification_matrix = rectification_matrix_l
camera_info_l.projection_matrix = projection_matrix_l
camera_info_l.distortion_coefficients = dist_coeffs_l
camera_info_r = CalibrationData(self.camera_name_r, self.frame_id_r, w, h)
camera_info_r.camera_matrix = camera_matrix_r
camera_info_r.rectification_matrix = rectification_matrix_r
camera_info_r.projection_matrix = projection_matrix_r
camera_info_r.distortion_coefficients = dist_coeffs_r
# save results
camera_info_l.save_camera_yaml_file(self.output_file_l)
print "==> saved left results to:", self.output_file_l
camera_info_r.save_camera_yaml_file(self.output_file_r)
print "==> saved right results to:", self.output_file_r
# convert baseline (invert transfrom as T and R bring right frame into left
# and we need transform from left to right for urdf!)
M = np.matrix(np.vstack((np.hstack((R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix
M_inv = M.I
T_inv = np.array(M_inv[:3,3]).flatten().tolist() # T as list (x, y, z)
R_inv = list(tf.transformations.euler_from_matrix(M_inv[:3,:3])) # convert R to (roll, pitch, yaw)
# save baseline
if (self.calibration_urdf_in != "" and self.calibration_urdf_out != ""):
attributes2update = {self.baseline_prop_prefix+'x': T_inv[0],
self.baseline_prop_prefix+'y': T_inv[1],
self.baseline_prop_prefix+'z': T_inv[2],
self.baseline_prop_prefix+'roll': R_inv[0],
self.baseline_prop_prefix+'pitch': R_inv[1],
self.baseline_prop_prefix+'yaw': R_inv[2]}
urdf_updater = CalibrationUrdfUpdater(self.calibration_urdf_in, self.calibration_urdf_out, self.verbose)
urdf_updater.update(attributes2update)
print "==> updated baseline in:", self.calibration_urdf_out
else:
print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..."
# verbose mode
if self.verbose:
print "--> results:"
np.set_printoptions(suppress=1)
print "left\n----"
print "rms left monocular calibration:", rms_l
print "camera matrix:\n", camera_matrix_l
print "distortion coefficients:\n", dist_coeffs_l
print "rectification matrix:\n", rectification_matrix_l
print "projection matrix:\n", projection_matrix_l
print
print "right\n-----"
print "rms right monocular calibration:", rms_r
print "camera matrix:\n", camera_matrix_r
print "distortion coefficients:\n", dist_coeffs_r
print "rectification matrix:\n", rectification_matrix_r
print "projection matrix:\n", projection_matrix_r
print
print "baseline (transform from left to right camera)\n--------"
print "R (in rpy):\n", R_inv
print "T (in xyz):\n", T_inv
def run_stereo_calibration(self):
'''
Runs the calibration
'''
# set up Checkerboard, CheckerboardDetector and MonoCalibrator
board = Checkerboard(self.pattern_size, self.square_size)
detector = CheckerboardDetector(board)
calibrator = StereoCalibrator(board, detector, self.folder,
self.image_prefix_l, self.image_prefix_r)
# run calibration
((rms_l, rms_r, rms_stereo), camera_matrix_l, dist_coeffs_l,
rectification_matrix_l, projection_matrix_l, camera_matrix_r,
dist_coeffs_r, rectification_matrix_r, projection_matrix_r, (h, w), R,
T) = calibrator.calibrate_stereo_camera(self.alpha)
print "==> successfully calibrated, stereo reprojection RMS (in pixels):", rms_stereo
# create CalibrationData object with results
camera_info_l = CalibrationData(self.camera_name_l, self.frame_id_l, w,
h)
camera_info_l.camera_matrix = camera_matrix_l
camera_info_l.rectification_matrix = rectification_matrix_l
camera_info_l.projection_matrix = projection_matrix_l
camera_info_l.distortion_coefficients = dist_coeffs_l
camera_info_r = CalibrationData(self.camera_name_r, self.frame_id_r, w,
h)
camera_info_r.camera_matrix = camera_matrix_r
camera_info_r.rectification_matrix = rectification_matrix_r
camera_info_r.projection_matrix = projection_matrix_r
camera_info_r.distortion_coefficients = dist_coeffs_r
# save results
camera_info_l.save_camera_yaml_file(self.output_file_l)
print "==> saved left results to:", self.output_file_l
camera_info_r.save_camera_yaml_file(self.output_file_r)
print "==> saved right results to:", self.output_file_r
# convert baseline (invert transfrom as T and R bring right frame into left
# and we need transform from left to right for urdf!)
M = np.matrix(np.vstack((np.hstack(
(R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix
M_inv = M.I
T_inv = np.array(M_inv[:3,
3]).flatten().tolist() # T as list (x, y, z)
R_inv = list(tf.transformations.euler_from_matrix(
M_inv[:3, :3])) # convert R to (roll, pitch, yaw)
# save baseline
if (self.calibration_urdf_in != ""
and self.calibration_urdf_out != ""):
attributes2update = {
self.baseline_prop_prefix + 'x': T_inv[0],
self.baseline_prop_prefix + 'y': T_inv[1],
self.baseline_prop_prefix + 'z': T_inv[2],
self.baseline_prop_prefix + 'roll': R_inv[0],
self.baseline_prop_prefix + 'pitch': R_inv[1],
self.baseline_prop_prefix + 'yaw': R_inv[2]
}
urdf_updater = CalibrationUrdfUpdater(self.calibration_urdf_in,
self.calibration_urdf_out,
self.verbose)
urdf_updater.update(attributes2update)
print "==> updated baseline in:", self.calibration_urdf_out
else:
print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..."
# verbose mode
if self.verbose:
print "--> results:"
np.set_printoptions(suppress=1)
print "left\n----"
print "rms left monocular calibration:", rms_l
print "camera matrix:\n", camera_matrix_l
print "distortion coefficients:\n", dist_coeffs_l
print "rectification matrix:\n", rectification_matrix_l
print "projection matrix:\n", projection_matrix_l
print
print "right\n-----"
print "rms right monocular calibration:", rms_r
print "camera matrix:\n", camera_matrix_r
print "distortion coefficients:\n", dist_coeffs_r
print "rectification matrix:\n", rectification_matrix_r
print "projection matrix:\n", projection_matrix_r
print
print "baseline (transform from left to right camera)\n--------"
print "R (in rpy):\n", R_inv
print "T (in xyz):\n", T_inv
def run_stereo_calibration(self):
'''
Runs the calibration
'''
# set up Checkerboard, CheckerboardDetector and MonoCalibrator
board = Checkerboard(self.pattern_size, self.pattern["square_size"])
detector = CheckerboardDetector(board)
attributes2update = {}
output = ""
camera_ref_saved = False;
for image_prefix_dep,\
camera_name_dep,\
frame_id_dep,\
output_file_dep,\
baseline_prop_prefix,\
is_part_of_stereo_system in zip(self.image_prefixes_dep,
self.camera_names_dep,
self.frame_ids_dep,
self.output_files_dep,
self.baseline_prop_prefixes_dep,
self.is_part_of_stereo_system):
if image_prefix_dep is not None:
print "Calibrating %s: \n\t Frame: %s \n\t Output File: %s \n\t Baseline Prefix: %s"%(camera_name_dep,frame_id_dep,output_file_dep, baseline_prop_prefix)
output += self.camera_name_ref +" -> " + camera_name_dep + ": \n"
calibrator = StereoCalibrator(board, detector, self.folder,
self.image_prefix_ref, image_prefix_dep)
# run calibration for stereo pair
(
(rms_ref, rms_dep, rms_stereo),
camera_matrix_ref, dist_coeffs_ref, rectification_matrix_ref, projection_matrix_ref,
camera_matrix_dep, dist_coeffs_dep, rectification_matrix_dep, projection_matrix_dep,
((h_ref, w_ref), (h_dep, w_dep)), R, T) = calibrator.calibrate_stereo_camera(self.alpha)
output += "==> successfully calibrated, stereo reprojection RMS (in pixels): "
output += str(rms_stereo)
output += "\n\n"
# create CalibrationData object with results
camera_info_ref = CalibrationData(
self.camera_name_ref, self.frame_id_ref, w_ref, h_ref)
camera_info_ref.camera_matrix = camera_matrix_ref
camera_info_ref.rectification_matrix = rectification_matrix_ref
camera_info_ref.projection_matrix = projection_matrix_ref
camera_info_ref.distortion_coefficients = dist_coeffs_ref
camera_info_dep = CalibrationData(
camera_name_dep, frame_id_dep, w_dep, h_dep)
camera_info_dep.camera_matrix = camera_matrix_dep
camera_info_dep.rectification_matrix = rectification_matrix_dep
camera_info_dep.projection_matrix = projection_matrix_dep
camera_info_dep.distortion_coefficients = dist_coeffs_dep
if output_file_dep is not None:
if not is_part_of_stereo_system:
# TODO: compute new projectionmatrix
(projection_matrix, _) = cv2.getOptimalNewCameraMatrix(
camera_matrix_dep, dist_coeffs_dep, (w_dep, h_dep),0 )
camera_info_dep.projection_matrix = np.array(np.hstack((
projection_matrix, np.matrix([0, 0, 0]).reshape(3, 1))))
camera_info_dep.rectification_matrix=[1,0,0, 0,1,0, 0,0,1]
# save results
if not camera_ref_saved:
camera_info_ref.save_camera_yaml_file(self.output_file_ref)
print "==> saved left results to:", self.output_file_ref
camera_ref_saved = True
#output += "==> saved left results to:", self.output_file_ref
camera_info_dep.save_camera_yaml_file(output_file_dep)
print "==> saved " + camera_name_dep + " results to:", output_file_dep
#output += "==> saved " + camera_name_dep + " results to:", output_file_dep
# convert baseline (invert transfrom as T and R bring right frame into left
# and we need transform from left to right for urdf!)
M = np.matrix(np.vstack((np.hstack(
(R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix
# TODO: tests with real hardware samples
# compute transformation from reference to parent frame of camera
# k_u = kinematic_utils()
# k_u.get_parent(frame_id_dep)
# M_parent = np.matrix(k_u.get_tf_to_parent(self.frame_id_ref))
# # resulting transformation
M_inv = M.I
# M_result = M_inv * M_parent
# # urdf specifies origin -> inverse
# M_inv = M_result.I
T_inv = np.array(
M_inv[:3, 3]).flatten().tolist() # T as list (x, y, z)
R_inv = list(tf.transformations.euler_from_matrix(
M_inv[:3, :3])) # convert R to (roll, pitch, yaw)
baseline_prop_prefix = 'offset_' + baseline_prop_prefix
attributes2update[baseline_prop_prefix + 'x'] = T_inv[0]
attributes2update[baseline_prop_prefix + 'y'] = T_inv[1]
attributes2update[baseline_prop_prefix + 'z'] = T_inv[2]
attributes2update[baseline_prop_prefix + 'roll'] = R_inv[0]
attributes2update[baseline_prop_prefix + 'pitch'] = R_inv[1]
attributes2update[baseline_prop_prefix + 'yaw'] = R_inv[2]
#verbose mode
if self.verbose:
print "--> results:"
np.set_printoptions(suppress=1)
print "left\n----"
print "rms left monocular calibration:", rms_ref
print "camera matrix:\n", camera_matrix_ref
print "distortion coefficients:\n", dist_coeffs_ref
print "rectification matrix:\n", rectification_matrix_ref
print "projection matrix:\n", projection_matrix_ref
print
print "right\n-----"
print "rms right monocular calibration:", rms_dep
print "camera matrix:\n", camera_matrix_dep
print "distortion coefficients:\n", dist_coeffs_dep
print "rectification matrix:\n", rectification_matrix_dep
print "projection matrix:\n", projection_matrix_dep
print
print "baseline (transform from left to right camera)\n--------"
print "R (in rpy):\n", R_inv
print "T (in xyz):\n", T_inv
# save baseline
if (self.calibration_offset_urdf != "" and self.calibration_default_urdf != ""):
for p in attributes2update.iteritems():
print "%s: %s"%p
urdf_updater = CalibrationUrdfUpdater(self.calibration_offset_urdf,
self.calibration_offset_urdf,
debug = self.verbose,
urdf_default=self.calibration_default_urdf)
urdf_updater.update(attributes2update)
print "==> updated baseline in:", self.calibration_offset_urdf
else:
print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..."
print output