Merge pull request #17 from bricerebsamen/brice/fix16-linearizeNumerically

Fix linearizeNumerically (#16)

gtsam/nonlinear/factorTesting.h

@@ -46,9 +46,9 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
 
   // Loop over all variables
   const double one_over_2delta = 1.0 / (2.0 * delta);
-  VectorValues dX = values.zeroVectors();
   for(Key key: factor) {
     // Compute central differences using the values struct.
+    VectorValues dX = values.zeroVectors();
     const size_t cols = dX.dim(key);
     Matrix J = Matrix::Zero(rows, cols);
     for (size_t col = 0; col < cols; ++col) {

gtsam/nonlinear/tests/testFactorTesting.cpp

@@ -0,0 +1,107 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------1------------------------------------------- */
+
+/**
+ * @file testFactorTesting.cpp
+ * @date September 18, 2014
+ * @author Brice Rebsamen
+ * @brief unit tests for testFactorJacobians and testExpressionJacobians
+ */
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/nonlinear/expressionTesting.h>
+#include <gtsam/slam/expressions.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+Vector3 bodyVelocity(const Pose3& w_t_b,
+                     const Vector3& vec_w,
+                     OptionalJacobian<3, 6> Hpose = boost::none,
+                     OptionalJacobian<3, 3> Hvel = boost::none) {
+  Matrix36 Hrot__pose;
+  Rot3 w_R_b = w_t_b.rotation(Hrot__pose);
+  Matrix33 Hvel__rot;
+  Vector3 vec_b = w_R_b.unrotate(vec_w, Hvel__rot, Hvel);
+  if (Hpose) {
+    *Hpose = Hvel__rot * Hrot__pose;
+  }
+  return vec_b;
+}
+
+// Functor used to create an expression for the measured wheel speed scaled
+// by the scale factor.
+class ScaledVelocityFunctor {
+ public:
+  explicit ScaledVelocityFunctor(double measured_wheel_speed)
+    : measured_velocity_(measured_wheel_speed, 0, 0) {}
+
+  // Computes the scaled measured velocity vector from the measured wheel speed
+  // and velocity scale factor. Also computes the corresponding jacobian
+  // (w.r.t. the velocity scale).
+  Vector3 operator()(double vscale,
+                     OptionalJacobian<3, 1> H = boost::none) const {
+    // The velocity scale factor value we are optimizing for is centered around
+    // 0, so we need to add 1 to it before scaling the velocity.
+    const Vector3 scaled_velocity = (vscale + 1.0) * measured_velocity_;
+    if (H) {
+      *H = measured_velocity_;
+    }
+    return scaled_velocity;
+  }
+
+ private:
+  Vector3 measured_velocity_;
+};
+
+/* ************************************************************************* */
+TEST(ExpressionTesting, Issue16) {
+  const double tol = 1e-4;
+  const double numerical_step = 1e-3;
+
+  // Note: name of keys matters: if we use 'p' instead of 'x' then this no
+  // longer repros the problem from issue 16. This is because the order of
+  // evaluation in linearizeNumerically depends on the key values. To repro
+  // we want to first evaluate the jacobian for the scale, then velocity,
+  // then pose.
+  const auto pose_key = Symbol('x', 1);
+  const auto vel_key = Symbol('v', 1);
+  const auto scale_key = Symbol('s', 1);
+
+  Values values;
+  values.insert<Pose3>(pose_key, Pose3());
+  values.insert<Vector3>(vel_key, Vector3(1, 0, 0));
+  values.insert<double>(scale_key, 0);
+
+  const Vector3_ body_vel(&bodyVelocity,
+                          Pose3_(pose_key),
+                          Vector3_(vel_key));
+  const Vector3_ scaled_measured_vel(ScaledVelocityFunctor(1),
+                                     Double_(scale_key));
+  const auto err_expr = body_vel - scaled_measured_vel;
+
+  const auto err = err_expr.value(values);
+  EXPECT_LONGS_EQUAL(3, err.size());
+  EXPECT(assert_equal(Vector3(Z_3x1), err));
+  EXPECT(internal::testExpressionJacobians(
+      "ScaleAndCompare", err_expr, values, numerical_step, tol));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+