diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index c8e31e1c5..898460b49 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -10,62 +10,80 @@ * -------------------------------------------------------------------------- */ /** - * @file RangeISAMExample_plaza1.cpp + * @file RangeISAMExample_plaza2.cpp * @brief A 2D Range SLAM example * @date June 20, 2013 - * @author FRank Dellaert + * @author Frank Dellaert */ -// Both relative poses and recovered trajectory poses will be stored as Pose2 objects +// Both relative poses and recovered trajectory poses will be stored as Pose2. #include +using gtsam::Pose2; -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols +// gtsam::Vectors are dynamic Eigen vectors, Vector3 is statically sized. +#include +using gtsam::Vector; +using gtsam::Vector3; + +// Unknown landmarks are of type Point2 (which is just a 2D Eigen vector). +#include +using gtsam::Point2; + +// Each variable in the system (poses and landmarks) must be identified with a +// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols +// (X1, X2, L1). Here we will use Symbols. #include +using gtsam::Symbol; -// We want to use iSAM2 to solve the range-SLAM problem incrementally +// We want to use iSAM2 to solve the range-SLAM problem incrementally. #include -// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, -// and initial guesses for any new variables used in the added factors +// iSAM2 requires as input a set set of new factors to be added stored in a +// factor graph, and initial guesses for any new variables in the added factors. #include #include -// We will use a non-liear solver to batch-inituialize from the first 150 frames +// We will use a non-linear solver to batch-initialize from the first 150 frames #include -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics SLAM problems. -#include +// Measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics SLAM problems: #include +#include #include -// Standard headers, added last, so we know headers above work on their own +// Timing, with functions below, provides nice facilities to benchmark. +#include +using gtsam::tictoc_print_; + + +// Standard headers, added last, so we know headers above work on their own. #include #include +#include +#include +#include -using namespace std; -using namespace gtsam; namespace NM = gtsam::noiseModel; -// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/ -// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html) +// data available at +// http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/ Datafile +// format (from +// http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html) // load the odometry // DR: Odometry Input (delta distance traveled and delta heading change) // Time (sec) Delta Dist. Trav. (m) Delta Heading (rad) -typedef pair TimedOdometry; -list readOdometry() { - list odometryList; - string data_file = findExampleDataFile("Plaza2_DR.txt"); - ifstream is(data_file.c_str()); +using TimedOdometry = std::pair; +std::list readOdometry() { + std::list odometryList; + std::string data_file = gtsam::findExampleDataFile("Plaza2_DR.txt"); + std::ifstream is(data_file.c_str()); while (is) { double t, distance_traveled, delta_heading; is >> t >> distance_traveled >> delta_heading; - odometryList.push_back( - TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading))); + odometryList.emplace_back(t, Pose2(distance_traveled, 0, delta_heading)); } is.clear(); /* clears the end-of-file and error flags */ return odometryList; @@ -73,90 +91,91 @@ list readOdometry() { // load the ranges from TD // Time (sec) Sender / Antenna ID Receiver Node ID Range (m) -typedef boost::tuple RangeTriple; -vector readTriples() { - vector triples; - string data_file = findExampleDataFile("Plaza2_TD.txt"); - ifstream is(data_file.c_str()); +using RangeTriple = boost::tuple; +std::vector readTriples() { + std::vector triples; + std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt"); + std::ifstream is(data_file.c_str()); while (is) { double t, sender, range; size_t receiver; is >> t >> sender >> receiver >> range; - triples.push_back(RangeTriple(t, receiver, range)); + triples.emplace_back(t, receiver, range); } is.clear(); /* clears the end-of-file and error flags */ return triples; } // main -int main (int argc, char** argv) { - +int main(int argc, char** argv) { // load Plaza2 data - list odometry = readOdometry(); -// size_t M = odometry.size(); + std::list odometry = readOdometry(); + // size_t M = odometry.size(); - vector triples = readTriples(); + std::vector triples = readTriples(); size_t K = triples.size(); // parameters - size_t minK = 150; // minimum number of range measurements to process initially - size_t incK = 25; // minimum number of range measurements to process after + size_t minK = + 150; // minimum number of range measurements to process initially + size_t incK = 25; // minimum number of range measurements to process after bool groundTruth = false; bool robust = true; // Set Noise parameters - Vector priorSigmas = Vector3(1,1,M_PI); + Vector priorSigmas = Vector3(1, 1, M_PI); Vector odoSigmas = Vector3(0.05, 0.01, 0.1); - double sigmaR = 100; // range standard deviation - const NM::Base::shared_ptr // all same type - priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior - odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry - gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust - tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust - rangeNoise = robust ? tukey : gaussian; + double sigmaR = 100; // range standard deviation + const NM::Base::shared_ptr // all same type + priorNoise = NM::Diagonal::Sigmas(priorSigmas), // prior + odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry + gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust + tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), + gaussian), // robust + rangeNoise = robust ? tukey : gaussian; // Initialize iSAM - ISAM2 isam; + gtsam::ISAM2 isam; // Add prior on first pose - Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, - M_PI - 2.02108900000000); - NonlinearFactorGraph newFactors; + Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.021089); + gtsam::NonlinearFactorGraph newFactors; newFactors.addPrior(0, pose0, priorNoise); - Values initial; + gtsam::Values initial; initial.insert(0, pose0); // initialize points - if (groundTruth) { // from TL file - initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778)); - initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278)); - initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678)); - initial.insert(symbol('L', 5), Point2(1.7095, -5.8122)); - } else { // drawn from sigma=1 Gaussian in matlab version - initial.insert(symbol('L', 1), Point2(3.5784, 2.76944)); - initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492)); - initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549)); - initial.insert(symbol('L', 5), Point2(0.714743, -0.204966)); + if (groundTruth) { // from TL file + initial.insert(Symbol('L', 1), Point2(-68.9265, 18.3778)); + initial.insert(Symbol('L', 6), Point2(-37.5805, 69.2278)); + initial.insert(Symbol('L', 0), Point2(-33.6205, 26.9678)); + initial.insert(Symbol('L', 5), Point2(1.7095, -5.8122)); + } else { // drawn from sigma=1 Gaussian in matlab version + initial.insert(Symbol('L', 1), Point2(3.5784, 2.76944)); + initial.insert(Symbol('L', 6), Point2(-1.34989, 3.03492)); + initial.insert(Symbol('L', 0), Point2(0.725404, -0.0630549)); + initial.insert(Symbol('L', 5), Point2(0.714743, -0.204966)); } // set some loop variables - size_t i = 1; // step counter - size_t k = 0; // range measurement counter + size_t i = 1; // step counter + size_t k = 0; // range measurement counter bool initialized = false; Pose2 lastPose = pose0; size_t countK = 0; // Loop over odometry gttic_(iSAM); - for(const TimedOdometry& timedOdometry: odometry) { - //--------------------------------- odometry loop ----------------------------------------- + for (const TimedOdometry& timedOdometry : odometry) { + //--------------------------------- odometry loop -------------------------- double t; Pose2 odometry; boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.push_back(BetweenFactor(i-1, i, odometry, odoNoise)); + newFactors.emplace_shared>(i - 1, i, odometry, + odoNoise); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); @@ -167,16 +186,17 @@ int main (int argc, char** argv) { while (k < K && t >= boost::get<0>(triples[k])) { size_t j = boost::get<1>(triples[k]); double range = boost::get<2>(triples[k]); - newFactors.push_back(RangeFactor(i, symbol('L', j), range,rangeNoise)); + newFactors.emplace_shared>( + i, Symbol('L', j), range, rangeNoise); k = k + 1; countK = countK + 1; } // Check whether to update iSAM 2 if ((k > minK) && (countK > incK)) { - if (!initialized) { // Do a full optimize for first minK ranges + if (!initialized) { // Do a full optimize for first minK ranges gttic_(batchInitialization); - LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial); + gtsam::LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial); initial = batchOptimizer.optimize(); gttoc_(batchInitialization); initialized = true; @@ -185,16 +205,16 @@ int main (int argc, char** argv) { isam.update(newFactors, initial); gttoc_(update); gttic_(calculateEstimate); - Values result = isam.calculateEstimate(); + gtsam::Values result = isam.calculateEstimate(); gttoc_(calculateEstimate); lastPose = result.at(i); - newFactors = NonlinearFactorGraph(); - initial = Values(); + newFactors = gtsam::NonlinearFactorGraph(); + initial = gtsam::Values(); countK = 0; } i += 1; - //--------------------------------- odometry loop ----------------------------------------- - } // end for + //--------------------------------- odometry loop -------------------------- + } // end for gttoc_(iSAM); // Print timings @@ -202,4 +222,3 @@ int main (int argc, char** argv) { exit(0); } -