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/14.1/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 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.")
rangemap = cvb.Image(os.path.join(cvb.install_path(
), "tutorial", "Metric", "Images", "RangeMapCalibrationPattern.tif"))
 
# create (intrinsically) calibrated dense point cloud
calibrator = cvb.Calibrator3D.load(os.path.join(
    cvb.install_path(), "tutorial", "Metric", "Images", "SICalibration.json"))
calibrator.range_map_ignore_value = 0
cloud_intrinsic = cvb.PointCloudFactory.create_dense(
    rangemap.planes[0], calibrator, cvb.PointCloudFlags.Float)
 
# 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 a rigid-body transformation.")
result = cvb.foundation.calculate_rigid_body_transformation_from_aqs12_piece(
    cloud_intrinsic, segmentor, aqs12)
calibrator.extrinsic_matrix = result[0]
 
rotation = calibrator.extrinsic_matrix.matrix.rotation_angles
print("transformation results:")
print("rotation about (x/y/z): ",
      rotation[0], "/", rotation[1], "/", rotation[2], "degree")
print("translation in (x/y/z): ", calibrator.extrinsic_matrix.translation[0], "/",
      calibrator.extrinsic_matrix.translation[1], "/", calibrator.extrinsic_matrix.translation[2], "mm")
 
# create point cloud
print("Creating calibrated point cloud.")
cloud = cvb.PointCloudFactory.create(
    rangemap.planes[0], calibrator, cvb.PointCloudFlags.Float)
 
if save:
    cloud.save("cloud.ply")