Main MRPT website > C++ reference for MRPT 1.5.6
SO_SE_average_unittest.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 
11 #include <mrpt/math/wrap2pi.h>
12 #include <gtest/gtest.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::poses;
16 using namespace mrpt::utils;
17 using namespace mrpt::math;
18 using namespace std;
19 
20 void run_test_so2_avrg(const double *angs, const size_t N, const double ang_correct_avr)
21 {
22  SO_average<2> so_avr;
23  for (size_t i=0;i<N;i++)
24  so_avr.append(angs[i]);
25  const double calc_avr = so_avr.get_average();
26  EXPECT_NEAR( mrpt::math::wrapToPi(ang_correct_avr-calc_avr), .0, 1e-6);
27 }
28 
29 TEST(SE2_SE3_avrg,SO2_average)
30 {
31  // Simple tests:
32  {
33  const double angs[] = {.1};
34  const double ang_correct_avr = .1;
35  run_test_so2_avrg(angs,sizeof(angs)/sizeof(angs[0]),ang_correct_avr);
36  }
37  {
38  const double angs[] = {.0, M_PI };
39  const double ang_correct_avr = .5*M_PI;
40  run_test_so2_avrg(angs,sizeof(angs)/sizeof(angs[0]),ang_correct_avr);
41  }
42  {
43  const double angs[] = {-0.75*M_PI, 0.75*M_PI };
44  const double ang_correct_avr = 1.0*M_PI;
45  run_test_so2_avrg(angs,sizeof(angs)/sizeof(angs[0]),ang_correct_avr);
46  }
47  {
48  const double angs [] = {-0.75*M_PI, 0.75*M_PI, 0.3*M_PI };
49  //const double angs_w[] = {1.0, 1.0, 0.1 };
50  const double ang_correct_avr = 2.3668403111754515;
51  run_test_so2_avrg(angs,sizeof(angs)/sizeof(angs[0]),ang_correct_avr);
52  }
53  // Test launching an exception when there is no data:
54  {
55  const double dummy [] = {0.};
56  try
57  {
58  run_test_so2_avrg(dummy,0,0);
59  GTEST_FAIL() << "An exception should have been raised before this point!!";
60  } catch (std::exception &)
61  {
62  // This error is expected, it's OK.
63  }
64  }
65 }
66 
67 void run_test_so3_avrg(const double *angs, const size_t N, const Eigen::Matrix3d & correct_avr)
68 {
69  SO_average<3> so_avr;
70  for (size_t i=0;i<N;i++) {
71  mrpt::poses::CPose3D rot(0,0,0, angs[3*i+0],angs[3*i+1],angs[3*i+2]);
72  so_avr.append(rot.getRotationMatrix());
73  }
74  Eigen::Matrix3d calc_avr = so_avr.get_average();
75  EXPECT_NEAR( (correct_avr-calc_avr).array().abs().sum(), .0, 1e-5);
76 }
77 
78 TEST(SE2_SE3_avrg,SO3_average)
79 {
80  // Simple tests:
81  {
82  const double angs[] = { .0,.0,.0 };
83  const Eigen::Matrix3d correct_avr = mrpt::poses::CPose3D(0,0,0, 0,0,0).getRotationMatrix();
84  run_test_so3_avrg(angs,sizeof(angs)/(3*sizeof(angs[0])),correct_avr);
85  }
86  {
87  const double angs[] = {-.75*M_PI,.0,.0, .75*M_PI,.0,.0 };
88  const Eigen::Matrix3d correct_avr = mrpt::poses::CPose3D(0,0,0, M_PI,0,0).getRotationMatrix();
89  run_test_so3_avrg(angs,sizeof(angs)/(3*sizeof(angs[0])),correct_avr);
90  }
91  {
92  const double angs[] = {.0,-0.2,.0, .0,0.2,.0 };
93  const Eigen::Matrix3d correct_avr = mrpt::poses::CPose3D(0,0,0, 0,0,0).getRotationMatrix();
94  run_test_so3_avrg(angs,sizeof(angs)/(3*sizeof(angs[0])),correct_avr);
95  }
96  {
97  const double angs[] = {.0,.0,.3, .0,.0,-.3 };
98  const Eigen::Matrix3d correct_avr = mrpt::poses::CPose3D(0,0,0, 0,0,0).getRotationMatrix();
99  run_test_so3_avrg(angs,sizeof(angs)/(3*sizeof(angs[0])),correct_avr);
100  }
101 }
Computes weighted and un-weighted averages of SO(2) orientations.
Definition: SO_SE_average.h:38
TEST(SE2_SE3_avrg, SO2_average)
#define M_PI
Definition: bits.h:78
void append(const double orientation_rad)
Adds a new orientation (radians) to the computation.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
void append(const Eigen::Matrix3d &M)
Adds a new orientation to the computation.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
void run_test_so3_avrg(const double *angs, const size_t N, const Eigen::Matrix3d &correct_avr)
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 run_test_so2_avrg(const double *angs, const size_t N, const double ang_correct_avr)
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