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

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020