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: d1962bc6a Wed Jan 15 17:38:30 2020 +0100 at miƩ ene 15 17:45:11 CET 2020