MRPT  2.0.1
pnp_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 #include <gtest/gtest.h>
10 #include <mrpt/vision/pnp_algos.h>
11 #include <Eigen/Dense>
12 #include <iostream>
13 
14 // Opencv 2.3 had a broken <opencv/eigen.h> in Ubuntu 14.04 Trusty => Disable
15 // PNP classes
16 #include <mrpt/config.h>
17 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM < 0x240
18 #undef MRPT_HAS_OPENCV
19 #define MRPT_HAS_OPENCV 0
20 #endif
21 
22 #if MRPT_HAS_OPENCV
23 
24 class CPnPTest : public ::testing::Test
25 {
26  public:
28 
29  Eigen::MatrixXd obj_pts, img_pts, pose_est;
30  Eigen::Matrix3d R, I3, R_est;
31  Eigen::Vector3d t, t_est;
32  int n;
33 
34  void SetUp() override
35  {
36  n = 6;
37 
38  obj_pts = Eigen::MatrixXd::Zero(3, n);
39  img_pts = Eigen::MatrixXd::Zero(3, n);
40  obj_pts << 0, 20, 10, 15, 14, 16, 0, 0, -10, -5, -2, -5, 0, 40, 0, 24,
41  21, 13;
42 
43  R << -0.3536, 0.3536, -0.8660, 0.9330, 0.0670, -0.3536, -0.0670,
44  -0.9330, -0.3536;
45 
46  t << 20, -30, 100;
47 
48  for (int i = 0; i < n; i++)
49  {
50  img_pts.col(i) = (R * obj_pts.col(i) + t);
51  img_pts.col(i) /= img_pts(2, i);
52  }
53 
54  I3 = Eigen::MatrixXd::Identity(3, 3);
55 
56  pose_est = Eigen::MatrixXd::Zero(6, 1);
57  }
58  void TearDown() override {}
59 };
60 
61 TEST_F(CPnPTest, p3p_TEST)
62 {
63  cpnp.p3p(obj_pts, img_pts, n, I3, pose_est);
64 
65  t_est << pose_est(0), pose_est(1), pose_est(2);
66 
67  double err_t = (t - t_est).norm();
68 
69  EXPECT_LE(err_t, 2);
70 }
71 
72 TEST_F(CPnPTest, rpnp_TEST)
73 {
74  cpnp.rpnp(obj_pts, img_pts, n, I3, pose_est);
75 
76  t_est << pose_est(0), pose_est(1), pose_est(2);
77 
78  double err_t = (t - t_est).norm();
79 
80  EXPECT_LE(err_t, 2);
81 }
82 
83 TEST_F(CPnPTest, ppnp_TEST)
84 {
85  cpnp.ppnp(obj_pts, img_pts, n, I3, pose_est);
86 
87  t_est << pose_est(0), pose_est(1), pose_est(2);
88 
89  double err_t = (t - t_est).norm();
90 
91  EXPECT_LE(err_t, 2);
92 }
93 
94 TEST_F(CPnPTest, posit_TEST)
95 {
96  cpnp.posit(obj_pts, img_pts, n, I3, pose_est);
97 
98  t_est << pose_est(0), pose_est(1), pose_est(2);
99 
100  double err_t = (t - t_est).norm();
101 
102  EXPECT_LE(err_t, 2);
103 }
104 
105 TEST_F(CPnPTest, lhm_TEST)
106 {
107  cpnp.lhm(obj_pts, img_pts, n, I3, pose_est);
108 
109  t_est << pose_est(0), pose_est(1), pose_est(2);
110 
111  double err_t = (t - t_est).norm();
112 
113  EXPECT_LE(err_t, 2);
114 }
115 
116 TEST_F(CPnPTest, dls_TEST)
117 {
118  cpnp.dls(obj_pts, img_pts, n, I3, pose_est);
119 
120  t_est << pose_est(0), pose_est(1), pose_est(2);
121 
122  double err_t = (t - t_est).norm();
123 
124  EXPECT_LE(err_t, 2);
125 }
126 
127 TEST_F(CPnPTest, epnp_TEST)
128 {
129  cpnp.epnp(obj_pts, img_pts, n, I3, pose_est);
130 
131  t_est << pose_est(0), pose_est(1), pose_est(2);
132 
133  double err_t = (t - t_est).norm();
134 
135  EXPECT_LE(err_t, 2);
136 }
137 
138 TEST_F(CPnPTest, DISABLED_upnp_TEST)
139 {
140  cpnp.upnp(obj_pts, img_pts, n, I3, pose_est);
141 
142  t_est << pose_est(0), pose_est(1), pose_est(2);
143 
144  double err_t = (t - t_est).norm();
145 
146  EXPECT_LE(err_t, 2);
147 }
148 
149 #endif
This class is used for Pose estimation from a known landmark using a monocular camera.
Definition: pnp_algos.h:50
TEST_F(Pose3DTests, DefaultValues)
PnP Algorithms toolkit for MRPT.
const float R
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020