SmartRangeExample_plaza1.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>
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 
51 using namespace std;
52 using namespace gtsam;
53 namespace 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)
61 typedef pair<double, Pose2> TimedOdometry;
62 list<TimedOdometry> readOdometry() {
63  list<TimedOdometry> odometryList;
64  string drFile = findExampleDataFile("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  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)
80 typedef std::tuple<double, size_t, double> RangeTriple;
81 vector<RangeTriple> readTriples() {
82  vector<RangeTriple> triples;
83  string tdFile = findExampleDataFile("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(RangeTriple(t, receiver, range));
91  }
92  is.clear(); /* clears the end-of-file and error flags */
93  return triples;
94 }
95 
96 // main
97 int 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(odoSigmas);
115  double sigmaR = 100; // range standard deviation
116  const NM::Base::shared_ptr // all same type
117  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
118  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
119  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), 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(0, pose0, 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(symbol('L', 1), Point2(-68.9265, 18.3778));
138  initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
139  initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
140  initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
141  }
142  Values landmarkEstimates = initial; // copy landmarks
143  initial.insert(0, 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(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(t, odometry) = timedOdometry;
169  printf("step %d, time = %g\n",(int)i,t);
170 
171  // add odometry factor
172  newFactors.push_back(
173  BetweenFactor<Pose2>(i - 1, i, odometry, 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  landmarkEstimates.insert(i, predictedPose);
180 
181  // Check if there are range factors to be added
182  while (k < K && t >= std::get<0>(triples[k])) {
183  size_t j = std::get<1>(triples[k]);
184  double range = std::get<2>(triples[k]);
185  if (i > start) {
186  if (smart && totalCount < minK) {
187  try {
188  smartFactors[j]->addRange(i, range);
189  printf("adding range %g for %d",range,(int)j);
190  } catch (const invalid_argument& e) {
191  printf("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('L', j), range,
198  rangeNoise);
199  // Throw out obvious outliers based on current landmark estimates
200  Vector error = factor.unwhitenedError(landmarkEstimates);
201  if (k <= 200 || std::abs(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, initial);
214  gttoc_(update);
215  gttic_(calculateEstimate);
217  gttoc_(calculateEstimate);
218  lastPose = result.at<Pose2>(i);
219  bool hasLandmarks = result.exists(symbol('L', ids[0]));
220  if (hasLandmarks) {
221  // update landmark estimates
222  landmarkEstimates = Values();
223  for(size_t jj: ids)
224  landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', 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(result);
232  initial.insert(symbol('L', jj), landmark);
233  landmarkEstimates.insert(symbol('L', jj), 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(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
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(0);
262 }
263 
const gtsam::Symbol key('X', 0)
A smart factor for range-only SLAM that does initialization and marginalization.
#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
#define M_PI
Definition: main.h:106
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
static Point3 landmark(0, 0, 5)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
list< TimedOdometry > readOdometry()
SharedDiagonal odoNoise
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
vector< RangeTriple > readTriples()
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
void tictoc_print_()
Definition: timing.h:268
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearISAM isam(relinearizeInterval)
Key symbol(unsigned char c, std::uint64_t j)
Class compose(const Class &g) const
Definition: Lie.h:48
std::tuple< double, size_t, double > RangeTriple
static Pose3 pose0
traits
Definition: chartTesting.h:28
auto priorNoise
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
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)
int main(int argc, char **argv)
static EIGEN_DEPRECATED const end_t end
void insert(Key j, const Value &val)
Definition: Values.cpp:155
static double error
Definition: testRot3.cpp:37
pair< double, Pose2 > TimedOdometry
Pose2 odometry(2.0, 0.0, 0.0)
2D Pose
#define gttoc_(label)
Definition: timing.h:250
bool exists(Key j) const
Definition: Values.cpp:93
#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