metric_aqs12.hpp

#pragma once
 
#include "../calibrator_3d.hpp"
 
#include "metric.hpp"
#include "_decl/decl_metric_calibration_configuration.hpp"
#include "_decl/decl_metric_segmentor_dense_point_cloud_aqs12.hpp"
#include "_decl/decl_metric_segmentor_range_map_aqs12.hpp"
#include "_detail/detail_metric_segmentor.hpp"
 
namespace Cvb
{
  CVB_BEGIN_INLINE_NS
 
  namespace Foundation
  {
    namespace Metric
    {
      using AQS12DensePointCloudSegmentorPtr = std::shared_ptr<AQS12DensePointCloudSegmentor>;
      using AQS12RangeMapSegmentorPtr        = std::shared_ptr<AQS12RangeMapSegmentor>;


      using AQS12CalibratorResult = std::pair<LaserPlaneHomographyCalibrator3DPtr, std::array<Point3D<double>, 12>>;
 
    }  // namespace Metric
  }  // namespace Foundation
 
  namespace Foundation
  {
    namespace Metric
    {

      inline AQS12CalibratorResult CreateCalibratorFromAqs12Piece(const ImagePlane &imagePlane,
                                                                  const AQS12RangeMapSegmentor &segmentor,
                                                                  const CalibrationConfiguration &config)
      {
        AQS12CalibratorResult result;
        result.first = std::dynamic_pointer_cast<LaserPlaneHomographyCalibrator3D>(
            Internal::DoResCallShareOut<Calibrator3D>([&](void *&handle) {
              return CVB_CALL_CAPI(CVMAQS12CreateIntrinsicCalibratorFromPiece(
                  imagePlane.Handle(), 0, segmentor.Handle(), config.Handle(), handle,
                  reinterpret_cast<CExports::CVC3DPointD *>(result.second.data())));
            }));
 
        return result;
      }


      inline AQS12CalibratorResult CreateCalibratorFromAqs12Piece(const ImagePlane &imagePlane,
                                                                  const AQS12RangeMapSegmentor &segmentor,
                                                                  const CalibrationConfiguration &config, Rect<int> aoi)
      {
 
        AQS12CalibratorResult result;
        result.first = std::dynamic_pointer_cast<LaserPlaneHomographyCalibrator3D>(
            Internal::DoResCallShareOut<Calibrator3D>([&](void *&handle) {
              return CVB_CALL_CAPI(CVMAQS12CreateIntrinsicCalibratorFromPieceRect(
                  imagePlane.Handle(), 0, aoi.Left(), aoi.Top(), aoi.Right(), aoi.Bottom(), segmentor.Handle(),
                  config.Handle(), handle, reinterpret_cast<CExports::CVC3DPointD *>(result.second.data())));
            }));
 
        return result;
      }


      inline TransformationResult
      CalculateCorrectionOfLaserPlaneInclinationFromAqs12Piece(const DensePointCloud &cloud,
                                                               const AQS12DensePointCloudSegmentor &segmentor,
                                                               const CalibrationConfiguration &config)
      {
        AffineMatrix3D trafo;
        AffineTransformationParameters trafoPara;
        std::vector<Point3D<double>> residuals(12);
 
        Internal::DoResCall([&]() {
          return CVB_CALL_CAPI(CVMAQS12CalculateCorrectionOfLaserPlaneInclinationWithParamsFromPiece(
              cloud.Handle(), segmentor.Handle(), config.Handle(),
              *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo),
              *reinterpret_cast<CExports::CVC3DAffineTransformationParameters *>(&trafoPara),
              reinterpret_cast<CExports::CVC3DPointD *>(residuals.data())));
        });
 
        // move transformation parameters if valid (in case of SpecificTransformationParameters)
        if (!std::isnan(trafoPara.InclinationX))
          return std::make_tuple(std::move(trafo), std::move(residuals), std::move(trafoPara));
 
        return std::make_tuple(std::move(trafo), std::move(residuals), Cvb::optional<AffineTransformationParameters>{});
      }


      inline TransformationResult
      CalculateCorrectionOfLaserPlaneInclinationFromAqs12Piece(const DensePointCloud &cloud,
                                                               const AQS12DensePointCloudSegmentor &segmentor,
                                                               const CalibrationConfiguration &config, Rect<int> aoi)
      {
        AffineMatrix3D trafo;
        AffineTransformationParameters trafoPara;
        std::vector<Point3D<double>> residuals(12);
 
        Internal::DoResCall([&]() {
          return CVB_CALL_CAPI(CVMAQS12CalculateCorrectionOfLaserPlaneInclinationWithParamsFromPieceRect(
              cloud.Handle(), aoi.Left(), aoi.Top(), aoi.Right(), aoi.Bottom(), segmentor.Handle(), config.Handle(),
              *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo),
              *reinterpret_cast<CExports::CVC3DAffineTransformationParameters *>(&trafoPara),
              reinterpret_cast<CExports::CVC3DPointD *>(residuals.data())));
        });
 
        // move transformation parameters if valid (in case of SpecificTransformationParameters)
        if (!std::isnan(trafoPara.InclinationX))
          return std::make_tuple(std::move(trafo), std::move(residuals), std::move(trafoPara));
 
        return std::make_tuple(std::move(trafo), std::move(residuals), Cvb::optional<AffineTransformationParameters>{});
      }


      inline RigidBodyTransformationResult CalculateRigidBodyTransformationFromAqs12Piece(
          const DensePointCloud &cloud, const AQS12DensePointCloudSegmentor &segmentor, const AQS12Piece &aqs12)
      {
        AffineMatrix3D trafo;
        AffineTransformationParameters trafoPara;
        std::vector<Point3D<double>> residuals(12);
 
        Internal::DoResCall([&]() {
          return CVB_CALL_CAPI(CVMAQS12CalculateRigidBodyTransformationWithParamsFromPiece(
              cloud.Handle(), segmentor.Handle(),
              reinterpret_cast<const CExports::CVC3DPointD *>(aqs12.Points().data()), aqs12.TopBasePlaneDistance(),
              *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo),
              *reinterpret_cast<CExports::CVC3DAffineTransformationParameters *>(&trafoPara),
              reinterpret_cast<CExports::CVC3DPointD *>(residuals.data())));
        });
 
        return std::make_tuple(std::move(trafo), std::move(residuals), std::move(trafoPara));
      }


      inline RigidBodyTransformationResult
      CalculateRigidBodyTransformationFromAqs12Piece(const DensePointCloud &cloud,
                                                     const AQS12DensePointCloudSegmentor &segmentor,
                                                     const AQS12Piece &aqs12, Cvb::Rect<int> aoi)
      {
        AffineMatrix3D trafo;
        AffineTransformationParameters trafoPara;
        std::vector<Point3D<double>> residuals(12);
 
        Internal::DoResCall([&]() {
          return CVB_CALL_CAPI(CVMAQS12CalculateRigidBodyTransformationWithParamsFromPieceRect(
              cloud.Handle(), aoi.Left(), aoi.Top(), aoi.Right(), aoi.Bottom(), segmentor.Handle(),
              reinterpret_cast<const CExports::CVC3DPointD *>(aqs12.Points().data()), aqs12.TopBasePlaneDistance(),
              *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo),
              *reinterpret_cast<CExports::CVC3DAffineTransformationParameters *>(&trafoPara),
              reinterpret_cast<CExports::CVC3DPointD *>(residuals.data())));
        });
 
        return std::make_tuple(std::move(trafo), std::move(residuals), std::move(trafoPara));
      }
    }  // namespace Metric
  }  // namespace Foundation
  CVB_END_INLINE_NS
}  // namespace Cvb