timeIncremental.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 
17 #include <gtsam/slam/dataset.h>
20 #include <gtsam/geometry/Pose2.h>
21 #include <gtsam/inference/Symbol.h>
22 #include <gtsam/nonlinear/ISAM2.h>
24 #include <gtsam/base/timing.h>
25 
26 #include <fstream>
27 #include <boost/archive/binary_oarchive.hpp>
28 #include <boost/archive/binary_iarchive.hpp>
29 #include <boost/serialization/export.hpp>
30 
31 using namespace std;
32 using namespace gtsam;
33 using namespace gtsam::symbol_shorthand;
34 
35 typedef Pose2 Pose;
36 
40 
41 //GTSAM_VALUE_EXPORT(Value);
42 //GTSAM_VALUE_EXPORT(Pose);
43 //GTSAM_VALUE_EXPORT(NonlinearFactor);
44 //GTSAM_VALUE_EXPORT(NoiseModelFactor);
45 //GTSAM_VALUE_EXPORT(NM1);
46 //GTSAM_VALUE_EXPORT(NM2);
47 //GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
48 //GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
49 //GTSAM_VALUE_EXPORT(BR);
50 //GTSAM_VALUE_EXPORT(noiseModel::Base);
51 //GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
52 //GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
53 //GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
54 //GTSAM_VALUE_EXPORT(noiseModel::Unit);
55 
57  // Compute degrees of freedom (observations - variables)
58  // In ocaml, +1 was added to the observations to account for the prior, but
59  // the factor graph already includes a factor for the prior/equality constraint.
60  // double dof = graph.size() - config.size();
61  int graph_dim = 0;
62  for(const std::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
63  graph_dim += nlf->dim();
64  }
65  double dof = graph_dim - config.dim(); // kaess: changed to dim
66  return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error
67 }
68 
69 int main(int argc, char *argv[]) {
70 
71  cout << "Loading data..." << endl;
72 
73  gttic_(Find_datafile);
74  //string datasetFile = findExampleDataFile("w10000");
75  string datasetFile = findExampleDataFile("victoria_park");
76  std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
77  load2D(datasetFile);
78  gttoc_(Find_datafile);
79 
80  NonlinearFactorGraph measurements = *data.first;
81  Values initial = *data.second;
82 
83  cout << "Playing forward time steps..." << endl;
84 
85  ISAM2 isam2;
86 
87  size_t nextMeasurement = 0;
88  for(size_t step=1; nextMeasurement < measurements.size(); ++step) {
89 
90  Values newVariables;
91  NonlinearFactorGraph newFactors;
92 
93  // Collect measurements and new variables for the current step
94  gttic_(Collect_measurements);
95  if(step == 1) {
96  // cout << "Initializing " << 0 << endl;
97  newVariables.insert(0, Pose());
98  // Add prior
99  newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3));
100  }
101  while(nextMeasurement < measurements.size()) {
102 
103  NonlinearFactor::shared_ptr measurementf = measurements[nextMeasurement];
104 
106  std::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
107  {
108  // Stop collecting measurements that are for future steps
109  if(measurement->key<1>() > step || measurement->key<2>() > step)
110  break;
111 
112  // Require that one of the nodes is the current one
113  if(measurement->key<1>() != step && measurement->key<2>() != step)
114  throw runtime_error("Problem in data file, out-of-sequence measurements");
115 
116  // Add a new factor
117  newFactors.push_back(measurement);
118 
119  // Initialize the new variable
120  if(measurement->key<1>() == step && measurement->key<2>() == step-1) {
121  if(step == 1)
122  newVariables.insert(step, measurement->measured().inverse());
123  else {
124  Pose prevPose = isam2.calculateEstimate<Pose>(step-1);
125  newVariables.insert(step, prevPose * measurement->measured().inverse());
126  }
127  // cout << "Initializing " << step << endl;
128  } else if(measurement->key<2>() == step && measurement->key<1>() == step-1) {
129  if(step == 1)
130  newVariables.insert(step, measurement->measured());
131  else {
132  Pose prevPose = isam2.calculateEstimate<Pose>(step-1);
133  newVariables.insert(step, prevPose * measurement->measured());
134  }
135  // cout << "Initializing " << step << endl;
136  }
137  }
139  std::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
140  {
141  Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
142 
143  // Stop collecting measurements that are for future steps
144  if(poseKey > step)
145  throw runtime_error("Problem in data file, out-of-sequence measurements");
146 
147  // Add new factor
148  newFactors.push_back(measurement);
149 
150  // Initialize new landmark
151  if(!isam2.getLinearizationPoint().exists(lmKey))
152  {
154  Rot2 measuredBearing = measurement->measured().bearing();
155  double measuredRange = measurement->measured().range();
156  newVariables.insert(lmKey,
157  pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0))));
158  }
159  }
160  else
161  {
162  throw std::runtime_error("Unknown factor type read from data file");
163  }
164  ++ nextMeasurement;
165  }
166  gttoc_(Collect_measurements);
167 
168  // Update iSAM2
169  gttic_(Update_ISAM2);
170  isam2.update(newFactors, newVariables);
171  gttoc_(Update_ISAM2);
172 
173  if(step % 100 == 0) {
174  gttic_(chi2);
175  Values estimate(isam2.calculateEstimate());
176  double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate);
177  cout << "chi2 = " << chi2 << endl;
178  gttoc_(chi2);
179  }
180 
182 
183  if(step % 1000 == 0) {
184  cout << "Step " << step << endl;
185  tictoc_print_();
186  }
187  }
188 
189  //try {
190  // {
191  // std::ofstream writerStream("incremental_init", ios::binary);
192  // boost::archive::binary_oarchive writer(writerStream);
193  // writer << isam2.calculateEstimate();
194  // writerStream.close();
195  // }
196  // {
197  // std::ofstream writerStream("incremental_graph", ios::binary);
198  // boost::archive::binary_oarchive writer(writerStream);
199  // writer << isam2.getFactorsUnsafe();
200  // writerStream.close();
201  // }
202  //} catch(std::exception& e) {
203  // cout << e.what() << endl;
204  //}
205 
207  Values values;
208 
209  //{
210  // std::ifstream readerStream("incremental_init", ios::binary);
211  // boost::archive::binary_iarchive reader(readerStream);
212  // reader >> values;
213  //}
214  //{
215  // std::ifstream readerStream("incremental_graph", ios::binary);
216  // boost::archive::binary_iarchive reader(readerStream);
217  // reader >> graph;
218  //}
219 
220  graph = isam2.getFactorsUnsafe();
221  values = isam2.calculateEstimate();
222 
223  // Compute marginals
224  try {
225  Marginals marginals(graph, values);
226  int i=0;
227  // Assign the keyvector to a named variable
228  auto keys = values.keys();
229  // Iterate over it in reverse
230  for (auto it1 = keys.rbegin(); it1 != keys.rend(); ++it1) {
231  Key key1 = *it1;
232  int j=0;
233  for (auto it2 = keys.rbegin(); it2 != keys.rend(); ++it2) {
234  Key key2 = *it2;
235  if(i != j) {
236  gttic_(jointMarginalInformation);
237  KeyVector keys(2);
238  keys[0] = key1;
239  keys[1] = key2;
240  JointMarginal info = marginals.jointMarginalInformation(keys);
241  gttoc_(jointMarginalInformation);
243  }
244  ++j;
245  if(j >= 50)
246  break;
247  }
248  ++i;
249  if(i >= 50)
250  break;
251  }
252  tictoc_print_();
253  for(Key key: values.keys()) {
254  gttic_(marginalInformation);
255  Matrix info = marginals.marginalInformation(key);
256  gttoc_(marginalInformation);
258  ++i;
259  }
260  } catch(std::exception& e) {
261  cout << e.what() << endl;
262  }
263  tictoc_print_();
264 
265  return 0;
266 }
const gtsam::Symbol key('X', 0)
const gtsam::Key poseKey
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:226
NoiseModelFactorN< Pose, Pose > NM2
#define gttic_(label)
Definition: timing.h:245
NoiseModelFactorN< Pose > NM1
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
Matrix marginalInformation(Key variable) const
Definition: Marginals.cpp:125
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
leaf::MyValues values
KeyVector keys() const
Definition: Values.cpp:218
Definition: BFloat16.h:88
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
NonlinearFactorGraph graph
std::shared_ptr< This > shared_ptr
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:217
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
else if n * info
size_t size() const
Definition: FactorGraph.h:334
static Point2 measurement(323.0, 240.0)
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:269
int main(int argc, char *argv[])
std::shared_ptr< BetweenFactor > shared_ptr
Definition: BetweenFactor.h:63
const Symbol key1('v', 1)
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:505
int data[]
void tictoc_print_()
Definition: timing.h:268
Array< double, 1, 3 > e(1./3., 0.5, 2.)
double chi2_red(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &config)
JointMarginal jointMarginalInformation(const KeyVector &variables) const
Definition: Marginals.cpp:145
traits
Definition: chartTesting.h:28
Pose2 Pose
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))
size_t dim() const
Definition: Values.cpp:242
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
const KeyVector keys
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
utility functions for loading datasets
BearingRangeFactor< Pose, Point2 > BR
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Values calculateEstimate() const
Definition: ISAM2.cpp:766
Marginals marginals(graph, result)
std::ptrdiff_t j
Timing utilities.
const Symbol key2('v', 2)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:01