Merge pull request #463 from borglab/feature/sfm_binary_measurement

Update translation averaging to use vector of binary measurements instead of map

gtsam.h

@@ -2569,10 +2569,12 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
-
-
 #include <gtsam/nonlinear/NonlinearEquality.h>
-template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
+template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
+               gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
+               gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
+               gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2,
+               gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
   NonlinearEquality(size_t j, const T& feasible);
@@ -2583,7 +2585,6 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
-
 #include <gtsam/sam/RangeFactor.h>
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NoiseModelFactor {
@@ -2881,6 +2882,19 @@ virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
   Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
 };
 
+#include <gtsam/sfm/BinaryMeasurement.h>
+template<T>
+class BinaryMeasurement {
+  BinaryMeasurement(size_t key1, size_t key2, const T& measured,
+                    const gtsam::noiseModel::Base* model);
+  size_t key1() const;
+  size_t key2() const;
+  T measured() const;
+};
+
+typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
+typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
+
 //*************************************************************************
 // Navigation
 //*************************************************************************

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 represents a measurement between two keys in a graph.
+ * A binary measurement is similar to a BetweenFactor, except that it does not contain 
+ * an error function. It is a measurement (along with a noise model) from one key to 
+ * another. Note that the direction is important. A measurement from key1 to key2 is not
+ * the same as the same measurement from key2 to key1. 
+ */
+
+#include <ostream>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#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:
+  /** Constructor */
+  BinaryMeasurement(Key key1, Key key2, const VALUE &measured,
+                    const SharedNoiseModel &model = nullptr) :
+      key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) {
+  }
+
+  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 BinaryMeasurement &expected, double tol = 1e-9) const {
+    const BinaryMeasurement<VALUE> *e = dynamic_cast<const BinaryMeasurement<VALUE> *> (&expected);
+    return e != nullptr && key1_ == e->key1_ &&
+        key2_ == e->key2_
+        && traits<VALUE>::Equals(this->measured_, e->measured_, tol) &&
+        noiseModel_->equals(*expected.noiseModel());
+  }
+
+  /** return the measured value */
+  VALUE measured() const {
+    return measured_;
+  }
+}; // \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

@@ -10,10 +10,10 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file TranslationRecovery.h
+ * @file TranslationRecovery.cpp
  * @author Frank Dellaert
  * @date March 2020
- * @brief test recovering translations when rotations are given.
+ * @brief Source code for recovering translations when rotations are given
  */
 
 #include <gtsam/sfm/TranslationRecovery.h>
@@ -33,30 +33,25 @@ 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;
 }
 
 void TranslationRecovery::addPrior(const double scale,
-                                   NonlinearFactorGraph* graph) const {
+                                   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;
 }
@@ -89,7 +82,8 @@ Values TranslationRecovery::run(const double scale) const {
 }
 
 TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
-    const Values& poses, const vector<KeyPair>& edges) {
+    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

@@ -13,15 +13,16 @@
  * @file TranslationRecovery.h
  * @author Frank Dellaert
  * @date March 2020
- * @brief test recovering translations when rotations are given.
+ * @brief Recovering translations in an epipolar graph when rotations are given.
  */
 
 #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,13 +60,14 @@ 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. 
    */
-  TranslationRecovery(const TranslationEdges& relativeTranslations,
+  TranslationRecovery(const TranslationEdges &relativeTranslations,
-                      const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams())
+                      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
       : relativeTranslations_(relativeTranslations), params_(lmParams) {
     params_.setVerbosityLM("Summary");
   }
@@ -83,7 +85,7 @@ class TranslationRecovery {
    * @param scale scale for first relative translation which fixes gauge.
    * @param graph factor graph to which prior is added.
    */
-  void addPrior(const double scale, NonlinearFactorGraph* graph) const;
+  void addPrior(const double scale, NonlinearFactorGraph *graph) const;
 
   /**
    * @brief Create random initial translations.
@@ -105,10 +107,11 @@ 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);
+      const Values &poses, const std::vector<KeyPair> &edges);
 };
 }  // namespace gtsam

gtsam/sfm/tests/testBinaryMeasurement.cpp

@@ -0,0 +1,70 @@
+/* ----------------------------------------------------------------------------
+
+ * 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;
+
+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));
+
+  BinaryMeasurement<Unit3> unit3MeasurementCopy(kKey1, kKey2, unit3Measured,
+                                                unit3_model);
+  EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
+}
+
+TEST(BinaryMeasurement, Rot3) {
+  // testing the accessors
+  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));
+
+  // testing the equals function
+  BinaryMeasurement<Rot3> rot3MeasurementCopy(kKey1, kKey2, rot3Measured,
+                                              rot3_model);
+  EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/sfm/tests/testTranslationFactor.cpp

@@ -51,8 +51,6 @@ TEST(TranslationFactor, ZeroError) {
   // Verify we get the expected error
   Vector expected = (Vector3() << 0, 0, 0).finished();
   EXPECT(assert_equal(expected, actualError, 1e-9));
-
-
 }
 
 /* ************************************************************************* */
@@ -67,13 +65,13 @@ TEST(TranslationFactor, NonZeroError) {
   Vector actualError(factor.evaluateError(T1, T2));
 
   // verify we get the expected error
-  Vector expected = (Vector3() << -1, 1/sqrt(2), 1/sqrt(2)).finished();
+  Vector expected = (Vector3() << -1, 1 / sqrt(2), 1 / sqrt(2)).finished();
   EXPECT(assert_equal(expected, actualError, 1e-9));
 }
 
 /* ************************************************************************* */
-Vector factorError(const Point3& T1, const Point3& T2,
+Vector factorError(const Point3 &T1, const Point3 &T2,
-                   const TranslationFactor& factor) {
+                   const TranslationFactor &factor) {
   return factor.evaluateError(T1, T2);
 }
 

tests/testTranslationRecovery.cpp

@@ -49,15 +49,17 @@ TEST(TranslationRecovery, BAL) {
       poses, {{0, 1}, {0, 2}, {1, 2}});
 
   // Check
-  const Pose3 wTa = poses.at<Pose3>(0), wTb = poses.at<Pose3>(1),
+  Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test
-              wTc = poses.at<Pose3>(2);
+  for(auto& unitTranslation : relativeTranslations) {
-  const Point3 Ta = wTa.translation(), Tb = wTb.translation(),
+    const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()), 
-               Tc = wTc.translation();
+                wTb = poses.at<Pose3>(unitTranslation.key2());
-  const Rot3 aRw = wTa.rotation().inverse();
+    const Point3 Ta = wTa.translation(), Tb = wTb.translation();
-  const Unit3 w_aZb = relativeTranslations.at({0, 1});
+    const Unit3 w_aZb = unitTranslation.measured();
-  EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
+    EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
-  const Unit3 w_aZc = relativeTranslations.at({0, 2});
+    if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) {
-  EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc));
+      w_aZb_stored = unitTranslation.measured();
+    }
+  }
 
   TranslationRecovery algorithm(relativeTranslations);
   const auto graph = algorithm.buildGraph();
@@ -69,10 +71,14 @@ TEST(TranslationRecovery, BAL) {
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
-  EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at<Point3>(1)));
+  EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at<Point3>(1)));
 
   // Check that the third translations is correct
-  Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
+  Point3 Ta = poses.at<Pose3>(0).translation();
+  Point3 Tb = poses.at<Pose3>(1).translation();
+  Point3 Tc = poses.at<Pose3>(2).translation();
+  Point3 expected = 
+      (Tc - Ta) * (scale / (Tb - Ta).norm());
   EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
 
   // TODO(frank): how to get stats back?