Foundation/CppMetricCalibrationInclinationLaserPlane

// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------


#include <iostream>

#include "cvb/point_cloud_factory.hpp"
#include "cvb/calibrator_3d.hpp"

#include "cvb/foundation/metric_aqs12.hpp"


Cvb::Foundation::Metric::AQS12::AQS12Piece GetAqs12(); 
void PrintTrafo(const Cvb::AffineMatrix3D& trafo);
void PrintResiduals(const std::array<Cvb::Point3D<double>, 12>& residuals);
void PrintAqs12Points(const std::vector< Cvb::Point3D<double>>& points);
bool CheckAccuracy(const std::array<Cvb::Point3D<double>, 12>& 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 calibrated (dense) point cloud
    const auto calibrationFile = Cvb::InstallPath() + CVB_LIT("tutorial/Metric/Images/SICalibration.json");
    auto calibrator = Cvb::Calibrator3D::Load<Cvb::Calibrator3D>(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
    Cvb::AffineMatrix3D affineTrafo;
    std::array<Cvb::Point3D<double>, 12> residuals;
    std::tie(affineTrafo,residuals) = Cvb::Foundation::Metric::CalculateCorrectionOfLaserPlaneInclinationFromAqs12Piece(cloud, *segmentor, *config);
    calibrator->SetCorrectionOfLaserPlaneInclination(affineTrafo);      

    // show results
    PrintTrafo(affineTrafo);
    PrintResiduals(residuals);
        
    // check intermediate results if residuals are not ok
    double desiredAccuracy = 0.05;
    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<Cvb::DensePointCloud>(rangemap->Plane(0), *calibrator, Cvb::PointCloudFlags::Float | Cvb::PointCloudFlags::XYZConfidence);
      std::cout << "The calibration was sucessful and accuracy is < " << desiredAccuracy << " mm. :)\n";
      std::cout << "Lattice size of calibrated cloud: " << calibratedCloud->LatticeSize().Width() << " x " << calibratedCloud->LatticeSize().Height() << "\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 coordindate 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::array<Cvb::Point3D<double>, 12>& 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& trafo)
{
  std::cout << "Estimated transformation:\n";
  std::cout << "translation: [";
  std::cout << trafo.Translation().X() << ", " << trafo.Translation().Y() << ", " << trafo.Translation().Z() << "]\n";

  std::cout << "transformation matrix: [\n";
  std::cout << trafo.Matrix()[0][0] << ", " << trafo.Matrix()[0][1] << ", " << trafo.Matrix()[0][2] << ",\n";
  std::cout << trafo.Matrix()[1][0] << ", " << trafo.Matrix()[1][1] << ", " << trafo.Matrix()[1][2] << ",\n";
  std::cout << trafo.Matrix()[2][0] << ", " << trafo.Matrix()[2][1] << ", " << trafo.Matrix()[2][2] << "]\n\n";
}

void PrintResiduals(const std::array<Cvb::Point3D<double>, 12>& 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";
}