SolverComparer.cpp
Go to the documentation of this file.
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 
36 #include <gtsam/slam/dataset.h>
37 #include <gtsam/geometry/Pose2.h>
38 #include <gtsam/nonlinear/ISAM2.h>
43 #include <gtsam/inference/Symbol.h>
44 #include <gtsam/base/timing.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 
62 using namespace std;
63 using namespace gtsam;
64 using namespace gtsam::symbol_shorthand;
65 namespace po = boost::program_options;
66 namespace br = boost::range;
67 
68 typedef Pose2 Pose;
69 
73 
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(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)
88 string outputFile;
89 string inputFile;
90 string datasetName;
96 bool dogleg;
97 bool batch;
98 bool compare;
99 bool perturb;
100 bool stats;
103 
106 
107 // Run functions for each mode
108 void runIncremental();
109 void runBatch();
110 void runCompare();
111 void runPerturb();
112 void runStats();
113 
114 /* ************************************************************************* */
115 int 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>(&outputFile)->default_value(""), "Write graph and solution to the specified file")
121  ("read-solution,i", po::value<string>(&inputFile)->default_value(""), "Read graph and solution from the specified file")
122  ("dataset,d", po::value<string>(&datasetName)->default_value(""), "Read a dataset file (if and only if --incremental is used)")
123  ("first-step,f", po::value<int>(&firstStep)->default_value(0), "First step to process from the dataset file")
124  ("last-step,l", po::value<int>(&lastStep)->default_value(-1), "Last step to process, or -1 to process until the end of the dataset")
125  ("threads", po::value<int>(&nThreads)->default_value(-1), "Number of threads, or -1 to use all processors")
126  ("relinSkip", po::value<int>(&relinSkip)->default_value(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>(&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(po::command_line_parser(argc, argv).options(desc).run(), vm);
136  po::notify(vm);
137 
138  batch = (vm.count("batch") > 0);
139  compare = (vm.count("compare") > 0);
140  perturb = (vm.count("perturb") > 0);
141  stats = (vm.count("stats") > 0);
142  const int modesSpecified = int(batch) + int(compare) + int(perturb) + int(stats);
143  incremental = (vm.count("incremental") > 0 || modesSpecified == 0);
144  dogleg = (vm.count("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(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(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(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(1);
166  }
167  if(batch && inputFile.empty()) {
168  cout << "In batch model, an input file must be specified\n" << desc << endl;
169  exit(1);
170  }
171  if(perturb && (inputFile.empty() || outputFile.empty())) {
172  cout << "In perturb mode, specify input and output files\n" << desc << endl;
173  exit(1);
174  }
175  if(stats && (datasetName.empty() || inputFile.empty())) {
176  cout << "In stats mode, specify dataset and input file\n" << desc << endl;
177  exit(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(datasetName);
195  std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
196  load2D(datasetFile);
197  datasetMeasurements = *data.first;
198  } catch(std::exception& e) {
199  cout << e.what() << endl;
200  exit(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(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 /* ************************************************************************* */
244 {
246  if(dogleg)
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  {
260  std::dynamic_pointer_cast<BetweenFactor<Pose> >(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(key1, 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(key1, 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(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(firstPose, Pose(), noiseModel::Unit::Create(3));
290  newVariables.insert(firstPose, Pose());
291 
292  isam2.update(newFactors, 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 
311  std::dynamic_pointer_cast<BetweenFactor<Pose> >(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(factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry
328  if(step == 1)
329  newVariables.insert(factor->key<1>(), measured.inverse());
330  else {
331  Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<2>());
332  newVariables.insert(factor->key<1>(), prevPose * measured.inverse());
333  }
334  }
335  } else {
336  if(!newVariables.exists(factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry
337  if(step == 1)
338  newVariables.insert(factor->key<2>(), measured);
339  else {
340  Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<1>());
341  newVariables.insert(factor->key<2>(), prevPose * measured);
342  }
343  }
344  }
345  }
347  std::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(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(lmKey))
360  {
361  Pose pose;
362  if(isam2.getLinearizationPoint().exists(poseKey))
363  pose = isam2.calculateEstimate<Pose>(poseKey);
364  else
365  pose = newVariables.at<Pose>(poseKey);
366  const auto& measured = factor->measured();
367  Rot2 measuredBearing = measured.bearing();
368  double measuredRange = measured.range();
369  newVariables.insert(lmKey,
370  pose.transformFrom(measuredBearing.rotate(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, newVariables);
385  gttoc_(Update_ISAM2);
386  } catch(std::exception& e) {
387  cout << e.what() << endl;
388  exit(1);
389  }
390 
391  if((step - firstPose) % 1000 == 0) {
392  try {
393  gttic_(chi2);
394  Values estimate(isam2.calculateEstimate());
395  double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate);
396  cout << "chi2 = " << chi2 << endl;
397  gttoc_(chi2);
398  } catch(std::exception& e) {
399  cout << e.what() << endl;
400  exit(1);
401  }
402  }
403 
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(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 /* ************************************************************************* */
467 void runBatch()
468 {
469  cout << "Creating batch optimizer..." << endl;
470 
472  measurements.addPrior(0, Pose(), noiseModel::Unit::Create(3));
473 
474  gttic_(Create_optimizer);
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(measurements, optimizer.values());
488  cout << "chi2 = " << chi2 << endl;
489  gttoc_(chi2);
490  } while(!checkConvergence(optimizer.params().relativeErrorTol,
491  optimizer.params().absoluteErrorTol, optimizer.params().errorTol,
492  lastError, optimizer.error(), optimizer.params().verbosity));
493 
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(1);
507  }
508  }
509 }
510 
511 /* ************************************************************************* */
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(soln1.keys(), soln2.keys(), std::back_inserter(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(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
545  double maxDiff = 0.0;
546  for(Key j: commonKeys)
547  maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
548  cout << " Maximum solution difference (norm of logmap): " << maxDiff << endl;
549 }
550 
551 /* ************************************************************************* */
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(key, noisev);
566  }
567  Values perturbed = initial.retract(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(1);
578  }
579 
580 }
581 
582 /* ************************************************************************* */
583 void runStats()
584 {
585  cout << "Gathering statistics..." << endl;
586  GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
587  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
589 
590  ofstream file;
591 
592  cout << "Writing SolverComparer_Stats_problemSizeHistogram.txt..." << endl;
593  file.open("SolverComparer_Stats_problemSizeHistogram.txt");
594  treeTraversal::ForestStatistics::Write(file, statistics.problemSizeHistogram);
595  file.close();
596 
597  cout << "Writing SolverComparer_Stats_numberOfChildrenHistogram.txt..." << endl;
598  file.open("SolverComparer_Stats_numberOfChildrenHistogram.txt");
599  treeTraversal::ForestStatistics::Write(file, statistics.numberOfChildrenHistogram);
600  file.close();
601 
602  cout << "Writing SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt..." << endl;
603  file.open("SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt");
604  treeTraversal::ForestStatistics::Write(file, statistics.problemSizeOfSecondLargestChildHistogram);
605  file.close();
606 }
const gtsam::Symbol key('X', 0)
Point2 measured(-17, 30)
const gtsam::Key poseKey
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
bool compare
void runCompare()
#define max(a, b)
Definition: datatypes.h:20
bool batch
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:226
#define gttic_(label)
Definition: timing.h:245
Pose2 Pose
const ValueType at(Key j) const
Definition: Values-inl.h:204
#define min(a, b)
Definition: datatypes.h:19
Vector2 Point2
Definition: Point2.h:32
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
static std::mt19937 rng
string compareFile2
KeyVector keys() const
Definition: Values.cpp:218
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Definition: Pose2.cpp:245
Definition: BFloat16.h:88
Struct to store gathered statistics about a forest.
Definition: statistics.h:31
iterator insert(const std::pair< Key, Vector > &key_value)
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
Definition: ISAM2.cpp:400
const Values & values() const
return values in current optimizer state
GTSAM_EXPORT double range(const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Definition: Pose2.cpp:270
void runIncremental()
NonlinearFactorGraph graph
std::shared_ptr< This > shared_ptr
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
bool stats
const GaussNewtonParams & params() const
int firstStep
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:217
static const SmartProjectionParams params
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
NoiseModelFactorN< Pose, Pose > NM2
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
double errorTol
The maximum total error to stop iterating (default 0.0)
string datasetName
size_t size() const
Definition: FactorGraph.h:334
string inputFile
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:269
NoiseModelFactorN< Pose > NM1
std::shared_ptr< BetweenFactor > shared_ptr
Definition: BetweenFactor.h:63
BearingRangeFactor< Pose, Point2 > BR
Eigen::VectorXd Vector
Definition: Vector.h:38
const Symbol key1('v', 1)
void runBatch()
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
idx_t idx_t idx_t idx_t idx_t idx_t idx_t real_t real_t idx_t * options
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
string compareFile1
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:505
int relinSkip
int data[]
int lastStep
void tictoc_print_()
Definition: timing.h:268
Array< double, 1, 3 > e(1./3., 0.5, 2.)
bool perturb
bool dogleg
Unit3_ normal(const OrientedPlane3_ &p)
OptimizationParams optimizationParams
Definition: ISAM2Params.h:151
std::map< Key, size_t > dims() const
Definition: Values.cpp:251
Key key() const
Definition: Symbol.cpp:40
void runPerturb()
double perturbationNoise
void runStats()
string outputFile
traits
Definition: chartTesting.h:28
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
Definition: Rot2.cpp:100
GaussianFactorGraph::shared_ptr iterate() override
NonlinearFactorGraph datasetMeasurements
Double_ range(const Point2_ &p, const Point2_ &q)
bool enablePartialRelinearizationCheck
Definition: ISAM2Params.h:219
size_t dim() const
Definition: Values.cpp:242
double chi2_red(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &config)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
A class for computing marginals in a NonlinearFactorGraph.
void tictoc_finishedIteration_()
Definition: timing.h:264
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
2D Pose
#define gttoc_(label)
Definition: timing.h:250
bool exists(Key j) const
Definition: Values.cpp:93
double error() const
return error in current optimizer state
ForestStatistics GatherStatistics(const FOREST &forest)
Definition: statistics.h:86
Values initial
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Values calculateEstimate() const
Definition: ISAM2.cpp:766
LinearSolverType linearSolverType
The type of linear solver to use in the nonlinear optimizer.
std::ptrdiff_t j
Timing utilities.
int main(int argc, char *argv[])
const Symbol key2('v', 2)
int nThreads
bool incremental


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:53