RangeISAMExample_plaza2.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  * -------------------------------------------------------------------------- */
11 
19 // Both relative poses and recovered trajectory poses will be stored as Pose2.
20 #include <gtsam/geometry/Pose2.h>
21 using gtsam::Pose2;
22 
23 // gtsam::Vectors are dynamic Eigen vectors, Vector3 is statically sized.
24 #include <gtsam/base/Vector.h>
25 using gtsam::Vector;
26 using gtsam::Vector3;
27 
28 // Unknown landmarks are of type Point2 (which is just a 2D Eigen vector).
29 #include <gtsam/geometry/Point2.h>
30 using 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>
36 using 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.
44 #include <gtsam/nonlinear/Values.h>
45 
46 // We will use a non-linear solver to batch-initialize from the first 150 frames
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>
53 #include <gtsam/slam/dataset.h>
54 
55 // Timing, with functions below, provides nice facilities to benchmark.
56 #include <gtsam/base/timing.h>
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 
68 namespace 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)
78 using TimedOdometry = std::pair<double, Pose2>;
79 std::list<TimedOdometry> readOdometry() {
80  std::list<TimedOdometry> odometryList;
81  std::string data_file = gtsam::findExampleDataFile("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(t, 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)
95 using RangeTriple = std::tuple<double, size_t, double>;
96 std::vector<RangeTriple> readTriples() {
97  std::vector<RangeTriple> triples;
98  std::string data_file = gtsam::findExampleDataFile("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(t, receiver, range);
105  }
106  is.clear(); /* clears the end-of-file and error flags */
107  return triples;
108 }
109 
110 // main
111 int 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(priorSigmas), // prior
133  looseNoise = NM::Isotropic::Sigma(2, 1000), // loose LM prior
134  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
135  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
136  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15),
137  gaussian), // robust
138  rangeNoise = robust ? tukey : gaussian;
139 
140  // Initialize 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(0, pose0, priorNoise);
148  initial.insert(0, 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(t, odometry) = timedOdometry;
170 
171  // add odometry factor
173  odoNoise);
174 
175  // predict pose and add as initial estimate
176  Pose2 predictedPose = lastPose.compose(odometry);
177  lastPose = predictedPose;
178  initial.insert(i, predictedPose);
179 
180  // Check if there are range factors to be added
181  while (k < K && t >= std::get<0>(triples[k])) {
182  size_t j = std::get<1>(triples[k]);
183  Symbol landmark_key('L', j);
184  double range = std::get<2>(triples[k]);
186  i, landmark_key, range, rangeNoise);
187  if (initializedLandmarks.count(landmark_key) == 0) {
188  std::cout << "adding landmark " << j << std::endl;
189  double x = normal(rng), y = normal(rng);
190  initial.insert(landmark_key, Point2(x, y));
191  initializedLandmarks.insert(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.
195  landmark_key, Point2(0, 0), 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, initial);
213  gttoc_(update);
214  gttic_(calculateEstimate);
215  gtsam::Values result = isam.calculateEstimate();
216  gttoc_(calculateEstimate);
217  lastPose = result.at<Pose2>(i);
218  newFactors = gtsam::NonlinearFactorGraph();
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>(landmark_key);
234  std::cout << landmark_key << ":" << p.transpose() << "\n";
235  }
236 
237  exit(0);
238 }
timing.h
Timing utilities.
RangeTriple
std::tuple< double, size_t, double > RangeTriple
Definition: RangeISAMExample_plaza2.cpp:95
relicense.update
def update(text)
Definition: relicense.py:46
Pose2.h
2D Pose
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::RangeFactor
Definition: sam/RangeFactor.h:35
gtsam::ISAM2
Definition: ISAM2.h:45
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::noiseModel::Base::shared_ptr
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
gtsam::noiseModel::Robust::Create
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:740
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
readOdometry
std::list< TimedOdometry > readOdometry()
Definition: RangeISAMExample_plaza2.cpp:79
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
RangeFactor.h
Serializable factor induced by a range measurement.
simple::pose0
static Pose3 pose0
Definition: testInitializePose3.cpp:56
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
odoNoise
SharedDiagonal odoNoise
Definition: testGaussianISAM2.cpp:40
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
main
int main(int argc, char **argv)
Definition: RangeISAMExample_plaza2.cpp:111
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
Point2.h
2D Point
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
odometry
Pose2 odometry(2.0, 0.0, 0.0)
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
dataset.h
utility functions for loading datasets
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gttoc_
#define gttoc_(label)
Definition: timing.h:250
BetweenFactor.h
gttic_
#define gttic_(label)
Definition: timing.h:245
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::tictoc_print_
void tictoc_print_()
Definition: timing.h:268
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
gtsam::normal
Unit3_ normal(const OrientedPlane3_ &p)
Definition: slam/expressions.h:121
y
Scalar * y
Definition: level1_cplx_impl.h:124
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
TimedOdometry
std::pair< double, Pose2 > TimedOdometry
Definition: RangeISAMExample_plaza2.cpp:78
readTriples
std::vector< RangeTriple > readTriples()
Definition: RangeISAMExample_plaza2.cpp:96
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
K
#define K
Definition: igam.h:8
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition: LossFunctions.cpp:28
p
float * p
Definition: Tutorial_Map_using.cpp:9
initial
Definition: testScenarioRunner.cpp:148
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
M_PI
#define M_PI
Definition: mconf.h:117
align_3::t
Point2 t(10, 10)
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:55