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/**
13 * @file SmartRangeExample_plaza1.cpp
14 * @brief A 2D Range SLAM example
15 * @date June 20, 2013
16 * @author FRank Dellaert
17 */
18
19// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
20#include <gtsam/geometry/Pose2.h>
21
22// Each variable in the system (poses and landmarks) must be identified with a unique key.
23// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
24// Here we will use Symbols
25#include <gtsam/inference/Symbol.h>
26
27// We want to use iSAM2 to solve the range-SLAM problem incrementally
28#include <gtsam/nonlinear/ISAM2.h>
29
30// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
31// and initial guesses for any new variables used in the added factors
32#include <gtsam/nonlinear/NonlinearFactorGraph.h>
33#include <gtsam/nonlinear/Values.h>
34
35// We will use a non-liear solver to batch-inituialize from the first 150 frames
36#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
37
38// In GTSAM, measurement functions are represented as 'factors'. Several common factors
39// have been provided with the library for solving robotics SLAM problems.
40#include <gtsam/slam/BetweenFactor.h>
41#include <gtsam/sam/RangeFactor.h>
42#include <gtsam_unstable/slam/SmartRangeFactor.h>
43
44// To find data files, we can use `findExampleDataFile`, declared here:
45#include <gtsam/slam/dataset.h>
46
47// Standard headers, added last, so we know headers above work on their own
48#include <fstream>
49#include <iostream>
50
51using namespace std;
52using namespace gtsam;
53namespace NM = gtsam::noiseModel;
54
55// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
56// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
57
58// load the odometry
59// DR: Odometry Input (delta distance traveled and delta heading change)
60// Time (sec) Delta Dist. Trav. (m) Delta Heading (rad)
61typedef pair<double, Pose2> TimedOdometry;
62list<TimedOdometry> readOdometry() {
63 list<TimedOdometry> odometryList;
64 string drFile = findExampleDataFile(name: "Plaza1_DR.txt");
65 ifstream is(drFile);
66 if (!is) throw runtime_error("Plaza1_DR.txt file not found");
67
68 while (is) {
69 double t, distance_traveled, delta_heading;
70 is >> t >> distance_traveled >> delta_heading;
71 odometryList.push_back(
72 x: TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
73 }
74 is.clear(); /* clears the end-of-file and error flags */
75 return odometryList;
76}
77
78// load the ranges from TD
79// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
80typedef std::tuple<double, size_t, double> RangeTriple;
81vector<RangeTriple> readTriples() {
82 vector<RangeTriple> triples;
83 string tdFile = findExampleDataFile(name: "Plaza1_TD.txt");
84 ifstream is(tdFile);
85 if (!is) throw runtime_error("Plaza1_TD.txt file not found");
86
87 while (is) {
88 double t, sender, receiver, range;
89 is >> t >> sender >> receiver >> range;
90 triples.push_back(x: RangeTriple(t, receiver, range));
91 }
92 is.clear(); /* clears the end-of-file and error flags */
93 return triples;
94}
95
96// main
97int main(int argc, char** argv) {
98
99 // load Plaza1 data
100 list<TimedOdometry> odometry = readOdometry();
101 vector<RangeTriple> triples = readTriples();
102 size_t K = triples.size();
103
104 // parameters
105 size_t start = 220, end=3000;
106 size_t minK = 100; // first batch of smart factors
107 size_t incK = 50; // minimum number of range measurements to process after
108 bool robust = true;
109 bool smart = true;
110
111 // Set Noise parameters
112 Vector priorSigmas = Vector3(1, 1, M_PI);
113 Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
114 auto odoNoise = NM::Diagonal::Sigmas(sigmas: odoSigmas);
115 double sigmaR = 100; // range standard deviation
116 const NM::Base::shared_ptr // all same type
117 priorNoise = NM::Diagonal::Sigmas(sigmas: priorSigmas), //prior
118 gaussian = NM::Isotropic::Sigma(dim: 1, sigma: sigmaR), // non-robust
119 tukey = NM::Robust::Create(robust: NM::mEstimator::Tukey::Create(k: 15), noise: gaussian), //robust
120 rangeNoise = robust ? tukey : gaussian;
121
122 // Initialize iSAM
123 ISAM2 isam;
124
125 // Add prior on first pose
126 Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
127 M_PI - 2.02108900000000);
128 NonlinearFactorGraph newFactors;
129 newFactors.addPrior(key: 0, prior: pose0, model: priorNoise);
130
131 ofstream os2("rangeResultLM.txt");
132 ofstream os3("rangeResultSR.txt");
133
134 // initialize points (Gaussian)
135 Values initial;
136 if (!smart) {
137 initial.insert(j: symbol(c: 'L', j: 1), val: Point2(-68.9265, 18.3778));
138 initial.insert(j: symbol(c: 'L', j: 6), val: Point2(-37.5805, 69.2278));
139 initial.insert(j: symbol(c: 'L', j: 0), val: Point2(-33.6205, 26.9678));
140 initial.insert(j: symbol(c: 'L', j: 5), val: Point2(1.7095, -5.8122));
141 }
142 Values landmarkEstimates = initial; // copy landmarks
143 initial.insert(j: 0, val: pose0);
144
145 // initialize smart range factors
146 size_t ids[] = { 1, 6, 0, 5 };
147 typedef std::shared_ptr<SmartRangeFactor> SmartPtr;
148 map<size_t, SmartPtr> smartFactors;
149 if (smart) {
150 for(size_t jj: ids) {
151 smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
152 newFactors.push_back(factor: smartFactors[jj]);
153 }
154 }
155
156 // set some loop variables
157 size_t i = 1; // step counter
158 size_t k = 0; // range measurement counter
159 Pose2 lastPose = pose0;
160 size_t countK = 0, totalCount=0;
161
162 // Loop over odometry
163 gttic_(iSAM);
164 for(const TimedOdometry& timedOdometry: odometry) {
165 //--------------------------------- odometry loop -----------------------------------------
166 double t;
167 Pose2 odometry;
168 std::tie(args&: t, args&: odometry) = timedOdometry;
169 printf(format: "step %d, time = %g\n",(int)i,t);
170
171 // add odometry factor
172 newFactors.push_back(
173 factor: BetweenFactor<Pose2>(i - 1, i, odometry, odoNoise));
174
175 // predict pose and add as initial estimate
176 Pose2 predictedPose = lastPose.compose(g: odometry);
177 lastPose = predictedPose;
178 initial.insert(j: i, val: predictedPose);
179 landmarkEstimates.insert(j: i, val: predictedPose);
180
181 // Check if there are range factors to be added
182 while (k < K && t >= std::get<0>(t&: triples[k])) {
183 size_t j = std::get<1>(t&: triples[k]);
184 double range = std::get<2>(t&: triples[k]);
185 if (i > start) {
186 if (smart && totalCount < minK) {
187 try {
188 smartFactors[j]->addRange(key: i, measuredRange: range);
189 printf(format: "adding range %g for %d",range,(int)j);
190 } catch (const invalid_argument& e) {
191 printf(format: "warning: omitting duplicate range %g for %d: %s", range,
192 (int)j, e.what());
193 }
194 cout << endl;
195 }
196 else {
197 RangeFactor<Pose2, Point2> factor(i, symbol(c: 'L', j), range,
198 rangeNoise);
199 // Throw out obvious outliers based on current landmark estimates
200 Vector error = factor.unwhitenedError(x: landmarkEstimates);
201 if (k <= 200 || std::abs(x: error[0]) < 5)
202 newFactors.push_back(factor);
203 }
204 totalCount += 1;
205 }
206 k = k + 1;
207 countK = countK + 1;
208 }
209
210 // Check whether to update iSAM 2
211 if (k >= minK && countK >= incK) {
212 gttic_(update);
213 isam.update(newFactors, newTheta: initial);
214 gttoc_(update);
215 gttic_(calculateEstimate);
216 Values result = isam.calculateEstimate();
217 gttoc_(calculateEstimate);
218 lastPose = result.at<Pose2>(j: i);
219 bool hasLandmarks = result.exists(j: symbol(c: 'L', j: ids[0]));
220 if (hasLandmarks) {
221 // update landmark estimates
222 landmarkEstimates = Values();
223 for(size_t jj: ids)
224 landmarkEstimates.insert(j: symbol(c: 'L', j: jj), val: result.at(j: symbol(c: 'L', j: jj)));
225 }
226 newFactors = NonlinearFactorGraph();
227 initial = Values();
228 if (smart && !hasLandmarks) {
229 cout << "initialize from smart landmarks" << endl;
230 for(size_t jj: ids) {
231 Point2 landmark = smartFactors[jj]->triangulate(x: result);
232 initial.insert(j: symbol(c: 'L', j: jj), val: landmark);
233 landmarkEstimates.insert(j: symbol(c: 'L', j: jj), val: landmark);
234 }
235 }
236 countK = 0;
237 for (const auto& [key, point] : result.extract<Point2>())
238 os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl;
239 if (smart) {
240 for(size_t jj: ids) {
241 Point2 landmark = smartFactors[jj]->triangulate(x: result);
242 os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
243 << endl;
244 }
245 }
246 }
247 i += 1;
248 if (i>end) break;
249 //--------------------------------- odometry loop -----------------------------------------
250 } // end for
251 gttoc_(iSAM);
252
253 // Print timings
254 tictoc_print_();
255
256 // Write result to file
257 Values result = isam.calculateEstimate();
258 ofstream os("rangeResult.txt");
259 for (const auto& [key, pose] : result.extract<Pose2>())
260 os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl;
261 exit(status: 0);
262}
263
264