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