MRPT  1.9.9
CPose3DPDFGrid_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 <CTraitsTest.h>
11 #include <gtest/gtest.h>
12 #include <mrpt/poses/CPose3D.h>
14 //#include <Eigen/Dense>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 template class mrpt::CTraitsTest<CPose3DPDFGrid>;
22 
23 const auto bb_min =
24  mrpt::math::TPose3D(1, 2, 3, DEG2RAD(-20), DEG2RAD(-30), DEG2RAD(-40));
25 const auto bb_max =
26  mrpt::math::TPose3D(3, 4, 5, DEG2RAD(20), DEG2RAD(30), DEG2RAD(40));
27 
28 TEST(CPose3DPDFGrid, uniformDistribution)
29 {
30  const auto gt_mean =
31  mrpt::math::TPose3D(2, 3, 4, DEG2RAD(.0), DEG2RAD(.0), DEG2RAD(.0));
32 
33  const double res_xyz = 0.25;
34  const double res_YPR = mrpt::DEG2RAD(10.0);
35 
36  auto grid = mrpt::poses::CPose3DPDFGrid(bb_min, bb_max, res_xyz, res_YPR);
37 
38  grid.uniformDistribution();
39 
40  auto [COV, MEAN] = grid.getCovarianceAndMean();
41 
42  EXPECT_NEAR(MEAN.x(), gt_mean.x, 1e-4);
43  EXPECT_NEAR(MEAN.y(), gt_mean.y, 1e-4);
44  EXPECT_NEAR(MEAN.z(), gt_mean.z, 1e-4);
45 
46  EXPECT_NEAR(MEAN.yaw(), gt_mean.yaw, 1e-4);
47  EXPECT_NEAR(MEAN.pitch(), gt_mean.pitch, 1e-4);
48  EXPECT_NEAR(MEAN.roll(), gt_mean.roll, 1e-4);
49 
50  for (int i = 0; i < 3; i++) EXPECT_GT(COV(i, i), mrpt::square(0.25));
51  for (int i = 4; i < 6; i++)
52  EXPECT_GT(COV(i, i), mrpt::square(mrpt::DEG2RAD(3.0)));
53 }
54 
55 TEST(CPose3DPDFGrid, setManualPDF)
56 {
57  const auto gt_mean = mrpt::math::TPose3D(
58  2, 3, 4, DEG2RAD(9.0), DEG2RAD(34.0), DEG2RAD(12.0));
59 
60  const double res_xyz = 0.25;
61  const double res_YPR = mrpt::DEG2RAD(10.0);
62 
63  auto grid = mrpt::poses::CPose3DPDFGrid(bb_min, bb_max, res_xyz, res_YPR);
64 
65  grid.fill(0);
66 
67  *grid.getByPos(gt_mean) = .1;
68 
69  grid.normalize();
70 
71  auto [COV, MEAN] = grid.getCovarianceAndMean();
72 
73  EXPECT_NEAR(MEAN.x(), gt_mean.x, 0.2);
74  EXPECT_NEAR(MEAN.y(), gt_mean.y, 0.2);
75  EXPECT_NEAR(MEAN.z(), gt_mean.z, 0.2);
76 
77  EXPECT_NEAR(MEAN.yaw(), gt_mean.yaw, 0.1);
78  EXPECT_NEAR(MEAN.pitch(), gt_mean.pitch, 0.1);
79  EXPECT_NEAR(MEAN.roll(), gt_mean.roll, 0.1);
80 
81  for (int i = 0; i < 6; i++) EXPECT_LT(COV(i, i), mrpt::square(0.05));
82 }
EXPECT_LT(out.final_rmse, 3.0)
EXPECT_GT(out.final_iters, 10UL)
EXPECT_NEAR(app.estimateMean.x(), app.GT_Ax, 0.1)
double DEG2RAD(const double x)
Degrees to radians.
STL namespace.
T square(const T x)
Inline function for the square of a number.
This base provides a set of functions for maths stuff.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
Declares a class that represents a Probability Distribution function (PDF) of a SE(3) pose (x...
const auto bb_max
const auto bb_min
TEST(CPose3DPDFGrid, uniformDistribution)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: dad381fd7 Sun Oct 20 13:36:46 2019 +0200 at dom oct 20 13:40:10 CEST 2019