Merge remote-tracking branch 'origin/develop' into feature/frobeniusfactor

cython/gtsam/tests/test_logging_optimizer.py

@@ -4,6 +4,12 @@ Author: Jing Wu and Frank Dellaert
 """
 # pylint: disable=invalid-name
 
+import sys
+if sys.version_info.major >= 3:
+    from io import StringIO
+else:
+    from cStringIO import StringIO
+
 import unittest
 from datetime import datetime
 
@@ -37,11 +43,19 @@ class TestOptimizeComet(GtsamTestCase):
         self.optimizer = gtsam.GaussNewtonOptimizer(
             graph, initial, self.params)
 
+        # setup output capture
+        self.capturedOutput = StringIO()
+        sys.stdout = self.capturedOutput
+
+    def tearDown(self):
+        """Reset print capture."""
+        sys.stdout = sys.__stdout__
+
     def test_simple_printing(self):
         """Test with a simple hook."""
 
         # Provide a hook that just prints
-        def hook(_, error: float):
+        def hook(_, error):
             print(error)
 
         # Only thing we require from optimizer is an iterate method
@@ -65,7 +79,7 @@ class TestOptimizeComet(GtsamTestCase):
                        + str(time.hour)+":"+str(time.minute)+":"+str(time.second))
 
         # I want to do some comet thing here
-        def hook(optimizer, error: float):
+        def hook(optimizer, error):
             comet.log_metric("Karcher error",
                              error, optimizer.iterations())
 

cython/gtsam/utils/logging_optimizer.py

@@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert
 """
 # pylint: disable=invalid-name
 
-from typing import TypeVar
-
 from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
 import gtsam
 
-T = TypeVar('T')
 
-
+def optimize(optimizer, check_convergence, hook):
-def optimize(optimizer: T, check_convergence, hook):
     """ Given an optimizer and a convergence check, iterate until convergence.
         After each iteration, hook(optimizer, error) is called.
         After the function, use values and errors to get the result.
@@ -36,8 +32,8 @@ def optimize(optimizer: T, check_convergence, hook):
         current_error = new_error
 
 
-def gtsam_optimize(optimizer: NonlinearOptimizer,
+def gtsam_optimize(optimizer,
-                   params: NonlinearOptimizerParams,
+                   params,
                    hook):
     """ Given an optimizer and params, iterate until convergence.
         After each iteration, hook(optimizer) is called.

gtsam/base/types.h

@@ -28,6 +28,7 @@
 #include <cstdint>
 
 #include <exception>
+#include <string>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/scalable_allocator.h>
@@ -54,7 +55,7 @@
 namespace gtsam {
 
   /// Function to demangle type name of variable, e.g. demangle(typeid(x).name())
-  std::string demangle(const char* name);
+  std::string GTSAM_EXPORT demangle(const char* name);
 
   /// Integer nonlinear key type
   typedef std::uint64_t Key;

gtsam/nonlinear/internal/ExecutionTrace.h

@@ -169,6 +169,12 @@ class ExecutionTrace {
       content.ptr->reverseAD2(dTdA, jacobians);
   }
 
+  ~ExecutionTrace() {
+    if (kind == Function) {
+      content.ptr->~CallRecord<Dim>();
+    }
+  }
+
   /// Define type so we can apply it as a meta-function
   typedef ExecutionTrace<T> type;
 };

gtsam/nonlinear/tests/testExecutionTrace.cpp

@@ -16,6 +16,7 @@
  * @brief unit tests for Expression internals
  */
 
+#include <gtsam/nonlinear/internal/CallRecord.h>
 #include <gtsam/nonlinear/internal/ExecutionTrace.h>
 #include <gtsam/geometry/Point2.h>
 

gtsam/sfm/TranslationFactor.h

@@ -0,0 +1,84 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 TranslationFactor.h
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief Binary factor for a relative translation direction measurement.
+ */
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Binary factor for a relative translation direction measurement
+ * w_aZb. The measurement equation is
+ *    w_aZb = Unit3(Tb - Ta)
+ * i.e., w_aZb is the translation direction from frame A to B, in world
+ * coordinates. Although Unit3 instances live on a manifold, following
+ * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
+ * ambient world coordinate frame:
+ *    normalized(Tb - Ta) - w_aZb.point3()
+ *
+ * @addtogroup SFM
+ */
+class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
+ private:
+  typedef NoiseModelFactor2<Point3, Point3> Base;
+  Point3 measured_w_aZb_;
+
+ public:
+  /// default constructor
+  TranslationFactor() {}
+
+  TranslationFactor(Key a, Key b, const Unit3& w_aZb,
+                    const SharedNoiseModel& noiseModel)
+      : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
+
+  /**
+   * @brief Caclulate error: (norm(Tb - Ta) - measurement)
+   * where Tb and Ta are Point3 translations and measurement is
+   * the Unit3 translation direction from a to b.
+   * 
+   * @param Ta translation for key a
+   * @param Tb translation for key b
+   * @param H1 optional jacobian in Ta
+   * @param H2 optional jacobian in Tb
+   * @return * Vector
+   */
+  Vector evaluateError(
+      const Point3& Ta, const Point3& Tb,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const override {
+    const Point3 dir = Tb - Ta;
+    Matrix33 H_predicted_dir;
+    const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
+    if (H1) *H1 = -H_predicted_dir;
+    if (H2) *H2 = H_predicted_dir;
+    return predicted - measured_w_aZb_;
+  }
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
+};  // \ TranslationFactor
+}  // namespace gtsam

gtsam/sfm/TranslationRecovery.cpp

@@ -0,0 +1,103 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 TranslationRecovery.h
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief test recovering translations when rotations are given.
+ */
+
+#include <gtsam/sfm/TranslationRecovery.h>
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/TranslationFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+
+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;
+    tie(a, b) = edge.first;
+    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 {
+  auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
+  auto edge = relativeTranslations_.begin();
+  Key a, b;
+  tie(a, b) = edge->first;
+  const Unit3 w_aZb = edge->second;
+  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 {
+  // Create a lambda expression that checks whether value exists and randomly
+  // initializes if not.
+  Values initial;
+  auto insert = [&initial](Key j) {
+    if (!initial.exists(j)) {
+      initial.insert<Point3>(j, Vector3::Random());
+    }
+  };
+
+  // Loop over measurements and add a random translation
+  for (auto edge : relativeTranslations_) {
+    Key a, b;
+    tie(a, b) = edge.first;
+    insert(a);
+    insert(b);
+  }
+  return initial;
+}
+
+Values TranslationRecovery::run(const double scale) const {
+  auto graph = buildGraph();
+  addPrior(scale, &graph);
+  const Values initial = initalizeRandomly();
+  LevenbergMarquardtOptimizer lm(graph, initial, params_);
+  Values result = lm.optimize();
+  return result;
+}
+
+TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
+    const Values& poses, const vector<KeyPair>& edges) {
+  TranslationEdges relativeTranslations;
+  for (auto edge : edges) {
+    Key a, b;
+    tie(a, b) = edge;
+    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;
+  }
+  return relativeTranslations;
+}

gtsam/sfm/TranslationRecovery.h

@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 TranslationRecovery.h
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief test recovering translations when rotations are given.
+ */
+
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <map>
+#include <utility>
+
+namespace gtsam {
+
+// Set up an optimization problem for the unknown translations Ti in the world
+// coordinate frame, given the known camera attitudes wRi with respect to the
+// world frame, and a set of (noisy) translation directions of type Unit3,
+// w_aZb. The measurement equation is
+//    w_aZb = Unit3(Tb - Ta)   (1)
+// i.e., w_aZb is the translation direction from frame A to B, in world
+// coordinates. Although Unit3 instances live on a manifold, following
+// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
+// ambient world coordinate frame.
+//
+// It is clear that we cannot recover the scale, nor the absolute position,
+// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
+// the translations Ta and Tb associated with the first measurement w_aZb,
+// clamping them to their initial values as given to this method. If no initial
+// values are given, we use the origin for Tb and set Tb to make (1) come
+// through, i.e.,
+//    Tb = s * wRa * Point3(w_aZb)     (2)
+// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
+// versions are supplied below corresponding to whether we have initial values
+// or not.
+class TranslationRecovery {
+ public:
+  using KeyPair = std::pair<Key, Key>;
+  using TranslationEdges = std::map<KeyPair, Unit3>;
+
+ private:
+  TranslationEdges relativeTranslations_;
+  LevenbergMarquardtParams params_;
+
+ public:
+  /**
+   * @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.
+   * @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,
+                      const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams())
+      : relativeTranslations_(relativeTranslations), params_(lmParams) {
+    params_.setVerbosityLM("Summary");
+  }
+
+  /**
+   * @brief Build the factor graph to do the optimization.
+   *
+   * @return NonlinearFactorGraph
+   */
+  NonlinearFactorGraph buildGraph() const;
+
+  /**
+   * @brief Add priors on ednpoints of first measurement edge.
+   *
+   * @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;
+
+  /**
+   * @brief Create random initial translations.
+   *
+   * @return Values
+   */
+  Values initalizeRandomly() const;
+
+  /**
+   * @brief Build and optimize factor graph.
+   *
+   * @param scale scale for first relative translation which fixes gauge.
+   * @return Values
+   */
+  Values run(const double scale = 1.0) const;
+
+  /**
+   * @brief Simulate translation direction measurements
+   *
+   * @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
+   * translation direction measurement between the cameras in KeyPair.
+   */
+  static TranslationEdges SimulateMeasurements(
+      const Values& poses, const std::vector<KeyPair>& edges);
+};
+}  // namespace gtsam

gtsam/sfm/tests/testTranslationFactor.cpp

@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testTranslationFactor.cpp
+ *  @brief Unit tests for TranslationFactor Class
+ *  @author Frank dellaert
+ *  @date March 2020
+ */
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/sfm/TranslationFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+// Create a noise model for the chordal error
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05));
+
+// Keys are deliberately *not* in sorted order to test that case.
+static const Key kKey1(2), kKey2(1);
+static const Unit3 kMeasured(1, 0, 0);
+
+/* ************************************************************************* */
+TEST(TranslationFactor, Constructor) {
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+}
+
+/* ************************************************************************* */
+TEST(TranslationFactor, ZeroError) {
+  // Create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+
+  // Use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2));
+
+  // Verify we get the expected error
+  Vector expected = (Vector3() << 0, 0, 0).finished();
+  EXPECT(assert_equal(expected, actualError, 1e-9));
+
+
+}
+
+/* ************************************************************************* */
+TEST(TranslationFactor, NonZeroError) {
+  // create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // set the linearization
+  Point3 T1(0, 1, 1), T2(0, 2, 2);
+
+  // use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2));
+
+  // verify we get the expected error
+  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,
+                   const TranslationFactor& factor) {
+  return factor.evaluateError(T1, T2);
+}
+
+TEST(TranslationFactor, Jacobian) {
+  // Create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+
+  // Use the factor to calculate the Jacobians
+  Matrix H1Actual, H2Actual;
+  factor.evaluateError(T1, T2, H1Actual, H2Actual);
+
+  // Use numerical derivatives to calculate the Jacobians
+  Matrix H1Expected, H2Expected;
+  H1Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorError, _1, T2, factor), T1);
+  H2Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorError, T1, _1, factor), T2);
+
+  // Verify the Jacobians are correct
+  EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
+  EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

tests/testTranslationRecovery.cpp

@@ -0,0 +1,87 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testTranslationRecovery.cpp
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief test recovering translations when rotations are given.
+ */
+
+#include <gtsam/sfm/TranslationRecovery.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/slam/dataset.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// We read the BAL file, which has 3 cameras in it, with poses. We then assume
+// the rotations are correct, but translations have to be estimated from
+// translation directions only. Since we have 3 cameras, A, B, and C, we can at
+// most create three relative measurements, let's call them w_aZb, w_aZc, and
+// bZc. These will be of type Unit3. We then call `recoverTranslations` which
+// sets up an optimization problem for the three unknown translations.
+TEST(TranslationRecovery, BAL) {
+  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfmData db;
+  bool success = readBAL(filename, db);
+  if (!success) throw runtime_error("Could not access file!");
+
+  // Get camera poses, as Values
+  size_t j = 0;
+  Values poses;
+  for (auto camera : db.cameras) {
+    poses.insert(j++, camera.pose());
+  }
+
+  // Simulate measurements
+  const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 2}, {1, 2}});
+
+  // Check
+  const Pose3 wTa = poses.at<Pose3>(0), wTb = poses.at<Pose3>(1),
+              wTc = poses.at<Pose3>(2);
+  const Point3 Ta = wTa.translation(), Tb = wTb.translation(),
+               Tc = wTc.translation();
+  const Rot3 aRw = wTa.rotation().inverse();
+  const Unit3 w_aZb = relativeTranslations.at({0, 1});
+  EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
+  const Unit3 w_aZc = relativeTranslations.at({0, 2});
+  EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc));
+
+  TranslationRecovery algorithm(relativeTranslations);
+  const auto graph = algorithm.buildGraph();
+  EXPECT_LONGS_EQUAL(3, graph.size());
+
+  // Translation recovery, version 1
+  const double scale = 2.0;
+  const auto result = algorithm.run(scale);
+
+  // 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)));
+
+  // Check that the third translations is correct
+  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?
+  // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */