lio-cpp/lio/include/sophus/interpolate_details.hpp

#ifndef SOPHUS_INTERPOLATE_DETAILS_HPP
#define SOPHUS_INTERPOLATE_DETAILS_HPP

#include "rxso2.hpp"
#include "rxso3.hpp"
#include "se2.hpp"
#include "se3.hpp"
#include "sim2.hpp"
#include "sim3.hpp"
#include "so2.hpp"
#include "so3.hpp"

namespace Sophus {
namespace interp_details {

template <class Group>
struct Traits;

template <class Scalar>
struct Traits<SO2<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {
    using std::abs;
    Scalar angle = foo_T_bar.log();
    return abs(abs(angle) - Constants<Scalar>::pi()) <
           Constants<Scalar>::epsilon();
  }
};

template <class Scalar>
struct Traits<RxSO2<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_bar) {
    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
  }
};

template <class Scalar>
struct Traits<SO3<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {
    using std::abs;
    Scalar angle = foo_T_bar.logAndTheta().theta;
    return abs(abs(angle) - Constants<Scalar>::pi()) <
           Constants<Scalar>::epsilon();
  }
};

template <class Scalar>
struct Traits<RxSO3<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_bar) {
    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
  }
};

template <class Scalar>
struct Traits<SE2<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {
    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
  }
};

template <class Scalar>
struct Traits<SE3<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {
    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
  }
};

template <class Scalar>
struct Traits<Sim2<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {
    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(
        foo_T_bar.rxso2().so2());
    ;
  }
};

template <class Scalar>
struct Traits<Sim3<Scalar>> {
  static bool constexpr supported = true;

  static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {
    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(
        foo_T_bar.rxso3().so3());
    ;
  }
};

}  // namespace interp_details
}  // namespace Sophus

#endif  // SOPHUS_INTERPOLATE_DETAILS_HPP