def calibrate_intrinsic_parameters(calibration_data, calibration_results_file):
    """Calibrate intrinsic parameters of the camera given different images
    taken for the Charuco board from different views, the resulting parameters
    are saved to the provided filename.

    Args:
        calibration_data (str):  directory of the stored images of the
        Charuco board.
        calibration_results_file (str):  filepath that will be used to write
        the calibration results in.
    """
    handler = CharucoBoardHandler()

    camera_matrix, dist_coeffs, error = handler.calibrate(
        calibration_data, visualize=False
    )
    camera_info = dict()
    camera_info["camera_matrix"] = dict()
    camera_info["camera_matrix"]["rows"] = 3
    camera_info["camera_matrix"]["cols"] = 3
    camera_info["camera_matrix"]["data"] = camera_matrix.flatten().tolist()
    camera_info["distortion_coefficients"] = dict()
    camera_info["distortion_coefficients"]["rows"] = 1
    camera_info["distortion_coefficients"]["cols"] = 5
    camera_info["distortion_coefficients"][
        "data"
    ] = dist_coeffs.flatten().tolist()

    with open(calibration_results_file, "w") as outfile:
        yaml.dump(
            camera_info, outfile, default_flow_style=False,
        )
    return
def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument(
        "-i",
        type=str,
        required=True,
        help="""Path to the image in which the points are visualized.""",
    )
    parser.add_argument(
        "-c",
        type=str,
        required=True,
        help="""Path to the camera calibration file.""",
    )
    parser.add_argument(
        "--detect-board-position",
        action="store_true",
        help="""Detect the board position from the image instead of using the
            pose from the calibration file.""",
    )
    args = parser.parse_args()

    camera_info = CameraCalibrationFile(args.c)
    camera_matrix = camera_info["camera_matrix"]
    distortion_coeffs = camera_info["distortion_coefficients"]
    handler = CharucoBoardHandler(
        BOARD_SIZE_X,
        BOARD_SIZE_Y,
        BOARD_SQUARE_SIZE,
        BOARD_MARKER_SIZE,
        camera_matrix,
        distortion_coeffs,
    )

    points = np.vstack([
        x.flatten() for x in np.meshgrid(
            np.arange(-0.3, 0.4, 0.1),
            np.arange(-0.3, 0.4, 0.1),
            [0.0],
        )
    ]).T

    image = cv2.imread(args.i)

    if args.detect_board_position:
        _, _, rvec, tvec = handler.detect_board(image)

        board_offset = np.array([0.1, 0.14, 0])
        points = points + board_offset
    else:
        # get rvec and tvec from projection matrix
        pose_mat = camera_info["projection_matrix"]
        tvec = pose_mat[0:3, 3]
        rvec = cv2.Rodrigues(pose_mat[:3, :3])[0]

    imgpoints, _ = cv2.projectPoints(
        points,
        rvec,
        tvec,
        camera_matrix,
        distortion_coeffs,
    )

    for imgpoint in imgpoints:
        cv2.drawMarker(image, tuple(imgpoint[0].astype(int)),
                       tuple([0, 0, 255]))

    cv2.imshow("foo", image)
    cv2.waitKey()
def calibrate_extrinsic_parameters(
    calibration_results_file,
    charuco_centralized_image_filename,
    extrinsic_calibration_filename,
    impose_cube=True,
):
    """Calibrate extrinsic parameters of the camera given one image taken for
    the Charuco board centered at (0, 0, 0) the resulting parameters are
    saved to the provided filename and a virtual cube is imposed on the
    board for verification.

    Args:
        calibration_results_file (str):  filepath that will be used to read
        the intrinsic calibration results.
        charuco_centralized_image_filename (str): filename of the image
        taken for the Charuco board centered at (0, 0, 0).
        extrinsic_calibration_filename (str):  filepath that will be used
        to write the extrinsic calibration results in.
        impose_cube (bool): boolean whether to output a virtual cube
        imposed on the first square of the board or not.
    """
    with open(calibration_results_file) as file:
        calibration_data = yaml.safe_load(file)

    def config_matrix(data):
        return np.array(data["data"]).reshape(data["rows"], data["cols"])

    camera_matrix = config_matrix(calibration_data["camera_matrix"])
    dist_coeffs = config_matrix(calibration_data["distortion_coefficients"])

    handler = CharucoBoardHandler(
        BOARD_SIZE_X,
        BOARD_SIZE_Y,
        BOARD_SQUARE_SIZE,
        BOARD_MARKER_SIZE,
        camera_matrix,
        dist_coeffs,
    )

    rvec, tvec = handler.detect_board_in_image(
        charuco_centralized_image_filename, visualize=False)

    # projection_matrix = np.zeros((4, 4))
    projection_matrix = utils.rodrigues_to_matrix(rvec)
    projection_matrix[0:3, 3] = tvec[:, 0]
    projection_matrix[3, 3] = 1

    calibration_data["projection_matrix"] = dict()
    calibration_data["projection_matrix"]["rows"] = 4
    calibration_data["projection_matrix"]["cols"] = 4
    calibration_data["projection_matrix"]["data"] = projection_matrix.flatten(
    ).tolist()

    with open(extrinsic_calibration_filename, "w") as outfile:
        yaml.dump(
            calibration_data,
            outfile,
            default_flow_style=False,
        )

    if impose_cube:
        new_object_points = (np.array(
            [
                [0, 0, 0],
                [0, 1, 0],
                [1, 0, 0],
                [1, 1, 0],
                [0, 0, 1],
                [0, 1, 1],
                [1, 0, 1],
                [1, 1, 1],
            ],
            dtype=np.float32,
        ) * 0.04)

        world_origin_points = (np.array(
            [
                [0, 0, 0],
                [0, 1, 0],
                [1, 0, 0],
                [0, 0, 1],
            ],
            dtype=np.float32,
        ) * 0.1)

        # cube
        point_pairs = (
            (0, 4),
            (1, 5),
            (2, 6),
            (3, 7),
            (0, 1),
            (0, 2),
            (1, 3),
            (2, 3),
            (4, 5),
            (4, 6),
            (5, 7),
            (6, 7),
        )

        img = cv2.imread(charuco_centralized_image_filename)
        imgpoints, _ = cv2.projectPoints(
            new_object_points,
            rvec,
            tvec,
            camera_matrix,
            dist_coeffs,
        )

        for p1, p2 in point_pairs:
            cv2.line(
                img,
                tuple(imgpoints[p1, 0]),
                tuple(imgpoints[p2, 0]),
                [200, 200, 0],
                thickness=2,
            )

        cv2.imshow("Imposed Cube", img)
        cv2.waitKey(0)
        cv2.destroyAllWindows()
def calibrate_mean_extrinsic_parameters(
    camera_matrix,
    dist_coeffs,
    charuco_centralized_image_dir,
    extrinsic_calibration_filename,
    impose_cube=True,
):
    """Calibrate extrinsic parameters of the camera given several imaeges taken for
    the Charuco board centered at (0, 0, 0). transform the extrinsic parameters into the
    fixed 'world' coordinate system.
    the resulting parameters are averaged for all images and saved to the provided filename.
    a virtual cube on the board as well as the world coordinates axes are imposed for verification.

    Args:
        camera_matrix, dist_coeffs:  output of the intrinsic calibration (either read from file or directly obtained from
        intrinsic calibration function.
        charuco_centralized_image_dir (str): directory containing images
        taken for the Charuco board centered at (0, 0, 0).
        extrinsic_calibration_filename (str):  filepath that will be used
        to write the extrinsic calibration results in.
        impose_cube (bool): boolean whether to output a virtual cube
        imposed on the first square of the board or not.
    """

    handler = CharucoBoardHandler(
        BOARD_SIZE_X,
        BOARD_SIZE_Y,
        BOARD_SQUARE_SIZE,
        BOARD_MARKER_SIZE,
        camera_matrix,
        dist_coeffs,
    )

    file_pattern = "*.png"
    pattern = os.path.join(charuco_centralized_image_dir, file_pattern)

    ind = 0
    projection_matrix = np.zeros((len(glob.glob(pattern)), 4, 4))

    for filename in glob.glob(pattern):
        img = cv2.imread(filename)

        rvec, tvec = handler.detect_board_in_image(filename, visualize=False)

        # geometric data of the calibration board with respect to the 'world' coordinates

        # inclination angle of the board (CAD data)
        alpha = 22
        alphr = np.radians(alpha)
        # half-width of the calibration board (CAD data)
        Dx = 0.105
        # projected (on the base) half-height of the calibration board (CAD data)
        Dy = 0.16054

        # thickness of the base-plate (measured)
        T1 = 0.00435
        # thickness of the calibration board plate (measured)
        T2 = 0.0045
        # y-distance of the pattern axis from the plate edge (measured)
        dy = 0.0378
        # x-distance of the pattern axis from the plate edge (measured)
        dx = 0.0047

        # x-distance of the pattern origin to the world origin
        tx = Dx - dx
        # y-distance of the pattern origin to the world origin
        ty = (Dy - T2 * np.sin(alphr) -
              T1 * np.tan(alphr)) * np.cos(alphr) - dy
        # z-distance of the pattern origin to the world origin
        tz = (T2 +
              (Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.sin(alphr) +
              T1 / np.cos(alphr))
        # resulting translation vector
        Tvec = np.array([tx, ty, -tz], dtype="float32")

        # rotation around the x-axis (pattern coordinate system)
        xrot = np.array([1, 0, 0]) * np.radians(-alpha)
        xMat = cv2.Rodrigues(xrot)[0]

        # (really dirty method to get) rotation angle of the calibration board
        zrot = int(filename[-6:-4])
        zrot = (zrot - 1) * 10
        zrot = np.radians(zrot)
        zrot = np.matmul(xMat, np.array([0, 0, 1])) * zrot
        zMat = cv2.Rodrigues(zrot)[0]

        # absolute world vectors equivalent to cv2 tvec and rvec:
        tvecW = np.matmul(cv2.Rodrigues(rvec)[0], Tvec) + tvec.T
        rvecW = cv2.Rodrigues(
            np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)))[0]
        #        embed()

        projection_matrix[ind, 0:4, 0:4] = utils.rodrigues_to_matrix(rvecW)
        projection_matrix[ind, 0:3, 3] = tvecW
        projection_matrix[ind, 3, 3] = 1

        ind += 1

        if impose_cube:
            new_object_points = (np.array(
                [
                    [0, 0, 0],
                    [0, 1, 0],
                    [1, 0, 0],
                    [1, 1, 0],
                    [0, 0, 1],
                    [0, 1, 1],
                    [1, 0, 1],
                    [1, 1, 1],
                ],
                dtype=np.float32,
            ) * 0.04)

            world_origin_points = (np.array(
                [
                    [0, 0, 0],
                    [0, 1, 0],
                    [1, 0, 0],
                    [0, 0, 1],
                ],
                dtype=np.float32,
            ) * 0.1)

            world_origin_points = (zMat @ xMat @ world_origin_points.T).T
            world_origin_points = world_origin_points + Tvec

            # cube
            point_pairs = (
                (0, 4),
                (1, 5),
                (2, 6),
                (3, 7),
                (0, 1),
                (0, 2),
                (1, 3),
                (2, 3),
                (4, 5),
                (4, 6),
                (5, 7),
                (6, 7),
            )

            img = cv2.imread(filename)
            imgpoints, _ = cv2.projectPoints(
                new_object_points,
                rvec,
                tvec,
                camera_matrix,
                dist_coeffs,
            )

            for p1, p2 in point_pairs:
                cv2.line(
                    img,
                    tuple(imgpoints[p1, 0]),
                    tuple(imgpoints[p2, 0]),
                    [200, 200, 0],
                    thickness=2,
                )

            # world origin
            point_pairs = (
                (0, 1),
                (0, 2),
                (0, 3),
            )

            imgpoints, _ = cv2.projectPoints(
                world_origin_points,
                rvec,
                tvec,
                camera_matrix,
                dist_coeffs,
            )

            for p1, p2 in point_pairs:
                cv2.line(
                    img,
                    tuple(imgpoints[p1, 0]),
                    tuple(imgpoints[p2, 0]),
                    [200, 200, 0],
                    thickness=2,
                )

            cv2.imshow("Imposed Cube", img)
            cv2.waitKey(100)

    cv2.destroyAllWindows()

    projection_matrix_std = np.std(projection_matrix, axis=0)
    projection_matrix = np.mean(projection_matrix, axis=0)

    print("Mean proj matrix:")
    print(projection_matrix)
    print("Std proj matrix:")
    print(projection_matrix_std)
    print("Rel std proj matrix:")
    print(projection_matrix_std / projection_matrix)

    # save all the data
    calibration_data = dict()
    calibration_data["camera_matrix"] = dict()
    calibration_data["camera_matrix"]["rows"] = 3
    calibration_data["camera_matrix"]["cols"] = 3
    calibration_data["camera_matrix"]["data"] = camera_matrix.flatten().tolist(
    )
    calibration_data["distortion_coefficients"] = dict()
    calibration_data["distortion_coefficients"]["rows"] = 1
    calibration_data["distortion_coefficients"]["cols"] = 5
    calibration_data["distortion_coefficients"]["data"] = dist_coeffs.flatten(
    ).tolist()

    calibration_data["projection_matrix"] = dict()
    calibration_data["projection_matrix"]["rows"] = 4
    calibration_data["projection_matrix"]["cols"] = 4
    calibration_data["projection_matrix"]["data"] = projection_matrix.flatten(
    ).tolist()

    calibration_data["projection_matrix_std"] = dict()
    calibration_data["projection_matrix_std"]["rows"] = 4
    calibration_data["projection_matrix_std"]["cols"] = 4
    calibration_data["projection_matrix_std"][
        "data"] = projection_matrix_std.flatten().tolist()

    with open(extrinsic_calibration_filename, "w") as outfile:
        yaml.dump(
            calibration_data,
            outfile,
            default_flow_style=False,
        )
Exemplo n.º 5
0
def calibrate_mean_extrinsic_parameters(
    camera_matrix,
    dist_coeffs,
    image_files,
    impose_cube=True,
):
    """Calibrate extrinsic parameters of the camera.

    Calibrate extrinsic parameters given several images taken of the Charuco
    board at defined poses.  Transform the extrinsic parameters into the fixed
    'world' coordinate system.  The resulting parameters are averaged for all
    images and saved to the provided filename.

    Args:
        camera_matrix, dist_coeffs:  output of the intrinsic calibration
            (either read from file or directly obtained from
        intrinsic calibration function.
        image_files (list): list of image files.
        taken for the Charuco board centered at (0, 0, 0).
        to write the extrinsic calibration results in.
        impose_cube (bool): boolean whether to output a virtual cube
        imposed on the first square of the board or not.

    Returns:
        The camera parameters including the camera pose.
    """

    handler = CharucoBoardHandler(
        BOARD_SIZE_X,
        BOARD_SIZE_Y,
        BOARD_SQUARE_SIZE,
        BOARD_MARKER_SIZE,
        camera_matrix,
        dist_coeffs,
    )

    camera_params = CameraParameters()

    ind = 0
    pose_matrix = np.zeros((len(image_files), 4, 4))

    for i, filename in enumerate(image_files):
        # verify that images are given in the expected order
        assert "{:04d}".format(i + 1) in filename

        img = cv2.imread(filename)

        camera_params.image_height = img.shape[0]
        camera_params.image_width = img.shape[1]

        rvec, tvec = handler.detect_board_in_image(filename, visualize=False)

        # geometric data of the calibration board with respect to the 'world'
        # coordinates

        # inclination angle of the board (CAD data)
        alpha = 22
        alphr = np.radians(alpha)
        # half-width of the calibration board (CAD data)
        Dx = 0.105
        # projected (on the base) half-height of the calibration board (CAD
        # data)
        Dy = 0.16054

        # thickness of the base-plate (measured)
        T1 = 0.00435
        # thickness of the calibration board plate (measured)
        T2 = 0.0045
        # y-distance of the pattern axis from the plate edge (measured)
        dy = 0.0378
        # x-distance of the pattern axis from the plate edge (measured)
        dx = 0.0047

        # x-distance of the pattern origin to the world origin
        tx = Dx - dx
        # y-distance of the pattern origin to the world origin
        ty = (Dy - T2 * np.sin(alphr) -
              T1 * np.tan(alphr)) * np.cos(alphr) - dy
        # z-distance of the pattern origin to the world origin
        tz = (T2 +
              (Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.sin(alphr) +
              T1 / np.cos(alphr))
        # resulting translation vector
        Tvec = np.array([tx, ty, -tz], dtype="float32")

        # rotation around the x-axis (pattern coordinate system)
        xrot = np.array([1, 0, 0]) * np.radians(-alpha)
        xMat = cv2.Rodrigues(xrot)[0]

        # get rotation angle of the calibration board
        zrot = i + 1
        zrot = (zrot - 1) * 10
        zrot = np.radians(zrot)
        zrot = np.matmul(xMat, np.array([0, 0, 1])) * zrot
        zMat = cv2.Rodrigues(zrot)[0]

        # absolute world vectors equivalent to cv2 tvec and rvec:
        tvecW = np.matmul(cv2.Rodrigues(rvec)[0], Tvec) + tvec.T
        rvecW = cv2.Rodrigues(
            np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)))[0]
        #        embed()

        pose_matrix[ind, 0:4, 0:4] = utils.rodrigues_to_matrix(rvecW)
        pose_matrix[ind, 0:3, 3] = tvecW
        pose_matrix[ind, 3, 3] = 1

        ind += 1

        if impose_cube:
            new_object_points = (np.array(
                [
                    [0, 0, 0],
                    [0, 1, 0],
                    [1, 0, 0],
                    [1, 1, 0],
                    [0, 0, 1],
                    [0, 1, 1],
                    [1, 0, 1],
                    [1, 1, 1],
                ],
                dtype=np.float32,
            ) * 0.04)

            world_origin_points = (np.array(
                [
                    [0, 0, 0],
                    [0, 1, 0],
                    [1, 0, 0],
                    [0, 0, 1],
                ],
                dtype=np.float32,
            ) * 0.1)

            world_origin_points = (zMat @ xMat @ world_origin_points.T).T
            world_origin_points = world_origin_points + Tvec

            # cube
            point_pairs = (
                (0, 4),
                (1, 5),
                (2, 6),
                (3, 7),
                (0, 1),
                (0, 2),
                (1, 3),
                (2, 3),
                (4, 5),
                (4, 6),
                (5, 7),
                (6, 7),
            )

            img = cv2.imread(filename)
            imgpoints, _ = cv2.projectPoints(
                new_object_points,
                rvec,
                tvec,
                camera_matrix,
                dist_coeffs,
            )

            for p1, p2 in point_pairs:
                cv2.line(
                    img,
                    tuple(imgpoints[p1, 0]),
                    tuple(imgpoints[p2, 0]),
                    [200, 200, 0],
                    thickness=2,
                )

            # world origin
            point_pairs = (
                (0, 1),
                (0, 2),
                (0, 3),
            )

            imgpoints, _ = cv2.projectPoints(
                world_origin_points,
                rvec,
                tvec,
                camera_matrix,
                dist_coeffs,
            )

            for p1, p2 in point_pairs:
                cv2.line(
                    img,
                    tuple(imgpoints[p1, 0].astype(int)),
                    tuple(imgpoints[p2, 0].astype(int)),
                    [200, 200, 0],
                    thickness=2,
                )

            cv2.imshow("Imposed Cube", img)
            cv2.waitKey(100)

    cv2.destroyAllWindows()

    camera_params.camera_matrix = camera_matrix
    camera_params.dist_coeffs = dist_coeffs
    camera_params.tf_world_to_camera_std = np.std(pose_matrix, axis=0)
    camera_params.tf_world_to_camera = np.mean(pose_matrix, axis=0)

    print("Mean proj matrix:")
    print(camera_params.tf_world_to_camera)
    print("Std proj matrix:")
    print(camera_params.tf_world_to_camera_std)
    print("Rel std proj matrix:")
    print(camera_params.tf_world_to_camera_std /
          camera_params.tf_world_to_camera)

    return camera_params
Exemplo n.º 6
0
def main():
    """Execute an action depending on arguments passed by the user."""
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument(
        "action",
        choices=["create_board", "detect_live", "detect_image", "calibrate"],
        help="""Action that is executed.""",
    )
    parser.add_argument(
        "--filename",
        type=str,
        help="""Filename used for saving or loading images (depending on the
            action).
        """,
    )
    parser.add_argument(
        "--calibration-data",
        type=str,
        help="""Path to the calibration data directory (only used for action
            'calibrate').
        """,
    )
    parser.add_argument(
        "--camera-info",
        type=str,
        help="""Camera info file, including intrinsic parameters.""",
    )
    parser.add_argument(
        "--no-gui",
        action="store_true",
        help="""Set to disable any GUI-based visualization.""",
    )
    args = parser.parse_args()

    camera_matrix = None
    distortion_coeffs = None
    if args.camera_info:
        camera_info = CameraCalibrationFile(args.camera_info)
        camera_matrix = camera_info["camera_matrix"]
        distortion_coeffs = camera_info["distortion_coefficients"]

    handler = CharucoBoardHandler(
        BOARD_SIZE_X,
        BOARD_SIZE_Y,
        BOARD_SQUARE_SIZE,
        BOARD_MARKER_SIZE,
        camera_matrix,
        distortion_coeffs,
    )

    if args.action == "create_board":
        if not args.filename:
            raise RuntimeError("Filename not specified.")
        handler.save_board(args.filename)
    elif args.action == "detect_live":
        handler.detect_board_in_camera_stream()
    elif args.action == "detect_image":
        if not args.filename:
            raise RuntimeError("Filename not specified.")
        handler.detect_board_in_image(args.filename, visualize=not args.no_gui)
    elif args.action == "calibrate":
        pattern = os.path.join(args.calibration_data, args.filename)
        files = glob.glob(pattern)
        handler.calibrate(
            files,
            visualize=not args.no_gui,
        )