Importing Frobenius error factors from Shonan effort

cython/gtsam/tests/test_FrobeniusFactor.py

@@ -0,0 +1,56 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+FrobeniusFactor unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error, invalid-name
+import unittest
+
+import numpy as np
+from gtsam import (SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
+                   FrobeniusWormholeFactor, SOn)
+
+id = SO4()
+v1 = np.array([0, 0, 0, 0.1, 0, 0])
+Q1 = SO4.Expmap(v1)
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q2 = SO4.Expmap(v2)
+
+
+class TestFrobeniusFactorSO4(unittest.TestCase):
+    """Test FrobeniusFactor factors."""
+
+    def test_frobenius_factor(self):
+        """Test creation of a factor that calculates the Frobenius norm."""
+        factor = FrobeniusFactorSO4(1, 2)
+        actual = factor.evaluateError(Q1, Q2)
+        expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
+        np.testing.assert_array_equal(actual, expected)
+
+    def test_frobenius_between_factor(self):
+        """Test creation of a Frobenius BetweenFactor."""
+        factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
+        actual = factor.evaluateError(Q1, Q2)
+        expected = np.zeros((16,))
+        np.testing.assert_array_almost_equal(actual, expected)
+
+    def test_frobenius_wormhole_factor(self):
+        """Test creation of a factor that calculates Shonan error."""
+        R1 = SO3.Expmap(v1[3:])
+        R2 = SO3.Expmap(v2[3:])
+        factor = FrobeniusWormholeFactor(1, 2, R1.between(R2), p=4)
+        I4 = SOn(4)
+        Q1 = I4.retract(v1)
+        Q2 = I4.retract(v2)
+        actual = factor.evaluateError(Q1, Q2)
+        expected = np.zeros((12,))
+        np.testing.assert_array_almost_equal(actual, expected, decimal=4)
+
+
+if __name__ == "__main__":
+    unittest.main()

gtsam.h

@@ -598,6 +598,7 @@ class SOn {
   // Standard Constructors
   SOn(size_t n);
   static gtsam::SOn FromMatrix(Matrix R);
+  static gtsam::SOn Lift(size_t n, Matrix R);
 
   // Testable
   void print(string s) const;
@@ -2835,6 +2836,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
   KarcherMeanFactor(const gtsam::KeyVector& keys);
 };
 
+#include <gtsam/slam/FrobeniusFactor.h>
+gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
+    gtsam::noiseModel::Base* model, size_t d);
+
+template<T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
+  FrobeniusFactor(size_t key1, size_t key2);
+  FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+template<T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
+  FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
+                          size_t p);
+  FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
+                          size_t p, gtsam::noiseModel::Base* model);
+  Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
+};
+
 //*************************************************************************
 // Navigation
 //*************************************************************************

gtsam/slam/FrobeniusFactor.cpp

@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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   FrobeniusFactor.cpp
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Various factors that minimize some Frobenius norm
+ */
+
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <gtsam/base/timing.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+namespace gtsam {
+
+//******************************************************************************
+boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
+    const SharedNoiseModel& model, size_t d, bool defaultToUnit) {
+  double sigma = 1.0;
+  if (model != nullptr) {
+    if (model->dim() != 6) {
+      if (!defaultToUnit)
+        throw std::runtime_error("Can only convert Pose3 noise models");
+    } else {
+      auto sigmas = model->sigmas().head(3).eval();
+      if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) {
+        if (!defaultToUnit)
+          throw std::runtime_error("Can only convert isotropic rotation noise");
+      } else {
+        sigma = sigmas(0);
+      }
+    }
+  }
+  return noiseModel::Isotropic::Sigma(d, sigma);
+}
+
+//******************************************************************************
+FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12,
+                                                 size_t p,
+                                                 const SharedNoiseModel& model)
+    : NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
+      M_(R12.matrix()),               // 3*3 in all cases
+      p_(p),                          // 4 for SO(4)
+      pp_(p * p),                     // 16 for SO(4)
+      dimension_(SOn::Dimension(p)),  // 6 for SO(4)
+      G_(pp_, dimension_)             // 16*6 for SO(4)
+{
+  // Calculate G matrix of vectorized generators
+  Matrix Z = Matrix::Zero(p, p);
+  for (size_t j = 0; j < dimension_; j++) {
+    const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j));
+    G_.col(j) = Eigen::Map<const Matrix>(X.data(), pp_, 1);
+  }
+  assert(noiseModel()->dim() == 3 * p_);
+}
+
+//******************************************************************************
+Vector FrobeniusWormholeFactor::evaluateError(
+    const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  gttic(FrobeniusWormholeFactorP_evaluateError);
+
+  const Matrix& M1 = Q1.matrix();
+  const Matrix& M2 = Q2.matrix();
+  assert(M1.rows() == p_ && M2.rows() == p_);
+
+  const size_t dim = 3 * p_;  // Stiefel manifold dimension
+  Vector fQ2(dim), hQ1(dim);
+
+  // Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
+  fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
+
+  // Vectorize M1*P*R12
+  const Matrix Q1PR12 = M1.leftCols<3>() * M_;
+  hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
+
+  // If asked, calculate Jacobian as (M \otimes M1) * G
+  if (H1) {
+    const size_t p2 = 2 * p_;
+    Matrix RPxQ = Matrix::Zero(dim, pp_);
+    RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0);
+    RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1);
+    RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2);
+    *H1 = -RPxQ * G_;
+  }
+  if (H2) {
+    const size_t p2 = 2 * p_;
+    Matrix PxQ = Matrix::Zero(dim, pp_);
+    PxQ.block(0, 0, p_, p_) = M2;
+    PxQ.block(p_, p_, p_, p_) = M2;
+    PxQ.block(p2, p2, p_, p_) = M2;
+    *H2 = PxQ * G_;
+  }
+
+  return fQ2 - hQ1;
+}
+
+//******************************************************************************
+
+}  // namespace gtsam

gtsam/slam/FrobeniusFactor.h

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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   FrobeniusFactor.h
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Various factors that minimize some Frobenius norm
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SOn.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
+ * BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
+ * model used to weight the Frobenius norm.  If the noise model passed is
+ * null we return a Dim-dimensional isotropic noise model with sigma=1.0. If
+ * not, we we check if the 3-dimensional noise model on rotations is
+ * isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
+ * error. If defaultToUnit == false throws an exception on unexepcted input.
+ */
+boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
+    const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
+
+/**
+ * FrobeniusPrior calculates the Frobenius norm between a given matrix and an
+ * element of SO(3) or SO(4).
+ */
+template <class Rot>
+class FrobeniusPrior : public NoiseModelFactor1<Rot> {
+  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  using MatrixNN = typename Rot::MatrixNN;
+  Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
+
+ public:
+  /// Constructor
+  FrobeniusPrior(Key j, const MatrixNN& M,
+                 const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) {
+    vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
+  }
+
+  /// Error is just Frobenius norm between Rot element and vectorized matrix M.
+  Vector evaluateError(const Rot& R,
+                       boost::optional<Matrix&> H = boost::none) const {
+    return R.vec(H) - vecM_;  // Jacobian is computed only when needed.
+  }
+};
+
+/**
+ * FrobeniusFactor calculates the Frobenius norm between rotation matrices.
+ * The template argument can be any fixed-size SO<N>.
+ */
+template <class Rot>
+class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
+  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+
+ public:
+  /// Constructor
+  FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1,
+                                    j2) {}
+
+  /// Error is just Frobenius norm between rotation matrices.
+  Vector evaluateError(const Rot& R1, const Rot& R2,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const {
+    Vector error = R2.vec(H2) - R1.vec(H1);
+    if (H1) *H1 = -*H1;
+    return error;
+  }
+};
+
+/**
+ * FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm
+ * of the rotation error between measured and predicted (rather than the
+ * Logmap of the error). This factor is only defined for fixed-dimension types,
+ * and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
+ */
+template <class Rot>
+class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
+  Rot R12_;  ///< measured rotation between R1 and R2
+  Eigen::Matrix<double, Rot::dimension, Rot::dimension>
+      R2hat_H_R1_;  ///< fixed derivative of R2hat wrpt R1
+  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+
+ public:
+  /// Constructor
+  FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
+                         const SharedNoiseModel& model = nullptr)
+      : NoiseModelFactor2<Rot, Rot>(
+            ConvertPose3NoiseModel(model, Dim), j1, j2),
+        R12_(R12),
+        R2hat_H_R1_(R12.inverse().AdjointMap()) {}
+
+  /// Error is Frobenius norm between R1*R12 and R2.
+  Vector evaluateError(const Rot& R1, const Rot& R2,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const {
+    const Rot R2hat = R1.compose(R12_);
+    Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
+    Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
+    if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
+    return error;
+  }
+};
+
+/**
+ * FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will
+ * land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects
+ * the SO(p) matrices down to a Stiefel manifold of p*d matrices.
+ * TODO(frank): template on D=2 or 3
+ */
+class FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> {
+  Matrix M_;                   ///< measured rotation between R1 and R2
+  size_t p_, pp_, dimension_;  ///< dimensionality constants
+  Matrix G_;                   ///< matrix of vectorized generators
+
+ public:
+  /// Constructor. Note we convert to 3*p-dimensional noise model.
+  FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4,
+                          const SharedNoiseModel& model = nullptr);
+
+  /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
+  /// projects down from SO(p) to the Stiefel manifold of px3 matrices.
+  Vector evaluateError(const SOn& Q1, const SOn& Q2,
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none) const;
+};
+
+}  // namespace gtsam

gtsam/slam/tests/testFrobeniusFactor.cpp

@@ -0,0 +1,244 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * testFrobeniusFactor.cpp
+ *
+ * @file   testFrobeniusFactor.cpp
+ * @date   March 2019
+ * @author Frank Dellaert
+ * @brief  Check evaluateError for various Frobenius norm
+ */
+
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SO3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+namespace so3 {
+SO3 id;
+Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
+SO3 R1 = SO3::Expmap(v1);
+Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
+SO3 R2 = SO3::Expmap(v2);
+SO3 R12 = R1.between(R2);
+}  // namespace so3
+
+/* ************************************************************************* */
+TEST(FrobeniusPriorSO3, evaluateError) {
+  using namespace ::so3;
+  auto factor = FrobeniusPrior<SO3>(1, R2.matrix());
+  Vector actual = factor.evaluateError(R1);
+  Vector expected = R1.vec() - R2.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, R1);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusPriorSO3, ClosestTo) {
+  // Example top-left of SO(4) matrix not quite on SO(3) manifold
+  Matrix3 M;
+  M << 0.79067393, 0.6051136, -0.0930814,   //
+      0.4155925, -0.64214347, -0.64324489,  //
+      -0.44948549, 0.47046326, -0.75917576;
+
+  SO3 expected = SO3::ClosestTo(M);
+
+  // manifold optimization gets same result as SVD solution in ClosestTo
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<FrobeniusPrior<SO3> >(1, M);
+
+  Values initial;
+  initial.insert(1, SO3(I_3x3));
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-6);
+  EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusPriorSO3, ChordalL2mean) {
+  // See Hartley13ijcv:
+  // Cost function C(R) = \sum FrobeniusPrior(R,R_i)
+  // Closed form solution = ClosestTo(C_e), where
+  // C_e = \sum R_i !!!!
+
+  // We will test by computing mean of R1=exp(v1) R1^T=exp(-v1):
+  using namespace ::so3;
+  SO3 expected;  // identity
+  Matrix3 M = R1.matrix() + R1.matrix().transpose();
+  EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6));
+  EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6));
+
+  // manifold optimization gets same result as ChordalMean
+  NonlinearFactorGraph graph;
+  graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix());
+  graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix().transpose());
+
+  Values initial;
+  initial.insert<SO3>(1, R1.inverse());
+  auto result = GaussNewtonOptimizer(graph, initial).optimize();
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1);  // Why so loose?
+  EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusFactorSO3, evaluateError) {
+  using namespace ::so3;
+  auto factor = FrobeniusFactor<SO3>(1, 2);
+  Vector actual = factor.evaluateError(R1, R2);
+  Vector expected = R2.vec() - R1.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, R1);
+  values.insert(2, R2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+// Commented out as SO(n) not yet supported (and might never be)
+// TEST(FrobeniusBetweenFactorSOn, evaluateError) {
+//   using namespace ::so3;
+//   auto factor =
+//       FrobeniusBetweenFactor<SOn>(1, 2, SOn::FromMatrix(R12.matrix()));
+//   Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()),
+//                                        SOn::FromMatrix(R2.matrix()));
+//   Vector expected = Vector9::Zero();
+//   EXPECT(assert_equal(expected, actual, 1e-9));
+
+//   Values values;
+//   values.insert(1, R1);
+//   values.insert(2, R2);
+//   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+// }
+
+/* ************************************************************************* */
+TEST(FrobeniusBetweenFactorSO3, evaluateError) {
+  using namespace ::so3;
+  auto factor = FrobeniusBetweenFactor<SO3>(1, 2, R12);
+  Vector actual = factor.evaluateError(R1, R2);
+  Vector expected = Vector9::Zero();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, R1);
+  values.insert(2, R2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+//******************************************************************************
+namespace so4 {
+SO4 id;
+Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
+SO4 Q1 = SO4::Expmap(v1);
+Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
+SO4 Q2 = SO4::Expmap(v2);
+}  // namespace so4
+
+/* ************************************************************************* */
+TEST(FrobeniusFactorSO4, evaluateError) {
+  using namespace ::so4;
+  auto factor = FrobeniusFactor<SO4>(1, 2, noiseModel::Unit::Create(6));
+  Vector actual = factor.evaluateError(Q1, Q2);
+  Vector expected = Q2.vec() - Q1.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, Q1);
+  values.insert(2, Q2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusBetweenFactorSO4, evaluateError) {
+  using namespace ::so4;
+  Matrix4 M{I_4x4};
+  M.topLeftCorner<3, 3>() = ::so3::R12.matrix();
+  auto factor = FrobeniusBetweenFactor<SO4>(1, 2, Q1.between(Q2));
+  Matrix H1, H2;
+  Vector actual = factor.evaluateError(Q1, Q2, H1, H2);
+  Vector expected = SO4::VectorN2::Zero();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, Q1);
+  values.insert(2, Q2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+//******************************************************************************
+namespace submanifold {
+SO4 id;
+Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
+SO3 R1 = SO3::Expmap(v1.tail<3>());
+SO4 Q1 = SO4::Expmap(v1);
+Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
+SO3 R2 = SO3::Expmap(v2.tail<3>());
+SO4 Q2 = SO4::Expmap(v2);
+SO3 R12 = R1.between(R2);
+}  // namespace submanifold
+
+/* ************************************************************************* */
+TEST(FrobeniusWormholeFactor, evaluateError) {
+  auto model = noiseModel::Isotropic::Sigma(6, 1.2);  // dimension = 6 not 16
+  for (const size_t p : {5, 4, 3}) {
+    Matrix M = Matrix::Identity(p, p);
+    M.topLeftCorner(3, 3) = submanifold::R1.matrix();
+    SOn Q1(M);
+    M.topLeftCorner(3, 3) = submanifold::R2.matrix();
+    SOn Q2(M);
+    auto factor =
+        FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model);
+    Matrix H1, H2;
+    factor.evaluateError(Q1, Q2, H1, H2);
+
+    // Test derivatives
+    Values values;
+    values.insert(1, Q1);
+    values.insert(2, Q2);
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+  }
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusWormholeFactor, equivalenceToSO3) {
+  using namespace ::submanifold;
+  auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
+  auto model = noiseModel::Isotropic::Sigma(6, 1.2);  // wrong dimension
+  auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
+  auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model);
+  const Eigen::Map<Matrix3> E3(factor3.evaluateError(R1, R2).data());
+  const Eigen::Map<Matrix43> E4(
+      factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
+  EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
+  EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

timing/timeFrobeniusFactor.cpp

@@ -0,0 +1,110 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    timeFrobeniusFactor.cpp
+ * @brief   time FrobeniusFactor with BAL file
+ * @author  Frank Dellaert
+ * @date    June 6, 2015
+ */
+
+#include <gtsam/base/timing.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/SO4.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/linear/PCGSolver.h>
+#include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/FrobeniusFactor.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
+
+int main(int argc, char* argv[]) {
+  // primitive argument parsing:
+  if (argc > 3) {
+    throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]");
+  }
+
+  string g2oFile;
+  try {
+    if (argc > 1)
+      g2oFile = argv[argc - 1];
+    else
+      g2oFile =
+          "/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/"
+          "datasets/randomTorus3D.g2o";
+    // g2oFile = findExampleDataFile("sphere_smallnoise.graph");
+  } catch (const exception& e) {
+    cerr << e.what() << '\n';
+    exit(1);
+  }
+
+  // Read G2O file
+  const auto factors = parse3DFactors(g2oFile);
+  const auto poses = parse3DPoses(g2oFile);
+
+  // Build graph
+  NonlinearFactorGraph graph;
+  // graph.add(NonlinearEquality<SO4>(0, SO4()));
+  auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
+  graph.add(PriorFactor<SO4>(0, SO4(), priorModel));
+  for (const auto& factor : factors) {
+    const auto& keys = factor->keys();
+    const auto& Tij = factor->measured();
+    const auto& model = factor->noiseModel();
+    graph.emplace_shared<FrobeniusWormholeFactor>(
+        keys[0], keys[1], SO3(Tij.rotation().matrix()), model);
+  }
+
+  boost::mt19937 rng(42);
+
+  // Set parameters to be similar to ceres
+  LevenbergMarquardtParams params;
+  LevenbergMarquardtParams::SetCeresDefaults(&params);
+  params.setLinearSolverType("MULTIFRONTAL_QR");
+  // params.setVerbosityLM("SUMMARY");
+  // params.linearSolverType = LevenbergMarquardtParams::Iterative;
+  // auto pcg = boost::make_shared<PCGSolverParameters>();
+  // pcg->preconditioner_ =
+  // boost::make_shared<SubgraphPreconditionerParameters>();
+  // boost::make_shared<BlockJacobiPreconditionerParameters>();
+  // params.iterativeParams = pcg;
+
+  // Optimize
+  for (size_t i = 0; i < 100; i++) {
+    gttic_(optimize);
+    Values initial;
+    initial.insert(0, SO4());
+    for (size_t j = 1; j < poses.size(); j++) {
+      initial.insert(j, SO4::Random(rng));
+    }
+    LevenbergMarquardtOptimizer lm(graph, initial, params);
+    Values result = lm.optimize();
+    cout << "cost = " << graph.error(result) << endl;
+  }
+
+  tictoc_finishedIteration_();
+  tictoc_print_();
+
+  return 0;
+}