Main MRPT website > C++ reference for MRPT 1.5.6
SO_SE_average.cpp
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 
10 #include "base-precomp.h" // Precompiled headers
11 
13 
14 using namespace mrpt;
15 using namespace mrpt::math;
16 using namespace mrpt::utils;
17 using namespace mrpt::poses;
18 
19 // ----------- SO_average<2> --------------------
21  enable_exception_on_undeterminate(false),
22  m_count(0),
23  m_accum_x(0),m_accum_y(0)
24 {
25 }
26 void SO_average<2>::clear() {
27  m_count=.0;
28  m_accum_x=m_accum_y=.0;
29 }
30 void SO_average<2>::append(const double orientation_rad) {
31  append(orientation_rad,1.0);
32 }
33 void SO_average<2>::append(const double orientation_rad, const double weight) {
34  m_count += weight;
35  m_accum_x += cos(orientation_rad) * weight;
36  m_accum_y += sin(orientation_rad) * weight;
37 }
38 double SO_average<2>::get_average() const
39 {
40  ASSERT_ABOVE_(m_count,0);
41  const double x = m_accum_x / m_count;
42  const double y = m_accum_y / m_count;
43  errno=0;
44  double ang = atan2(y,x);
45  if (errno==EDOM)
46  {
47  if (enable_exception_on_undeterminate)
48  throw std::runtime_error("[SO_average<2>::get_average()] Undetermined average value");
49  else ang = 0;
50  }
51  return ang;
52 }
53 
54 // ----------- SO_average<3> --------------------
56  enable_exception_on_undeterminate(false),
57  m_count(0),
58  m_accum_rot()
59 {
60  clear();
61 }
62 void SO_average<3>::clear() {
63  m_count=.0;
64  m_accum_rot.setZero();
65 }
66 void SO_average<3>::append(const Eigen::Matrix3d &M) {
67  append(M,1.0);
68 }
69 void SO_average<3>::append(const Eigen::Matrix3d &M, const double weight) {
70  m_count += weight;
71  m_accum_rot += weight*M;
72 }
73 // See: eq. (3.7) in "MEANS AND AVERAGING IN THE GROUP OF ROTATIONS", MAHER MOAKHER, 2002.
74 Eigen::Matrix3d SO_average<3>::get_average() const
75 {
76  ASSERT_ABOVE_(m_count,0);
77  const Eigen::Matrix3d MtM = m_accum_rot.transpose() * m_accum_rot;
78 
79  Eigen::JacobiSVD<Eigen::Matrix3d> svd(MtM, Eigen::ComputeFullU);
80  const Eigen::Vector3d vs = svd.singularValues();
81 
82  errno=0;
83  const double d1 = 1.0/sqrt(vs[0]);
84  const double d2 = 1.0/sqrt(vs[1]);
85  const double d3 = mrpt::utils::sign( m_accum_rot.determinant() )/sqrt(vs[2]);
86  if (errno!=0)
87  {
88  if (enable_exception_on_undeterminate)
89  throw std::runtime_error("[SO_average<3>::get_average()] Undetermined average value");
90  else return Eigen::Matrix3d::Identity();
91  }
92 
93  Eigen::Matrix3d D = Eigen::Matrix3d::Zero();
94  D(0,0)=d1; D(1,1)=d2; D(2,2)=d3;
95  Eigen::Matrix3d ret = m_accum_rot * svd.matrixU() * D * svd.matrixU().transpose();
96  return ret;
97 }
98 
99 // ----------- SE_average<2> --------------------
101  enable_exception_on_undeterminate(false),
102  m_count(0),
103  m_accum_x(0),m_accum_y(0),
104  m_rot_part()
105 {
106  clear();
107 }
108 void SE_average<2>::clear() {
109  m_count=.0;
110  m_accum_x=m_accum_y=.0;
111  m_rot_part.clear();
112 }
114  append(p,1.0);
115 }
116 void SE_average<2>::append(const mrpt::poses::CPose2D &p, const double weight) {
117  m_count += weight;
118  m_accum_x += weight * p.x();
119  m_accum_y += weight * p.y();
120  m_rot_part.append(p.phi(), weight);
121 }
123 {
124  ASSERT_ABOVE_(m_count,0);
125  ret_mean.x( m_accum_x / m_count );
126  ret_mean.y( m_accum_y / m_count );
127  const_cast<SO_average<2>*>(&m_rot_part)->enable_exception_on_undeterminate = this->enable_exception_on_undeterminate;
128  ret_mean.phi ( m_rot_part.get_average() );
129 }
130 
131 // ----------- SE_average<3> --------------------
133  enable_exception_on_undeterminate(false),
134  m_count(0),
135  m_accum_x(0),m_accum_y(0),m_accum_z(0),
136  m_rot_part()
137 {
138  clear();
139 }
140 void SE_average<3>::clear() {
141  m_count=.0;
142  m_accum_x=m_accum_y=m_accum_z=.0;
143  m_rot_part.clear();
144 }
146  append(p,1.0);
147 }
148 void SE_average<3>::append(const mrpt::poses::CPose3D &p, const double weight) {
149  m_count += weight;
150  m_accum_x += weight * p.x();
151  m_accum_y += weight * p.y();
152  m_accum_z += weight * p.z();
153  m_rot_part.append(p.getRotationMatrix(), weight);
154 }
156 {
157  ASSERT_ABOVE_(m_count,0);
158  ret_mean.x( m_accum_x / m_count );
159  ret_mean.y( m_accum_y / m_count );
160  ret_mean.z( m_accum_z / m_count );
161  const_cast<SO_average<3>*>(&m_rot_part)->enable_exception_on_undeterminate = this->enable_exception_on_undeterminate;
162  ret_mean.setRotationMatrix( m_rot_part.get_average() );
163 }
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void append(const mrpt::poses::CPose3D &p)
Adds a new pose to the computation.
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
#define ASSERT_ABOVE_(__A, __B)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
class BASE_IMPEXP SO_average
Computes weighted and un-weighted averages of SO(2) or SO(3) orientations.
Definition: SO_SE_average.h:25
int sign(T x)
Returns the sign of X as "1" or "-1".
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
void append(const double orientation_rad)
Adds a new orientation (radians) to the computation.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:181
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
void append(const Eigen::Matrix3d &M)
Adds a new orientation to the computation.
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
Eigen::Matrix3d get_average() const
Returns the average orientation.
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.
void get_average(mrpt::poses::CPose3D &out_mean) const
Returns the average pose.
double get_average() const
Returns the average orientation (radians).



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