SmartRangeExample_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 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
33 #include <gtsam/nonlinear/Values.h>
34 
35 // We will use a non-liear solver to batch-inituialize from the first 150 frames
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.
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 
50 using namespace std;
51 using namespace gtsam;
52 namespace 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)
60 typedef pair<double, Pose2> TimedOdometry;
61 list<TimedOdometry> readOdometry() {
62  list<TimedOdometry> odometryList;
63  string drFile = findExampleDataFile("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  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)
79 typedef std::tuple<double, size_t, double> RangeTriple;
80 vector<RangeTriple> readTriples() {
81  vector<RangeTriple> triples;
82  string tdFile = findExampleDataFile("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(RangeTriple(t, receiver, range));
90  }
91  is.clear(); /* clears the end-of-file and error flags */
92  return triples;
93 }
94 
95 // main
96 int 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(priorSigmas), //prior
115  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
116  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
117  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), 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(0, pose0, priorNoise);
127 
128  // initialize points (Gaussian)
129  Values initial;
130  initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
131  initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
132  initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
133  initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
134  Values landmarkEstimates = initial; // copy landmarks
135  initial.insert(0, 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(t, odometry) = timedOdometry;
150 
151  // add odometry factor
152  newFactors.push_back(
153  BetweenFactor<Pose2>(i - 1, i, odometry,
154  NM::Diagonal::Sigmas(odoSigmas)));
155 
156  // predict pose and add as initial estimate
157  Pose2 predictedPose = lastPose.compose(odometry);
158  lastPose = predictedPose;
159  initial.insert(i, predictedPose);
160  landmarkEstimates.insert(i, predictedPose);
161 
162  // Check if there are range factors to be added
163  while (k < K && t >= std::get<0>(triples[k])) {
164  size_t j = std::get<1>(triples[k]);
165  double range = std::get<2>(triples[k]);
166  RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range, rangeNoise);
167  // Throw out obvious outliers based on current landmark estimates
168  Vector error = factor.unwhitenedError(landmarkEstimates);
169  if (k <= 200 || std::abs(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, initial);
179  gttoc_(update);
180  gttic_(calculateEstimate);
182  gttoc_(calculateEstimate);
183  lastPose = result.at<Pose2>(i);
184  // update landmark estimates
185  landmarkEstimates = Values();
186  landmarkEstimates.insert(symbol('L', 1), result.at(symbol('L', 1)));
187  landmarkEstimates.insert(symbol('L', 6), result.at(symbol('L', 6)));
188  landmarkEstimates.insert(symbol('L', 0), result.at(symbol('L', 0)));
189  landmarkEstimates.insert(symbol('L', 5), result.at(symbol('L', 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
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(0);
211 }
212 
const gtsam::Symbol key('X', 0)
#define gttic_(label)
Definition: timing.h:245
def update(text)
Definition: relicense.py:46
Eigen::Vector3d Vector3
Definition: Vector.h:43
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
const ValueType at(Key j) const
Definition: Values-inl.h:204
std::pair< double, Pose2 > TimedOdometry
Vector2 Point2
Definition: Point2.h:32
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Values initial
Definition: BFloat16.h:88
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
Definition: ISAM2.cpp:400
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:704
vector< RangeTriple > readTriples()
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
SharedDiagonal odoNoise
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
list< TimedOdometry > readOdometry()
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
void tictoc_print_()
Definition: timing.h:268
NonlinearISAM isam(relinearizeInterval)
Key symbol(unsigned char c, std::uint64_t j)
Class compose(const Class &g) const
Definition: Lie.h:48
static Pose3 pose0
traits
Definition: chartTesting.h:28
auto priorNoise
int main(int argc, char **argv)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
double x() const
get x
Definition: Pose3.h:292
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
std::vector< float > Values
pair< double, Pose2 > TimedOdometry
ofstream os("timeSchurFactors.csv")
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
Double_ range(const Point2_ &p, const Point2_ &q)
std::tuple< double, size_t, double > RangeTriple
void insert(Key j, const Value &val)
Definition: Values.cpp:155
static double error
Definition: testRot3.cpp:37
Pose2 odometry(2.0, 0.0, 0.0)
2D Pose
#define gttoc_(label)
Definition: timing.h:250
#define abs(x)
Definition: datatypes.h:17
utility functions for loading datasets
Values calculateEstimate() const
Definition: ISAM2.cpp:766
std::ptrdiff_t j
Point2 t(10, 10)
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594
double y() const
get y
Definition: Pose3.h:297
All noise models live in the noiseModel namespace.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:52