Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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/utils/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> class BASE_IMPEXP SO_average;
26 
27  /** Computes weighted and un-weighted averages of SE(2) or SE(3) poses
28  * \sa SO_average, SE_traits<2>, SE_traits<3>, CPose3D, CPose2D */
29  template <size_t DOF> class BASE_IMPEXP 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 corresponding 2D points, then projecting the result back to the closest-point in the manifold.
35  * Shortly explained in [these slides](http://ingmec.ual.es/~jlblanco/papers/blanco2013tutorial-manifolds-introduction-robotics.pdf)
36  * \note Class introduced in MRPT 1.3.1
37  * \sa SE_traits */
38  template <> class BASE_IMPEXP SO_average<2>
39  {
40  public:
41  SO_average(); //!< Constructor
42  void clear(); //!< Resets the accumulator
43  void append(const double orientation_rad); //!< Adds a new orientation (radians) to the computation \sa get_average
44  void append(const double orientation_rad, const double weight); //!< Adds a new orientation (radians) to the weighted-average computation \sa get_average
45  /** Returns the average orientation (radians).
46  * \exception std::logic_error If no data point were inserted.
47  * \exception std::runtime_error Upon undeterminate average value (ie the average lays exactly on the origin point) and \a enable_exception_on_undeterminate is set to true (otherwise, the 0 orientation would be returned)
48  * \sa append */
49  double get_average() const;
50  bool enable_exception_on_undeterminate; //!< (Default=false) Set to true if you want to raise an exception on undetermined average values.
51  private:
52  double m_count;
53  double m_accum_x,m_accum_y;
54  }; // end SO_average<2>
55 
56  /** Computes weighted and un-weighted averages of SO(3) orientations.
57  * Add values to average with \a append(), when done call \a get_average().
58  * Use \a clear() to reset the accumulator and start a new average computation.
59  * Theoretical base: Average on SO(3) manifolds is computed by averaging the corresponding matrices, then projecting the result back to the closest matrix in the manifold.
60  * Shortly explained in [these slides](http://ingmec.ual.es/~jlblanco/papers/blanco2013tutorial-manifolds-introduction-robotics.pdf)
61  * See also: eq. (3.7) in "MEANS AND AVERAGING IN THE GROUP OF ROTATIONS", MAHER MOAKHER, 2002.
62  * \note Class introduced in MRPT 1.3.1
63  * \sa SE_traits */
64  template <> class BASE_IMPEXP SO_average<3>
65  {
66  public:
67  SO_average(); //!< Constructor
68  void clear(); //!< Resets the accumulator
69  void append(const Eigen::Matrix3d &M); //!< Adds a new orientation to the computation \sa get_average
70  void append(const Eigen::Matrix3d &M, const double weight); //!< Adds a new orientation to the weighted-average computation \sa get_average
71  /** Returns the average orientation.
72  * \exception std::logic_error If no data point were inserted.
73  * \exception std::runtime_error Upon undeterminate average value (ie there was a problem with the SVD) and \a enable_exception_on_undeterminate is set to true (otherwise, the 0 orientation would be returned)
74  * \sa append */
75  Eigen::Matrix3d get_average() const;
76  bool enable_exception_on_undeterminate; //!< (Default=false) Set to true if you want to raise an exception on undetermined average values.
77  private:
78  double m_count;
79  Eigen::Matrix3d m_accum_rot;
80  }; // end SO_average<3>
81 
82  /** Computes weighted and un-weighted averages of SE(2) poses.
83  * Add values to average with \a append(), when done call \a get_average().
84  * Use \a clear() to reset the accumulator and start a new average computation.
85  * Theoretical base: See SO_average<2> for the rotation part. The translation is a simple arithmetic mean in Euclidean space.
86  * \note Class introduced in MRPT 1.3.1
87  * \sa SE_traits */
88  template <> class BASE_IMPEXP SE_average<2>
89  {
90  public:
91  SE_average(); //!< Constructor
92  void clear(); //!< Resets the accumulator
93  void append(const mrpt::poses::CPose2D &p); //!< Adds a new pose to the computation \sa get_average
94  void append(const mrpt::poses::CPose2D &p, const double weight); //!< Adds a new pose to the weighted-average computation \sa get_average
95  /** Returns the average pose.
96  * \exception std::logic_error If no data point were inserted.
97  * \exception std::runtime_error Upon undeterminate average value (ie the average lays exactly on the origin point) and \a enable_exception_on_undeterminate is set to true (otherwise, the 0 orientation would be returned)
98  * \sa append */
99  void get_average(mrpt::poses::CPose2D &out_mean) const;
100  bool enable_exception_on_undeterminate; //!< (Default=false) Set to true if you want to raise an exception on undetermined average values.
101  private:
102  double m_count;
103  double m_accum_x,m_accum_y;
105  }; // end SE_average<2>
106 
107  /** Computes weighted and un-weighted averages of SE(3) poses.
108  * Add values to average with \a append(), when done call \a get_average().
109  * Use \a clear() to reset the accumulator and start a new average computation.
110  * Theoretical base: See SO_average<3> for the rotation part. The translation is a simple arithmetic mean in Euclidean space.
111  * \note Class introduced in MRPT 1.3.1
112  * \sa SE_traits */
113  template <> class BASE_IMPEXP SE_average<3>
114  {
115  public:
116  SE_average(); //!< Constructor
117  void clear(); //!< Resets the accumulator
118  void append(const mrpt::poses::CPose3D &p); //!< Adds a new pose to the computation \sa get_average
119  void append(const mrpt::poses::CPose3D &p, const double weight); //!< Adds a new pose to the weighted-average computation \sa get_average
120  /** Returns the average pose.
121  * \exception std::logic_error If no data point were inserted.
122  * \exception std::runtime_error Upon undeterminate average value (ie the average lays exactly on the origin point) and \a enable_exception_on_undeterminate is set to true (otherwise, the 0 orientation would be returned)
123  * \sa append */
124  void get_average(mrpt::poses::CPose3D &out_mean) const;
125  bool enable_exception_on_undeterminate; //!< (Default=false) Set to true if you want to raise an exception on undetermined average values.
126  private:
127  double m_count;
128  double m_accum_x,m_accum_y,m_accum_z;
130  }; // end SE_average<3>
131 
132  /** @} */ // end of grouping
133 
134  } // End of namespace
135 } // End of namespace
136 
137 #endif
Computes weighted and un-weighted averages of SO(2) orientations.
Definition: SO_SE_average.h:38
class BASE_IMPEXP SE_average
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...
class BASE_IMPEXP SO_average
Computes weighted and un-weighted averages of SO(2) or SO(3) orientations.
Definition: SO_SE_average.h:25
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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...
Definition: SO_SE_average.h:76
GLfloat GLfloat p
Definition: glew.h:10113
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Computes weighted and un-weighted averages of SO(3) orientations.
Definition: SO_SE_average.h:64
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:50



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018