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, -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.
T square(const T x)
Inline function for the square of a number.
constexpr double DEG2RAD(const double x)
Degrees to radians.
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: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 1.9.9 Git: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019