FrobeniusFactors now work for any Matrix Lie group with vec

gtsam/slam/FrobeniusFactor.h

@@ -33,11 +33,10 @@ namespace gtsam {
  * model with sigma=1.0.
  * If not, we we check if the d-dimensional noise model on rotations is
  * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
- * error.
+ * error. If the noise model is a robust error model, we use the sigmas of the
- * If the noise model is a robust error model, we use the sigmas of the
  * underlying noise model.
  *
- * If defaultToUnit == false throws an exception on unexepcted input.
+ * If defaultToUnit == false throws an exception on unexpected input.
  */
 GTSAM_EXPORT SharedNoiseModel
 ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
@@ -47,29 +46,30 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
  * FrobeniusPrior calculates the Frobenius norm between a given matrix and an
  * element of SO(3) or SO(4).
  */
-template <class Rot>
+template <class T>
-class FrobeniusPrior : public NoiseModelFactorN<Rot> {
+class FrobeniusPrior : public NoiseModelFactorN<T> {
-  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
+  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
-  using MatrixNN = typename Rot::MatrixNN;
+  inline constexpr static auto Dim = N * N;
+  using MatrixNN = Eigen::Matrix<double, N, N>;
   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
 
  public:
 
   // Provide access to the Matrix& version of evaluateError:
-  using NoiseModelFactor1<Rot>::evaluateError;
+  using NoiseModelFactor1<T>::evaluateError;
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// Constructor
   FrobeniusPrior(Key j, const MatrixNN& M,
                  const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
+      : NoiseModelFactorN<T>(ConvertNoiseModel(model, Dim), j) {
     vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
   }
 
-  /// Error is just Frobenius norm between Rot element and vectorized matrix M.
+  /// Error is just Frobenius norm between T element and vectorized matrix M.
-  Vector evaluateError(const Rot& R, OptionalMatrixType H) const override {
+  Vector evaluateError(const T& g, OptionalMatrixType H) const override {
-    return R.vec(H) - vecM_;  // Jacobian is computed only when needed.
+    return g.vec(H) - vecM_;  // Jacobian is computed only when needed.
   }
 };
 
@@ -77,24 +77,24 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
  * FrobeniusFactor calculates the Frobenius norm between rotation matrices.
  * The template argument can be any fixed-size SO<N>.
  */
-template <class Rot>
+template <class T>
-class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
+class FrobeniusFactor : public NoiseModelFactorN<T, T> {
-  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
+  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
+  inline constexpr static auto Dim = N * N;
 
  public:
 
   // Provide access to the Matrix& version of evaluateError:
-  using NoiseModelFactor2<Rot, Rot>::evaluateError;
+  using NoiseModelFactor2<T, T>::evaluateError;
 
   /// Constructor
   FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
+      : NoiseModelFactorN<T, T>(ConvertNoiseModel(model, Dim), j1, j2) {}
-                                    j2) {}
 
   /// Error is just Frobenius norm between rotation matrices.
-  Vector evaluateError(const Rot& R1, const Rot& R2,
+  Vector evaluateError(const T& T1, const T& T2,
                        OptionalMatrixType H1, OptionalMatrixType H2) const override {
-    Vector error = R2.vec(H2) - R1.vec(H1);
+    Vector error = T2.vec(H2) - T1.vec(H1);
     if (H1) *H1 = -*H1;
     return error;
   }
@@ -106,17 +106,18 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
  * 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>
+template <class T>
-class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
+class FrobeniusBetweenFactor : public NoiseModelFactorN<T, T> {
-  Rot R12_;  ///< measured rotation between R1 and R2
+  T T12_;  ///< measured rotation between T1 and T2
-  Eigen::Matrix<double, Rot::dimension, Rot::dimension>
+  Eigen::Matrix<double, T::dimension, T::dimension>
-      R2hat_H_R1_;  ///< fixed derivative of R2hat wrpt R1
+      T2hat_H_T1_;  ///< fixed derivative of T2hat wrpt T1
-  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
+  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
-
+  inline constexpr static auto Dim = N * N;
+    
  public:
 
   // Provide access to the Matrix& version of evaluateError:
-  using NoiseModelFactor2<Rot, Rot>::evaluateError;
+  using NoiseModelFactor2<T, T>::evaluateError;
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
@@ -124,12 +125,11 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
   /// @{
 
   /// Construct from two keys and measured rotation
-  FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
+  FrobeniusBetweenFactor(Key j1, Key j2, const T& T12,
                          const SharedNoiseModel& model = nullptr)
-      : NoiseModelFactorN<Rot, Rot>(
+      : NoiseModelFactorN<T, T>(ConvertNoiseModel(model, Dim), j1, j2),
-            ConvertNoiseModel(model, Dim), j1, j2),
+        T12_(T12),
-        R12_(R12),
+        T2hat_H_T1_(T12.inverse().AdjointMap()) {}
-        R2hat_H_R1_(R12.inverse().AdjointMap()) {}
 
   /// @}
   /// @name Testable
@@ -139,10 +139,10 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
   void
   print(const std::string &s,
         const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
-    std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
+    std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(T).name())
               << ">(" << keyFormatter(this->key1()) << ","
               << keyFormatter(this->key2()) << ")\n";
-    traits<Rot>::Print(R12_, "  R12: ");
+    traits<T>::Print(T12_, "  T12: ");
     this->noiseModel_->print("  noise model: ");
   }
 
@@ -150,21 +150,21 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
   bool equals(const NonlinearFactor &expected,
               double tol = 1e-9) const override {
     auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
-    return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
+    return e != nullptr && NoiseModelFactorN<T, T>::equals(*e, tol) &&
-           traits<Rot>::Equals(this->R12_, e->R12_, tol);
+           traits<T>::Equals(this->T12_, e->T12_, tol);
   }
 
   /// @}
   /// @name NoiseModelFactorN methods 
   /// @{
 
-  /// Error is Frobenius norm between R1*R12 and R2.
+  /// Error is Frobenius norm between T1*T12 and T2.
-  Vector evaluateError(const Rot& R1, const Rot& R2,
+  Vector evaluateError(const T& T1, const T& T2,
                        OptionalMatrixType H1, OptionalMatrixType H2) const override {
-    const Rot R2hat = R1.compose(R12_);
+    const T T2hat = T1.compose(T12_);
-    Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
+    Eigen::Matrix<double, Dim, T::dimension> vec_H_T2hat;
-    Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
+    Vector error = T2.vec(H2) - T2hat.vec(H1 ? &vec_H_T2hat : nullptr);
-    if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
+    if (H1) *H1 = -vec_H_T2hat * T2hat_H_T1_;
     return error;
   }
   /// @}

gtsam/slam/slam.i

@@ -403,21 +403,21 @@ gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
 gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
                                                 size_t d);
 
-template <T = {gtsam::SO3, gtsam::SO4}>
+template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
 virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
   FrobeniusFactor(size_t key1, size_t key2);
   FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
 
-  gtsam::Vector evaluateError(const T& R1, const T& R2);
+  gtsam::Vector evaluateError(const T& T1, const T& T2);
 };
 
-template <T = {gtsam::SO3, gtsam::SO4}>
+template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
 virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
-  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& T12);
-  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12,
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& T12,
                          gtsam::noiseModel::Base* model);
 
-  gtsam::Vector evaluateError(const T& R1, const T& R2);
+  gtsam::Vector evaluateError(const T& T1, const T& T2);
 };
 
 #include <gtsam/slam/TriangulationFactor.h>

gtsam/slam/tests/testFrobeniusFactor.cpp

@@ -9,18 +9,20 @@
 
  * -------------------------------------------------------------------------- */
 
-/**
+ /**
- * testFrobeniusFactor.cpp
+  * testFrobeniusFactor.cpp
- *
+  *
- * @file   testFrobeniusFactor.cpp
+  * @file   testFrobeniusFactor.cpp
- * @date   March 2019
+  * @date   March 2019
- * @author Frank Dellaert
+  * @author Frank Dellaert
- * @brief  Check evaluateError for various Frobenius norm
+  * @brief  Check evaluateError for various Frobenius norms
- */
+  */
 
 #include <gtsam/base/lieProxies.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/SO3.h>
 #include <gtsam/geometry/SO4.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -30,17 +32,16 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-using namespace std;
 using namespace gtsam;
 
-//******************************************************************************
+/* ************************************************************************* */
 namespace so3 {
-SO3 id;
+  SO3 id;
-Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
+  Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
-SO3 R1 = SO3::Expmap(v1);
+  SO3 R1 = SO3::Expmap(v1);
-Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
+  Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
-SO3 R2 = SO3::Expmap(v2);
+  SO3 R2 = SO3::Expmap(v2);
-SO3 R12 = R1.between(R2);
+  SO3 R12 = R1.between(R2);
 }  // namespace so3
 
 /* ************************************************************************* */
@@ -61,8 +62,8 @@ 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.4155925, -0.64214347, -0.64324489,  //
-      -0.44948549, 0.47046326, -0.75917576;
+    -0.44948549, 0.47046326, -0.75917576;
 
   SO3 expected = SO3::ClosestTo(M);
 
@@ -89,7 +90,7 @@ TEST(FrobeniusPriorSO3, ChordalL2mean) {
   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));
+  EXPECT(assert_equal(expected, SO3::ChordalMean({ R1, R1.inverse() }), 1e-6));
 
   // manifold optimization gets same result as ChordalMean
   NonlinearFactorGraph graph;
@@ -148,13 +149,13 @@ TEST(FrobeniusBetweenFactorSO3, evaluateError) {
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
 
-//******************************************************************************
+/* ************************************************************************* */
 namespace so4 {
-SO4 id;
+  SO4 id;
-Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
+  Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
-SO4 Q1 = SO4::Expmap(v1);
+  SO4 Q1 = SO4::Expmap(v1);
-Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
+  Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
-SO4 Q2 = SO4::Expmap(v2);
+  SO4 Q2 = SO4::Expmap(v2);
 }  // namespace so4
 
 /* ************************************************************************* */
@@ -174,7 +175,7 @@ TEST(FrobeniusFactorSO4, evaluateError) {
 /* ************************************************************************* */
 TEST(FrobeniusBetweenFactorSO4, evaluateError) {
   using namespace ::so4;
-  Matrix4 M{I_4x4};
+  Matrix4 M{ I_4x4 };
   M.topLeftCorner<3, 3>() = ::so3::R12.matrix();
   auto factor = FrobeniusBetweenFactor<SO4>(1, 2, Q1.between(Q2));
   Matrix H1, H2;
@@ -188,6 +189,79 @@ TEST(FrobeniusBetweenFactorSO4, evaluateError) {
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
 
+/* ************************************************************************* */
+namespace pose2 {
+  Pose2 id;
+  Pose2 P1 = Pose2(0.1, 0.2, 0.3);
+  Pose2 P2 = Pose2(0.4, 0.5, 0.6);
+}  // namespace pose2
+
+/* ************************************************************************* */
+TEST(FrobeniusFactorPose2, evaluateError) {
+  using namespace ::pose2;
+  auto factor = FrobeniusFactor<Pose2>(1, 2, noiseModel::Unit::Create(3));
+  Vector actual = factor.evaluateError(P1, P2);
+  Vector expected = P2.vec() - P1.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, P1);
+  values.insert(2, P2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusBetweenFactorPose2, evaluateError) {
+  using namespace ::pose2;
+  auto factor = FrobeniusBetweenFactor<Pose2>(1, 2, P1.between(P2));
+  Matrix H1, H2;
+  Vector actual = factor.evaluateError(P1, P2, H1, H2);
+  Vector expected = Vector9::Zero();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, P1);
+  values.insert(2, P2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+namespace pose3 {
+  Pose3 id;
+  Pose3 P1 = Pose3(Rot3::Expmap(Vector3(0.1, 0.2, 0.3)), Vector3(0.4, 0.5, 0.6));
+  Pose3 P2 = Pose3(Rot3::Expmap(Vector3(0.2, 0.3, 0.4)), Vector3(0.7, 0.8, 0.9));
+}  // namespace pose3
+
+/* ************************************************************************* */
+TEST(FrobeniusFactorPose3, evaluateError) {
+  using namespace ::pose3;
+  auto factor = FrobeniusFactor<Pose3>(1, 2, noiseModel::Unit::Create(12));
+  Vector actual = factor.evaluateError(P1, P2);
+  Vector expected = P2.vec() - P1.vec();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, P1);
+  values.insert(2, P2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+TEST(FrobeniusBetweenFactorPose3, evaluateError) {
+  using namespace ::pose3;
+  auto factor = FrobeniusBetweenFactor<Pose3>(1, 2, P1.between(P2));
+  Matrix H1, H2;
+  Vector actual = factor.evaluateError(P1, P2, H1, H2);
+  Vector expected(16);
+  expected.setZero();
+  EXPECT(assert_equal(expected, actual, 1e-9));
+
+  Values values;
+  values.insert(1, P1);
+  values.insert(2, P2);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;