match_3d.hpp

#pragma once
 
#include "../_cexports/c_match_3d.h"
 
#include "../affine_matrix_3d.hpp"
#include "../global.hpp"
#include "../point_cloud.hpp"
 
namespace Cvb
{
 
CVB_BEGIN_INLINE_NS
 
 
namespace Match3D
{
 
enum class CorrespondenceType
{
  ClassicICP = CExports::CVM3DCT_ClassicICP,
 
    ConsensusICP = CExports::CVM3DCT_ConsensusICP
};
 
class MatchingParameters;
class MatchingResult;
 
inline MatchingResult
IcpMatch(const PointCloud &scene, const PointCloud &model, const MatchingParameters &parameters);
 
class MatchingParameters final
{
  friend MatchingResult
  IcpMatch(const PointCloud &model, const PointCloud &scene, const MatchingParameters &parameters);
 
public:  
 
  MatchingParameters() noexcept
  {
  }
 
 
  MatchingParameters(std::size_t maxScenePoints, std::size_t maxModelPoints) noexcept
  {
    SetMaxModelPoints(maxModelPoints);
    SetMaxScenePoints(maxScenePoints);
  }
 
 
  MatchingParameters(std::size_t maxScenePoints,
    std::size_t maxModelPoints,
    int maxIterations, double tolerance,
    double minImprovement, double correspondenceThreshold,
    double convergenceRadius, bool prealign, CorrespondenceType correspondenceMethod) noexcept
  {
    SetMaxModelPoints(maxModelPoints);
    SetMaxScenePoints(maxScenePoints);
 
    params_.major = static_cast<Cvb::CExports::cvbint32_t>(0);
    params_.minor = static_cast<Cvb::CExports::cvbint32_t>(1);
    SetMaxIterations(maxIterations);
    SetTolerance(tolerance);
    SetMinImprovement(minImprovement);
    SetCorrespondenceThreshold(correspondenceThreshold);
    SetConvergenceRadius(convergenceRadius);
    SetPrealign(prealign);
    SetCorrespondenceMethod(correspondenceMethod);
  }
 
 
  std::size_t MaxModelPoints() const noexcept 
  { 
    return static_cast<std::size_t>(maxModelPoints_); 
  }
 
 
  void SetMaxModelPoints(std::size_t maxModelPoints) noexcept
  {
    maxModelPoints_ = static_cast<std::int64_t>(maxModelPoints);
  }
 
 
  std::size_t MaxScenePoints() const noexcept 
  { 
    return static_cast<std::size_t>(maxScenePoints_); 
  }
 
 
  void SetMaxScenePoints(std::size_t maxScenePoints) noexcept
  {
    maxScenePoints_ = static_cast<std::int64_t>(maxScenePoints);
  }
 
 
  int MaxIterations() const noexcept 
  { 
    return static_cast<int>(params_.MaxIterations); 
  }
 
 
  void SetMaxIterations(int value) noexcept 
  { 
    params_.MaxIterations = static_cast<Cvb::CExports::cvbval_t>(value); 
  }
 
 
  double Tolerance() const noexcept 
  { 
    return params_.Tolerance; 
  }
 
 
  void SetTolerance(double value) noexcept 
  { 
    params_.Tolerance = value; 
  }
 
 
  double MinImprovement() const noexcept 
  { 
    return params_.MinImprovement; 
  }
 
 
  void SetMinImprovement(double value) noexcept 
  { 
    params_.MinImprovement = value; 
  }
 
 
  double CorrespondenceThreshold() const noexcept 
  { 
    return params_.CorrespondenceThreshold; 
  }
 
 
  void SetCorrespondenceThreshold(double value) noexcept 
  { 
    params_.CorrespondenceThreshold = value; 
  }
 
 
  double ConvergenceRadius() const noexcept 
  { 
    return params_.ConvergenceRadius; 
  }
 
 
  void SetConvergenceRadius(double value) noexcept 
  { 
    params_.ConvergenceRadius = value; 
  }
 
 
  bool Prealign() const noexcept 
  { 
    return static_cast<bool>(params_.Prealign); 
  }
 
 
  void SetPrealign(bool value) noexcept 
  { 
    params_.Prealign = static_cast<Cvb::CExports::cvbbool_t>(value); 
  }
 
 
  CorrespondenceType CorrespondenceMethod() const noexcept 
  { 
    return static_cast<CorrespondenceType>(params_.CorrespondenceMethod); 
  }
 
 
  void SetCorrespondenceMethod(CorrespondenceType value) noexcept 
  { 
    params_.CorrespondenceMethod = static_cast<Cvb::CExports::CVM3DCorrespondenceType>(value); 
  }
 
private:
  std::int64_t maxModelPoints_ = 0;
  std::int64_t maxScenePoints_ = 0;
 
  CExports::CVM3DMatchParameters params_ = CExports::CVM3DDefaultParameters;
};
 
class MatchingResult final
{
  friend MatchingResult
  IcpMatch(const PointCloud &scene, const PointCloud& model, const MatchingParameters &parameters);
 
public:
 
  double DistanceRMS() const noexcept 
  { 
    return rmsDistance_; 
  }
 
 
  AffineMatrix3D Transformation() const noexcept 
  { 
    return transformation_; 
  }
 
 
  int NumIterations() const noexcept 
  { 
    return numIterations_; 
  }
 
private:
  MatchingResult() noexcept = default;
 
  double rmsDistance_ = std::numeric_limits<double>::quiet_NaN();
  CExports::cvbval_t numIterations_ = 0;
  AffineMatrix3D transformation_;
};
 
 
inline MatchingResult
IcpMatch(const PointCloud &scene, const PointCloud &model, const MatchingParameters &parameters)
{
  MatchingResult result;
 
  Internal::DoResCall([&]() {
    return CVB_CALL_CAPI(
      CVM3DMatchPointCloudsAdvanced(
        scene.Handle(),
        parameters.maxScenePoints_, 
        model.Handle(),
        parameters.maxModelPoints_,
        parameters.params_,
        result.rmsDistance_,
        result.numIterations_,
        *reinterpret_cast<CExports::CVC3DTransformation*>(&result.transformation_)));
    });
  
  return result;
}
 
 
inline double DistanceRMS(const PointCloud &scene, const PointCloud& model)
{
  return Internal::DoResCallValueOut<double>([&](double &value) {
    return CVB_CALL_CAPI(CVM3DRmsDistanceOfPointClouds(scene.Handle(), model.Handle(), value));
  });
}
 
}
 
CVB_END_INLINE_NS
 
}