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/14.1/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 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.")
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 calibration configuration object
config = cvb.foundation.CalibrationConfiguration.create(aqs12)
config.is_homography_calculated = False
config.is_correction_of_laser_plane_inclination_calculated = True
 
# 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.")
result = cvb.foundation.calculate_correction_of_laser_plane_inclination_from_aqs12_piece(
    cloud_intrinsic, segmentor, config)
calibrator.correction_of_laser_plane_inclination = result[0]
 
# check residuals
desired_accuracy = 0.05  # mm
if check_accuracy(result[1], desired_accuracy):
    print("The calibration was successful and accuracy is < ",
          desired_accuracy, "mm.")
 
    # create calibrated cloud
    print("Creating calibrated point cloud.")
    cloud = cvb.PointCloudFactory.create(
        rangemap.planes[0], calibrator, cvb.PointCloudFlags.Float)
 
    # 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...")
 
    # segement AQS12 facesmap
    print("Extracting AQ12 faces on intrinsically calibrated point cloud.")
    facesAqs12 = segmentor.face_segmentation_from_piece(cloud_intrinsic)
 
    # save image with segmentated faces:
    if save:
        facesAqs12.save("AQS12faces.bmp")
 
    # extract AQS12 points on range map (might take some time...)
    print("Extracting AQ12 corner points on intrinsically calibrated point cloud.")
    aqs12points = segmentor.extract_projected_points_from_piece(
        cloud_intrinsic)