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 17f21cf9a..43a9c588e 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -185,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 on %d",range,(int)j,(int)i); + } catch (const invalid_argument& e) { + printf("warning: omitting duplicate range %g for %d on %d",range,(int)j,(int)i); + } + cout << endl; } else { RangeFactor factor(i, symbol('L', j), range, diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index c90a9646f..c05633345 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -59,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(); 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()) } /* ************************************************************************* */