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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019