Merged in feature/BAD_using_charts (pull request #41)

Working on a prototype of wrapping external types

gtsam/base/chartTesting.h

@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 chartTesting.h
+ * @brief
+ * @date November, 2014
+ * @author Paul Furgale
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Testable.h>
+#include <CppUnitLite/TestResult.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/Failure.h>
+
+namespace gtsam {
+// Do a full concept check and test the invertibility of local() vs. retract().
+template<typename T>
+void testDefaultChart(TestResult& result_,
+                      const std::string& name_,
+                      const T& value) {
+  typedef typename gtsam::DefaultChart<T> Chart;
+  typedef typename Chart::vector Vector;
+
+  // First, check the basic chart concept. This checks that the interface is satisfied.
+  // The rest of the function is even more detailed, checking the correctness of the chart.
+  BOOST_CONCEPT_ASSERT((ChartConcept<Chart>));
+
+  T other = value;
+  // Check for the existence of a print function.
+  gtsam::traits::print<T>()(value, "value");
+  gtsam::traits::print<T>()(other, "other");
+
+  // Check for the existence of "equals"
+  EXPECT(gtsam::traits::equals<T>()(value, other, 1e-12));
+
+  // Check that the dimension of the local value matches the chart dimension.
+  Vector dx = Chart::local(value, other);
+  EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size());
+  // And that the "local" of a value vs. itself is zero.
+  EXPECT(assert_equal(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size()))));
+
+  // Test the invertibility of retract/local
+  dx.setRandom();
+  T updated = Chart::retract(value, dx);
+  Vector invdx = Chart::local(value, updated);
+  EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9));
+
+  // And test that negative steps work as well.
+  dx = -dx;
+  updated = Chart::retract(value, dx);
+  invdx = Chart::local(value, updated);
+  EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9));
+}
+}  // namespace gtsam
+
+/// \brief Perform a concept check on the default chart for a type.
+/// \param value An instantiation of the type to be tested.
+#define CHECK_CHART_CONCEPT(value) \
+    { gtsam::testDefaultChart(result_, name_, value); }

gtsam/base/numericalDerivative.h

@@ -30,6 +30,13 @@
 
 #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>
+
+#include <CppUnitLite/TestResult.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/Failure.h>
 
 namespace gtsam {
 
@@ -516,4 +523,72 @@ 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 numerical derivatives out the other side.
+template<typename FactorType>
+JacobianFactor computeNumericalDerivativeJacobianFactor(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);
 }
+
+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 = computeNumericalDerivativeJacobianFactor(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));
+}
+
+} // namespace gtsam
+
+/// \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 numerical_derivative_step The step to use when computing the numerical derivative Jacobians
+/// \param tolerance The numerical tolerance to use when comparing Jacobians.
+#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
+    { gtsam::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); }
+

gtsam/geometry/tests/testRot3.cpp

@@ -22,6 +22,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/lieProxies.h>
+#include <gtsam/base/ChartTesting.h>
 
 #include <boost/math/constants/constants.hpp>
 
@@ -38,6 +39,14 @@ static Point3 P(0.2, 0.7, -2.0);
 static double error = 1e-9, epsilon = 0.001;
 static const Matrix I3 = eye(3);
 
+/* ************************************************************************* */
+TEST( Rot3, chart)
+{
+  Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1);
+  Rot3 rot3(R);
+  CHECK_CHART_CONCEPT(rot3);
+}
+
 /* ************************************************************************* */
 TEST( Rot3, constructor)
 {

gtsam/navigation/tests/testMagFactor.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/navigation/MagFactor.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/LieScalar.h>
 
 #include <CppUnitLite/TestHarness.h>
 

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -17,6 +17,8 @@
  * @brief Expressions for Block Automatic Differentiation
  */
 
+#pragma once
+
 #include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Testable.h>

gtsam_unstable/nonlinear/expressionTesting.h

@@ -0,0 +1,53 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 expressionTesting.h
+ * @date September 18, 2014
+ * @author Frank Dellaert
+ * @author Paul Furgale
+ * @brief Test harness methods for expressions.
+ */
+
+#pragma once
+
+#include "Expression.h"
+#include "ExpressionFactor.h"
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
+#include <CppUnitLite/TestResult.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/Failure.h>
+#include <gtsam/base/numericalDerivative.h>
+
+namespace gtsam {
+
+template<typename T>
+void testExpressionJacobians(TestResult& result_,
+                             const std::string& name_,
+                             const gtsam::Expression<T>& expression,
+                             const gtsam::Values& values,
+                             double nd_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, nd_step, tolerance);
+}
+}  // namespace gtsam
+
+/// \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 numerical_derivative_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, numerical_derivative_step, tolerance) \
+    { gtsam::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); }

gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp

@@ -29,7 +29,6 @@
 
 #include <gtsam_unstable/nonlinear/ceres_example.h>
 
-#undef CHECK
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/list_of.hpp>

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>
@@ -423,6 +424,29 @@ 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;