1 #ifndef SOPHUS_AVERAGE_HPP 2 #define SOPHUS_AVERAGE_HPP 14 template <
class SequenceContainer>
16 SequenceContainer
const& foo_Ts_bar,
int max_num_iterations) {
17 size_t N = foo_Ts_bar.size();
20 using Group =
typename SequenceContainer::value_type;
21 using Scalar =
typename Group::Scalar;
22 using Tangent =
typename Group::Tangent;
26 Group foo_T_average = foo_Ts_bar.front();
27 Scalar w = Scalar(1. / N);
28 for (
int i = 0; i < max_num_iterations; ++i) {
31 for (Group
const& foo_T_bar : foo_Ts_bar) {
32 average += w * (foo_T_average.inverse() * foo_T_bar).log();
34 Group foo_T_newaverage = foo_T_average * Group::exp(average);
35 if (squaredNorm<Tangent>(
36 (foo_T_newaverage.inverse() * foo_T_average).log()) <
38 return foo_T_newaverage;
41 foo_T_average = foo_T_newaverage;
47 template <
class SequenceContainer,
48 class Scalar =
typename SequenceContainer::value_type::Scalar>
50 std::is_same<typename SequenceContainer::value_type, SO2<Scalar>>
::value,
52 average(SequenceContainer
const& foo_Ts_bar) {
58 Scalar w = Scalar(1. / N);
62 average += w * (foo_T_average.
inverse() * foo_T_bar).log();
71 template <
class Scalar>
76 template <
class Scalar>
78 return sR.
so3().unit_quaternion();
81 template <
class SequenceContainer,
82 class Scalar =
typename SequenceContainer::value_type::Scalar>
84 SequenceContainer
const& foo_Ts_bar) {
88 Eigen::Matrix<Scalar, 4, Eigen::Dynamic> Q(4, N);
90 Scalar w = Scalar(1. / N);
91 for (
auto const& foo_T_bar : foo_Ts_bar) {
96 Eigen::Matrix<Scalar, 4, 4> QQt = Q * Q.transpose();
98 Eigen::EigenSolver<Eigen::Matrix<Scalar, 4, 4>> es(QQt);
100 std::complex<Scalar> max_eigenvalue = es.eigenvalues()[0];
101 Eigen::Matrix<std::complex<Scalar>, 4, 1> max_eigenvector =
102 es.eigenvectors().col(0);
104 for (
int i = 1; i < 4; i++) {
106 max_eigenvalue = es.eigenvalues()[i];
107 max_eigenvector = es.eigenvectors().col(i);
110 Eigen::Quaternion<Scalar> quat;
112 max_eigenvector[0].real(),
113 max_eigenvector[1].real(),
114 max_eigenvector[2].real(),
115 max_eigenvector[3].real();
123 template <
class SequenceContainer,
124 class Scalar =
typename SequenceContainer::value_type::Scalar>
126 std::is_same<typename SequenceContainer::value_type, SO3<Scalar>>
::value,
128 average(SequenceContainer
const& foo_Ts_bar) {
133 template <
class SequenceContainer,
134 class Scalar =
typename SequenceContainer::value_type::Scalar>
136 std::is_same<typename SequenceContainer::value_type, RxSO3<Scalar>>
::value,
138 average(SequenceContainer
const& foo_Ts_bar) {
142 Scalar scale_sum = Scalar(0);
146 scale_sum += log(foo_T_bar.scale());
152 template <
class SequenceContainer,
153 class Scalar =
typename SequenceContainer::value_type::Scalar>
155 std::is_same<typename SequenceContainer::value_type, SE2<Scalar>>
::value,
157 average(SequenceContainer
const& foo_Ts_bar,
int max_num_iterations = 20) {
163 template <
class SequenceContainer,
164 class Scalar =
typename SequenceContainer::value_type::Scalar>
166 std::is_same<typename SequenceContainer::value_type, SE3<Scalar>>
::value,
168 average(SequenceContainer
const& foo_Ts_bar,
int max_num_iterations = 20) {
172 template <
class SequenceContainer,
173 class Scalar =
typename SequenceContainer::value_type::Scalar>
175 std::is_same<typename SequenceContainer::value_type, Sim3<Scalar>>
::value,
177 average(SequenceContainer
const& foo_Ts_bar,
int max_num_iterations = 20) {
183 #endif // SOPHUS_AVERAGE_HPP
#define SOPHUS_ENSURE(expr, description,...)
EIGEN_STRONG_INLINE iterator end()
static SOPHUS_FUNC SO2< Scalar > exp(Tangent const &theta)
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
EIGEN_STRONG_INLINE iterator begin()
T value(details::expression_node< T > *n)
void getQuaternion(T const &)
SOPHUS_FUNC SO3< Scalar > so3() const
Eigen::Quaternion< Scalar > averageUnitQuaternion(SequenceContainer const &foo_Ts_bar)
typename std::enable_if< B, T >::type enable_if_t
Eigen::Quaternion< Scalar > getUnitQuaternion(SO3< Scalar > const &R)
optional< typename SequenceContainer::value_type > iterativeMean(SequenceContainer const &foo_Ts_bar, int max_num_iterations)
enable_if_t< std::is_same< typename SequenceContainer::value_type, SO2< Scalar > >::value, optional< typename SequenceContainer::value_type > > average(SequenceContainer const &foo_Ts_bar)
constexpr nullopt_t nullopt
SOPHUS_FUNC SO2< Scalar > inverse() const
CONTAINER::Scalar norm(const CONTAINER &v)
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.