MRPT  2.0.1
SO_SE_average.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/CMatrixFixed.h>
12 #include <mrpt/poses/CPose2D.h>
13 #include <mrpt/poses/CPose3D.h>
14 
15 namespace mrpt::poses
16 {
17 /** \addtogroup poses_grp
18  * @{ */
19 
20 /** Computes weighted and un-weighted averages of SO(2) or SO(3) orientations
21  * \sa SE_average, Lie::SE<>, Lie::SO<3>, CPose3D, CPose2D */
22 template <size_t DOF>
23 class SO_average;
24 
25 /** Computes weighted and un-weighted averages of SE(2) or SE(3) poses
26  * \sa SO_average, Lie::SE<>, Lie::SO<3>, SE<3>, CPose3D, CPose2D */
27 template <size_t DOF>
28 class SE_average;
29 
30 /** Computes weighted and un-weighted averages of SO(2) orientations.
31  * Add values to average with \a append(), when done call \a get_average().
32  * Use \a clear() to reset the accumulator and start a new average computation.
33  * Theoretical base: Average on SO(2) manifolds is computed by averaging the
34  * corresponding 2D points, then projecting the result back to the closest-point
35  * in the manifold.
36  * Shortly explained in [these
37  * slides](http://ingmec.ual.es/~jlblanco/papers/blanco2013tutorial-manifolds-introduction-robotics.pdf)
38  * \note Class introduced in MRPT 1.3.1
39  * \sa SE_traits */
40 template <>
41 class SO_average<2>
42 {
43  public:
44  /** Constructor */
45  SO_average();
46  /** Resets the accumulator */
47  void clear();
48  /** Adds a new orientation (radians) to the computation \sa get_average */
49  void append(const double orientation_rad);
50  /** Adds a new orientation (radians) to the weighted-average computation \sa
51  * get_average */
52  void append(const double orientation_rad, const double weight);
53  /** Returns the average orientation (radians).
54  * \exception std::logic_error If no data point were inserted.
55  * \exception std::runtime_error Upon undeterminate average value (ie the
56  * average lays exactly on the origin point) and \a
57  * enable_exception_on_undeterminate is set to true (otherwise, the 0
58  * orientation would be returned)
59  * \sa append */
60  double get_average() const;
61  /** (Default=false) Set to true if you want to raise an exception on
62  * undetermined average values. */
63  bool enable_exception_on_undeterminate{false};
64 
65  private:
66  double m_count{0};
67  double m_accum_x{0}, m_accum_y{0};
68 }; // end SO_average<2>
69 
70 /** Computes weighted and un-weighted averages of SO(3) orientations.
71  * Add values to average with \a append(), when done call \a get_average().
72  * Use \a clear() to reset the accumulator and start a new average computation.
73  * Theoretical base: Average on SO(3) manifolds is computed by averaging the
74  * corresponding matrices, then projecting the result back to the closest matrix
75  * in the manifold.
76  * Shortly explained in [these
77  * slides](http://ingmec.ual.es/~jlblanco/papers/blanco2013tutorial-manifolds-introduction-robotics.pdf)
78  * See also: eq. (3.7) in "MEANS AND AVERAGING IN THE GROUP OF ROTATIONS",
79  * MAHER MOAKHER, 2002.
80  * \note Class introduced in MRPT 1.3.1
81  * \sa SE_traits */
82 template <>
83 class SO_average<3>
84 {
85  public:
86  /** Constructor */
87  SO_average();
88  /** Resets the accumulator */
89  void clear();
90  /** Adds a new orientation to the computation \sa get_average */
91  void append(const mrpt::math::CMatrixDouble33& M);
92  /** Adds a new orientation to the weighted-average computation \sa
93  * get_average */
94  void append(const mrpt::math::CMatrixDouble33& M, const double weight);
95  /** Returns the average orientation.
96  * \exception std::logic_error If no data point were inserted.
97  * \exception std::runtime_error Upon undeterminate average value (ie there
98  * was a problem with the SVD) and \a enable_exception_on_undeterminate is
99  * set to true (otherwise, the 0 orientation would be returned)
100  * \sa append */
101  mrpt::math::CMatrixDouble33 get_average() const;
102  /** (Default=false) Set to true if you want to raise an exception on
103  * undetermined average values. */
104  bool enable_exception_on_undeterminate{false};
105 
106  private:
107  double m_count{0};
109 }; // end SO_average<3>
110 
111 /** Computes weighted and un-weighted averages of SE(2) poses.
112  * Add values to average with \a append(), when done call \a get_average().
113  * Use \a clear() to reset the accumulator and start a new average computation.
114  * Theoretical base: See SO_average<2> for the rotation part. The translation
115  * is a simple arithmetic mean in Euclidean space.
116  * \note Class introduced in MRPT 1.3.1
117  * \sa SE_traits */
118 template <>
119 class SE_average<2>
120 {
121  public:
122  /** Constructor */
123  SE_average();
124  /** Resets the accumulator */
125  void clear();
126  /** Adds a new pose to the computation \sa get_average */
127  void append(const mrpt::poses::CPose2D& p);
128  /** Adds a new pose to the weighted-average computation \sa get_average */
129  void append(const mrpt::poses::CPose2D& p, const double weight);
130  void append(const mrpt::math::TPose2D& p, const double weight);
131  /** Returns the average pose.
132  * \exception std::logic_error If no data point were inserted.
133  * \exception std::runtime_error Upon undeterminate average value (ie the
134  * average lays exactly on the origin point) and \a
135  * enable_exception_on_undeterminate is set to true (otherwise, the 0
136  * orientation would be returned)
137  * \sa append */
138  void get_average(mrpt::poses::CPose2D& out_mean) const;
139  /** (Default=false) Set to true if you want to raise an exception on
140  * undetermined average values. */
141  bool enable_exception_on_undeterminate{false};
142 
143  private:
144  double m_count{0};
145  double m_accum_x{0}, m_accum_y{0};
147 }; // end SE_average<2>
148 
149 /** Computes weighted and un-weighted averages of SE(3) poses.
150  * Add values to average with \a append(), when done call \a get_average().
151  * Use \a clear() to reset the accumulator and start a new average computation.
152  * Theoretical base: See SO_average<3> for the rotation part. The translation
153  * is a simple arithmetic mean in Euclidean space.
154  * \note Class introduced in MRPT 1.3.1
155  * \sa SE_traits */
156 template <>
157 class SE_average<3>
158 {
159  public:
160  /** Constructor */
161  SE_average();
162  /** Resets the accumulator */
163  void clear();
164  /** Adds a new pose to the computation \sa get_average */
165  void append(const mrpt::poses::CPose3D& p);
166  /** Adds a new pose to the weighted-average computation \sa get_average */
167  void append(const mrpt::poses::CPose3D& p, const double weight);
168  void append(const mrpt::math::TPose3D& p, const double weight);
169  /** Returns the average pose.
170  * \exception std::logic_error If no data point were inserted.
171  * \exception std::runtime_error Upon undeterminate average value (ie the
172  * average lays exactly on the origin point) and \a
173  * enable_exception_on_undeterminate is set to true (otherwise, the 0
174  * orientation would be returned)
175  * \sa append */
176  void get_average(mrpt::poses::CPose3D& out_mean) const;
177  /** (Default=false) Set to true if you want to raise an exception on
178  * undetermined average values. */
179  bool enable_exception_on_undeterminate{false};
180 
181  private:
182  double m_count{0};
183  double m_accum_x{0}, m_accum_y{0}, m_accum_z{0};
185 }; // end SE_average<3>
186 
187 /** @} */ // end of grouping
188 
189 } // namespace mrpt::poses
Computes weighted and un-weighted averages of SO(2) orientations.
Definition: SO_SE_average.h:41
Computes weighted and un-weighted averages of SE(2) or SE(3) poses.
Definition: SO_SE_average.h:28
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Computes weighted and un-weighted averages of SO(3) orientations.
Definition: SO_SE_average.h:83
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
Computes weighted and un-weighted averages of SO(2) or SO(3) orientations.
Definition: SO_SE_average.h:23
mrpt::math::CMatrixDouble33 m_accum_rot
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020