add binary measurement class

gtsam/sfm/BinaryMeasurement.h

@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ * @file BinaryMeasurement.h
+ * @author Akshay Krishnan
+ * @date July 2020
+ * @brief Binary measurement for representing edges in the epipolar graph
+ */
+
+#include <gtsam/inference/Key.h>
+#include <gtsam/linear/NoiseModel.h>
+
+namespace gtsam {
+
+template<class VALUE> 
+class BinaryMeasurement {
+    // Check that VALUE type is testable
+    BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
+
+public:
+    typedef VALUE T;
+
+    // shorthand for a smart pointer to a measurement
+    typedef typename boost::shared_ptr<BinaryMeasurement> shared_ptr;
+
+private:
+    Key key1_, key2_;   /** Keys */
+
+    VALUE measured_; /** The measurement */
+
+    SharedNoiseModel noiseModel_; /** Noise model */
+
+public:
+    /** default constructor - only use for serialization */
+    BinaryMeasurement() {}
+
+    /** Constructor */
+    BinaryMeasurement(Key key1, Key key2, const VALUE& measured,
+        const SharedNoiseModel& model = nullptr) :
+      key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) {
+    }
+
+    virtual ~BinaryMeasurement() {}
+
+    Key key1() const { return key1_;  }
+
+    Key key2() const { return key2_;  }
+
+    const SharedNoiseModel& noiseModel() const { return noiseModel_; }
+
+    /** implement functions needed for Testable */
+
+    /** print */
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      std::cout << s << "BinaryMeasurement("
+          << keyFormatter(this->key1()) << ","
+          << keyFormatter(this->key2()) << ")\n";
+      traits<T>::Print(measured_, "  measured: ");
+      this->noiseModel_->print("  noise model: ");
+    }
+
+    /** equals */
+    bool equals(const BetweenMeasurement& expected, double tol=1e-9) const {
+      const BetweenMeasurement<VALUE> *e =  dynamic_cast<const  BetweenMeasurement<VALUE>*> (&expected);
+      return e != nullptr && key1_ == expected.key1() && 
+             key2_ == expected.key2() 
+             && traits<VALUE>::Equals(this->measured_, e->measured_, tol) && 
+             noiseModel_.equals(expected.noiseModel());
+    }
+
+    /** return the measured */
+    const VALUE& measured() const {
+      return measured_;
+    }
+
+    /** number of variables attached to this measurement */
+    std::size_t size() const {
+      return 2;
+    }
+  }; // \class BetweenMeasurement
+}

gtsam/sfm/TranslationFactor.h

@@ -36,6 +36,8 @@ namespace gtsam {
  *    normalized(Tb - Ta) - w_aZb.point3()
  *
  * @addtogroup SFM
+ * 
+ * 
  */
 class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
  private:

gtsam/sfm/TranslationRecovery.cpp

@@ -33,15 +33,12 @@ using namespace gtsam;
 using namespace std;
 
 NonlinearFactorGraph TranslationRecovery::buildGraph() const {
-  auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
   NonlinearFactorGraph graph;
 
   // Add all relative translation edges
   for (auto edge : relativeTranslations_) {
-    Key a, b;
+    graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(), 
-    tie(a, b) = edge.first;
+      edge.measured(), edge.noiseModel());
-    const Unit3 w_aZb = edge.second;
-    graph.emplace_shared<TranslationFactor>(a, b, w_aZb, noiseModel);
   }
 
   return graph;
@@ -49,14 +46,12 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
 
 void TranslationRecovery::addPrior(const double scale,
                                    NonlinearFactorGraph* graph) const {
-  auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
+  //TODO(akshay-krishnan): make this an input argument                                     
+  auto priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
   auto edge = relativeTranslations_.begin();
-  Key a, b;
+  graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0), priorNoiseModel);
-  tie(a, b) = edge->first;
+  graph->emplace_shared<PriorFactor<Point3> >(edge->key2(), scale * edge->measured().point3(),
-  const Unit3 w_aZb = edge->second;
+                                              edge->noiseModel());
-  graph->emplace_shared<PriorFactor<Point3> >(a, Point3(0, 0, 0), noiseModel);
-  graph->emplace_shared<PriorFactor<Point3> >(b, scale * w_aZb.point3(),
-                                              noiseModel);
 }
 
 Values TranslationRecovery::initalizeRandomly() const {
@@ -71,10 +66,8 @@ Values TranslationRecovery::initalizeRandomly() const {
 
   // Loop over measurements and add a random translation
   for (auto edge : relativeTranslations_) {
-    Key a, b;
+    insert(edge.key1());
-    tie(a, b) = edge.first;
+    insert(edge.key2());
-    insert(a);
-    insert(b);
   }
   return initial;
 }
@@ -90,6 +83,7 @@ Values TranslationRecovery::run(const double scale) const {
 
 TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
     const Values& poses, const vector<KeyPair>& edges) {
+  auto edgeNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
   TranslationEdges relativeTranslations;
   for (auto edge : edges) {
     Key a, b;
@@ -97,7 +91,7 @@ TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
     const Pose3 wTa = poses.at<Pose3>(a), wTb = poses.at<Pose3>(b);
     const Point3 Ta = wTa.translation(), Tb = wTb.translation();
     const Unit3 w_aZb(Tb - Ta);
-    relativeTranslations[edge] = w_aZb;
+    relativeTranslations.emplace_back(a, b, w_aZb, edgeNoiseModel);
   }
   return relativeTranslations;
 }

gtsam/sfm/TranslationRecovery.h

@@ -19,9 +19,10 @@
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/BinaryMeasurement.h>
 
-#include <map>
 #include <utility>
+#include <vector>
 
 namespace gtsam {
 
@@ -48,7 +49,7 @@ namespace gtsam {
 class TranslationRecovery {
  public:
   using KeyPair = std::pair<Key, Key>;
-  using TranslationEdges = std::map<KeyPair, Unit3>;
+  using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
 
  private:
   TranslationEdges relativeTranslations_;
@@ -59,7 +60,8 @@ class TranslationRecovery {
    * @brief Construct a new Translation Recovery object
    *
    * @param relativeTranslations the relative translations, in world coordinate
-   * frames, indexed in a map by a pair of Pose keys.
+   * frames, vector of BinaryMeasurements of Unit3, where each key of a measurement 
+   * is a point in 3D. 
    * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
    * used to modify the parameters for the LM optimizer. By default, uses the
    * default LM parameters. 
@@ -105,8 +107,9 @@ class TranslationRecovery {
    *
    * @param poses SE(3) ground truth poses stored as Values
    * @param edges pairs (a,b) for which a measurement w_aZb will be generated.
-   * @return TranslationEdges map from a KeyPair to the simulated Unit3
+   * @return TranslationEdges vector of binary measurements where the keys are 
-   * translation direction measurement between the cameras in KeyPair.
+   * the cameras and the measurement is the simulated Unit3 translation 
+   * direction between the cameras.
    */
   static TranslationEdges SimulateMeasurements(
       const Values& poses, const std::vector<KeyPair>& edges);

gtsam/sfm/tests/testBinaryMeasurement.cpp

@@ -0,0 +1,61 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  testBinaryMeasurement.cpp
+ *  @brief Unit tests for BinaryMeasurement class
+ *  @author Akshay Krishnan
+ *  @date July 2020
+ */
+
+#include <gtsam/sfm/BinaryMeasurement.h>
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/geometry/Pose3.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+// Keys are deliberately *not* in sorted order to test that case.
+static const Key kKey1(2), kKey2(1);
+
+// Create noise models for unit3 and rot3
+static SharedNoiseModel unit3_model(noiseModel::Isotropic::Sigma(2, 0.05));
+static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05));
+
+const Unit3 unit3Measured(Vector3(1, 1, 1));
+const Rot3 rot3Measured;
+
+TEST(BinaryMeasurement, Unit3) {
+  BinaryMeasurement<Unit3> unit3Measurement(kKey1, kKey2, unit3Measured,
+                                            unit3_model);
+  EXPECT_LONGS_EQUAL(unit3Measurement.key1(), kKey1);
+  EXPECT_LONGS_EQUAL(unit3Measurement.key2(), kKey2);
+  EXPECT(unit3Measurement.measured().equals(unit3Measured));
+}
+
+TEST(BinaryMeasurement, Rot3) {
+  BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
+                                            rot3_model);
+  EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
+  EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
+  EXPECT(rot3Measurement.measured().equals(rot3Measured));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */