Foundation/Cvb++/CppMetricCalibrationInclinationLaserPlane

This example program is located in your CVB installation under %CVB%Tutorial/Foundation/Cvb++/CppMetricCalibrationInclinationLaserPlane.

main.cpp:

// Example for **extrinsic calibration**, including the **correction of an inclined laser plane**, in a
// laser triangulation system using the AQS12 target.
 
// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------
 
#include <iostream>
 
#include "cvb/calibrator_3d.hpp"
#include "cvb/point_cloud_factory.hpp"
 
#include "cvb/foundation/metric_aqs12.hpp"
 
Cvb::Foundation::Metric::AQS12::AQS12Piece GetAqs12();
void PrintTrafo(const Cvb::AffineMatrix3D &transformation);
void PrintTrafo(
    const Cvb::AffineTransformationParameters &transformationParameters);
void PrintResiduals(const std::vector<Cvb::Point3D<double>> &residuals);
void PrintAqs12Points(const std::vector<Cvb::Point3D<double>> &points);
bool CheckAccuracy(const std::vector<Cvb::Point3D<double>> &residuals,
                   double desiredAccuracy);
 
int main() {
  try {
    std::cout << "Estimation of an affine transformation (correcting an "
                 "inclined laser plane)\n\n";
 
    // load range map of AQS12
    const auto rangemapFile =
        Cvb::InstallPath() +
        CVB_LIT("tutorial/Metric/Images/RangeMapCalibrationPattern.tif");
    Cvb::ImagePtr rangemap = Cvb::Image::Load(rangemapFile);
    std::cout << "Rangemap loaded with size of " << rangemap->Width() << " x "
              << rangemap->Height() << " from " << rangemapFile << ".\n";
 
    // create calibration configuration object
    auto aqs12 = GetAqs12();
    auto config =
        Cvb::Foundation::Metric::CalibrationConfiguration::Create(aqs12);
    config->SetHomographyCalculated(false);
 
    // create intrinsically calibrated dense point cloud
    const auto calibrationFile =
        Cvb::InstallPath() +
        CVB_LIT(
            "tutorial/Metric/Images/SICalibration.json"); // calibration file
                                                          // with homography and
                                                          // scaling
    auto calibrator =
        Cvb::Calibrator3D::Load<Cvb::LaserPlaneCalibrator3D>(calibrationFile);
    calibrator->SetRangeMapIgnoreValue(0.0);
    auto cloud = Cvb::PointCloudFactory::Create<Cvb::DensePointCloud>(
        rangemap->Plane(0), *calibrator,
        Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
    std::cout
        << "Dense point cloud created from rangemap and calibration file with "
        << cloud->NumPoints() << " points.\n\n";
 
    // create AQS12 segmentor for dense point clouds
    auto segmentor =
        Cvb::Foundation::Metric::AQS12DensePointCloudSegmentor::Create(
            Cvb::Foundation::Metric::SegmentationMethod::KmeansClustering);
 
    // estimate calibration parameters
    auto [transformation, residuals, transformationParameters] =
        Cvb::Foundation::Metric::
            CalculateCorrectionOfLaserPlaneInclinationFromAqs12Piece(
                *cloud, *segmentor, *config);
    calibrator->SetCorrectionOfLaserPlaneInclination(transformation,
                                                     transformationParameters);
 
    // show results
    PrintTrafo(transformation);
    if (transformationParameters)
      PrintTrafo(*transformationParameters);
 
    // check intermediate results if residuals are not ok
    double desiredAccuracy = 0.05;
    if (residuals) {
      PrintResiduals(*residuals);
      if (!CheckAccuracy(*residuals, desiredAccuracy)) {
        std::cout << "Results do not have desired accuracy. Check face "
                     "segmentation and extracted AQS12 points...\n";
        auto facesAqs12 = segmentor->FaceSegmentationFromPiece(*cloud);
        auto pointsAqs12 = segmentor->ExtractProjectedPointsFromPiece(*cloud);
        PrintAqs12Points(pointsAqs12);
      } else {
        // create calibrated cloud
        auto calibratedCloud = Cvb::PointCloudFactory::Create(
            rangemap->Plane(0), *calibrator,
            Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
        std::cout << "The calibration was successful and accuracy is < "
                  << desiredAccuracy << " mm. :)\n";
      }
    }
 
  } catch (const std::exception &error) {
    std::cout << error.what() << std::endl;
  }
 
  return 0;
}
 
Cvb::Foundation::Metric::AQS12::AQS12Piece GetAqs12() {
  // reference points in mm in a right-handed coordinate system
  std::array<Cvb::Point3D<double>, 12> points = {{
      {20.0018, 44.9941, 15.0000},
      {24.0018, 39.9942, 14.9994},
      {23.9994, 24.9972, 15.0001},
      {20.0021, 20.0035, 15.0011},
      {15.9994, 25.0079, 15.0016},
      {16.0000, 39.9919, 15.0010},
      {20.0095, 59.9985, 4.9902},
      {32.0093, 44.9958, 4.9909},
      {32.0052, 19.9925, 4.9920},
      {20.0021, 4.9961, 4.9939},
      {8.0024, 19.9980, 5.0009},
      {8.0065, 45.0009, 4.9984},
  }};
 
  return Cvb::Foundation::Metric::AQS12::AQS12Piece(points, 0);
}
 
bool CheckAccuracy(const std::vector<Cvb::Point3D<double>> &residuals,
                   double desiredAccuracy) {
  for (auto residual : residuals) {
    if (std::abs(residual.X()) > desiredAccuracy ||
        std::abs(residual.Y()) > desiredAccuracy ||
        std::abs(residual.Z()) > desiredAccuracy) {
      return false;
    }
  }
  return true;
}
 
void PrintTrafo(const Cvb::AffineMatrix3D &transformation) {
  std::cout << "Estimated transformation:\n";
  std::cout << "translation: [";
  std::cout << transformation.Translation().X() << ", "
            << transformation.Translation().Y() << ", "
            << transformation.Translation().Z() << "]\n";
 
  std::cout << "transformation matrix: [\n";
  std::cout << transformation.Matrix()[0][0] << ", "
            << transformation.Matrix()[0][1] << ", "
            << transformation.Matrix()[0][2] << ",\n";
  std::cout << transformation.Matrix()[1][0] << ", "
            << transformation.Matrix()[1][1] << ", "
            << transformation.Matrix()[1][2] << ",\n";
  std::cout << transformation.Matrix()[2][0] << ", "
            << transformation.Matrix()[2][1] << ", "
            << transformation.Matrix()[2][2] << "]\n\n";
}
 
void PrintTrafo(
    const Cvb::AffineTransformationParameters &transformationParameters) {
  std::cout << "Rotation angles about X,Y,Z axis in [degree]: "
            << transformationParameters.RotationAngles[0] << ", "
            << transformationParameters.RotationAngles[1] << ", "
            << transformationParameters.RotationAngles[2] << "\n";
  std::cout << "Shear Syx, Syz: " << transformationParameters.Syx << ", "
            << transformationParameters.Syz << "\n";
  std::cout << "Inclination of laser plane about X,Z axis in [degree]: "
            << transformationParameters.InclinationX << ", "
            << transformationParameters.InclinationZ << "\n";
  std::cout << "Scale in X,Y,Z: " << transformationParameters.Scale.X << ", "
            << transformationParameters.Scale.Y << ", "
            << transformationParameters.Scale.Z << "\n\n";
}
 
void PrintResiduals(const std::vector<Cvb::Point3D<double>> &residuals) {
  std::cout << "Residuals:\n";
  for (auto residual : residuals) {
    std::cout << residual.X() << " " << residual.Y() << " " << residual.Z()
              << "\n";
  }
  std::cout << "\n";
}
 
void PrintAqs12Points(const std::vector<Cvb::Point3D<double>> &points) {
  std::cout << "AQS12 points\n";
  for (auto point : points) {
    std::cout << point.X() << " " << point.Y() << " " << point.Z() << "\n";
  }
  std::cout << "\n";
}