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 timeIncremental.cpp
13 * @brief Overall timing tests for incremental solving
14 * @author Richard Roberts
15 */
16
17#include <gtsam/slam/dataset.h>
18#include <gtsam/slam/BetweenFactor.h>
19#include <gtsam/sam/BearingRangeFactor.h>
20#include <gtsam/geometry/Pose2.h>
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/nonlinear/ISAM2.h>
23#include <gtsam/nonlinear/Marginals.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
31using namespace std;
32using namespace gtsam;
33using namespace gtsam::symbol_shorthand;
34
35typedef Pose2 Pose;
36
37typedef NoiseModelFactorN<Pose> NM1;
38typedef NoiseModelFactorN<Pose,Pose> NM2;
39typedef BearingRangeFactor<Pose,Point2> BR;
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
56double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
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(values: config) / dof; // kaess: added factor 2, graph.error returns half of actual error
67}
68
69int 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(name: "victoria_park");
76 std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
77 load2D(filename: 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(j: 0, val: Pose());
98 // Add prior
99 newFactors.addPrior(key: 0, prior: Pose(), model: noiseModel::Unit::Create(dim: 3));
100 }
101 while(nextMeasurement < measurements.size()) {
102
103 NonlinearFactor::shared_ptr measurementf = measurements[nextMeasurement];
104
105 if(BetweenFactor<Pose>::shared_ptr measurement =
106 std::dynamic_pointer_cast<BetweenFactor<Pose> >(r: 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(factor: 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(j: step, val: measurement->measured().inverse());
123 else {
124 Pose prevPose = isam2.calculateEstimate<Pose>(key: step-1);
125 newVariables.insert(j: step, val: 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(j: step, val: measurement->measured());
131 else {
132 Pose prevPose = isam2.calculateEstimate<Pose>(key: step-1);
133 newVariables.insert(j: step, val: prevPose * measurement->measured());
134 }
135 // cout << "Initializing " << step << endl;
136 }
137 }
138 else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
139 std::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(r: 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(factor: measurement);
149
150 // Initialize new landmark
151 if(!isam2.getLinearizationPoint().exists(j: lmKey))
152 {
153 Pose pose = isam2.calculateEstimate<Pose>(key: poseKey);
154 Rot2 measuredBearing = measurement->measured().bearing();
155 double measuredRange = measurement->measured().range();
156 newVariables.insert(j: lmKey,
157 val: pose.transformFrom(point: measuredBearing.rotate(p: 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, newTheta: newVariables);
171 gttoc_(Update_ISAM2);
172
173 if(step % 100 == 0) {
174 gttic_(chi2);
175 Values estimate(isam2.calculateEstimate());
176 double chi2 = chi2_red(graph: isam2.getFactorsUnsafe(), config: estimate);
177 cout << "chi2 = " << chi2 << endl;
178 gttoc_(chi2);
179 }
180
181 tictoc_finishedIteration_();
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
206 NonlinearFactorGraph graph;
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(variables: keys);
241 gttoc_(jointMarginalInformation);
242 tictoc_finishedIteration_();
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(variable: key);
256 gttoc_(marginalInformation);
257 tictoc_finishedIteration_();
258 ++i;
259 }
260 } catch(std::exception& e) {
261 cout << e.what() << endl;
262 }
263 tictoc_print_();
264
265 return 0;
266}
267