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_plaza2.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
43// To find data files, we can use `findExampleDataFile`, declared here:
44#include <gtsam/slam/dataset.h>
45
46// Standard headers, added last, so we know headers above work on their own
47#include <fstream>
48#include <iostream>
49
50using namespace std;
51using namespace gtsam;
52namespace NM = gtsam::noiseModel;
53
54// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
55// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
56
57// load the odometry
58// DR: Odometry Input (delta distance traveled and delta heading change)
59// Time (sec) Delta Dist. Trav. (m) Delta Heading (rad)
60typedef pair<double, Pose2> TimedOdometry;
61list<TimedOdometry> readOdometry() {
62 list<TimedOdometry> odometryList;
63 string drFile = findExampleDataFile(name: "Plaza2_DR.txt");
64 ifstream is(drFile);
65 if (!is) throw runtime_error("Plaza2_DR.txt file not found");
66
67 while (is) {
68 double t, distance_traveled, delta_heading;
69 is >> t >> distance_traveled >> delta_heading;
70 odometryList.push_back(
71 x: TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
72 }
73 is.clear(); /* clears the end-of-file and error flags */
74 return odometryList;
75}
76
77// load the ranges from TD
78// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
79typedef std::tuple<double, size_t, double> RangeTriple;
80vector<RangeTriple> readTriples() {
81 vector<RangeTriple> triples;
82 string tdFile = findExampleDataFile(name: "Plaza2_TD.txt");
83 ifstream is(tdFile);
84 if (!is) throw runtime_error("Plaza2_TD.txt file not found");
85
86 while (is) {
87 double t, sender, receiver, range;
88 is >> t >> sender >> receiver >> range;
89 triples.push_back(x: RangeTriple(t, receiver, range));
90 }
91 is.clear(); /* clears the end-of-file and error flags */
92 return triples;
93}
94
95// main
96int main(int argc, char** argv) {
97
98 // load Plaza1 data
99 list<TimedOdometry> odometry = readOdometry();
100// size_t M = odometry.size();
101
102 vector<RangeTriple> triples = readTriples();
103 size_t K = triples.size();
104
105 // parameters
106 size_t incK = 50; // minimum number of range measurements to process after
107 bool robust = false;
108
109 // Set Noise parameters
110 Vector priorSigmas = Vector3(0.01, 0.01, 0.01);
111 Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
112 double sigmaR = 100; // range standard deviation
113 const NM::Base::shared_ptr // all same type
114 priorNoise = NM::Diagonal::Sigmas(sigmas: priorSigmas), //prior
115 odoNoise = NM::Diagonal::Sigmas(sigmas: odoSigmas), // odometry
116 gaussian = NM::Isotropic::Sigma(dim: 1, sigma: sigmaR), // non-robust
117 tukey = NM::Robust::Create(robust: NM::mEstimator::Tukey::Create(k: 15), noise: gaussian), //robust
118 rangeNoise = robust ? tukey : gaussian;
119
120 // Initialize iSAM
121 ISAM2 isam;
122
123 // Add prior on first pose
124 Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000);
125 NonlinearFactorGraph newFactors;
126 newFactors.addPrior(key: 0, prior: pose0, model: priorNoise);
127
128 // initialize points (Gaussian)
129 Values initial;
130 initial.insert(j: symbol(c: 'L', j: 1), val: Point2(-68.9265, 18.3778));
131 initial.insert(j: symbol(c: 'L', j: 6), val: Point2(-37.5805, 69.2278));
132 initial.insert(j: symbol(c: 'L', j: 0), val: Point2(-33.6205, 26.9678));
133 initial.insert(j: symbol(c: 'L', j: 5), val: Point2(1.7095, -5.8122));
134 Values landmarkEstimates = initial; // copy landmarks
135 initial.insert(j: 0, val: pose0);
136
137 // set some loop variables
138 size_t i = 1; // step counter
139 size_t k = 0; // range measurement counter
140 Pose2 lastPose = pose0;
141 size_t countK = 0;
142
143 // Loop over odometry
144 gttic_(iSAM);
145 for(const TimedOdometry& timedOdometry: odometry) {
146 //--------------------------------- odometry loop -----------------------------------------
147 double t;
148 Pose2 odometry;
149 std::tie(args&: t, args&: odometry) = timedOdometry;
150
151 // add odometry factor
152 newFactors.push_back(
153 factor: BetweenFactor<Pose2>(i - 1, i, odometry,
154 NM::Diagonal::Sigmas(sigmas: odoSigmas)));
155
156 // predict pose and add as initial estimate
157 Pose2 predictedPose = lastPose.compose(g: odometry);
158 lastPose = predictedPose;
159 initial.insert(j: i, val: predictedPose);
160 landmarkEstimates.insert(j: i, val: predictedPose);
161
162 // Check if there are range factors to be added
163 while (k < K && t >= std::get<0>(t&: triples[k])) {
164 size_t j = std::get<1>(t&: triples[k]);
165 double range = std::get<2>(t&: triples[k]);
166 RangeFactor<Pose2, Point2> factor(i, symbol(c: 'L', j), range, rangeNoise);
167 // Throw out obvious outliers based on current landmark estimates
168 Vector error = factor.unwhitenedError(x: landmarkEstimates);
169 if (k <= 200 || std::abs(x: error[0]) < 5)
170 newFactors.push_back(factor);
171 k = k + 1;
172 countK = countK + 1;
173 }
174
175 // Check whether to update iSAM 2
176 if (countK > incK) {
177 gttic_(update);
178 isam.update(newFactors, newTheta: initial);
179 gttoc_(update);
180 gttic_(calculateEstimate);
181 Values result = isam.calculateEstimate();
182 gttoc_(calculateEstimate);
183 lastPose = result.at<Pose2>(j: i);
184 // update landmark estimates
185 landmarkEstimates = Values();
186 landmarkEstimates.insert(j: symbol(c: 'L', j: 1), val: result.at(j: symbol(c: 'L', j: 1)));
187 landmarkEstimates.insert(j: symbol(c: 'L', j: 6), val: result.at(j: symbol(c: 'L', j: 6)));
188 landmarkEstimates.insert(j: symbol(c: 'L', j: 0), val: result.at(j: symbol(c: 'L', j: 0)));
189 landmarkEstimates.insert(j: symbol(c: 'L', j: 5), val: result.at(j: symbol(c: 'L', j: 5)));
190 newFactors = NonlinearFactorGraph();
191 initial = Values();
192 countK = 0;
193 }
194 i += 1;
195 //--------------------------------- odometry loop -----------------------------------------
196 } // end for
197 gttoc_(iSAM);
198
199 // Print timings
200 tictoc_print_();
201
202 // Write result to file
203 Values result = isam.calculateEstimate();
204 ofstream os2("rangeResultLM.txt");
205 for (const auto& [key, point] : result.extract<Point2>())
206 os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl;
207 ofstream os("rangeResult.txt");
208 for (const auto& [key, pose] : result.extract<Pose2>())
209 os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl;
210 exit(status: 0);
211}
212
213