foundation/MetricCalibration

# @brief Example for **intrinsic and extrinsic calibration**
# of a laser triangulation system using the AQS12 target.
 
"""
CVBpy Example Script for AQS12 Calibration - Use Case 1.
 
This example shows how to calibrate range maps acquired by a modular laser 
triangulation setup (camera and laser separated), where the intrinsic calibration
parameters are not given.
 
See also use case 1 described in the CVB Metric documentation:
https://help.commonvisionblox.com/NextGen/15.0/md_theory_of_operation_tools__metric.html#calibration_setup
 
This example program estimates homography and an affine transformation.
The affine transformation:
- corrects errors induced by an incline laser plane
- corrects scaling in x, y, z
- moves the point cloud to the coordinate system given by the reference 
points of the AQS12
"""
 
import os
 
import cvb
import cvb.foundation
 
 
def print_trafo(trafo: cvb.AffineMatrix3D) -> None:
    print("Estimated transformation:")
    print("Translation:")
    print(f"[{trafo.translation.x}, {trafo.translation.y}, "
          f"{trafo.translation.z}]")
    print("Transformation matrix:")
    print(f"[[{trafo.matrix.at(0, 0)}, "
          f"{trafo.matrix.at(0, 1)}, "
          f"{trafo.matrix.at(0, 2)}],")
    print(f"[{trafo.matrix.at(1, 0)}, "
          f"{trafo.matrix.at(1, 1)}, "
          f"{trafo.matrix.at(1, 2)}],")
    print(f"[{trafo.matrix.at(2, 0)}, "
          f"{trafo.matrix.at(2, 1)}, "
          f"{trafo.matrix.at(2, 2)}]]")
 
 
def print_trafo_parameters(atp: cvb.AffineTransformationParameters) -> None:
    print("Rotation angles about X, Y, Z axis in degrees:")
    print(f"{atp.rotation_angles.x}, {atp.rotation_angles.y}, "
          f"{atp.rotation_angles.z}")
    print("Shear Syx, Syz:")
    print(f"{atp.s_yx}, {atp.s_yz}")
    print("Inclination of laser plane about X, Z axis in degrees:")
    print(f"{atp.inclination_x}, {atp.inclination_z}")
    print("Scale in X, Y, Z:")
    print(f"{atp.scale.x}, {atp.scale.y}, {atp.scale.z}")
 
 
def print_point_3d_list(points: list[cvb.Point3D]) -> None:
    data_list = list()
    for p in points:
        data_list.append(f"[{p.x}, {p.y}, {p.z}]")
    delimiter = ",\n"
    print(f"[{delimiter.join(data_list)}]")
 
 
def print_residuals(points: list[cvb.Point3D]) -> None:
    print("Residuals:")
    print_point_3d_list(points)
 
 
def print_aqs12_points(points: list[cvb.Point3D]) -> None:
    print("AQS12 points:")
    print_point_3d_list(points)
 
 
def create_aqs12():
    # list of known point coordinates of the AQS12
    points = [
        cvb.Point3D(20.0018, 44.9941, 15.0000),
        cvb.Point3D(24.0018, 39.9942, 14.9994),
        cvb.Point3D(23.9994, 24.9972, 15.0001),
        cvb.Point3D(20.0021, 20.0035, 15.0011),
        cvb.Point3D(15.9994, 25.0079, 15.0016),
        cvb.Point3D(16.0000, 39.9919, 15.0010),
        cvb.Point3D(20.0095, 59.9985,  4.9902),
        cvb.Point3D(32.0093, 44.9958,  4.9909),
        cvb.Point3D(32.0052, 19.9925,  4.9920),
        cvb.Point3D(20.0021,  4.9961,  4.9939),
        cvb.Point3D(8.0024, 19.9980,  5.0009),
        cvb.Point3D(8.0065, 45.0009,  4.9984)]
    return cvb.foundation.AQS12Piece(points, 0)
 
 
def check_accuracy(residuals: list[cvb.Point3D], desired_accuracy: float):
    for point in residuals:
        if (abs(point.x) > desired_accuracy or
                abs(point.y) > desired_accuracy or
                abs(point.z) > desired_accuracy):
            return False
    return True
 
 
# If you like to save intermediate and final results, turn this flag on:
save = False
 
print("Estimation of homography and affine transformation (correcting an "
      "inclined laser plane)")
 
# load range map of the calibration target AQS12
print("Loading range map.")
range_map_file = os.path.join(cvb.install_path(),
                              "tutorial", "Metric", "Images",
                              "RangeMapCalibrationPattern.tif")
range_map = cvb.Image(range_map_file)
 
print(f"Range map loaded with size of {range_map.width} x {range_map.height} "
      f"from {range_map_file}.")
 
# create calibration configuration object
aqs12 = create_aqs12()
config = cvb.foundation.CalibrationConfiguration.create(aqs12)
 
# create AQS12 segmentor for the range maps
print("Estimating homography and affine matrix.")
segmentor = cvb.foundation.AQS12RangeMapSegmentor.create(
    cvb.foundation.SegmentationMethod.KmeansClustering)
 
# estimate calibration parameters
calibrator_, residuals_ = cvb.foundation.create_calibrator_from_aqs12_piece(
    range_map.planes[0], segmentor, config)
 
transformation_, transformation_parameters_ = \
    calibrator_.correction_of_laser_plane_inclination
 
# show result
if transformation_:
    print_trafo(transformation_)
 
print_residuals(residuals_)
 
if transformation_parameters_:
    print_trafo_parameters(transformation_parameters_)
 
# check residuals
desired_accuracy_ = 0.05  # mm
if check_accuracy(residuals_, desired_accuracy_):
    print("The calibration was successful and accuracy is < "
          f"{desired_accuracy_} mm.")
 
    # create calibrated cloud
    print("Creating calibrated point cloud.")
    cloud = cvb.PointCloudFactory.create(
        range_map.planes[0], calibrator_,
        cvb.PointCloudFlags.Float | cvb.PointCloudFlags.XYZConfidence)
 
    # save calibrated point cloud
    if save:
        cloud.save("cloud.ply")
else:
    print("Results do not have desired accuracy. Check face segmentation and "
          "extracted AQS12 points...")
 
    # segment AQS12 faces
    print("Segmenting AQ12 faces on range map.")
    faces_aqs12 = segmentor.face_segmentation_from_piece(range_map.planes[0])
 
    # save image with segmented faces:
    if save:
        faces_aqs12.save("AQS12faces.bmp")
 
    # extract AQS12 points on range map (might take some time...)
    print("Extracting AQ12 corner points on range map.")
    points_aqs12 = segmentor.extract_projected_points_from_piece(
        range_map.planes[0])
    print_aqs12_points(points_aqs12)