metric.hpp

#pragma once
 
#include "../affine_matrix_3d.hpp"
#include "../_cexports/c_metric.h"
 
#include "../point_cloud.hpp"
#include "_decl/decl_metric_calibration_configuration.hpp"
#include "_decl/decl_metric_segmentor_dense_point_cloud.hpp"
#include "_decl/decl_metric_segmentor_range_map.hpp"
 
namespace Cvb
{
  CVB_BEGIN_INLINE_NS
 
  namespace Foundation
  {
    namespace Metric
    {
      using DensePointCloudSegmentorPtr = std::shared_ptr<DensePointCloudSegmentor>;
      using RangeMapSegmentorPtr        = std::shared_ptr<RangeMapSegmentor>;

      enum class CalibrationPiece
      {
        AQS12
      };
      enum class CalibrationPiece {…};

      enum class SegmentationMethod
      {
        KmeansClustering = CExports::CVMSM_KmeansClustering
      };
      enum class SegmentationMethod {…};


      using TransformationResult = std::tuple<AffineMatrix3D, Cvb::optional<std::vector<Point3D<double>>>,
                                              Cvb::optional<AffineTransformationParameters>>;


      using RigidBodyTransformationResult =
          std::tuple<AffineMatrix3D, Cvb::optional<std::vector<Point3D<double>>>, AffineTransformationParameters>;
    }  // namespace Metric
  }  // namespace Foundation
 
  namespace Foundation
  {

    namespace Metric
    {

      inline AffineMatrix3D CalculateAffineTransformation(const std::vector<Point3D<double>> &referencePoints,
                                                          const std::vector<Point3D<double>> &measuredPoints)
      {
        if (referencePoints.size() != measuredPoints.size())
          throw std::invalid_argument("vector of reference and measured points must have the same size");
 
        AffineMatrix3D trafo;
        Internal::DoResCall([&]() {
          return CVB_CALL_CAPI(
              CVMCalculateAffineTransformation(reinterpret_cast<const CExports::CVC3DPointD *>(referencePoints.data()),
                                               reinterpret_cast<const CExports::CVC3DPointD *>(measuredPoints.data()),
                                               static_cast<CExports::cvbdim_t>(referencePoints.size()),
                                               *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo)));
        });
 
        return trafo;
      }
      inline AffineMatrix3D CalculateAffineTransformation(const std::vector<Point3D<double>> &referencePoints, {…}


      inline TransformationResult CalculateCorrectionOfLaserPlaneInclination(
          const std::vector<Point3D<double>> &referencePoints, const std::vector<Point3D<double>> &measuredPoints,
          CalibrationConfiguration::ExtrinsicCalibrationModel model =
              CalibrationConfiguration::ExtrinsicCalibrationModel::SpecificTransformationParameters,
          bool estimateEncoderStep = true, bool calculateResiduals = true)
      {
        if (referencePoints.size() != measuredPoints.size())
          throw std::invalid_argument("vector of reference and measured points must have the same size");
 
        AffineMatrix3D trafo;
        AffineTransformationParameters trafoPara;
 
        if (calculateResiduals)
        {
          auto residuals = std::vector<Point3D<double>>(referencePoints.size());
 
          Internal::DoResCall([&]() {
            return CVB_CALL_CAPI(CVMCalculateCorrectionOfLaserPlaneInclinationWithParams(
                reinterpret_cast<const CExports::CVC3DPointD *>(referencePoints.data()),
                reinterpret_cast<const CExports::CVC3DPointD *>(measuredPoints.data()),
                static_cast<CExports::cvbdim_t>(referencePoints.size()),
                static_cast<CExports::CVMExtrinsicTransformationModel>(model),
                static_cast<CExports::cvbbool_t>(estimateEncoderStep),
                *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo),
                *reinterpret_cast<CExports::CVC3DAffineTransformationParameters *>(&trafoPara),
                reinterpret_cast<CExports::CVC3DPointD *>(residuals.data())));
          });
 
          if (!std::isnan(trafoPara.InclinationX))
            return std::make_tuple(std::move(trafo), std::move(residuals), std::move(trafoPara));
          else
            return std::make_tuple(std::move(trafo), std::move(residuals),
                                   Cvb::optional<AffineTransformationParameters>{});
        }
        else
        {
          Internal::DoResCall([&]() {
            return CVB_CALL_CAPI(CVMCalculateCorrectionOfLaserPlaneInclinationWithParams(
                reinterpret_cast<const CExports::CVC3DPointD *>(referencePoints.data()),
                reinterpret_cast<const CExports::CVC3DPointD *>(measuredPoints.data()),
                static_cast<CExports::cvbdim_t>(referencePoints.size()),
                static_cast<CExports::CVMExtrinsicTransformationModel>(model),
                static_cast<CExports::cvbbool_t>(estimateEncoderStep),
                *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo),
                *reinterpret_cast<CExports::CVC3DAffineTransformationParameters *>(&trafoPara), nullptr));
          });
 
          if (!std::isnan(trafoPara.InclinationX))
            return std::make_tuple(std::move(trafo), Cvb::optional<std::vector<Point3D<double>>>{},
                                   std::move(trafoPara));
          else
            return std::make_tuple(std::move(trafo), Cvb::optional<std::vector<Point3D<double>>>{},
                                   Cvb::optional<AffineTransformationParameters>{});
        }
      }
      inline TransformationResult CalculateCorrectionOfLaserPlaneInclination( {…}


      inline RigidBodyTransformationResult
      CalculateRigidBodyTransformation(const std::vector<Point3D<double>> &referencePoints,
                                       const std::vector<Point3D<double>> &measuredPoints,
                                       bool calculateResiduals = true)
      {
        if (referencePoints.size() != measuredPoints.size())
          throw std::invalid_argument("vector of reference and measured points must have the same size");
 
        AffineMatrix3D trafo;
        AffineTransformationParameters trafoPara;
 
        if (calculateResiduals)
        {
          auto residuals = std::vector<Point3D<double>>(referencePoints.size());
 
          Internal::DoResCall([&]() {
            return CVB_CALL_CAPI(CVMCalculateRigidBodyTransformationWithParams(
                reinterpret_cast<const CExports::CVC3DPointD *>(referencePoints.data()),
                reinterpret_cast<const CExports::CVC3DPointD *>(measuredPoints.data()),
                static_cast<CExports::cvbdim_t>(referencePoints.size()),
                *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));
        }
        else
        {
          Internal::DoResCall([&]() {
            return CVB_CALL_CAPI(CVMCalculateRigidBodyTransformationWithParams(
                reinterpret_cast<const CExports::CVC3DPointD *>(referencePoints.data()),
                reinterpret_cast<const CExports::CVC3DPointD *>(measuredPoints.data()),
                static_cast<CExports::cvbdim_t>(referencePoints.size()),
                *reinterpret_cast<CExports::CVC3DTransformation *>(&trafo),
                *reinterpret_cast<CExports::CVC3DAffineTransformationParameters *>(&trafoPara), nullptr));
          });
 
          return std::make_tuple(std::move(trafo), Cvb::optional<std::vector<Point3D<double>>>{}, std::move(trafoPara));
        }
      }
      CalculateRigidBodyTransformation(const std::vector<Point3D<double>> &referencePoints, {…}
    }  // namespace Metric
  }  // namespace Foundation
  CVB_END_INLINE_NS
}  // namespace Cvb