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 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));
}     
}
}
CVB_END_INLINE_NS
}