MRPT  1.9.9
SO_SE_average_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
11 #include <mrpt/math/wrap2pi.h>
13 #include <Eigen/Dense>
14 
15 using namespace mrpt;
16 using namespace mrpt::poses;
17 using namespace mrpt::math;
18 using namespace std;
19 
21  const double* angs, const size_t N, const double ang_correct_avr)
22 {
23  SO_average<2> so_avr;
24  for (size_t i = 0; i < N; i++) 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;
36  angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
37  }
38  {
39  const double angs[] = {.0, M_PI};
40  const double ang_correct_avr = .5 * M_PI;
42  angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
43  }
44  {
45  const double angs[] = {-0.75 * M_PI, 0.75 * M_PI};
46  const double ang_correct_avr = 1.0 * M_PI;
48  angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
49  }
50  {
51  const double angs[] = {-0.75 * M_PI, 0.75 * M_PI, 0.3 * M_PI};
52  // const double angs_w[] = {1.0, 1.0, 0.1 };
53  const double ang_correct_avr = 2.3668403111754515;
55  angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
56  }
57  // Test launching an exception when there is no data:
58  {
59  const double dummy[] = {0.};
60  try
61  {
62  run_test_so2_avrg(dummy, 0, 0);
63  GTEST_FAIL()
64  << "An exception should have been raised before this point!!";
65  }
66  catch (std::exception&)
67  {
68  // This error is expected, it's OK.
69  }
70  }
71 }
72 
74  const double* angs, const size_t N,
75  const mrpt::math::CMatrixDouble33& correct_avr)
76 {
77  SO_average<3> so_avr;
78  for (size_t i = 0; i < N; i++)
79  {
81  0, 0, 0, angs[3 * i + 0], angs[3 * i + 1], angs[3 * i + 2]);
82  so_avr.append(rot.getRotationMatrix());
83  }
84  const auto calc_avr = so_avr.get_average();
85  EXPECT_NEAR((correct_avr - calc_avr).array().abs().sum(), .0, 1e-5);
86 }
87 
88 TEST(SE2_SE3_avrg, SO3_average)
89 {
90  // Simple tests:
91  {
92  const double angs[] = {.0, .0, .0};
93  const auto correct_avr =
94  mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
96  angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
97  }
98  {
99  const double angs[] = {-.75 * M_PI, .0, .0, .75 * M_PI, .0, .0};
100  const auto correct_avr =
101  mrpt::poses::CPose3D(0, 0, 0, M_PI, 0, 0).getRotationMatrix();
103  angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
104  }
105  {
106  const double angs[] = {.0, -0.2, .0, .0, 0.2, .0};
107  const auto correct_avr =
108  mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
110  angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
111  }
112  {
113  const double angs[] = {.0, .0, .3, .0, .0, -.3};
114  const auto correct_avr =
115  mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
117  angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
118  }
119 }
void append(const mrpt::math::CMatrixDouble33 &M)
Adds a new orientation to the computation.
Computes weighted and un-weighted averages of SO(2) orientations.
Definition: SO_SE_average.h:41
TEST(SE2_SE3_avrg, SO2_average)
STL namespace.
void append(const double orientation_rad)
Adds a new orientation (radians) to the computation.
This base provides a set of functions for maths stuff.
void run_test_so3_avrg(const double *angs, const size_t N, const mrpt::math::CMatrixDouble33 &correct_avr)
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double get_average() const
Returns the average orientation (radians).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Computes weighted and un-weighted averages of SO(3) orientations.
Definition: SO_SE_average.h:83
mrpt::math::CMatrixDouble33 get_average() const
Returns the average orientation.
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
void run_test_so2_avrg(const double *angs, const size_t N, const double ang_correct_avr)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019