MRPT  2.0.5
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-2020, 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, -20.0_deg, -30.0_deg, -40.0_deg);
25 const auto bb_max = mrpt::math::TPose3D(3, 4, 5, 20.0_deg, 30.0_deg, 40.0_deg);
26 
27 TEST(CPose3DPDFGrid, uniformDistribution)
28 {
29  const auto gt_mean = mrpt::math::TPose3D(2, 3, 4, .0_deg, .0_deg, .0_deg);
30 
31  const double res_xyz = 0.25;
32  const double res_YPR = mrpt::DEG2RAD(10.0);
33 
34  auto grid = mrpt::poses::CPose3DPDFGrid(bb_min, bb_max, res_xyz, res_YPR);
35 
36  grid.uniformDistribution();
37 
38  auto [COV, MEAN] = grid.getCovarianceAndMean();
39 
40  EXPECT_NEAR(MEAN.x(), gt_mean.x, 1e-4);
41  EXPECT_NEAR(MEAN.y(), gt_mean.y, 1e-4);
42  EXPECT_NEAR(MEAN.z(), gt_mean.z, 1e-4);
43 
44  EXPECT_NEAR(MEAN.yaw(), gt_mean.yaw, 1e-4);
45  EXPECT_NEAR(MEAN.pitch(), gt_mean.pitch, 1e-4);
46  EXPECT_NEAR(MEAN.roll(), gt_mean.roll, 1e-4);
47 
48  for (int i = 0; i < 3; i++) EXPECT_GT(COV(i, i), mrpt::square(0.25));
49  for (int i = 4; i < 6; i++)
50  EXPECT_GT(COV(i, i), mrpt::square(mrpt::DEG2RAD(3.0)));
51 }
52 
53 TEST(CPose3DPDFGrid, setManualPDF)
54 {
55  const auto gt_mean =
56  mrpt::math::TPose3D(2, 3, 4, 9.0_deg, 34.0_deg, 12.0_deg);
57 
58  const double res_xyz = 0.25;
59  const double res_YPR = mrpt::DEG2RAD(10.0);
60 
61  auto grid = mrpt::poses::CPose3DPDFGrid(bb_min, bb_max, res_xyz, res_YPR);
62 
63  grid.fill(0);
64 
65  *grid.getByPos(gt_mean) = .1;
66 
67  grid.normalize();
68 
69  auto [COV, MEAN] = grid.getCovarianceAndMean();
70 
71  EXPECT_NEAR(MEAN.x(), gt_mean.x, 0.2);
72  EXPECT_NEAR(MEAN.y(), gt_mean.y, 0.2);
73  EXPECT_NEAR(MEAN.z(), gt_mean.z, 0.2);
74 
75  EXPECT_NEAR(MEAN.yaw(), gt_mean.yaw, 0.1);
76  EXPECT_NEAR(MEAN.pitch(), gt_mean.pitch, 0.1);
77  EXPECT_NEAR(MEAN.roll(), gt_mean.roll, 0.1);
78 
79  for (int i = 0; i < 6; i++) EXPECT_LT(COV(i, i), mrpt::square(0.05));
80 }
EXPECT_LT(out.final_rmse, 3.0)
EXPECT_GT(out.final_iters, 10UL)
STL namespace.
This base provides a set of functions for maths stuff.
constexpr double DEG2RAD(const double x)
Degrees to radians.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
return_t square(const num_t x)
Inline function for the square of a number.
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:24
Declares a class that represents a Probability Distribution function (PDF) of a SE(3) pose (x...
const auto bb_max
const auto bb_min
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
TEST(CPose3DPDFGrid, uniformDistribution)



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: fc2d69e97 Fri Jul 10 18:20:57 2020 +0200 at vie jul 10 18:30:13 CEST 2020