Added factor for Karcher mean constraint: fixes the geodesic mean to impose a global gauge constraint.

gtsam/slam/KarcherMeanFactor-inl.h

@@ -0,0 +1,62 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, 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 KarcherMeanFactor.cpp
+ * @author Frank Dellaert
+ * @date March 2019
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/KarcherMeanFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+
+using namespace std;
+
+namespace gtsam {
+
+template <class T>
+T FindKarcherMean(const vector<T>& rotations) {
+  // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
+  // No closed form solution.
+  NonlinearFactorGraph graph;
+  static const Key kKey(0);
+  for (const auto& R : rotations) {
+    graph.emplace_shared<PriorFactor<T> >(kKey, R);
+  }
+  Values initial;
+  initial.insert<T>(kKey, T());
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  return result.at<T>(kKey);
+}
+
+template <class T>
+template <typename CONTAINER>
+KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
+    : NonlinearFactor(keys) {
+  if (d <= 0) {
+    throw std::invalid_argument(
+        "KarcherMeanFactor needs dimension for dynamic types.");
+  }
+  // Create the constant Jacobian made of D*D identity matrices,
+  // where D is the dimensionality of the manifold.
+  const auto I = Eigen::MatrixXd::Identity(d, d);
+  std::map<Key, Matrix> terms;
+  for (Key j : keys) {
+    terms[j] = I;
+  }
+  jacobian_ =
+      boost::make_shared<JacobianFactor>(terms, Eigen::VectorXd::Zero(d));
+}
+}  // namespace gtsam

gtsam/slam/KarcherMeanFactor.h

@@ -0,0 +1,70 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, 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 KarcherMeanFactor.h
+ * @author Frank Dellaert
+ * @date March 2019ƒ
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/linear/JacobianFactor.h>
+
+#include <map>
+#include <vector>
+
+namespace gtsam {
+/**
+ * Optimize for the Karcher mean, minimizing the geodesic distance to each of
+ * the given rotations, ,by constructing a factor graph out of simple
+ * PriorFactors.
+ */
+template <class T>
+T FindKarcherMean(const std::vector<T>& rotations);
+
+/**
+ * The KarcherMeanFactor creates a constraint on all SO(3) variables with
+ * given keys that the Karcher mean (see above) will stay the same. Note the
+ * mean itself is irrelevant to the constraint and is not a parameter: the
+ * constraint is implemented as enforcing that the sum of local updates is
+ * equal to zero, hence creating a rank-3 constraint. Note it is implemented as
+ * a soft constraint, as typically it is used to fix a gauge freedom.
+ * */
+template <class T>
+class KarcherMeanFactor : public NonlinearFactor {
+  /// Constant Jacobian made of d*d identity matrices
+  boost::shared_ptr<JacobianFactor> jacobian_;
+
+  enum {D = traits<T>::dimension};
+
+ public:
+  /// Construct from given keys.
+  template <typename CONTAINER>
+  KarcherMeanFactor(const CONTAINER& keys, int d=D);
+
+  /// Destructor
+  virtual ~KarcherMeanFactor() {}
+
+  /// Calculate the error of the factor: always zero
+  double error(const Values& c) const override { return 0; }
+
+  /// get the dimension of the factor (number of rows on linearization)
+  size_t dim() const override { return D; }
+
+  /// linearize to a GaussianFactor
+  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
+    return jacobian_;
+  }
+};
+// \KarcherMeanFactor
+}  // namespace gtsam

gtsam/slam/tests/testKarcherMeanFactor.cpp

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, 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 testKarcherMeanFactor.cpp
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+#include <gtsam/slam/KarcherMeanFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// Rot3 version
+/* ************************************************************************* */
+static const Rot3 R = Rot3::Expmap(Vector3(0.1, 0, 0));
+
+/* ************************************************************************* */
+// Check that optimizing for Karcher mean (which minimizes Between distance)
+// gets correct result.
+TEST(KarcherMean, FindRot3) {
+  std::vector<Rot3> rotations = {R, R.inverse()};
+  Rot3 expected;
+  auto actual = FindKarcherMean(rotations);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+// Check that the InnerConstraint factor leaves the mean unchanged.
+TEST(KarcherMean, FactorRot3) {
+  // Make a graph with two variables, one between, and one InnerConstraint
+  // The optimal result should satisfy the between, while moving the other
+  // variable to make the mean the same as before.
+  // Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
+  // R*R*R, i.e. geodesic length is 3 rather than 2.
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<BetweenFactor<Rot3>>(1, 2, R * R * R);
+  std::vector<Key> keys{1, 2};
+  graph.emplace_shared<KarcherMeanFactor<Rot3>>(keys);
+
+  Values initial;
+  initial.insert<Rot3>(1, R.inverse());
+  initial.insert<Rot3>(2, R);
+  const auto expected = FindKarcherMean<Rot3>({R, R.inverse()});
+
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  const auto actual =
+      FindKarcherMean<Rot3>({result.at<Rot3>(1), result.at<Rot3>(2)});
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(
+      assert_equal(R * R * R, result.at<Rot3>(1).between(result.at<Rot3>(2))));
+}
+
+/* ************************************************************************* */
+// SO(4) version
+/* ************************************************************************* */
+static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished());
+
+/* ************************************************************************* */
+TEST(KarcherMean, FindSO4) {
+  std::vector<SO4> rotations = {Q, Q.inverse()};
+  auto expected = SO4();  //::ChordalMean(rotations);
+  auto actual = FindKarcherMean(rotations);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(KarcherMean, FactorSO4) {
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<BetweenFactor<SO4>>(1, 2, Q * Q * Q);
+  std::vector<Key> keys{1, 2};
+  graph.emplace_shared<KarcherMeanFactor<SO4>>(keys);
+
+  Values initial;
+  initial.insert<SO4>(1, Q.inverse());
+  initial.insert<SO4>(2, Q);
+  const auto expected = FindKarcherMean<SO4>({Q, Q.inverse()});
+
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  const auto actual =
+      FindKarcherMean<SO4>({result.at<SO4>(1), result.at<SO4>(2)});
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(),
+                      result.at<SO4>(1).between(result.at<SO4>(2))));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */