foundation/MetricCalibrationInclinationLaserPlane

"""
CVBpy Example Script for AQS12 Calibration - Use Case 2.
 
This example shows how to calibrate range maps acquired by a compact 3D 
sensor where the intrinsic calibration parameters are given, but the 
laser plane is not exactly vertical to the direction of movement.
 
See also use case 2 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 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, desired_accuracy):
    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 an affine transformation (correcting an inclined laser "
      "plane)")
 
# load range map of the calibration target AQS12
print("Loading range map and calibration file.")
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)
config.is_homography_calculated = False
 
# create (intrinsically) calibrated dense point cloud
calibrator_file = os.path.join(cvb.install_path(),
                               "tutorial", "Metric", "Images",
                               "SICalibration.json")
calibrator = cvb.Calibrator3D.load(calibrator_file)
calibrator.range_map_ignore_value = 0
cloud_intrinsic = cvb.PointCloudFactory.create_dense(
    range_map.planes[0], calibrator,
    cvb.PointCloudFlags.Float)
 
print("Dense point cloud created from range map and calibration file "
      f"with {cloud_intrinsic.num_points} points.")
 
# create AQS12 object with known reference coordinates of corner points
aqs12 = create_aqs12()
 
# create segmentor (segmenting AQS12 faces on dense point cloud)
segmentor = cvb.foundation.AQS12DensePointCloudSegmentor.create(
    cvb.foundation.SegmentationMethod.KmeansClustering)
 
# estimate calibration parameters
print("Estimating affine transformation.")
transformation_, residuals_, transformation_parameters_ = \
    cvb.foundation.calculate_correction_of_laser_plane_inclination_from_aqs12_piece(
        cloud_intrinsic, segmentor, config)
 
calibrator.correction_of_laser_plane_inclination = \
    transformation_, transformation_parameters_
 
# show results
if transformation_:
    print_trafo(transformation_)
 
if transformation_parameters_:
    print_trafo_parameters(transformation_parameters_)
 
# check residuals
desired_accuracy_ = 0.05  # mm
if residuals_:
    print_residuals(residuals_)
    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.")
        calibrator_cloud = cvb.PointCloudFactory.create(
            range_map.planes[0], calibrator,
            cvb.PointCloudFlags.Float | cvb.PointCloudFlags.XYZConfidence)
 
        # save calibrated point cloud
        if save:
            calibrator_cloud.save("cloud.ply")
    else:
        print(
            "Results do not have desired accuracy. Check face segmentation and "
            "extracted AQS12 points...")
 
        # segment AQS12 faces map
        print("Extracting AQ12 faces on intrinsically calibrated point cloud.")
        faces_aqs12 = segmentor.face_segmentation_from_piece(cloud_intrinsic)
 
        # 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 intrinsically calibrated "
              "point cloud.")
        points_aqs12 = segmentor.extract_projected_points_from_piece(
            cloud_intrinsic)
        print_aqs12_points(points_aqs12)