interpolate_details.hpp

Go to the documentation of this file.

#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