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



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019