1/*
2 * testInvDepthFactor.cpp
3 *
4 * Created on: Apr 13, 2012
5 * Author: cbeall3
6 */
7
8#include <CppUnitLite/TestHarness.h>
9
10#include <gtsam/base/numericalDerivative.h>
11#include <gtsam/base/Testable.h>
12#include <gtsam/geometry/PinholeCamera.h>
13#include <gtsam/geometry/Cal3_S2.h>
14
15#include <gtsam_unstable/geometry/InvDepthCamera3.h>
16
17using namespace std;
18using namespace gtsam;
19
20static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
21Pose3 level_pose = Pose3(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), gtsam::Point3(0,0,1));
22PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
23
24/* ************************************************************************* */
25TEST( InvDepthFactor, Project1) {
26
27 // landmark 5 meters infront of camera
28 Point3 landmark(5, 0, 1);
29
30 Point2 expected_uv = level_camera.project(pw: landmark);
31
32 InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
33 Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.).finished());
34 double inv_depth(1./4);
35 Point2 actual_uv = inv_camera.project(pw: inv_landmark, rho: inv_depth);
36 EXPECT(assert_equal(expected_uv, actual_uv,1e-8));
37 EXPECT(assert_equal(Point2(640,480), actual_uv));
38}
39
40/* ************************************************************************* */
41TEST( InvDepthFactor, Project2) {
42
43 // landmark 1m to the left and 1m up from camera
44 // inv landmark xyz is same as camera xyz, so depth actually doesn't matter
45 Point3 landmark(1, 1, 2);
46 Point2 expected = level_camera.project(pw: landmark);
47
48 InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
49 Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(x: 1.0/sqrt(x: 2.0))).finished());
50 double inv_depth(1/sqrt(x: 3.0));
51 Point2 actual = inv_camera.project(pw: diag_landmark, rho: inv_depth);
52 EXPECT(assert_equal(expected, actual));
53}
54
55/* ************************************************************************* */
56TEST( InvDepthFactor, Project3) {
57
58 // landmark 1m to the left and 1m up from camera
59 // inv depth landmark xyz at origion
60 Point3 landmark(1, 1, 2);
61 Point2 expected = level_camera.project(pw: landmark);
62
63 InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
64 Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(x: 2./sqrt(x: 2.0))).finished());
65 double inv_depth( 1./sqrt(x: 1.0+1+4));
66 Point2 actual = inv_camera.project(pw: diag_landmark, rho: inv_depth);
67 EXPECT(assert_equal(expected, actual));
68}
69
70/* ************************************************************************* */
71TEST( InvDepthFactor, Project4) {
72
73 // landmark 4m to the left and 1m up from camera
74 // inv depth landmark xyz at origion
75 Point3 landmark(1, 4, 2);
76 Point2 expected = level_camera.project(pw: landmark);
77
78 InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
79 Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(x: 4.0/1), atan(x: 2./sqrt(x: 1.+16.))).finished());
80 double inv_depth(1./sqrt(x: 1.+16.+4.));
81 Point2 actual = inv_camera.project(pw: diag_landmark, rho: inv_depth);
82 EXPECT(assert_equal(expected, actual));
83}
84
85
86/* ************************************************************************* */
87Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_depth) {
88 return InvDepthCamera3<Cal3_S2>(pose,K).project(pw: landmark, rho: inv_depth); }
89
90TEST( InvDepthFactor, Dproject_pose)
91{
92 Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished());
93 double inv_depth(1./4);
94 Matrix expected = numericalDerivative31(h: project_,x1: level_pose, x2: landmark, x3: inv_depth);
95 InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
96 Matrix actual;
97 inv_camera.project(pw: landmark, rho: inv_depth, H1: actual, H2: {}, H3: {});
98 EXPECT(assert_equal(expected,actual,1e-6));
99}
100
101/* ************************************************************************* */
102TEST( InvDepthFactor, Dproject_landmark)
103{
104 Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished());
105 double inv_depth(1./4);
106 Matrix expected = numericalDerivative32(h: project_,x1: level_pose, x2: landmark, x3: inv_depth);
107 InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
108 Matrix actual;
109 inv_camera.project(pw: landmark, rho: inv_depth, H1: {}, H2: actual, H3: {});
110 EXPECT(assert_equal(expected,actual,1e-7));
111}
112
113/* ************************************************************************* */
114TEST( InvDepthFactor, Dproject_inv_depth)
115{
116 Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished());
117 double inv_depth(1./4);
118 Matrix expected = numericalDerivative33(h: project_,x1: level_pose, x2: landmark, x3: inv_depth);
119 InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
120 Matrix actual;
121 inv_camera.project(pw: landmark, rho: inv_depth, H1: {}, H2: {}, H3: actual);
122 EXPECT(assert_equal(expected,actual,1e-7));
123}
124
125/* ************************************************************************* */
126TEST(InvDepthFactor, backproject)
127{
128 Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2).finished());
129 double inv_depth(1./4);
130 InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
131 Point2 z = inv_camera.project(pw: expected, rho: inv_depth);
132
133 const auto [actual_vec, actual_inv] = inv_camera.backproject(pi: z, depth: 4);
134 EXPECT(assert_equal(expected,actual_vec,1e-7));
135 EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
136}
137
138/* ************************************************************************* */
139TEST(InvDepthFactor, backproject2)
140{
141 // backwards facing camera
142 Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished());
143 double inv_depth(1./10);
144 InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::Ypr(y: 1.5,p: 0.1, r: -1.5), Point3(-5, -5, 2)),K);
145 Point2 z = inv_camera.project(pw: expected, rho: inv_depth);
146
147 const auto [actual_vec, actual_inv] = inv_camera.backproject(pi: z, depth: 10);
148 EXPECT(assert_equal(expected,actual_vec,1e-7));
149 EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
150}
151
152/* ************************************************************************* */
153int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
154/* ************************************************************************* */
155