sfm changes to docs, format, wrapper

gtsam.h

@@ -2894,28 +2894,6 @@ class BinaryMeasurement {
 typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
 
-// std::vector<gtsam::BinaryMeasurement<Unit3>::shared_ptr>
-class BinaryMeasurementUnit3s
-{
-  BinaryMeasurementUnit3s();
-  size_t size() const;
-  gtsam::BinaryMeasurementUnit3* at(size_t i) const;
-  void push_back(const gtsam::BinaryMeasurementUnit3* measurement);
-};
-
-// std::vector<gtsam::BinaryMeasurement<Rot3>::shared_ptr>
-class BinaryMeasurementRot3s
-{
-  BinaryMeasurementRot3s();
-  size_t size() const;
-  gtsam::BinaryMeasurementRot3* at(size_t i) const;
-  void push_back(const gtsam::BinaryMeasurementRot3* measurement);
-};
-
-#include <gtsam/sfm/TranslationRecovery.h>
-class TranslationRecovery {
-  TranslationRecovery()
-}
 //*************************************************************************
 // Navigation
 //*************************************************************************

gtsam/sfm/BinaryMeasurement.h

@@ -15,12 +15,15 @@
  * @file BinaryMeasurement.h
  * @author Akshay Krishnan
  * @date July 2020
- * @brief Binary measurement for representing edges in the epipolar graph
+ * @brief Binary measurement for representing edges in the epipolar 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>
@@ -32,66 +35,58 @@ namespace gtsam {
 
 template<class VALUE>
 class BinaryMeasurement {
-    // Check that VALUE type is testable
+  // Check that VALUE type is testable
-    BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
+  BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
 
-public:
+ public:
-    typedef VALUE T;
+  typedef VALUE T;
 
-    // shorthand for a smart pointer to a measurement
+  // shorthand for a smart pointer to a measurement
-    typedef typename boost::shared_ptr<BinaryMeasurement> shared_ptr;
+  typedef typename boost::shared_ptr<BinaryMeasurement> shared_ptr;
 
-private:
+ private:
-    Key key1_, key2_;   /** Keys */
+  Key key1_, key2_;   /** Keys */
 
-    VALUE measured_; /** The measurement */
+  VALUE measured_; /** The measurement */
 
-    SharedNoiseModel noiseModel_; /** Noise model */
+  SharedNoiseModel noiseModel_; /** Noise model */
 
-public:
+ public:
-    /** default constructor - only use for serialization */
+  /** Constructor */
-    BinaryMeasurement() {}
+  BinaryMeasurement(Key key1, Key key2, const VALUE &measured,
-
+                    const SharedNoiseModel &model = nullptr) :
-    /** 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 key1() const { return key1_;  }
+  Key key2() const { return key2_; }
 
-    Key key2() const { return key2_;  }
+  const SharedNoiseModel &noiseModel() const { return noiseModel_; }
 
-    const SharedNoiseModel& noiseModel() const { return noiseModel_; }
+  /** implement functions needed for Testable */
 
-    /** 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: ");
+  }
 
-    /** print */
+  /** equals */
-    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
-      std::cout << s << "BinaryMeasurement("
+    const BinaryMeasurement<VALUE> *e = dynamic_cast<const BinaryMeasurement<VALUE> *> (&expected);
-          << keyFormatter(this->key1()) << ","
+    return e != nullptr && key1_ == e->key1_ &&
-          << keyFormatter(this->key2()) << ")\n";
+        key2_ == e->key2_
-      traits<T>::Print(measured_, "  measured: ");
+        && traits<VALUE>::Equals(this->measured_, e->measured_, tol) &&
-      this->noiseModel_->print("  noise model: ");
+        noiseModel_->equals(*expected.noiseModel());
-    }
+  }
 
-    /** equals */
+  /** return the measured value */
-    bool equals(const BinaryMeasurement& expected, double tol=1e-9) const {
+  VALUE measured() const {
-      const BinaryMeasurement<VALUE> *e =  dynamic_cast<const  BinaryMeasurement<VALUE>*> (&expected);
+    return measured_;
-      return e != nullptr && key1_ == e->key1_ && 
+  }
-             key2_ == e->key2_
+}; // \class BetweenMeasurement
-             && traits<VALUE>::Equals(this->measured_, e->measured_, tol) && 
-             noiseModel_->equals(expected.noiseModel());
-    }
-
-    /** return the measured */
-    VALUE measured() const {
-      return measured_;
-    }
-  }; // \class BetweenMeasurement
-
-  using BinaryMeasurementUnit3s = std::vector<gtsam::BinaryMeasurement<Unit3>::shared_ptr>;
-  using BinaryMeasurementRot3s = std::vector<gtsam::BinaryMeasurement<Rot3>::shared_ptr>;
 }

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>
@@ -38,14 +38,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
   // Add all relative translation edges
   for (auto edge : relativeTranslations_) {
     graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
-      edge.measured(), edge.noiseModel());
+                                            edge.measured(), edge.noiseModel());
   }
 
   return graph;
 }
 
 void TranslationRecovery::addPrior(const double scale,
-                                   NonlinearFactorGraph* graph) const {
+                                   NonlinearFactorGraph *graph) const {
   //TODO(akshay-krishnan): make this an input argument                                     
   auto priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
   auto edge = relativeTranslations_.begin();
@@ -82,7 +82,7 @@ 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) {

gtsam/sfm/TranslationRecovery.h

@@ -13,7 +13,7 @@
  * @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>
@@ -66,8 +66,8 @@ class TranslationRecovery {
    * 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");
   }
@@ -85,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.
@@ -112,6 +112,6 @@ class TranslationRecovery {
    * 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

@@ -27,7 +27,6 @@
 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
@@ -43,14 +42,24 @@ TEST(BinaryMeasurement, Unit3) {
   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);
+                                          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));
 }
 
 /* ************************************************************************* */

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