testNonlinearISAM.cpp
Go to the documentation of this file.
1 
6 #include <CppUnitLite/TestHarness.h>
7 
8 #include <gtsam/slam/BetweenFactor.h>
9 #include <gtsam/sam/BearingRangeFactor.h>
10 #include <gtsam/slam/dataset.h>
11 #include <gtsam/nonlinear/NonlinearEquality.h>
12 #include <gtsam/nonlinear/NonlinearISAM.h>
13 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
14 #include <gtsam/nonlinear/Values.h>
15 #include <gtsam/inference/Symbol.h>
16 #include <gtsam/linear/Sampler.h>
17 #include <gtsam/geometry/Pose2.h>
18 
19 #include <iostream>
20 #include <sstream>
21 
22 using namespace std;
23 using namespace gtsam;
24 
25 const double tol=1e-5;
26 
27 /* ************************************************************************* */
28 TEST(testNonlinearISAM, markov_chain ) {
29  int reorder_interval = 2;
30  NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
31  NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
32 
33  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5));
34  Sampler sampler(model, 42u);
35 
36  // create initial graph
37  Pose2 cur_pose; // start at origin
38  NonlinearFactorGraph start_factors;
39  start_factors.emplace_shared<NonlinearEquality<Pose2>>(0, cur_pose);
40 
41  Values init;
42  Values expected;
43  init.insert(0, cur_pose);
44  expected.insert(0, cur_pose);
45  isamChol.update(start_factors, init);
46  isamQR.update(start_factors, init);
47 
48  // loop for a period of time to verify memory usage
49  size_t nrPoses = 21;
50  Pose2 z(1.0, 2.0, 0.1);
51  for (size_t i=1; i<=nrPoses; ++i) {
52  NonlinearFactorGraph new_factors;
53  new_factors.emplace_shared<BetweenFactor<Pose2>>(i-1, i, z, model);
54  Values new_init;
55 
56  cur_pose = cur_pose.compose(z);
57  new_init.insert(i, cur_pose.retract(sampler.sample()));
58  expected.insert(i, cur_pose);
59  isamChol.update(new_factors, new_init);
60  isamQR.update(new_factors, new_init);
61  }
62 
63  // verify values - all but the last one should be very close
64  Values actualChol = isamChol.estimate();
65  for (size_t i=0; i<nrPoses; ++i) {
66  EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), tol));
67  }
68  Values actualQR = isamQR.estimate();
69  for (size_t i=0; i<nrPoses; ++i) {
70  EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), tol));
71  }
72 }
73 
74 /* ************************************************************************* */
75 TEST(testNonlinearISAM, markov_chain_with_disconnects ) {
76  int reorder_interval = 2;
77  NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
78  NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
79 
80  SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5));
81  SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector2(2.0, 2.0));
82  Sampler sampler(model3, 42u);
83 
84  // create initial graph
85  Pose2 cur_pose; // start at origin
86  NonlinearFactorGraph start_factors;
87  start_factors.emplace_shared<NonlinearEquality<Pose2>>(0, cur_pose);
88 
89  Values init;
90  Values expected;
91  init.insert(0, cur_pose);
92  expected.insert(0, cur_pose);
93  isamChol.update(start_factors, init);
94  isamQR.update(start_factors, init);
95 
96  size_t nrPoses = 21;
97 
98  // create a constrained constellation of landmarks
99  Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3;
100  Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.);
101  expected.insert(lm1, landmark1);
102  expected.insert(lm2, landmark2);
103  expected.insert(lm3, landmark3);
104 
105  // loop for a period of time to verify memory usage
106  Pose2 z(1.0, 2.0, 0.1);
107  for (size_t i=1; i<=nrPoses; ++i) {
108  NonlinearFactorGraph new_factors;
109  new_factors.emplace_shared<BetweenFactor<Pose2>>(i-1, i, z, model3);
110  Values new_init;
111 
112  cur_pose = cur_pose.compose(z);
113  new_init.insert(i, cur_pose.retract(sampler.sample()));
114  expected.insert(i, cur_pose);
115 
116  // Add a floating landmark constellation
117  if (i == 7) {
118  new_factors.addPrior(lm1, landmark1, model2);
119  new_factors.addPrior(lm2, landmark2, model2);
120  new_factors.addPrior(lm3, landmark3, model2);
121 
122  // Initialize to origin
123  new_init.insert(lm1, Point2(0,0));
124  new_init.insert(lm2, Point2(0,0));
125  new_init.insert(lm3, Point2(0,0));
126  }
127 
128  isamChol.update(new_factors, new_init);
129  isamQR.update(new_factors, new_init);
130  }
131 
132  // verify values - all but the last one should be very close
133  Values actualChol = isamChol.estimate();
134  for (size_t i=0; i<nrPoses; ++i)
135  EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), tol));
136 
137  Values actualQR = isamQR.estimate();
138  for (size_t i=0; i<nrPoses; ++i)
139  EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), tol));
140 
141  // Check landmarks
142  EXPECT(assert_equal(expected.at<Point2>(lm1), actualChol.at<Point2>(lm1), tol));
143  EXPECT(assert_equal(expected.at<Point2>(lm2), actualChol.at<Point2>(lm2), tol));
144  EXPECT(assert_equal(expected.at<Point2>(lm3), actualChol.at<Point2>(lm3), tol));
145 
146  EXPECT(assert_equal(expected.at<Point2>(lm1), actualQR.at<Point2>(lm1), tol));
147  EXPECT(assert_equal(expected.at<Point2>(lm2), actualQR.at<Point2>(lm2), tol));
148  EXPECT(assert_equal(expected.at<Point2>(lm3), actualQR.at<Point2>(lm3), tol));
149 }
150 
151 /* ************************************************************************* */
152 TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
153  int reorder_interval = 2;
154  NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
155  NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
156 
157  SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5));
158  SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector2(2.0, 2.0));
159  Sampler sampler(model3, 42u);
160 
161  // create initial graph
162  Pose2 cur_pose; // start at origin
163  NonlinearFactorGraph start_factors;
164  start_factors.emplace_shared<NonlinearEquality<Pose2>>(0, cur_pose);
165 
166  Values init;
167  Values expected;
168  init.insert(0, cur_pose);
169  expected.insert(0, cur_pose);
170  isamChol.update(start_factors, init);
171  isamQR.update(start_factors, init);
172 
173  size_t nrPoses = 21;
174 
175  // create a constrained constellation of landmarks
176  Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3;
177  Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.);
178  expected.insert(lm1, landmark1);
179  expected.insert(lm2, landmark2);
180  expected.insert(lm3, landmark3);
181 
182  // loop for a period of time to verify memory usage
183  Pose2 z(1.0, 2.0, 0.1);
184  for (size_t i=1; i<=nrPoses; ++i) {
185  NonlinearFactorGraph new_factors;
186  new_factors.emplace_shared<BetweenFactor<Pose2>>(i-1, i, z, model3);
187  Values new_init;
188 
189  cur_pose = cur_pose.compose(z);
190  new_init.insert(i, cur_pose.retract(sampler.sample()));
191  expected.insert(i, cur_pose);
192 
193  // Add a floating landmark constellation
194  if (i == 7) {
195  new_factors.addPrior(lm1, landmark1, model2);
196  new_factors.addPrior(lm2, landmark2, model2);
197  new_factors.addPrior(lm3, landmark3, model2);
198 
199  // Initialize to origin
200  new_init.insert(lm1, Point2(0,0));
201  new_init.insert(lm2, Point2(0,0));
202  new_init.insert(lm3, Point2(0,0));
203  }
204 
205  // Reconnect with observation later
206  if (i == 15) {
207  new_factors.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
208  i, lm1, cur_pose.bearing(landmark1), cur_pose.range(landmark1), model2);
209  }
210 
211  isamChol.update(new_factors, new_init);
212  isamQR.update(new_factors, new_init);
213  }
214 
215  // verify values - all but the last one should be very close
216  Values actualChol = isamChol.estimate();
217  for (size_t i=0; i<nrPoses; ++i)
218  EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), 1e-4));
219 
220  Values actualQR = isamQR.estimate();
221  for (size_t i=0; i<nrPoses; ++i)
222  EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), 1e-4));
223 
224  // Check landmarks
225  EXPECT(assert_equal(expected.at<Point2>(lm1), actualChol.at<Point2>(lm1), tol));
226  EXPECT(assert_equal(expected.at<Point2>(lm2), actualChol.at<Point2>(lm2), tol));
227  EXPECT(assert_equal(expected.at<Point2>(lm3), actualChol.at<Point2>(lm3), tol));
228 
229  EXPECT(assert_equal(expected.at<Point2>(lm1), actualQR.at<Point2>(lm1), tol));
230  EXPECT(assert_equal(expected.at<Point2>(lm2), actualQR.at<Point2>(lm2), tol));
231  EXPECT(assert_equal(expected.at<Point2>(lm3), actualQR.at<Point2>(lm3), tol));
232 }
233 
234 /* ************************************************************************* */
235 TEST(testNonlinearISAM, loop_closures ) {
236  int relinearizeInterval = 100;
237  NonlinearISAM isam(relinearizeInterval);
238 
239  // Create a Factor Graph and Values to hold the new data
240  NonlinearFactorGraph graph;
241  Values initialEstimate;
242 
243  vector<string> lines;
244  lines.emplace_back("VERTEX2 0 0.000000 0.000000 0.000000");
245  lines.emplace_back("EDGE2 1 0 1.030390 0.011350 -0.012958");
246  lines.emplace_back("VERTEX2 1 1.030390 0.011350 -0.012958");
247  lines.emplace_back("EDGE2 2 1 1.013900 -0.058639 -0.013225");
248  lines.emplace_back("VERTEX2 2 2.043445 -0.060422 -0.026183");
249  lines.emplace_back("EDGE2 3 2 1.027650 -0.007456 0.004833");
250  lines.emplace_back("VERTEX2 3 3.070548 -0.094779 -0.021350");
251  lines.emplace_back("EDGE2 4 3 -0.012016 1.004360 1.566790");
252  lines.emplace_back("VERTEX2 4 3.079976 0.909609 1.545440");
253  lines.emplace_back("EDGE2 5 4 1.016030 0.014565 -0.016304");
254  lines.emplace_back("VERTEX2 5 3.091176 1.925681 1.529136");
255  lines.emplace_back("EDGE2 6 5 1.023890 0.006808 0.010981");
256  lines.emplace_back("VERTEX2 6 3.127018 2.948966 1.540117");
257  lines.emplace_back("EDGE2 7 6 0.957734 0.003159 0.010901");
258  lines.emplace_back("VERTEX2 7 3.153237 3.906347 1.551018");
259  lines.emplace_back("EDGE2 8 7 -1.023820 -0.013668 -3.093240");
260  lines.emplace_back("VERTEX2 8 3.146655 2.882457 -1.542222");
261  lines.emplace_back("EDGE2 9 8 1.023440 0.013984 -0.007802");
262  lines.emplace_back("EDGE2 9 5 0.033943 0.032439 -3.127400");
263  lines.emplace_back("VERTEX2 9 3.189873 1.859834 -1.550024");
264  lines.emplace_back("EDGE2 10 9 1.003350 0.022250 0.023491");
265  lines.emplace_back("EDGE2 10 3 0.044020 0.988477 -1.563530");
266  lines.emplace_back("VERTEX2 10 3.232959 0.857162 -1.526533");
267  lines.emplace_back("EDGE2 11 10 0.977245 0.019042 -0.028623");
268  lines.emplace_back("VERTEX2 11 3.295225 -0.118283 -1.555156");
269  lines.emplace_back("EDGE2 12 11 -0.996880 -0.025512 -3.126915");
270  lines.emplace_back("VERTEX2 12 3.254125 0.878076 1.601114");
271  lines.emplace_back("EDGE2 13 12 0.990646 0.018396 -0.016519");
272  lines.emplace_back("VERTEX2 13 3.205708 1.867709 1.584594");
273  lines.emplace_back("EDGE2 14 13 0.945873 0.008893 -0.002602");
274  lines.emplace_back("EDGE2 14 8 0.015808 0.021059 3.128310");
275  lines.emplace_back("VERTEX2 14 3.183765 2.813370 1.581993");
276  lines.emplace_back("EDGE2 15 14 1.000010 0.006428 0.028234");
277  lines.emplace_back("EDGE2 15 7 -0.014728 -0.001595 -0.019579");
278  lines.emplace_back("VERTEX2 15 3.166141 3.813245 1.610227");
279 
280  auto model = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5));
281 
282  // Loop over the different poses, adding the observations to iSAM incrementally
283  for (const string& str : lines) {
284  // scan the tag
285  string tag;
286  istringstream is(str);
287  if (!(is >> tag))
288  break;
289 
290  // Check if vertex
291  const auto indexedPose = parseVertexPose(is, tag);
292  if (indexedPose) {
293  Key id = indexedPose->first;
294  initialEstimate.insert(Symbol('x', id), indexedPose->second);
295  if (id == 0) {
296  noiseModel::Diagonal::shared_ptr priorNoise =
297  noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001));
298  graph.addPrior(Symbol('x', id), Pose2(0, 0, 0), priorNoise);
299  } else {
300  isam.update(graph, initialEstimate);
301 
302  // Clear the factor graph and values for the next iteration
303  graph.resize(0);
304  initialEstimate.clear();
305  }
306  }
307 
308  // check if edge
309  const auto betweenPose = parseEdge(is, tag);
310  if (betweenPose) {
311  size_t id1, id2;
312  tie(id1, id2) = betweenPose->first;
313  graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2),
314  Symbol('x', id1), betweenPose->second, model);
315  }
316  }
317  EXPECT_LONGS_EQUAL(16, isam.estimate().size())
318 }
319 
320 /* ************************************************************************* */
321 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
322 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
model2
auto model2
Definition: testFunctorizedFactor.cpp:40
gtsam::Values::size
size_t size() const
Definition: Values.h:178
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
TestHarness.h
TEST
TEST(testNonlinearISAM, markov_chain)
Definition: testNonlinearISAM.cpp:28
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
NonlinearISAM.h
relinearizeInterval
int relinearizeInterval
Definition: doc/Code/VisualISAMExample.cpp:1
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:44
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
landmark2
Point2 landmark2(7.0, 1.5)
gtsam::EliminatePreferCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:539
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:259
landmark1
Point2 landmark1(5.0, 1.5)
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
dataset.h
utility functions for loading datasets
BetweenFactor.h
gtsam::BearingRangeFactor
Definition: sam/BearingRangeFactor.h:34
main
int main()
Definition: testNonlinearISAM.cpp:321
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::parseVertexPose
std::optional< IndexedPose > parseVertexPose(std::istream &is, const std::string &tag)
Definition: dataset.cpp:173
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:916
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::NonlinearISAM::estimate
Values estimate() const
Definition: NonlinearISAM.cpp:80
Symbol.h
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
NonlinearEquality.h
lines
void lines()
Definition: geo_hyperplane.cpp:88
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1958
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
TestResult
Definition: TestResult.h:26
str
Definition: pytypes.h:1560
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition: Sampler.cpp:62
gtsam::Pose2::bearing
Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Definition: Pose2.cpp:260
gtsam
traits
Definition: ABC.h:17
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:156
gtsam::NonlinearISAM
Definition: NonlinearISAM.h:27
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::Pose2::range
double range(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Definition: Pose2.cpp:285
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
gtsam::parseEdge
std::optional< IndexedEdge > parseEdge(std::istream &is, const std::string &tag)
Definition: dataset.cpp:299
Sampler.h
sampling from a NoiseModel
model3
const SharedNoiseModel model3
Definition: testPoseRotationPrior.cpp:22
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
init
Definition: TutorialInplaceLU.cpp:2
tol
const double tol
Definition: testNonlinearISAM.cpp:25
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::Sampler
Definition: Sampler.h:31
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Symbol
Definition: inference/Symbol.h:37
BearingRangeFactor.h
a single factor contains both the bearing and the range to prevent handle to pair bearing and range f...
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:804

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:07:01