1/* ----------------------------------------------------------------------------
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 * Atlanta, Georgia 30332-0415
4 * All Rights Reserved
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 * See LICENSE for the license information
7 * -------------------------------------------------------------------------- */
8
9/**
10 * @file testInvDepthFactor3.cpp
11 * @brief Unit tests inverse depth parametrization
12 *
13 * @author cbeall3
14 * @author Dominik Van Opdenbosch
15 * @date Apr 13, 2012
16 */
17
18#include <CppUnitLite/TestHarness.h>
19
20#include <gtsam/geometry/PinholeCamera.h>
21#include <gtsam/nonlinear/NonlinearEquality.h>
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <gtsam/inference/Symbol.h>
25#include <gtsam/base/numericalDerivative.h>
26
27#include <gtsam_unstable/slam/InvDepthFactor3.h>
28
29using namespace std;
30using namespace gtsam;
31
32static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
33static SharedNoiseModel sigma(noiseModel::Unit::Create(dim: 2));
34
35// camera pose at (0,0,1) looking straight along the x-axis.
36Pose3 level_pose = Pose3(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), gtsam::Point3(0,0,1));
37PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
38
39typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
40typedef NonlinearEquality<Pose3> PoseConstraint;
41
42Matrix factorError(const Pose3& pose, const Vector5& point, double invDepth,
43 const InverseDepthFactor& factor) {
44 return factor.evaluateError(x: pose, x: point, x: invDepth);
45}
46
47/* ************************************************************************* */
48TEST( InvDepthFactor, optimize) {
49
50 // landmark 5 meters infront of camera (camera center at (0,0,1))
51 Point3 landmark(5, 0, 1);
52
53 // get expected projection using pinhole camera
54 Point2 expected_uv = level_camera.project(pw: landmark);
55
56 InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
57 Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.).finished());
58 // initialize inverse depth with "incorrect" depth of 1/4
59 // in reality this is 1/5, but initial depth is guessed
60 double inv_depth(1./4);
61
62 gtsam::NonlinearFactorGraph graph;
63 Values initial;
64
65 graph.emplace_shared<InverseDepthFactor>(args&: expected_uv, args&: sigma,
66 args: Symbol('x',1), args: Symbol('l',1), args: Symbol('d',1), args&: K);
67 graph.emplace_shared<PoseConstraint>(args: Symbol('x', 1), args&: level_pose);
68 initial.insert(j: Symbol('x',1), val: level_pose);
69 initial.insert(j: Symbol('l',1), val: inv_landmark);
70 initial.insert(j: Symbol('d',1), val: inv_depth);
71
72 LevenbergMarquardtParams lmParams;
73 Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
74
75 // with a single factor the incorrect initialization of 1/4 should not move!
76 EXPECT(assert_equal(initial, result, 1e-9));
77
78 /// Add a second camera
79
80 // add a camera 2 meters to the right
81 Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
82 PinholeCamera<Cal3_S2> right_camera(right_pose, *K);
83
84 // projection measurement of landmark into right camera
85 // this measurement disagrees with the depth initialization
86 // and will push it to 1/5
87 Point2 right_uv = right_camera.project(pw: landmark);
88
89 InverseDepthFactor::shared_ptr factor1(new InverseDepthFactor(right_uv, sigma,
90 Symbol('x',2), Symbol('l',1),Symbol('d',1),K));
91 graph.push_back(factor: factor1);
92
93 graph.emplace_shared<PoseConstraint>(args: Symbol('x',2),args&: right_pose);
94
95 initial.insert(j: Symbol('x',2), val: right_pose);
96
97 Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
98
99 Point3 result2_lmk = InvDepthCamera3<Cal3_S2>::invDepthTo3D(
100 pw: result2.at<Vector5>(j: Symbol('l',1)),
101 rho: result2.at<double>(j: Symbol('d',1)));
102 EXPECT(assert_equal(landmark, result2_lmk, 1e-9));
103
104 // TODO: need to add priors to make this work with
105 // Values result2 = optimize<NonlinearFactorGraph>(graph, initial,
106 // NonlinearOptimizationParameters(),MULTIFRONTAL, GN);
107}
108
109
110/* ************************************************************************* */
111TEST( InvDepthFactor, Jacobian3D ) {
112
113 // landmark 5 meters infront of camera (camera center at (0,0,1))
114 Point3 landmark(5, 0, 1);
115
116 // get expected projection using pinhole camera
117 Point2 expected_uv = level_camera.project(pw: landmark);
118
119 // get expected landmark representation using backprojection
120 InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
121 const auto [inv_landmark, inv_depth] = inv_camera.backproject(pi: expected_uv, depth: 5);
122 Vector5 expected_inv_landmark((Vector(5) << 0., 0., 1., 0., 0.).finished());
123
124 CHECK(assert_equal(expected_inv_landmark, inv_landmark, 1e-6));
125 CHECK(assert_equal(inv_depth, 1./5, 1e-6));
126
127 Symbol poseKey('x',1);
128 Symbol pointKey('l',1);
129 Symbol invDepthKey('d',1);
130 InverseDepthFactor factor(expected_uv, sigma, poseKey, pointKey, invDepthKey, K);
131
132 std::vector<Matrix> actualHs(3);
133 factor.unwhitenedError(x: {{poseKey, genericValue(v: level_pose)},
134 {pointKey, genericValue(v: inv_landmark)},
135 {invDepthKey,genericValue(v: inv_depth)}},
136 H&: actualHs);
137
138 const Matrix& H1Actual = actualHs.at(n: 0);
139 const Matrix& H2Actual = actualHs.at(n: 1);
140 const Matrix& H3Actual = actualHs.at(n: 2);
141
142 // Use numerical derivatives to verify the Jacobians
143 Matrix H1Expected, H2Expected, H3Expected;
144
145 std::function<Matrix(const Pose3 &, const Vector5 &, const double &)>
146 func = std::bind(f: &factorError, args: std::placeholders::_1, args: std::placeholders::_2, args: std::placeholders::_3, args&: factor);
147 H1Expected = numericalDerivative31(h: func, x1: level_pose, x2: inv_landmark, x3: inv_depth);
148 H2Expected = numericalDerivative32(h: func, x1: level_pose, x2: inv_landmark, x3: inv_depth);
149 H3Expected = numericalDerivative33(h: func, x1: level_pose, x2: inv_landmark, x3: inv_depth);
150
151 // Verify the Jacobians
152 CHECK(assert_equal(H1Expected, H1Actual, 1e-6))
153 CHECK(assert_equal(H2Expected, H2Actual, 1e-6))
154 CHECK(assert_equal(H3Expected, H3Actual, 1e-6))
155}
156
157/* ************************************************************************* */
158int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
159/* ************************************************************************* */
160