More progress on easy Jacobian testing

gtsam/base/numericalDerivative.h

@@ -30,6 +30,9 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/nonlinear/Values.h>
 
 namespace gtsam {
 
@@ -516,4 +519,43 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
       boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
       delta);
 }
+
+// The benefit of this method is that it does not need to know what types are involved
+// to evaluate the factor. If all the machinery of gtsam is working correctly, we should
+// get the correct finite differences out the other side.
+template<typename FactorType>
+JacobianFactor computeFiniteDifferenceJacobianFactor(const FactorType& factor,
+                                                     const Values& values,
+                                                     double fd_step) {
+  Eigen::VectorXd e = factor.unwhitenedError(values);
+  const size_t rows = e.size();
+
+  std::map<Key, Matrix> jacobians;
+  typename FactorType::const_iterator key_it = factor.begin();
+  VectorValues dX = values.zeroVectors();
+  for(; key_it != factor.end(); ++key_it) {
+    size_t key = *key_it;
+    // Compute central differences using the values struct.
+    const size_t cols = dX.dim(key);
+    Matrix J = Matrix::Zero(rows, cols);
+    for(size_t col = 0; col < cols; ++col) {
+      Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
+      dx[col] = fd_step;
+      dX[key] = dx;
+      Values eval_values = values.retract(dX);
+      Eigen::VectorXd left = factor.unwhitenedError(eval_values);
+      dx[col] = -fd_step;
+      dX[key] = dx;
+      eval_values = values.retract(dX);
+      Eigen::VectorXd right = factor.unwhitenedError(eval_values);
+      J.col(col) = (left - right) * (1.0/(2.0 * fd_step));
+    }
+    jacobians[key] = J;
+  }
+
+  // Next step...return JacobianFactor
+  return JacobianFactor(jacobians, -e);
 }
+
+} // namespace gtsam
+

gtsam_unstable/nonlinear/ExpressionTesting.h

@@ -0,0 +1,111 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Expression.h
+ * @date September 18, 2014
+ * @author Frank Dellaert
+ * @author Paul Furgale
+ * @brief Expressions for Block Automatic Differentiation
+ */
+
+#pragma once
+
+#include "Expression.h"
+#include "ExpressionFactor.h"
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Matrix.h>
+#include <CppUnitLite/TestResult.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/Failure.h>
+#include <gtsam/base/numericalDerivative.h>
+
+namespace gtsam {
+
+template<typename FactorType>
+void testFactorJacobians(TestResult& result_,
+                         const std::string& name_,
+                         const FactorType& f,
+                         const gtsam::Values& values,
+                         double fd_step,
+                         double tolerance) {
+  // Check linearization
+  JacobianFactor expected = computeFiniteDifferenceJacobianFactor(f, values, fd_step);
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+
+  typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
+  Jacobian evalJ = jf->jacobianUnweighted();
+  Jacobian estJ = expected.jacobianUnweighted();
+  EXPECT(assert_equal(evalJ.first, estJ.first, tolerance));
+  EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance));
+  EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance));
+}
+
+template<typename T>
+void testExpressionJacobians(TestResult& result_,
+                             const std::string& name_,
+                             const gtsam::Expression<T>& expression,
+                             const gtsam::Values& values,
+                             double fd_step,
+                             double tolerance) {
+  // Create factor
+  size_t size = traits::dimension<T>::value;
+  ExpressionFactor<T> f(noiseModel::Unit::Create(size), expression.value(values), expression);
+  testFactorJacobians(result_, name_, f, values, fd_step, tolerance);
+}
+
+
+
+// Do a full concept check and test the invertibility of
+// local() vs. retract().
+template<typename T>
+void testDefaultChart(const T& value) {
+  T other = value;
+  gtsam::traits::print<T>()(value, "value");
+  gtsam::traits::print<T>()(other, "other");
+  EXPECT_TRUE(gtsam::traits::equals<T>()(value, other, 1e-12));
+
+  typedef typename gtsam::DefaultChart<T> Chart;
+  typedef typename Chart::vector Vector;
+
+  Vector dx = Chart::local(value, other);
+  EXPECT_EQ(Chart::getDimension(value), dx.size());
+
+  dx.setRandom();
+  T updated = Chart::retract(value, dx);
+  Vector invdx = Chart::local(value, updated);
+  EXPECT_TRUE(assert_equal(dx, invdx, 1e-9));
+
+  dx = -dx;
+  updated = Chart::retract(value, dx);
+  invdx = Chart::local(value, updated);
+  EXPECT_TRUE(assert_equal(dx, invdx, 1e-9));
+}
+
+/// \brief Check the Jacobians produced by a factor against finite differences.
+/// \param factor The factor to test.
+/// \param values Values filled in for testing the Jacobians.
+/// \param finite_difference_step The step to use when computing the finite difference Jacobians
+/// \param tolerance The numerical tolerance to use when comparing Jacobians.
+#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, finite_difference_step, tolerance) \
+    { testFactorJacobians(result_, name_, factor, values, finite_difference_step, tolerance); }
+
+/// \brief Check the Jacobians produced by an expression against finite differences.
+/// \param expression The expression to test.
+/// \param values Values filled in for testing the Jacobians.
+/// \param finite_difference_step The step to use when computing the finite difference Jacobians
+/// \param tolerance The numerical tolerance to use when comparing Jacobians.
+#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, finite_difference_step, tolerance) \
+    { testExpressionJacobians(result_, name_, expression, values, finite_difference_step, tolerance); }
+
+}  // namespace gtsam

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -19,6 +19,7 @@
 
 #include <gtsam_unstable/slam/expressions.h>
 #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam_unstable/nonlinear/ExpressionTesting.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/PriorFactor.h>
@@ -425,6 +426,30 @@ TEST(ExpressionFactor, composeTernary) {
   EXPECT( assert_equal(expected, *jf,1e-9));
 }
 
+TEST(ExpressionFactor, tree_finite_differences) {
+
+  // Create some values
+  Values values;
+  values.insert(1, Pose3());
+  values.insert(2, Point3(0, 0, 1));
+  values.insert(3, Cal3_S2());
+
+  // Create leaves
+  Pose3_ x(1);
+  Point3_ p(2);
+  Cal3_S2_ K(3);
+
+  // Create expression tree
+  Point3_ p_cam(x, &Pose3::transform_to, p);
+  Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
+  Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
+
+  const double fd_step = 1e-5;
+  const double tolerance = 1e-5;
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance);
+}
+
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;