105 lines
2.6 KiB
C++
105 lines
2.6 KiB
C++
|
#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
|