diff --git a/.cproject b/.cproject
index 1dcc51dfe..d42d2b0e0 100644
--- a/.cproject
+++ b/.cproject
@@ -2777,6 +2777,14 @@
true
true
+
+ make
+ -j4
+ testCustomChartExpression.run
+ true
+ true
+ true
+
make
-j2
diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index 30136d9f2..f2c4566bb 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -524,71 +524,5 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const 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
-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 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
-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 gf = f.linearize(values);
- boost::shared_ptr jf = //
- boost::dynamic_pointer_cast(gf);
-
- typedef std::pair 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); }
-
diff --git a/gtsam_unstable/nonlinear/expressionTesting.h b/gtsam_unstable/nonlinear/expressionTesting.h
index ae2c1e7e5..92c8f71e8 100644
--- a/gtsam_unstable/nonlinear/expressionTesting.h
+++ b/gtsam_unstable/nonlinear/expressionTesting.h
@@ -30,19 +30,88 @@
namespace gtsam {
+/**
+ * Linearize a nonlinear factor using numerical differentiation
+ * 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.
+ */
+JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
+ const Values& values, double delta = 1e-5) {
+
+ // We will fill a map of Jacobians
+ std::map jacobians;
+
+ // Get size
+ Eigen::VectorXd e = factor.whitenedError(values);
+ const size_t rows = e.size();
+
+ // Loop over all variables
+ VectorValues dX = values.zeroVectors();
+ BOOST_FOREACH(Key key, factor) {
+ // 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] = delta;
+ dX[key] = dx;
+ Values eval_values = values.retract(dX);
+ Eigen::VectorXd left = factor.whitenedError(eval_values);
+ dx[col] = -delta;
+ dX[key] = dx;
+ eval_values = values.retract(dX);
+ Eigen::VectorXd right = factor.whitenedError(eval_values);
+ J.col(col) = (left - right) * (1.0 / (2.0 * delta));
+ }
+ jacobians[key] = J;
+ }
+
+ // Next step...return JacobianFactor
+ return JacobianFactor(jacobians, -e);
+}
+
+namespace internal {
+// CPPUnitLite-style test for linearization of a factor
+void testFactorJacobians(TestResult& result_, const std::string& name_,
+ const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
+ double tolerance) {
+
+ // Create expected value by numerical differentiation
+ JacobianFactor expected = linearizeNumerically(factor, values, delta);
+
+ // Create actual value by linearize
+ boost::shared_ptr actual = //
+ boost::dynamic_pointer_cast(factor.linearize(values));
+
+ // Check cast result and then equality
+ CHECK(actual);
+ EXPECT( assert_equal(expected, *actual, tolerance));
+}
+}
+
+/// \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::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); }
+
+namespace internal {
+// CPPUnitLite-style test for linearization of an ExpressionFactor
template
-void testExpressionJacobians(TestResult& result_,
- const std::string& name_,
- const gtsam::Expression& expression,
- const gtsam::Values& values,
- double nd_step,
- double tolerance) {
+void testExpressionJacobians(TestResult& result_, const std::string& name_,
+ const gtsam::Expression& expression, const gtsam::Values& values,
+ double nd_step, double tolerance) {
// Create factor
size_t size = traits::dimension::value;
- ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression);
+ ExpressionFactor f(noiseModel::Unit::Create(size),
+ expression.value(values), expression);
testFactorJacobians(result_, name_, f, values, nd_step, tolerance);
}
-} // namespace gtsam
+}
+} // namespace gtsam
/// \brief Check the Jacobians produced by an expression against finite differences.
/// \param expression The expression to test.
@@ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_,
/// \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::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); }