Duplicate measurements are disallowed -> leads to duplicate keys in JacobianFactor

gtsam/slam/SmartFactorBase.h

@@ -118,14 +118,17 @@ public:
   }
 
   /**
-   * Add a new measurement and pose key
+   * Add a new measurement and pose/camera key
-   * @param measured_i is the 2m dimensional projection of a single landmark
+   * @param measured is the 2m dimensional projection of a single landmark
-   * @param poseKey is the index corresponding to the camera observing the landmark
+   * @param key is the index corresponding to the camera observing the landmark
-   * @param sharedNoiseModel is the measurement noise
    */
-  void add(const Z& measured_i, const Key& cameraKey_i) {
+  void add(const Z& measured, const Key& key) {
-    this->measured_.push_back(measured_i);
+    if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
-    this->keys_.push_back(cameraKey_i);
+      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->add(measurements.at(i), cameraKeys.at(i));
-      this->keys_.push_back(cameraKeys.at(i));
     }
   }
 

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);
+          try {
-          printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
+            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<Pose2, Point2> factor(i, symbol('L', j), range,

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();

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())
 }
 /* ************************************************************************* */