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 RangeISAMExample_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.
20#include <gtsam/geometry/Pose2.h>
21using gtsam::Pose2;
22
23// gtsam::Vectors are dynamic Eigen vectors, Vector3 is statically sized.
24#include <gtsam/base/Vector.h>
25using gtsam::Vector;
26using gtsam::Vector3;
27
28// Unknown landmarks are of type Point2 (which is just a 2D Eigen vector).
29#include <gtsam/geometry/Point2.h>
30using gtsam::Point2;
31
32// Each variable in the system (poses and landmarks) must be identified with a
33// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
34// (X1, X2, L1). Here we will use Symbols.
35#include <gtsam/inference/Symbol.h>
36using gtsam::Symbol;
37
38// We want to use iSAM2 to solve the range-SLAM problem incrementally.
39#include <gtsam/nonlinear/ISAM2.h>
40
41// iSAM2 requires as input a set set of new factors to be added stored in a
42// factor graph, and initial guesses for any new variables in the added factors.
43#include <gtsam/nonlinear/NonlinearFactorGraph.h>
44#include <gtsam/nonlinear/Values.h>
45
46// We will use a non-linear solver to batch-initialize from the first 150 frames
47#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
48
49// Measurement functions are represented as 'factors'. Several common factors
50// have been provided with the library for solving robotics SLAM problems:
51#include <gtsam/sam/RangeFactor.h>
52#include <gtsam/slam/BetweenFactor.h>
53#include <gtsam/slam/dataset.h>
54
55// Timing, with functions below, provides nice facilities to benchmark.
56#include <gtsam/base/timing.h>
57using gtsam::tictoc_print_;
58
59// Standard headers, added last, so we know headers above work on their own.
60#include <fstream>
61#include <iostream>
62#include <random>
63#include <set>
64#include <string>
65#include <utility>
66#include <vector>
67
68namespace NM = gtsam::noiseModel;
69
70// Data is second UWB ranging dataset, B2 or "plaza 2", from
71// "Navigating with Ranging Radios: Five Data Sets with Ground Truth"
72// by Joseph Djugash, Bradley Hamner, and Stephan Roth
73// https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf
74
75// load the odometry
76// DR: Odometry Input (delta distance traveled and delta heading change)
77// Time (sec) Delta Distance Traveled (m) Delta Heading (rad)
78using TimedOdometry = std::pair<double, Pose2>;
79std::list<TimedOdometry> readOdometry() {
80 std::list<TimedOdometry> odometryList;
81 std::string data_file = gtsam::findExampleDataFile(name: "Plaza2_DR.txt");
82 std::ifstream is(data_file.c_str());
83
84 while (is) {
85 double t, distance_traveled, delta_heading;
86 is >> t >> distance_traveled >> delta_heading;
87 odometryList.emplace_back(args&: t, args: Pose2(distance_traveled, 0, delta_heading));
88 }
89 is.clear(); /* clears the end-of-file and error flags */
90 return odometryList;
91}
92
93// load the ranges from TD
94// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
95using RangeTriple = std::tuple<double, size_t, double>;
96std::vector<RangeTriple> readTriples() {
97 std::vector<RangeTriple> triples;
98 std::string data_file = gtsam::findExampleDataFile(name: "Plaza2_TD.txt");
99 std::ifstream is(data_file.c_str());
100
101 while (is) {
102 double t, range, sender, receiver;
103 is >> t >> sender >> receiver >> range;
104 triples.emplace_back(args&: t, args&: receiver, args&: range);
105 }
106 is.clear(); /* clears the end-of-file and error flags */
107 return triples;
108}
109
110// main
111int main(int argc, char** argv) {
112 // load Plaza2 data
113 std::list<TimedOdometry> odometry = readOdometry();
114 size_t M = odometry.size();
115 std::cout << "Read " << M << " odometry entries." << std::endl;
116
117 std::vector<RangeTriple> triples = readTriples();
118 size_t K = triples.size();
119 std::cout << "Read " << K << " range triples." << std::endl;
120
121 // parameters
122 size_t minK =
123 150; // minimum number of range measurements to process initially
124 size_t incK = 25; // minimum number of range measurements to process after
125 bool robust = true;
126
127 // Set Noise parameters
128 Vector priorSigmas = Vector3(1, 1, M_PI);
129 Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
130 double sigmaR = 100; // range standard deviation
131 const NM::Base::shared_ptr // all same type
132 priorNoise = NM::Diagonal::Sigmas(sigmas: priorSigmas), // prior
133 looseNoise = NM::Isotropic::Sigma(dim: 2, sigma: 1000), // loose LM prior
134 odoNoise = NM::Diagonal::Sigmas(sigmas: odoSigmas), // odometry
135 gaussian = NM::Isotropic::Sigma(dim: 1, sigma: sigmaR), // non-robust
136 tukey = NM::Robust::Create(robust: NM::mEstimator::Tukey::Create(k: 15),
137 noise: gaussian), // robust
138 rangeNoise = robust ? tukey : gaussian;
139
140 // Initialize iSAM
141 gtsam::ISAM2 isam;
142
143 // Add prior on first pose
144 Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.021089);
145 gtsam::NonlinearFactorGraph newFactors;
146 newFactors.addPrior(key: 0, prior: pose0, model: priorNoise);
147 gtsam::Values initial;
148 initial.insert(j: 0, val: pose0);
149
150 // We will initialize landmarks randomly, and keep track of which landmarks we
151 // already added with a set.
152 std::mt19937_64 rng;
153 std::normal_distribution<double> normal(0.0, 100.0);
154 std::set<Symbol> initializedLandmarks;
155
156 // set some loop variables
157 size_t i = 1; // step counter
158 size_t k = 0; // range measurement counter
159 bool initialized = false;
160 Pose2 lastPose = pose0;
161 size_t countK = 0;
162
163 // Loop over odometry
164 gttic_(iSAM);
165 for (const TimedOdometry& timedOdometry : odometry) {
166 //--------------------------------- odometry loop --------------------------
167 double t;
168 Pose2 odometry;
169 std::tie(args&: t, args&: odometry) = timedOdometry;
170
171 // add odometry factor
172 newFactors.emplace_shared<gtsam::BetweenFactor<Pose2>>(args: i - 1, args&: i, args&: odometry,
173 args: 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
180 // Check if there are range factors to be added
181 while (k < K && t >= std::get<0>(t&: triples[k])) {
182 size_t j = std::get<1>(t&: triples[k]);
183 Symbol landmark_key('L', j);
184 double range = std::get<2>(t&: triples[k]);
185 newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>(
186 args&: i, args&: landmark_key, args&: range, args: rangeNoise);
187 if (initializedLandmarks.count(x: landmark_key) == 0) {
188 std::cout << "adding landmark " << j << std::endl;
189 double x = normal(rng), y = normal(rng);
190 initial.insert(j: landmark_key, val: Point2(x, y));
191 initializedLandmarks.insert(x: landmark_key);
192 // We also add a very loose prior on the landmark in case there is only
193 // one sighting, which cannot fully determine the landmark.
194 newFactors.emplace_shared<gtsam::PriorFactor<Point2>>(
195 args&: landmark_key, args: Point2(0, 0), args: looseNoise);
196 }
197 k = k + 1;
198 countK = countK + 1;
199 }
200
201 // Check whether to update iSAM 2
202 if ((k > minK) && (countK > incK)) {
203 if (!initialized) { // Do a full optimize for first minK ranges
204 std::cout << "Initializing at time " << k << std::endl;
205 gttic_(batchInitialization);
206 gtsam::LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
207 initial = batchOptimizer.optimize();
208 gttoc_(batchInitialization);
209 initialized = true;
210 }
211 gttic_(update);
212 isam.update(newFactors, newTheta: initial);
213 gttoc_(update);
214 gttic_(calculateEstimate);
215 gtsam::Values result = isam.calculateEstimate();
216 gttoc_(calculateEstimate);
217 lastPose = result.at<Pose2>(j: i);
218 newFactors = gtsam::NonlinearFactorGraph();
219 initial = gtsam::Values();
220 countK = 0;
221 }
222 i += 1;
223 //--------------------------------- odometry loop --------------------------
224 } // end for
225 gttoc_(iSAM);
226
227 // Print timings
228 tictoc_print_();
229
230 // Print optimized landmarks:
231 gtsam::Values finalResult = isam.calculateEstimate();
232 for (auto&& landmark_key : initializedLandmarks) {
233 Point2 p = finalResult.at<Point2>(j: landmark_key);
234 std::cout << landmark_key << ":" << p.transpose() << "\n";
235 }
236
237 exit(status: 0);
238}
239