def run_mono_calibration(self): ''' Runs the calibration ''' print "==> starting monocular calibration" # set up Checkerboard, CheckerboardDetector and MonoCalibrator board = Checkerboard(self.pattern_size, self.square_size) detector = CheckerboardDetector(board) calibrator = MonoCalibrator(board, detector, self.folder, self.image_prefix) # run calibration (rms, camera_matrix, projection_matrix, dist_coeffs, (h, w), _, _) = calibrator.calibrate_monocular_camera(self.alpha) print "==> successfully calibrated, reprojection RMS (in pixels):", rms # create CalibrationData object with results camera_info = CalibrationData(self.camera_name, self.frame_id, w, h) camera_info.camera_matrix = camera_matrix camera_info.distortion_coefficients = dist_coeffs camera_info.projection_matrix = projection_matrix camera_info.dist_coeffs = dist_coeffs # save results camera_info.save_camera_yaml_file(self.output_file) print "==> saved results to:", self.output_file # verbose mode if self.verbose: print "--> results:" np.set_printoptions(suppress=1) print "camera matrix:\n", camera_matrix print "distortion coefficients:\n", dist_coeffs print "projection matrix:\n", projection_matrix
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