Pose3SLAMExample_changeKeys.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  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/slam/dataset.h>
22 #include <fstream>
23 
24 using namespace std;
25 using namespace gtsam;
26 
27 int main(const int argc, const char *argv[]) {
28 
29  // Read graph from file
30  string g2oFile;
31  if (argc < 2)
32  g2oFile = findExampleDataFile("pose3example.txt");
33  else
34  g2oFile = argv[1];
35 
37  Values::shared_ptr initial;
38  bool is3D = true;
39  std::tie(graph, initial) = readG2o(g2oFile, is3D);
40 
41  bool add = false;
42  Key firstKey = 8646911284551352320;
43 
44  std::cout << "Using reference key: " << firstKey << std::endl;
45  if(add)
46  std::cout << "adding key " << std::endl;
47  else
48  std::cout << "subtracting key " << std::endl;
49 
50 
51  if (argc < 3) {
52  std::cout << "Please provide output file to write " << std::endl;
53  } else {
54  const string inputFileRewritten = argv[2];
55  std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
56  // Additional: rewrite input with simplified keys 0,1,...
57  Values simpleInitial;
58  for (const auto k : initial->keys()) {
59  Key key;
60  if (add)
61  key = k + firstKey;
62  else
63  key = k - firstKey;
64 
65  simpleInitial.insert(key, initial->at(k));
66  }
67  NonlinearFactorGraph simpleGraph;
68  for(const std::shared_ptr<NonlinearFactor>& factor: *graph) {
69  std::shared_ptr<BetweenFactor<Pose3> > pose3Between =
70  std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
71  if (pose3Between){
72  Key key1, key2;
73  if(add){
74  key1 = pose3Between->key<1>() + firstKey;
75  key2 = pose3Between->key<2>() + firstKey;
76  }else{
77  key1 = pose3Between->key<1>() - firstKey;
78  key2 = pose3Between->key<2>() - firstKey;
79  }
80  NonlinearFactor::shared_ptr simpleFactor(
81  new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));
82  simpleGraph.add(simpleFactor);
83  }
84  }
85  writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
86  }
87  return 0;
88 }
const gtsam::Symbol key('X', 0)
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Definition: dataset.cpp:636
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
int main(const int argc, const char *argv[])
const Symbol key1('v', 1)
Key key() const
Definition: Symbol.cpp:40
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:621
traits
Definition: chartTesting.h:28
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
std::shared_ptr< This > shared_ptr
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
void insert(Key j, const Value &val)
Definition: Values.cpp:155
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
const Symbol key2('v', 2)


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