MRPT  2.0.4
robust_kernels_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 <gtest/gtest.h>
12 
13 using namespace mrpt;
14 using namespace mrpt::math;
15 using namespace std;
16 
17 // Test data tables:
18 // Lists of: E2 (squared error) ; PARAM2 (squared kernel parameter) ; R2
19 // (expected robustified output) ; R' (1st derivative) ; R'' (2nd
20 // derivative)
21 // ============= Kernel: None (plain least squares)
22 const double list_test_kernel_none[][5] = {{0.0, .0, 0.0, 1.0, 0.0},
23  {1.0, .0, 1.0, 1.0, 0.0},
24  {10.0, .0, 10.0, 1.0, 0.0}};
25 
26 // ============= Kernel: Pseudo-Huber
27 const double list_test_kernel_pshb[][5] = {
28  {0.0, 1.0, 0.0, 1.0, -0.5},
29  {0.0, 4.0, 0.0, 1.0, -0.125},
30  {0.0, 9.0, 0.0, 1.0, -0.0555556},
31  {1.0, 1.0, 0.828427, 0.707107, -0.176777},
32  {1.0, 4.0, 0.944272, 0.894427, -0.0894427},
33  {1.0, 9.0, 0.973666, 0.948683, -0.0474342},
34  {4.0, 1.0, 2.47214, 0.447214, -0.0447214},
35  {4.0, 4.0, 3.31371, 0.707107, -0.0441942},
36  {4.0, 9.0, 3.63331, 0.83205, -0.0320019}};
37 
38 template <TRobustKernelType KERNEL_TYPE>
39 void tester_robust_kernel(const double table[][5], const size_t N)
40 {
42 
43  for (size_t i = 0; i < N; i++)
44  {
45  const double e2 = table[i][0];
46  const double param2 = table[i][1];
47 
48  double rhop, rhopp;
49  rKernel.param_sq = param2;
50  double r2 = rKernel.eval(e2, rhop, rhopp);
51 
52  const double expected_r2 = table[i][2];
53  const double expected_rhop = table[i][3];
54  const double expected_rhopp = table[i][4];
55 
56  EXPECT_NEAR(r2, expected_r2, 1e-5);
57  EXPECT_NEAR(rhop, expected_rhop, 1e-5);
58  EXPECT_NEAR(rhopp, expected_rhopp, 1e-5);
59  }
60 }
61 
62 TEST(RobustKernels, PlainLeastSquares)
63 {
64  const size_t N =
65  sizeof(list_test_kernel_none) / sizeof(list_test_kernel_none[0]);
66  tester_robust_kernel<rkLeastSquares>(list_test_kernel_none, N);
67 }
68 
69 TEST(RobustKernels, PseudoHuber)
70 {
71  const size_t N =
72  sizeof(list_test_kernel_pshb) / sizeof(list_test_kernel_pshb[0]);
73  tester_robust_kernel<rkPseudoHuber>(list_test_kernel_pshb, N);
74 }
const double list_test_kernel_pshb[][5]
STL namespace.
void tester_robust_kernel(const double table[][5], const size_t N)
This base provides a set of functions for maths stuff.
const double list_test_kernel_none[][5]
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
TEST(RobustKernels, PlainLeastSquares)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020