Merge pull request #453 from borglab/feature/better_frobenius_factors

Better frobenius factors

gtsam.h

@@ -2369,6 +2369,7 @@ virtual class NonlinearOptimizer {
   double error() const;
   int iterations() const;
   gtsam::Values values() const;
+  gtsam::NonlinearFactorGraph graph() const;
   gtsam::GaussianFactorGraph* iterate() const;
 };
 

gtsam/geometry/SOn-inl.h

@@ -26,13 +26,13 @@ using namespace std;
 
 namespace gtsam {
 
-// Implementation for N>5 just uses dynamic version
+// Implementation for N>=5 just uses dynamic version
 template <int N>
 typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
   return SOn::Hat(xi);
 }
 
-// Implementation for N>5 just uses dynamic version
+// Implementation for N>=5 just uses dynamic version
 template <int N>
 typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
   return SOn::Vee(X);
@@ -99,12 +99,8 @@ typename SO<N>::VectorN2 SO<N>::vec(
   if (H) {
     // Calculate P matrix of vectorized generators
     // TODO(duy): Should we refactor this as the jacobian of Hat?
+    Matrix P = VectorizedGenerators(n);
     const size_t d = dim();
-    Matrix P(n2, d);
-    for (size_t j = 0; j < d; j++) {
-      const auto X = Hat(Eigen::VectorXd::Unit(d, j));
-      P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
-    }
     H->resize(n2, d);
     for (size_t i = 0; i < n; i++) {
       H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);

gtsam/geometry/SOn.h

@@ -290,7 +290,34 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
    * */
   VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
                    boost::none) const;
-  /// @}
+
+  /// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
+  template <int N_ = N, typename = IsFixed<N_>>
+  static Matrix VectorizedGenerators() {
+    constexpr size_t N2 = static_cast<size_t>(N * N);
+    Matrix G(N2, dimension);
+    for (size_t j = 0; j < dimension; j++) {
+      const auto X = Hat(Vector::Unit(dimension, j));
+      G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1);
+    }
+    return G;
+  }
+
+  /// Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
+  template <int N_ = N, typename = IsDynamic<N_>>
+  static Matrix VectorizedGenerators(size_t n = 0) {
+    const size_t n2 = n * n, dim = Dimension(n);
+    Matrix G(n2, dim);
+    for (size_t j = 0; j < dim; j++) {
+      const auto X = Hat(Vector::Unit(dim, j));
+      G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
+    }
+    return G;
+  }
+
+  /// @{
+  /// @name Serialization
+  /// @{
 
   template <class Archive>
   friend void save(Archive&, SO&, const unsigned int);
@@ -300,6 +327,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   friend void serialize(Archive&, SO&, const unsigned int);
   friend class boost::serialization::access;
   friend class Rot3;  // for serialize
+
+  /// @}
 };
 
 using SOn = SO<Eigen::Dynamic>;

gtsam/nonlinear/NonlinearFactor.h

@@ -296,6 +296,8 @@ protected:
   typedef NoiseModelFactor1<VALUE> This;
 
 public:
+  /// @name Constructors
+  /// @{
 
   /** Default constructor for I/O only */
   NoiseModelFactor1() {}
@@ -309,16 +311,23 @@ public:
    *  @param noiseModel shared pointer to noise model
    *  @param key1 by which to look up X value in Values
    */
-  NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
+  NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1)
-    Base(noiseModel, cref_list_of<1>(key1)) {}
+      : Base(noiseModel, cref_list_of<1>(key1)) {}
 
-  /** Calls the 1-key specific version of evaluateError, which is pure virtual
+  /// @}
-   *  so must be implemented in the derived class.
+  /// @name NoiseModelFactor methods
+  /// @{
+
+  /**
+   * Calls the 1-key specific version of evaluateError below, which is pure
+   * virtual so must be implemented in the derived class.
    */
-  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
+  Vector unwhitenedError(
-    if(this->active(x)) {
+      const Values &x,
-      const X& x1 = x.at<X>(keys_[0]);
+      boost::optional<std::vector<Matrix> &> H = boost::none) const override {
-      if(H) {
+    if (this->active(x)) {
+      const X &x1 = x.at<X>(keys_[0]);
+      if (H) {
         return evaluateError(x1, (*H)[0]);
       } else {
         return evaluateError(x1);
@@ -328,16 +337,22 @@ public:
     }
   }
 
+  /// @}
+  /// @name Virtual methods
+  /// @{
+
   /**
    *  Override this method to finish implementing a unary factor.
    *  If the optional Matrix reference argument is specified, it should compute
    *  both the function evaluation and its derivative in X.
    */
-  virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
+  virtual Vector
-      boost::none) const = 0;
+  evaluateError(const X &x,
+                boost::optional<Matrix &> H = boost::none) const = 0;
+
+  /// @}
 
 private:
-
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>

gtsam/nonlinear/NonlinearOptimizer.h

@@ -105,14 +105,17 @@ public:
    */
   const Values& optimizeSafely();
 
-  /// return error
+  /// return error in current optimizer state
   double error() const;
 
-  /// return number of iterations
+  /// return number of iterations in current optimizer state
   size_t iterations() const;
 
-  /// return values
+  /// return values in current optimizer state
-  const Values& values() const;
+  const Values &values() const;
+
+  /// return the graph with nonlinear factors
+  const NonlinearFactorGraph &graph() const { return graph_; }
 
   /// @}
 

gtsam/slam/BetweenFactor.h

@@ -67,9 +67,11 @@ namespace gtsam {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
-    /** implement functions needed for Testable */
+    /// @}
+    /// @name Testable
+    /// @{
 
-    /** print */
+    /// print with optional string
     void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
@@ -78,15 +80,17 @@ namespace gtsam {
       this->noiseModel_->print("  noise model: ");
     }
 
-    /** equals */
+    /// assert equality up to a tolerance
     bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
     }
 
-    /** implement functions needed to derive from Factor */
+    /// @}
+    /// @name NoiseModelFactor2 methods 
+    /// @{
 
-    /** vector of errors */
+    /// evaluate error, returns vector of errors size of tangent space
     Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
       boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
@@ -102,15 +106,15 @@ namespace gtsam {
 #endif
     }
 
-    /** return the measured */
+    /// @}
+    /// @name Standard interface 
+    /// @{
+
+    /// return the measurement
     const VALUE& measured() const {
       return measured_;
     }
-
+    /// @}
-    /** number of variables attached to this factor */
-    std::size_t size() const {
-      return 2;
-    }
 
   private:
 

gtsam/slam/EssentialMatrixConstraint.h

@@ -87,11 +87,6 @@ public:
     return measuredE_;
   }
 
-  /** number of variables attached to this factor */
-  std::size_t size() const {
-    return 2;
-  }
-
 private:
 
   /** Serialization function */

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
       p_(p),            // 4 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
+    : public NoiseModelFactor2<SOn, SOn> {
   Matrix M_;                    ///< measured rotation between R1 and R2
-  size_t p_, pp_, dimension_;  ///< dimensionality constants
+  size_t p_, pp_;               ///< dimensionality constants
-  Matrix G_;                   ///< matrix of vectorized generators
+  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

gtsam_unstable/slam/BetweenFactorEM.h

@@ -403,11 +403,6 @@ public:
     return measured_;
   }
 
-  /** number of variables attached to this factor */
-  std::size_t size() const {
-    return 2;
-  }
-
   size_t dim() const override {
     return model_inlier_->R().rows() + model_inlier_->R().cols();
   }

gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h

@@ -203,11 +203,6 @@ namespace gtsam {
 
     /* ************************************************************************* */
 
-    /** number of variables attached to this factor */
-    std::size_t size() const {
-      return 1;
-    }
-
     size_t dim() const override {
       return model_->R().rows() + model_->R().cols();
     }

gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h

@@ -401,11 +401,6 @@ namespace gtsam {
 
     /* ************************************************************************* */
 
-    /** number of variables attached to this factor */
-    std::size_t size() const {
-      return 1;
-    }
-
     size_t dim() const override {
       return model_inlier_->R().rows() + model_inlier_->R().cols();
     }

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();