Allow to pass in pre-computed generators. Should save some energy.

gtsam/slam/FrobeniusFactor.cpp

@@ -52,23 +52,40 @@ boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
 }
 
 //******************************************************************************
-FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12,
+FrobeniusWormholeFactor::FrobeniusWormholeFactor(
-                                                 size_t p,
+    Key j1, Key j2, const Rot3 &R12, size_t p, const SharedNoiseModel &model,
-                                                 const SharedNoiseModel& model)
+    const boost::shared_ptr<Matrix> &G)
     : NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
-      M_(R12.matrix()),               // 3*3 in all cases
+      M_(R12.matrix()), // 3*3 in all cases
-      p_(p),                          // 4 for SO(4)
+      p_(p),            // 4 for SO(4)
-      pp_(p * p),                     // 16 for SO(4)
+      pp_(p * p),       // 16 for SO(4)
-      dimension_(SOn::Dimension(p)),  // 6 for SO(4)
+      G_(G) {
-      G_(pp_, dimension_)             // 16*6 for SO(4)
+  if (noiseModel()->dim() != 3 * p_)
-{
+    throw std::invalid_argument(
-  // Calculate G matrix of vectorized generators
+        "FrobeniusWormholeFactor: model with incorrect dimension.");
-  Matrix Z = Matrix::Zero(p, p);
+  if (!G) {
-  for (size_t j = 0; j < dimension_; j++) {
+    G_ = boost::make_shared<Matrix>();
-    const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j));
+    *G_ = SOn::VectorizedGenerators(p); // expensive!
-    G_.col(j) = Eigen::Map<const Matrix>(X.data(), pp_, 1);
   }
-  assert(noiseModel()->dim() == 3 * p_);
+  if (G_->rows() != pp_ || G_->cols() != SOn::Dimension(p))
+    throw std::invalid_argument("FrobeniusWormholeFactor: passed in generators "
+                                "of incorrect dimension.");
+}
+
+//******************************************************************************
+void FrobeniusWormholeFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
+  std::cout << s << "FrobeniusWormholeFactor<" << p_ << ">(" << keyFormatter(key1()) << ","
+            << keyFormatter(key2()) << ")\n";
+  traits<Matrix>::Print(M_, "  M: ");
+  noiseModel_->print("  noise model: ");
+}
+
+//******************************************************************************
+bool FrobeniusWormholeFactor::equals(const NonlinearFactor &expected,
+                                     double tol) const {
+  auto e = dynamic_cast<const FrobeniusWormholeFactor *>(&expected);
+  return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) &&
+         p_ == e->p_ && M_ == e->M_;
 }
 
 //******************************************************************************
@@ -98,7 +115,7 @@ Vector FrobeniusWormholeFactor::evaluateError(
     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_;
+    *H1 = -RPxQ * (*G_);
   }
   if (H2) {
     const size_t p2 = 2 * p_;
@@ -106,7 +123,7 @@ Vector FrobeniusWormholeFactor::evaluateError(
     PxQ.block(0, 0, p_, p_) = M2;
     PxQ.block(p_, p_, p_, p_) = M2;
     PxQ.block(p2, p2, p_, p_) = M2;
-    *H2 = PxQ * G_;
+    *H2 = PxQ * (*G_);
   }
 
   return fQ2 - hQ1;

gtsam/slam/FrobeniusFactor.h

@@ -92,14 +92,17 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
  * and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
  */
 template <class Rot>
-class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
+GTSAM_EXPORT 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
+  /// @name Constructor
+  /// @{
+
+  /// Construct from two keys and measured rotation
   FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
                          const SharedNoiseModel& model = nullptr)
       : NoiseModelFactor2<Rot, Rot>(
@@ -107,6 +110,33 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
         R12_(R12),
         R2hat_H_R1_(R12.inverse().AdjointMap()) {}
 
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print with optional string
+  void
+  print(const std::string &s,
+        const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
+    std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
+              << ">(" << keyFormatter(this->key1()) << ","
+              << keyFormatter(this->key2()) << ")\n";
+    traits<Rot>::Print(R12_, "  R12: ");
+    this->noiseModel_->print("  noise model: ");
+  }
+
+  /// assert equality up to a tolerance
+  bool equals(const NonlinearFactor &expected,
+              double tol = 1e-9) const override {
+    auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
+    return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) &&
+           traits<Rot>::Equals(this->R12_, e->R12_, tol);
+  }
+
+  /// @}
+  /// @name NoiseModelFactor2 methods 
+  /// @{
+
   /// Error is Frobenius norm between R1*R12 and R2.
   Vector evaluateError(const Rot& R1, const Rot& R2,
                        boost::optional<Matrix&> H1 = boost::none,
@@ -117,6 +147,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
     if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
     return error;
   }
+  /// @}
 };
 
 /**
@@ -125,21 +156,46 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
  * the SO(p) matrices down to a Stiefel manifold of p*d matrices.
  * TODO(frank): template on D=2 or 3
  */
-class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> {
+class GTSAM_EXPORT FrobeniusWormholeFactor
-  Matrix M_;                   ///< measured rotation between R1 and R2
+    : public NoiseModelFactor2<SOn, SOn> {
-  size_t p_, pp_, dimension_;  ///< dimensionality constants
+  Matrix M_;                    ///< measured rotation between R1 and R2
-  Matrix G_;                   ///< matrix of vectorized generators
+  size_t p_, pp_;               ///< dimensionality constants
+  boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
+
+public:
+  /// @name Constructor
+  /// @{
 
- public:
   /// Constructor. Note we convert to 3*p-dimensional noise model.
-  FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4,
+  /// To save memory and mallocs, pass in the vectorized Lie algebra generators:
-                          const SharedNoiseModel& model = nullptr);
+  ///    G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(p));
+  FrobeniusWormholeFactor(Key j1, Key j2, const Rot3 &R12, size_t p = 4,
+                          const SharedNoiseModel &model = nullptr,
+                          const boost::shared_ptr<Matrix> &G = nullptr);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print with optional string
+  void
+  print(const std::string &s,
+        const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
+
+  /// assert equality up to a tolerance
+  bool equals(const NonlinearFactor &expected,
+              double tol = 1e-9) const override;
+
+  /// @}
+  /// @name NoiseModelFactor2 methods 
+  /// @{
 
   /// 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 override;
+  /// @}
 };
 
 }  // namespace gtsam

timing/timeFrobeniusFactor.cpp

@@ -13,12 +13,11 @@
  * @file    timeFrobeniusFactor.cpp
  * @brief   time FrobeniusFactor with BAL file
  * @author  Frank Dellaert
- * @date    June 6, 2015
+ * @date    2019
  */
 
 #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>
@@ -51,10 +50,7 @@ int main(int argc, char* argv[]) {
     if (argc > 1)
       g2oFile = argv[argc - 1];
     else
-      g2oFile =
+      g2oFile = findExampleDataFile("sphere_smallnoise.graph");
-          "/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);
@@ -66,15 +62,16 @@ int main(int argc, char* argv[]) {
 
   // Build graph
   NonlinearFactorGraph graph;
-  // graph.add(NonlinearEquality<SO4>(0, SO4()));
+  // graph.add(NonlinearEquality<SOn>(0, SOn::identity(4)));
   auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
-  graph.add(PriorFactor<SO4>(0, SO4(), priorModel));
+  graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel));
+  auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4));
   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], Rot3(Tij.rotation().matrix()), 4, model);
+        keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model, G);
   }
 
   std::mt19937 rng(42);
@@ -95,9 +92,9 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < 100; i++) {
     gttic_(optimize);
     Values initial;
-    initial.insert(0, SO4());
+    initial.insert(0, SOn::identity(4));
     for (size_t j = 1; j < poses.size(); j++) {
-      initial.insert(j, SO4::Random(rng));
+      initial.insert(j, SOn::Random(rng, 4));
     }
     LevenbergMarquardtOptimizer lm(graph, initial, params);
     Values result = lm.optimize();