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
Beispiel #2
0
    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