Main MRPT website > C++ reference for MRPT 1.5.3
average.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_AVERAGE_HPP
2 #define SOPHUS_AVERAGE_HPP
3 
4 #include "common.hpp"
5 #include "rxso3.hpp"
6 #include "se2.hpp"
7 #include "se3.hpp"
8 #include "sim3.hpp"
9 #include "so2.hpp"
10 #include "so3.hpp"
11 
12 namespace Sophus {
13 
14 template <class SequenceContainer>
16  SequenceContainer const& foo_Ts_bar, int max_num_iterations) {
17  size_t N = foo_Ts_bar.size();
18  SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
19 
20  using Group = typename SequenceContainer::value_type;
21  using Scalar = typename Group::Scalar;
22  using Tangent = typename Group::Tangent;
23 
24  // This implements the algorithm in the beginning of Sec. 4.2 in
25  // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
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) {
29  Tangent average;
30  setToZero<Tangent>(average);
31  for (Group const& foo_T_bar : foo_Ts_bar) {
32  average += w * (foo_T_average.inverse() * foo_T_bar).log();
33  }
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;
39  }
40 
41  foo_T_average = foo_T_newaverage;
42  }
43  return nullopt;
44 }
45 
46 // Mean implementation for SO(2).
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) {
53  // This implements rotational part of Proposition 12 from Sec. 6.2 of
54  // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
55  size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
56  SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
57  SO2<Scalar> foo_T_average = foo_Ts_bar.front();
58  Scalar w = Scalar(1. / N);
59 
60  Scalar average(0);
61  for (SO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
62  average += w * (foo_T_average.inverse() * foo_T_bar).log();
63  }
64  return foo_T_average * SO2<Scalar>::exp(average);
65 }
66 
67 namespace details {
68 template <class T>
69 void getQuaternion(T const&);
70 
71 template <class Scalar>
72 Eigen::Quaternion<Scalar> getUnitQuaternion(SO3<Scalar> const& R) {
73  return R.unit_quaternion();
74 }
75 
76 template <class Scalar>
77 Eigen::Quaternion<Scalar> getUnitQuaternion(RxSO3<Scalar> const& sR) {
78  return sR.so3().unit_quaternion();
79 }
80 
81 template <class SequenceContainer,
82  class Scalar = typename SequenceContainer::value_type::Scalar>
83 Eigen::Quaternion<Scalar> averageUnitQuaternion(
84  SequenceContainer const& foo_Ts_bar) {
85  // This: http://stackoverflow.com/a/27410865/1221742
86  size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
87  SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
88  Eigen::Matrix<Scalar, 4, Eigen::Dynamic> Q(4, N);
89  int i = 0;
90  Scalar w = Scalar(1. / N);
91  for (auto const& foo_T_bar : foo_Ts_bar) {
92  Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs();
93  ++i;
94  }
95 
96  Eigen::Matrix<Scalar, 4, 4> QQt = Q * Q.transpose();
97  // TODO: Figure out why we can't use SelfAdjointEigenSolver here.
98  Eigen::EigenSolver<Eigen::Matrix<Scalar, 4, 4>> es(QQt);
99 
100  std::complex<Scalar> max_eigenvalue = es.eigenvalues()[0];
101  Eigen::Matrix<std::complex<Scalar>, 4, 1> max_eigenvector =
102  es.eigenvectors().col(0);
103 
104  for (int i = 1; i < 4; i++) {
105  if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) {
106  max_eigenvalue = es.eigenvalues()[i];
107  max_eigenvector = es.eigenvectors().col(i);
108  }
109  }
110  Eigen::Quaternion<Scalar> quat;
111  quat.coeffs() << //
112  max_eigenvector[0].real(), //
113  max_eigenvector[1].real(), //
114  max_eigenvector[2].real(), //
115  max_eigenvector[3].real();
116  return quat;
117 }
118 } // namespace details
119 
120 // Mean implementation for SO(3).
121 //
122 // TODO: Detect degenerated cases and return nullopt.
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) {
129  return SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar));
130 }
131 
132 // Mean implementation for R x SO(3).
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) {
139  size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
140 
141  SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
142  Scalar scale_sum = Scalar(0);
143  using std::log;
144  using std::exp;
145  for (RxSO3<Scalar> const& foo_T_bar : foo_Ts_bar) {
146  scale_sum += log(foo_T_bar.scale());
147  }
148  return RxSO3<Scalar>(exp(scale_sum / N),
150 }
151 
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) {
158  // TODO: Implement Proposition 12 from Sec. 6.2 of
159  // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
160  return iterativeMean(foo_Ts_bar, max_num_iterations);
161 }
162 
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) {
169  return iterativeMean(foo_Ts_bar, max_num_iterations);
170 }
171 
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) {
178  return iterativeMean(foo_Ts_bar, max_num_iterations);
179 }
180 
181 } // namespace Sophus
182 
183 #endif // SOPHUS_AVERAGE_HPP
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
static SOPHUS_FUNC SO2< Scalar > exp(Tangent const &theta)
Definition: so2.hpp:241
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
Definition: so3.hpp:554
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
T value(details::expression_node< T > *n)
Definition: exprtk.hpp:12104
void getQuaternion(T const &)
SOPHUS_FUNC SO3< Scalar > so3() const
Definition: rxso3.hpp:309
Eigen::Quaternion< Scalar > averageUnitQuaternion(SequenceContainer const &foo_Ts_bar)
Definition: average.hpp:83
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:201
Eigen::Quaternion< Scalar > getUnitQuaternion(SO3< Scalar > const &R)
Definition: average.hpp:72
optional< typename SequenceContainer::value_type > iterativeMean(SequenceContainer const &foo_Ts_bar, int max_num_iterations)
Definition: average.hpp:15
enable_if_t< std::is_same< typename SequenceContainer::value_type, SO2< Scalar > >::value, optional< typename SequenceContainer::value_type > > average(SequenceContainer const &foo_Ts_bar)
Definition: average.hpp:52
constexpr nullopt_t nullopt
Definition: common.hpp:162
SOPHUS_FUNC SO2< Scalar > inverse() const
Definition: so2.hpp:119
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.



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Mon Oct 30 10:27:08 UTC 2017