12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed parsing error and cleaned up

release/4.3a0
Frank Dellaert 2022-03-19 15:19:25 -04:00
parent 383d81298c
commit cf5e3729e0
1 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
examples/RangeISAMExample_plaza2.cpp
View File

@ -67,10 +67,10 @@ using gtsam::tictoc_print_;
namespace NM = gtsam::noiseModel; namespace NM = gtsam::noiseModel;
// data available at // Data is second UWB ranging dataset, B2 or "plaza 2", from
// http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/ Datafile // "Navigating with Ranging Radios: Five Data Sets with Ground Truth"
// format (from // by Joseph Djugash, Bradley Hamner, and Stephan Roth
// http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html) // https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf
// load the odometry // load the odometry
// DR: Odometry Input (delta distance traveled and delta heading change) // DR: Odometry Input (delta distance traveled and delta heading change)
@ -99,8 +99,7 @@ std::vector<RangeTriple> readTriples() {
std::ifstream is(data_file.c_str()); std::ifstream is(data_file.c_str());
while (is) { while (is) {
double t, sender, range; double t, range, sender, receiver;
size_t receiver;
is >> t >> sender >> receiver >> range; is >> t >> sender >> receiver >> range;
triples.emplace_back(t, receiver, range); triples.emplace_back(t, receiver, range);
} }
@ -112,10 +111,12 @@ std::vector<RangeTriple> readTriples() {
int main(int argc, char** argv) { int main(int argc, char** argv) {
// load Plaza2 data // load Plaza2 data
std::list<TimedOdometry> odometry = readOdometry(); std::list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size(); size_t M = odometry.size();
std::cout << "Read " << M << " odometry entries." << std::endl;
std::vector<RangeTriple> triples = readTriples(); std::vector<RangeTriple> triples = readTriples();
size_t K = triples.size(); size_t K = triples.size();
std::cout << "Read " << K << " range triples." << std::endl;
// parameters // parameters
size_t minK = size_t minK =
@ -149,7 +150,7 @@ int main(int argc, char** argv) {
// We will initialize landmarks randomly, and keep track of which landmarks we // We will initialize landmarks randomly, and keep track of which landmarks we
// already added with a set. // already added with a set.
std::mt19937_64 rng; std::mt19937_64 rng;
std::normal_distribution<double> normal(0.0, 1.0); std::normal_distribution<double> normal(0.0, 100.0);
std::set<Symbol> initializedLandmarks; std::set<Symbol> initializedLandmarks;
// set some loop variables // set some loop variables
@ -184,6 +185,7 @@ int main(int argc, char** argv) {
newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>( newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>(
i, landmark_key, range, rangeNoise); i, landmark_key, range, rangeNoise);
if (initializedLandmarks.count(landmark_key) == 0) { if (initializedLandmarks.count(landmark_key) == 0) {
std::cout << "adding landmark " << j << std::endl;
double x = normal(rng), y = normal(rng); double x = normal(rng), y = normal(rng);
initial.insert(landmark_key, Point2(x, y)); initial.insert(landmark_key, Point2(x, y));
initializedLandmarks.insert(landmark_key); initializedLandmarks.insert(landmark_key);
@ -199,6 +201,7 @@ int main(int argc, char** argv) {
// Check whether to update iSAM 2 // Check whether to update iSAM 2
if ((k > minK) && (countK > incK)) { 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
std::cout << "Initializing at time " << k << std::endl;
gttic_(batchInitialization); gttic_(batchInitialization);
gtsam::LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial); gtsam::LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
initial = batchOptimizer.optimize(); initial = batchOptimizer.optimize();