move linear algebra functions for jacobian factor graph to a new file

gtsam/linear/GaussianBayesNet.cpp

@@ -19,7 +19,7 @@
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 
-#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/JacobianFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/VectorValues.h>
 

gtsam/linear/GaussianBayesTree.cpp

@@ -18,7 +18,7 @@
  */
 
 #include <gtsam/linear/GaussianBayesTree.h>
-#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/JacobianFactorGraph.h>
 
 namespace gtsam {
 

gtsam/linear/JacobianFactor.cpp

@@ -536,98 +536,4 @@ namespace gtsam {
 			model_ = noiseModel::Diagonal::Sigmas(sigmas);
 	}
 
-  /* ************************************************************************* */
-  Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
-    Errors e;
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
-      e.push_back((*Ai)*x);
-    }
-    return e;
-  }
-
-  /* ************************************************************************* */
-  void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) {
-    multiplyInPlace(fg,x,e.begin());
-  }
-
-  /* ************************************************************************* */
-  void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) {
-    Errors::iterator ei = e;
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
-      *ei = (*Ai)*x;
-      ei++;
-    }
-  }
-
-
-  /* ************************************************************************* */
-  // x += alpha*A'*e
-  void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x) {
-    // For each factor add the gradient contribution
-    Errors::const_iterator ei = e.begin();
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
-      Ai->transposeMultiplyAdd(alpha,*(ei++),x);
-    }
-  }
-
-  /* ************************************************************************* */
-  VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) {
-    // It is crucial for performance to make a zero-valued clone of x
-    VectorValues g = VectorValues::Zero(x0);
-    Errors e;
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-      e.push_back(factor->error_vector(x0));
-    }
-    transposeMultiplyAdd(fg, 1.0, e, g);
-    return g;
-  }
-
-  /* ************************************************************************* */
-  void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) {
-    // Zero-out the gradient
-    g.setZero();
-    Errors e;
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-      e.push_back(-factor->getb());
-    }
-    transposeMultiplyAdd(fg, 1.0, e, g);
-  }
-
-  /* ************************************************************************* */
-  void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
-    Index i = 0 ;
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-      r[i] = factor->getb();
-      i++;
-    }
-    VectorValues Ax = VectorValues::SameStructure(r);
-    multiply(fg,x,Ax);
-    axpy(-1.0,Ax,r);
-  }
-
-  /* ************************************************************************* */
-  void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
-    r.vector() = Vector::Zero(r.dim());
-    Index i = 0;
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-      SubVector &y = r[i];
-      for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
-        y += factor->getA(j) * x[*j];
-      }
-      ++i;
-    }
-  }
-
-  /* ************************************************************************* */
-  void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
-    x.vector() = Vector::Zero(x.dim());
-    Index i = 0;
-    BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-      for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
-        x[*j] += factor->getA(j).transpose() * r[i];
-      }
-      ++i;
-    }
-  }
-
 }

gtsam/linear/JacobianFactor.h

@@ -318,45 +318,5 @@ namespace gtsam {
     }
 
   }; // JacobianFactor
-
-  /** return A*x */
-  Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x);
-
-  /* In-place version e <- A*x that overwrites e. */
-  void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e);
-
-  /* In-place version e <- A*x that takes an iterator. */
-  void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e);
-
-  /** x += alpha*A'*e */
-  void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x);
-
-  /**
-   * Compute the gradient of the energy function,
-   * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
-   * centered around \f$ x = x_0 \f$.
-   * The gradient is \f$ A^T(Ax-b) \f$.
-   * @param fg The Jacobian factor graph $(A,b)$
-   * @param x0 The center about which to compute the gradient
-   * @return The gradient as a VectorValues
-   */
-  VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0);
-
-  /**
-   * Compute the gradient of the energy function,
-   * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
-   * centered around zero.
-   * The gradient is \f$ A^T(Ax-b) \f$.
-   * @param fg The Jacobian factor graph $(A,b)$
-   * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
-   * @return The gradient as a VectorValues
-   */
-  void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g);
-
-  // matrix-vector operations
-  void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r);
-  void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r);
-  void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x);
-
 } // gtsam
 

gtsam/linear/JacobianFactorGraph.cpp

@@ -0,0 +1,108 @@
+/*
+ * JacobianFactorGraph.cpp
+ *
+ *  Created on: Jun 6, 2012
+ *      Author: ydjian
+ */
+
+#include <gtsam/linear/JacobianFactorGraph.h>
+#include <boost/foreach.hpp>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
+  Errors e;
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
+    e.push_back((*Ai)*x);
+  }
+  return e;
+}
+
+/* ************************************************************************* */
+void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) {
+  multiplyInPlace(fg,x,e.begin());
+}
+
+/* ************************************************************************* */
+void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) {
+  Errors::iterator ei = e;
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
+    *ei = (*Ai)*x;
+    ei++;
+  }
+}
+
+
+/* ************************************************************************* */
+// x += alpha*A'*e
+void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x) {
+  // For each factor add the gradient contribution
+  Errors::const_iterator ei = e.begin();
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
+    Ai->transposeMultiplyAdd(alpha,*(ei++),x);
+  }
+}
+
+/* ************************************************************************* */
+VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) {
+  // It is crucial for performance to make a zero-valued clone of x
+  VectorValues g = VectorValues::Zero(x0);
+  Errors e;
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
+    e.push_back(factor->error_vector(x0));
+  }
+  transposeMultiplyAdd(fg, 1.0, e, g);
+  return g;
+}
+
+/* ************************************************************************* */
+void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) {
+  // Zero-out the gradient
+  g.setZero();
+  Errors e;
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
+    e.push_back(-factor->getb());
+  }
+  transposeMultiplyAdd(fg, 1.0, e, g);
+}
+
+/* ************************************************************************* */
+void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
+  Index i = 0 ;
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
+    r[i] = factor->getb();
+    i++;
+  }
+  VectorValues Ax = VectorValues::SameStructure(r);
+  multiply(fg,x,Ax);
+  axpy(-1.0,Ax,r);
+}
+
+/* ************************************************************************* */
+void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
+  r.vector() = Vector::Zero(r.dim());
+  Index i = 0;
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
+    SubVector &y = r[i];
+    for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
+      y += factor->getA(j) * x[*j];
+    }
+    ++i;
+  }
+}
+
+/* ************************************************************************* */
+void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
+  x.vector() = Vector::Zero(x.dim());
+  Index i = 0;
+  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
+    for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
+      x[*j] += factor->getA(j).transpose() * r[i];
+    }
+    ++i;
+  }
+}
+}
+
+

gtsam/linear/JacobianFactorGraph.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
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * JacobianFactorGraph.cpp
+ *
+ *  Created on: Jun 6, 2012
+ *      Author: ydjian
+ */
+#pragma once
+
+#include <gtsam/linear/Errors.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/VectorValues.h>
+
+namespace gtsam {
+
+  /**
+   * Linear Algebra Operations for the JacobianFactorGraph
+   */
+
+  /** return A*x */
+  Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x);
+
+  /** In-place version e <- A*x that overwrites e. */
+  void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e);
+
+  /** In-place version e <- A*x that takes an iterator. */
+  void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e);
+
+  /** x += alpha*A'*e */
+  void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x);
+
+  /**
+   * Compute the gradient of the energy function,
+   * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
+   * centered around \f$ x = x_0 \f$.
+   * The gradient is \f$ A^T(Ax-b) \f$.
+   * @param fg The Jacobian factor graph $(A,b)$
+   * @param x0 The center about which to compute the gradient
+   * @return The gradient as a VectorValues
+   */
+  VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0);
+
+  /**
+   * Compute the gradient of the energy function,
+   * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
+   * centered around zero.
+   * The gradient is \f$ A^T(Ax-b) \f$.
+   * @param fg The Jacobian factor graph $(A,b)$
+   * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
+   * @return The gradient as a VectorValues
+   */
+  void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g);
+
+  /* matrix-vector operations */
+  void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r);
+  void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r);
+  void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x);
+
+}

gtsam/linear/SimpleSPCGSolver.cpp

@@ -13,7 +13,7 @@
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/JacobianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/EliminationTree.h>
 #include <boost/foreach.hpp>

gtsam/nonlinear/ISAM2.cpp

@@ -26,6 +26,7 @@ using namespace boost::assign;
 #include <gtsam/linear/GaussianJunctionTree.h>
 #include <gtsam/inference/BayesTree-inl.h>
 #include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/JacobianFactorGraph.h>
 
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>

tests/testGaussianFactorGraphB.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/JacobianFactorGraph.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Matrix.h>

tests/testGaussianISAM2.cpp

@@ -10,6 +10,7 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/GaussianBayesTree.h>
+#include <gtsam/linear/JacobianFactorGraph.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/slam/smallExample.h>
 #include <gtsam/slam/planarSLAM.h>