1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file testVisualISAM2.cpp
14 * @brief Test convergence of visualSLAM example.
15 * @author Duy-Nguyen Ta
16 * @author Frank Dellaert
17 */
18
19#include <CppUnitLite/TestHarness.h>
20
21#include <examples/SFMdata.h>
22#include <gtsam/geometry/Point2.h>
23#include <gtsam/inference/Symbol.h>
24#include <gtsam/nonlinear/ISAM2.h>
25#include <gtsam/nonlinear/NonlinearFactorGraph.h>
26#include <gtsam/nonlinear/Values.h>
27#include <gtsam/slam/ProjectionFactor.h>
28
29#include <vector>
30
31using namespace std;
32using namespace gtsam;
33
34/* ************************************************************************* */
35TEST(testVisualISAM2, all)
36{
37 Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
38
39 auto measurementNoise = noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0);
40
41 // Create ground truth data
42 vector<Point3> points = createPoints();
43 vector<Pose3> poses = createPoses();
44
45 // Set the parameters
46 ISAM2Params parameters;
47 parameters.relinearizeThreshold = 0.01;
48 parameters.relinearizeSkip = 1;
49 ISAM2 isam(parameters);
50
51 // Create a Factor Graph and Values to hold the new data
52 NonlinearFactorGraph graph;
53 Values initialEstimate;
54
55 // Loop over the poses, adding the observations to iSAM incrementally
56 for (size_t i = 0; i < poses.size(); ++i)
57 {
58 // Add factors for each landmark observation
59 for (size_t j = 0; j < points.size(); ++j)
60 {
61 PinholeCamera<Cal3_S2> camera(poses[i], *K);
62 Point2 measurement = camera.project(pw: points[j]);
63 graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
64 args&: measurement, args&: measurementNoise, args: Symbol('x', i), args: Symbol('l', j), args&: K);
65 }
66
67 // Add an initial guess for the current pose
68 // Intentionally initialize the variables off from the ground truth
69 static Pose3 kDeltaPose(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25),
70 Point3(0.05, -0.10, 0.20));
71 initialEstimate.insert(j: Symbol('x', i), val: poses[i] * kDeltaPose);
72
73 // Treat first iteration as special case
74 if (i == 0)
75 {
76 // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
77 static auto kPosePrior = noiseModel::Diagonal::Sigmas(
78 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3))
79 .finished());
80 graph.addPrior(key: Symbol('x', 0), prior: poses[0], model: kPosePrior);
81
82 // Add a prior on landmark l0
83 static auto kPointPrior = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
84 graph.addPrior(key: Symbol('l', 0), prior: points[0], model: kPointPrior);
85
86 // Add initial guesses to all observed landmarks
87 // Intentionally initialize the variables off from the ground truth
88 static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
89 for (size_t j = 0; j < points.size(); ++j)
90 initialEstimate.insert<Point3>(j: Symbol('l', j), val: points[j] + kDeltaPoint);
91 }
92 else
93 {
94 // Update iSAM with the new factors
95 isam.update(newFactors: graph, newTheta: initialEstimate);
96
97 // Do an extra update to converge withing these 8 iterations
98 isam.update();
99
100 // Optimize
101 Values currentEstimate = isam.calculateEstimate();
102
103 // reset for next iteration
104 graph.resize(size: 0);
105 initialEstimate.clear();
106 }
107 } // for loop
108
109 auto result = isam.calculateEstimate();
110 EXPECT_LONGS_EQUAL(16, result.size());
111 for (size_t j = 0; j < points.size(); ++j)
112 {
113 Symbol key('l', j);
114 EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01));
115 }
116}
117
118/* ************************************************************************* */
119int main()
120{
121 TestResult tr;
122 return TestRegistry::runAllTests(result&: tr);
123}
124/* ************************************************************************* */
125