diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 032d61a4a..7b1667e12 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -108,7 +108,7 @@ int main (int argc, char** argv) { // Set Noise parameters Vector priorSigmas = Vector3(1,1,M_PI); - Vector odoSigmas = Vector3(0.05, 0.01, 0.2); + 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 @@ -157,7 +157,7 @@ int main (int argc, char** argv) { boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.push_back(BetweenFactor(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); + newFactors.push_back(BetweenFactor(i-1, i, odometry, odoNoise)); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 489a4adf4..6e55eb50c 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -118,14 +118,17 @@ public: } /** - * Add a new measurement and pose key - * @param measured_i is the 2m dimensional projection of a single landmark - * @param poseKey is the index corresponding to the camera observing the landmark - * @param sharedNoiseModel is the measurement noise + * Add a new measurement and pose/camera key + * @param measured is the 2m dimensional projection of a single landmark + * @param key is the index corresponding to the camera observing the landmark */ - void add(const Z& measured_i, const Key& cameraKey_i) { - this->measured_.push_back(measured_i); - this->keys_.push_back(cameraKey_i); + void add(const Z& measured, const Key& key) { + if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) { + throw std::invalid_argument( + "SmartFactorBase::add: adding duplicate measurement for key."); + } + this->measured_.push_back(measured); + this->keys_.push_back(key); } /** @@ -133,8 +136,7 @@ public: */ void add(ZVector& measurements, KeyVector& cameraKeys) { for (size_t i = 0; i < measurements.size(); i++) { - this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(cameraKeys.at(i)); + this->add(measurements.at(i), cameraKeys.at(i)); } } diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 0ee601d26..24bab3feb 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartRangeExample_plaza2.cpp + * @file SmartRangeExample_plaza1.cpp * @brief A 2D Range SLAM example * @date June 20, 2013 * @author FRank Dellaert @@ -42,6 +42,9 @@ #include #include +// To find data files, we can use `findExampleDataFile`, declared here: +#include + // Standard headers, added last, so we know headers above work on their own #include #include @@ -59,10 +62,9 @@ namespace NM = gtsam::noiseModel; typedef pair TimedOdometry; list readOdometry() { list odometryList; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found"); + string drFile = findExampleDataFile("Plaza1_DR.txt"); + ifstream is(drFile); + if (!is) throw runtime_error("Plaza1_DR.txt file not found"); while (is) { double t, distance_traveled, delta_heading; @@ -79,10 +81,9 @@ list readOdometry() { typedef boost::tuple RangeTriple; vector readTriples() { vector triples; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found"); + string tdFile = findExampleDataFile("Plaza1_TD.txt"); + ifstream is(tdFile); + if (!is) throw runtime_error("Plaza1_TD.txt file not found"); while (is) { double t, sender, receiver, range; @@ -98,8 +99,6 @@ int main(int argc, char** argv) { // load Plaza1 data list odometry = readOdometry(); -// size_t M = odometry.size(); - vector triples = readTriples(); size_t K = triples.size(); @@ -112,11 +111,11 @@ int main(int argc, char** argv) { // Set Noise parameters Vector priorSigmas = Vector3(1, 1, M_PI); - Vector odoSigmas = Vector3(0.05, 0.01, 0.2); + Vector odoSigmas = Vector3(0.05, 0.01, 0.1); + auto odoNoise = NM::Diagonal::Sigmas(odoSigmas); 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; @@ -130,10 +129,8 @@ int main(int argc, char** argv) { NonlinearFactorGraph newFactors; newFactors.push_back(PriorFactor(0, pose0, priorNoise)); - ofstream os2( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); - ofstream os3( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt"); + ofstream os2("rangeResultLM.txt"); + ofstream os3("rangeResultSR.txt"); // initialize points (Gaussian) Values initial; @@ -170,11 +167,11 @@ int main(int argc, char** argv) { double t; Pose2 odometry; boost::tie(t, odometry) = timedOdometry; + printf("step %d, time = %g\n",(int)i,t); // add odometry factor newFactors.push_back( - BetweenFactor(i - 1, i, odometry, - NM::Diagonal::Sigmas(odoSigmas))); + BetweenFactor(i - 1, i, odometry, odoNoise)); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); @@ -188,8 +185,13 @@ int main(int argc, char** argv) { double range = boost::get<2>(triples[k]); if (i > start) { if (smart && totalCount < minK) { - smartFactors[j]->addRange(i, range); - printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl; + try { + smartFactors[j]->addRange(i, range); + printf("adding range %g for %d",range,(int)j); + } catch (const invalid_argument& e) { + printf("warning: omitting duplicate range %g for %d",range,(int)j); + } + cout << endl; } else { RangeFactor factor(i, symbol('L', j), range, @@ -254,8 +256,7 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); - ofstream os( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt"); + ofstream os("rangeResult.txt"); for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 5f33a215b..a83eb06ac 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -41,6 +41,9 @@ #include #include +// To find data files, we can use `findExampleDataFile`, declared here: +#include + // Standard headers, added last, so we know headers above work on their own #include #include @@ -58,10 +61,9 @@ namespace NM = gtsam::noiseModel; typedef pair TimedOdometry; list readOdometry() { list odometryList; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found"); + string drFile = findExampleDataFile("Plaza2_DR.txt"); + ifstream is(drFile); + if (!is) throw runtime_error("Plaza2_DR.txt file not found"); while (is) { double t, distance_traveled, delta_heading; @@ -78,10 +80,9 @@ list readOdometry() { typedef boost::tuple RangeTriple; vector readTriples() { vector triples; - ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt"); - if (!is) - throw runtime_error( - "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found"); + string tdFile = findExampleDataFile("Plaza2_TD.txt"); + ifstream is(tdFile); + if (!is) throw runtime_error("Plaza2_TD.txt file not found"); while (is) { double t, sender, receiver, range; @@ -201,13 +202,11 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); - ofstream os2( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); + ofstream os2("rangeResultLM.txt"); for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl; - ofstream os( - "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt"); + ofstream os("rangeResult.txt"); for(const Values::ConstFiltered::KeyValuePair& it: result.filter()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index b3d71d05f..c05633345 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -13,7 +13,6 @@ #include #include #include -#include #include #include @@ -60,6 +59,10 @@ class SmartRangeFactor: public NoiseModelFactor { /// Add a range measurement to a pose with given key. void addRange(Key key, double measuredRange) { + if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) { + throw std::invalid_argument( + "SmartRangeFactor::addRange: adding duplicate measurement for key."); + } keys_.push_back(key); measurements_.push_back(measuredRange); size_t n = keys_.size(); @@ -89,7 +92,6 @@ class SmartRangeFactor: public NoiseModelFactor { * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -100,7 +102,7 @@ class SmartRangeFactor: public NoiseModelFactor { Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional bestCircle2; + auto bestCircle2 = boost::make_optional(false, circle1); // fixes issue #38 // loop over all circles for (const Circle2& it : circles) { @@ -136,7 +138,6 @@ class SmartRangeFactor: public NoiseModelFactor { } else { throw std::runtime_error("triangulate failed"); } - // gttoc_(triangulate); } /** diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index ead807138..8a6aab6b7 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -46,7 +46,7 @@ TEST( SmartRangeFactor, constructor ) { TEST( SmartRangeFactor, addRange ) { SmartRangeFactor f(sigma); f.addRange(1, 10); - f.addRange(1, 12); + f.addRange(2, 12); LONGS_EQUAL(2, f.size()) } /* ************************************************************************* */