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* @file SolverComparer.cpp
13* @brief Incremental and batch solving, timing, and accuracy comparisons
14* @author Richard Roberts
15* @date August, 2013
16*
17* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
18*
19* Solve in incremental and write to file w_inc:
20* ./SolverComparer --incremental -d w10000 -o w_inc
21*
22* You can then perturb that initialization to get batch something to optimize.
23* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
24* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
25*
26* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
27* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
28*
29* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
30* ./SolverComparer --compare w_inc w_batch
31*
32*/
33
34#include <gtsam/slam/BetweenFactor.h>
35#include <gtsam/sam/BearingRangeFactor.h>
36#include <gtsam/slam/dataset.h>
37#include <gtsam/geometry/Pose2.h>
38#include <gtsam/nonlinear/ISAM2.h>
39#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
40#include <gtsam/nonlinear/Marginals.h>
41#include <gtsam/linear/GaussianJunctionTree.h>
42#include <gtsam/linear/GaussianEliminationTree.h>
43#include <gtsam/inference/Symbol.h>
44#include <gtsam/base/timing.h>
45#include <gtsam/base/treeTraversal-inst.h>
46#include <gtsam/config.h> // for GTSAM_USE_TBB
47
48#include <boost/archive/binary_iarchive.hpp>
49#include <boost/archive/binary_oarchive.hpp>
50#include <boost/program_options.hpp>
51#include <boost/range/algorithm/set_algorithm.hpp>
52
53#include <fstream>
54#include <iostream>
55#include <random>
56
57#ifdef GTSAM_USE_TBB
58#include <tbb/task_arena.h> // tbb::task_arena
59#include <tbb/task_group.h> // tbb::task_group
60#endif
61
62using namespace std;
63using namespace gtsam;
64using namespace gtsam::symbol_shorthand;
65namespace po = boost::program_options;
66namespace br = boost::range;
67
68typedef Pose2 Pose;
69
70typedef NoiseModelFactorN<Pose> NM1;
71typedef NoiseModelFactorN<Pose,Pose> NM2;
72typedef BearingRangeFactor<Pose,Point2> BR;
73
74double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
75 // Compute degrees of freedom (observations - variables)
76 // In ocaml, +1 was added to the observations to account for the prior, but
77 // the factor graph already includes a factor for the prior/equality constraint.
78 // double dof = graph.size() - config.size();
79 int graph_dim = 0;
80 for(const std::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
81 graph_dim += (int)nlf->dim();
82 }
83 double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
84 return 2. * graph.error(values: config) / dof; // kaess: added factor 2, graph.error returns half of actual error
85}
86
87// Global variables (these are only set once at program startup and never modified after)
88string outputFile;
89string inputFile;
90string datasetName;
91int firstStep;
92int lastStep;
93int nThreads;
94int relinSkip;
95bool incremental;
96bool dogleg;
97bool batch;
98bool compare;
99bool perturb;
100bool stats;
101double perturbationNoise;
102string compareFile1, compareFile2;
103
104Values initial;
105NonlinearFactorGraph datasetMeasurements;
106
107// Run functions for each mode
108void runIncremental();
109void runBatch();
110void runCompare();
111void runPerturb();
112void runStats();
113
114/* ************************************************************************* */
115int main(int argc, char *argv[]) {
116
117 po::options_description desc("Available options");
118 desc.add_options()
119 ("help", "Print help message")
120 ("write-solution,o", po::value<string>(v: &outputFile)->default_value(v: ""), "Write graph and solution to the specified file")
121 ("read-solution,i", po::value<string>(v: &inputFile)->default_value(v: ""), "Read graph and solution from the specified file")
122 ("dataset,d", po::value<string>(v: &datasetName)->default_value(v: ""), "Read a dataset file (if and only if --incremental is used)")
123 ("first-step,f", po::value<int>(v: &firstStep)->default_value(v: 0), "First step to process from the dataset file")
124 ("last-step,l", po::value<int>(v: &lastStep)->default_value(v: -1), "Last step to process, or -1 to process until the end of the dataset")
125 ("threads", po::value<int>(v: &nThreads)->default_value(v: -1), "Number of threads, or -1 to use all processors")
126 ("relinSkip", po::value<int>(v: &relinSkip)->default_value(v: 10), "Fluid relinearization check every arg steps")
127 ("incremental", "Run in incremental mode using ISAM2 (default)")
128 ("dogleg", "When in incremental mode, solve with Dogleg instead of Gauss-Newton in iSAM2")
129 ("batch", "Run in batch mode, requires an initialization from --read-solution")
130 ("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files")
131 ("perturb", po::value<double>(v: &perturbationNoise), "Perturb a solution file with the specified noise")
132 ("stats", "Gather factorization statistics about the dataset, writes text-file histograms")
133 ;
134 po::variables_map vm;
135 po::store(options: po::command_line_parser(argc, argv).options(desc).run(), m&: vm);
136 po::notify(m&: vm);
137
138 batch = (vm.count(x: "batch") > 0);
139 compare = (vm.count(x: "compare") > 0);
140 perturb = (vm.count(x: "perturb") > 0);
141 stats = (vm.count(x: "stats") > 0);
142 const int modesSpecified = int(batch) + int(compare) + int(perturb) + int(stats);
143 incremental = (vm.count(x: "incremental") > 0 || modesSpecified == 0);
144 dogleg = (vm.count(x: "dogleg") > 0);
145 if(compare) {
146 const vector<string>& compareFiles = vm["compare"].as<vector<string> >();
147 if(compareFiles.size() != 2) {
148 cout << "Must specify two files with --compare";
149 exit(status: 1);
150 }
151 compareFile1 = compareFiles[0];
152 compareFile2 = compareFiles[1];
153 }
154
155 if(modesSpecified > 1) {
156 cout << "Only one of --incremental, --batch, --compare, --perturb, and --stats may be specified\n" << desc << endl;
157 exit(status: 1);
158 }
159 if((incremental || batch) && datasetName.empty()) {
160 cout << "In incremental and batch modes, a dataset must be specified\n" << desc << endl;
161 exit(status: 1);
162 }
163 if(!(incremental || batch || stats) && !datasetName.empty()) {
164 cout << "A dataset may only be specified in incremental or batch modes\n" << desc << endl;
165 exit(status: 1);
166 }
167 if(batch && inputFile.empty()) {
168 cout << "In batch model, an input file must be specified\n" << desc << endl;
169 exit(status: 1);
170 }
171 if(perturb && (inputFile.empty() || outputFile.empty())) {
172 cout << "In perturb mode, specify input and output files\n" << desc << endl;
173 exit(status: 1);
174 }
175 if(stats && (datasetName.empty() || inputFile.empty())) {
176 cout << "In stats mode, specify dataset and input file\n" << desc << endl;
177 exit(status: 1);
178 }
179
180 // Read input file
181 if(!inputFile.empty())
182 {
183 cout << "Reading input file " << inputFile << endl;
184 std::ifstream readerStream(inputFile.c_str(), ios::binary);
185 boost::archive::binary_iarchive reader(readerStream);
186 reader >> initial;
187 }
188
189 // Read dataset
190 if(!datasetName.empty())
191 {
192 cout << "Loading dataset " << datasetName << endl;
193 try {
194 string datasetFile = findExampleDataFile(name: datasetName);
195 std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
196 load2D(filename: datasetFile);
197 datasetMeasurements = *data.first;
198 } catch(std::exception& e) {
199 cout << e.what() << endl;
200 exit(status: 1);
201 }
202 }
203
204#ifdef GTSAM_USE_TBB
205 tbb::task_arena arena;
206 tbb::task_group tg;
207 if(nThreads > 0) {
208 cout << "Using " << nThreads << " threads" << endl;
209 arena.initialize(nThreads);
210 } else
211 cout << "Using threads for all processors" << endl;
212#else
213 if(nThreads > 0) {
214 std::cout << "GTSAM is not compiled with TBB, so threading is disabled and the --threads option cannot be used." << endl;
215 exit(status: 1);
216 }
217#endif
218
219#ifdef GTSAM_USE_TBB
220 arena.execute([&]{
221 tg.run_and_wait([&]{
222#endif
223 // Run mode
224 if(incremental)
225 runIncremental();
226 else if(batch)
227 runBatch();
228 else if(compare)
229 runCompare();
230 else if(perturb)
231 runPerturb();
232 else if(stats)
233 runStats();
234#ifdef GTSAM_USE_TBB
235 });
236 });
237#endif
238
239 return 0;
240}
241
242/* ************************************************************************* */
243void runIncremental()
244{
245 ISAM2Params params;
246 if(dogleg)
247 params.optimizationParams = ISAM2DoglegParams();
248 params.relinearizeSkip = relinSkip;
249 params.enablePartialRelinearizationCheck = true;
250 ISAM2 isam2(params);
251
252 // Look for the first measurement to use
253 cout << "Looking for first measurement from step " << firstStep << endl;
254 size_t nextMeasurement = 0;
255 bool havePreviousPose = false;
256 Key firstPose = 0;
257 while(nextMeasurement < datasetMeasurements.size())
258 {
259 if(BetweenFactor<Pose>::shared_ptr factor =
260 std::dynamic_pointer_cast<BetweenFactor<Pose> >(r: datasetMeasurements[nextMeasurement]))
261 {
262 Key key1 = factor->key<1>(), key2 = factor->key<2>();
263 if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
264 // We found an odometry starting at firstStep
265 firstPose = std::min(a: key1, b: key2);
266 break;
267 }
268 if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) {
269 // We found an odometry joining firstStep with a previous pose
270 havePreviousPose = true;
271 firstPose = std::max(a: key1, b: key2);
272 break;
273 }
274 }
275 ++ nextMeasurement;
276 }
277
278 if(nextMeasurement == datasetMeasurements.size()) {
279 cout << "The supplied first step is past the end of the dataset" << endl;
280 exit(status: 1);
281 }
282
283 // If we didn't find an odometry linking to a previous pose, create a first pose and a prior
284 if(!havePreviousPose) {
285 cout << "Looks like " << firstPose << " is the first time step, so adding a prior on it" << endl;
286 NonlinearFactorGraph newFactors;
287 Values newVariables;
288
289 newFactors.addPrior(key: firstPose, prior: Pose(), model: noiseModel::Unit::Create(dim: 3));
290 newVariables.insert(j: firstPose, val: Pose());
291
292 isam2.update(newFactors, newTheta: newVariables);
293 }
294
295 cout << "Playing forward time steps..." << endl;
296
297 for (size_t step = firstPose;
298 nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep);
299 ++step)
300 {
301 Values newVariables;
302 NonlinearFactorGraph newFactors;
303
304 // Collect measurements and new variables for the current step
305 gttic_(Collect_measurements);
306 while(nextMeasurement < datasetMeasurements.size()) {
307
308 NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
309
310 if(BetweenFactor<Pose>::shared_ptr factor =
311 std::dynamic_pointer_cast<BetweenFactor<Pose> >(r: measurementf))
312 {
313 // Stop collecting measurements that are for future steps
314 if(factor->key<1>() > step || factor->key<2>() > step)
315 break;
316
317 // Require that one of the nodes is the current one
318 if(factor->key<1>() != step && factor->key<2>() != step)
319 throw runtime_error("Problem in data file, out-of-sequence measurements");
320
321 // Add a new factor
322 newFactors.push_back(factor);
323 const auto& measured = factor->measured();
324
325 // Initialize the new variable
326 if(factor->key<1>() > factor->key<2>()) {
327 if(!newVariables.exists(j: factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry
328 if(step == 1)
329 newVariables.insert(j: factor->key<1>(), val: measured.inverse());
330 else {
331 Pose prevPose = isam2.calculateEstimate<Pose>(key: factor->key<2>());
332 newVariables.insert(j: factor->key<1>(), val: prevPose * measured.inverse());
333 }
334 }
335 } else {
336 if(!newVariables.exists(j: factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry
337 if(step == 1)
338 newVariables.insert(j: factor->key<2>(), val: measured);
339 else {
340 Pose prevPose = isam2.calculateEstimate<Pose>(key: factor->key<1>());
341 newVariables.insert(j: factor->key<2>(), val: prevPose * measured);
342 }
343 }
344 }
345 }
346 else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
347 std::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(r: measurementf))
348 {
349 Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
350
351 // Stop collecting measurements that are for future steps
352 if(poseKey > step)
353 throw runtime_error("Problem in data file, out-of-sequence measurements");
354
355 // Add new factor
356 newFactors.push_back(factor);
357
358 // Initialize new landmark
359 if(!isam2.getLinearizationPoint().exists(j: lmKey))
360 {
361 Pose pose;
362 if(isam2.getLinearizationPoint().exists(j: poseKey))
363 pose = isam2.calculateEstimate<Pose>(key: poseKey);
364 else
365 pose = newVariables.at<Pose>(j: poseKey);
366 const auto& measured = factor->measured();
367 Rot2 measuredBearing = measured.bearing();
368 double measuredRange = measured.range();
369 newVariables.insert(j: lmKey,
370 val: pose.transformFrom(point: measuredBearing.rotate(p: Point2(measuredRange, 0.0))));
371 }
372 }
373 else
374 {
375 throw std::runtime_error("Unknown factor type read from data file");
376 }
377 ++ nextMeasurement;
378 }
379 gttoc_(Collect_measurements);
380
381 // Update iSAM2
382 try {
383 gttic_(Update_ISAM2);
384 isam2.update(newFactors, newTheta: newVariables);
385 gttoc_(Update_ISAM2);
386 } catch(std::exception& e) {
387 cout << e.what() << endl;
388 exit(status: 1);
389 }
390
391 if((step - firstPose) % 1000 == 0) {
392 try {
393 gttic_(chi2);
394 Values estimate(isam2.calculateEstimate());
395 double chi2 = chi2_red(graph: isam2.getFactorsUnsafe(), config: estimate);
396 cout << "chi2 = " << chi2 << endl;
397 gttoc_(chi2);
398 } catch(std::exception& e) {
399 cout << e.what() << endl;
400 exit(status: 1);
401 }
402 }
403
404 tictoc_finishedIteration_();
405
406 if((step - firstPose) % 1000 == 0) {
407 cout << "Step " << step << endl;
408 tictoc_print_();
409 }
410 }
411
412 if(!outputFile.empty())
413 {
414 try {
415 cout << "Writing output file " << outputFile << endl;
416 std::ofstream writerStream(outputFile.c_str(), ios::binary);
417 boost::archive::binary_oarchive writer(writerStream);
418 Values estimates = isam2.calculateEstimate();
419 writer << estimates;
420 } catch(std::exception& e) {
421 cout << e.what() << endl;
422 exit(status: 1);
423 }
424 }
425
426 tictoc_print_();
427
428 // Compute marginals
429 //try {
430 // Marginals marginals(graph, values);
431 // int i=0;
432 // for (Key key1: boost::adaptors::reverse(values.keys())) {
433 // int j=0;
434 // for (Key key12: boost::adaptors::reverse(values.keys())) {
435 // if(i != j) {
436 // gttic_(jointMarginalInformation);
437 // KeyVector keys(2);
438 // keys[0] = key1;
439 // keys[1] = key2;
440 // JointMarginal info = marginals.jointMarginalInformation(keys);
441 // gttoc_(jointMarginalInformation);
442 // tictoc_finishedIteration_();
443 // }
444 // ++j;
445 // if(j >= 50)
446 // break;
447 // }
448 // ++i;
449 // if(i >= 50)
450 // break;
451 // }
452 // tictoc_print_();
453 // for(Key key: values.keys()) {
454 // gttic_(marginalInformation);
455 // Matrix info = marginals.marginalInformation(key);
456 // gttoc_(marginalInformation);
457 // tictoc_finishedIteration_();
458 // ++i;
459 // }
460 //} catch(std::exception& e) {
461 // cout << e.what() << endl;
462 //}
463 //tictoc_print_();
464}
465
466/* ************************************************************************* */
467void runBatch()
468{
469 cout << "Creating batch optimizer..." << endl;
470
471 NonlinearFactorGraph measurements = datasetMeasurements;
472 measurements.addPrior(key: 0, prior: Pose(), model: noiseModel::Unit::Create(dim: 3));
473
474 gttic_(Create_optimizer);
475 GaussNewtonParams params;
476 params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY;
477 GaussNewtonOptimizer optimizer(measurements, initial, params);
478 gttoc_(Create_optimizer);
479 double lastError;
480 do {
481 lastError = optimizer.error();
482 gttic_(Iterate_optimizer);
483 optimizer.iterate();
484 gttoc_(Iterate_optimizer);
485 cout << "Error: " << lastError << " -> " << optimizer.error() /*<< ", lambda: " << optimizer.lambda()*/ << endl;
486 gttic_(chi2);
487 double chi2 = chi2_red(graph: measurements, config: optimizer.values());
488 cout << "chi2 = " << chi2 << endl;
489 gttoc_(chi2);
490 } while(!checkConvergence(relativeErrorTreshold: optimizer.params().relativeErrorTol,
491 absoluteErrorTreshold: optimizer.params().absoluteErrorTol, errorThreshold: optimizer.params().errorTol,
492 currentError: lastError, newError: optimizer.error(), verbosity: optimizer.params().verbosity));
493
494 tictoc_finishedIteration_();
495 tictoc_print_();
496
497 if(!outputFile.empty())
498 {
499 try {
500 cout << "Writing output file " << outputFile << endl;
501 std::ofstream writerStream(outputFile.c_str(), ios::binary);
502 boost::archive::binary_oarchive writer(writerStream);
503 writer << optimizer.values();
504 } catch(std::exception& e) {
505 cout << e.what() << endl;
506 exit(status: 1);
507 }
508 }
509}
510
511/* ************************************************************************* */
512void runCompare()
513{
514 Values soln1, soln2;
515
516 cout << "Reading solution file " << compareFile1 << endl;
517 {
518 std::ifstream readerStream(compareFile1.c_str(), ios::binary);
519 boost::archive::binary_iarchive reader(readerStream);
520 reader >> soln1;
521 }
522
523 cout << "Reading solution file " << compareFile2 << endl;
524 {
525 std::ifstream readerStream(compareFile2.c_str(), ios::binary);
526 boost::archive::binary_iarchive reader(readerStream);
527 reader >> soln2;
528 }
529
530 // Check solution for equality
531 cout << "Comparing solutions..." << endl;
532 KeyVector missingKeys;
533 br::set_symmetric_difference(rng1: soln1.keys(), rng2: soln2.keys(), out: std::back_inserter(x&: missingKeys));
534 if(!missingKeys.empty()) {
535 cout << " Keys unique to one solution file: ";
536 for(size_t i = 0; i < missingKeys.size(); ++i) {
537 cout << DefaultKeyFormatter(missingKeys[i]);
538 if(i != missingKeys.size() - 1)
539 cout << ", ";
540 }
541 cout << endl;
542 }
543 KeyVector commonKeys;
544 br::set_intersection(rng1: soln1.keys(), rng2: soln2.keys(), out: std::back_inserter(x&: commonKeys));
545 double maxDiff = 0.0;
546 for(Key j: commonKeys)
547 maxDiff = std::max(a: maxDiff, b: soln1.at(j).localCoordinates_(value: soln2.at(j)).norm());
548 cout << " Maximum solution difference (norm of logmap): " << maxDiff << endl;
549}
550
551/* ************************************************************************* */
552void runPerturb()
553{
554 // Set up random number generator
555 std::mt19937 rng;
556 std::normal_distribution<double> normal(0.0, perturbationNoise);
557
558 // Perturb values
559 VectorValues noise;
560 for(const auto& [key, dim]: initial.dims())
561 {
562 Vector noisev(dim);
563 for(Vector::Index i = 0; i < noisev.size(); ++i)
564 noisev(i) = normal(rng);
565 noise.insert(j: key, value: noisev);
566 }
567 Values perturbed = initial.retract(delta: noise);
568
569 // Write results
570 try {
571 cout << "Writing output file " << outputFile << endl;
572 std::ofstream writerStream(outputFile.c_str(), ios::binary);
573 boost::archive::binary_oarchive writer(writerStream);
574 writer << perturbed;
575 } catch(std::exception& e) {
576 cout << e.what() << endl;
577 exit(status: 1);
578 }
579
580}
581
582/* ************************************************************************* */
583void runStats()
584{
585 cout << "Gathering statistics..." << endl;
586 GaussianFactorGraph linear = *datasetMeasurements.linearize(linearizationPoint: initial);
587 GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(graph: linear)));
588 treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(forest: jt);
589
590 ofstream file;
591
592 cout << "Writing SolverComparer_Stats_problemSizeHistogram.txt..." << endl;
593 file.open(s: "SolverComparer_Stats_problemSizeHistogram.txt");
594 treeTraversal::ForestStatistics::Write(outStream&: file, histogram: statistics.problemSizeHistogram);
595 file.close();
596
597 cout << "Writing SolverComparer_Stats_numberOfChildrenHistogram.txt..." << endl;
598 file.open(s: "SolverComparer_Stats_numberOfChildrenHistogram.txt");
599 treeTraversal::ForestStatistics::Write(outStream&: file, histogram: statistics.numberOfChildrenHistogram);
600 file.close();
601
602 cout << "Writing SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt..." << endl;
603 file.open(s: "SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt");
604 treeTraversal::ForestStatistics::Write(outStream&: file, histogram: statistics.problemSizeOfSecondLargestChildHistogram);
605 file.close();
606}
607