foundation/MetricCalibrationRigidBodyTransformation

"""
CVBpy Example Script for AQS12 Calibration - Use Case 3.
 
This example shows how to bring an intrinsically calibrated point cloud to 
a world coordinate system via a rigid body transformation.
This workflow is recommended for area 3D compact sensors.
 
See also use case 3 described in the CVB Metric documentation:
https://help.commonvisionblox.com/NextGen/15.0/md_theory_of_operation_tools__metric.html#calibration_setup
 
The rigid body transformation solely inlcudes rotation and translation.
"""
 
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_extrinsic_matrix(matrix: cvb.AffineMatrix3D) -> None:
    rotation = matrix.matrix.rotation_angles
    print("Transformation results:")
    print("Rotation about x, y, and z in degrees:")
    print(f"{rotation[0]}, {rotation[1]}, {rotation[2]}")
    print("Translation about x, y, and z in millimeters:")
    print(f"{matrix.translation.x}, {matrix.translation.y}, "
          f"{matrix.translation.z}")
 
 
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)
 
 
# If you like to save the calibrated point cloud, turn this flag on:
save = False
 
print("Estimation of a rigid body transformation (rotation and translation)")
 
# load range map of the calibration target AQS12
print("Loading range map and calibration file.")
range_map_file = os.path.join(
    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 AQS12 object with known reference coordinates of corner points
aqs12 = create_aqs12()
 
# 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 | cvb.PointCloudFlags.XYZConfidence)
 
print("Dense point cloud created from range map and calibration file "
      f"with {cloud_intrinsic.num_points} points.")
 
# create segmentor (segmenting AQS12 faces on dense point cloud)
segmentor = cvb.foundation.AQS12DensePointCloudSegmentor.create(
    cvb.foundation.SegmentationMethod.KmeansClustering)
 
# estimate calibration parameters
print("Estimating a rigid-body transformation.")
transformation_, residuals_, transformation_parameters_ = \
    cvb.foundation.calculate_rigid_body_transformation_from_aqs12_piece(
        cloud_intrinsic, segmentor, aqs12)
calibrator.extrinsic_matrix = transformation_
 
# show results
if transformation_:
    print_trafo(transformation_)
 
if transformation_parameters_:
    print_trafo_parameters(transformation_parameters_)
 
if residuals_:
    print_residuals(residuals_)
 
print_extrinsic_matrix(calibrator.extrinsic_matrix)
 
# create calibrated point cloud
print("Creating calibrated point cloud.")
cloud = cvb.PointCloudFactory.create(
    range_map.planes[0], calibrator,
    cvb.PointCloudFlags.Float | cvb.PointCloudFlags.XYZConfidence)
 
if save:
    cloud.save("cloud.ply")