Merged in feature/small_improvements (pull request #208)

Feature/small_improvements

gtsam/geometry/Pose3.cpp

@@ -120,9 +120,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
   Rot3 R = Rot3::Expmap(omega.vector());
   double theta2 = omega.dot(omega);
   if (theta2 > std::numeric_limits<double>::epsilon()) {
-    double omega_v = omega.dot(v);          // translation parallel to axis
+    Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis
     Point3 omega_cross_v = omega.cross(v);    // points towards axis
-    Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2;
+    Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
     return Pose3(R, t);
   } else {
     return Pose3(R, v);

gtsam/geometry/SO3.cpp

@@ -47,7 +47,7 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
     // get components of axis \omega, where is a unit vector
     const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
 
-    const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
+    const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2;
     const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
                  wz_sintheta = wz * sintheta;
 
@@ -130,7 +130,6 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
 
 /* ************************************************************************* */
 Matrix3 SO3::ExpmapDerivative(const Vector3& omega)    {
-  using std::cos;
   using std::sin;
 
   const double theta2 = omega.dot(omega);

gtsam/geometry/tests/testPoint3.cpp

@@ -133,6 +133,17 @@ TEST (Point3, distance) {
   EXPECT(assert_equal(numH2, H2, 1e-8));
 }
 
+/* ************************************************************************* */
+TEST(Point3, cross) {
+  Matrix aH1, aH2;
+  boost::function<Point3(const Point3&, const Point3&)> f =
+      boost::bind(&Point3::cross, _1, _2, boost::none, boost::none);
+  const Point3 omega(0, 1, 0), theta(4, 6, 8);
+  omega.cross(theta, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testPose3.cpp

@@ -715,7 +715,42 @@ TEST( Pose3, ExpmapDerivative1) {
 }
 
 /* ************************************************************************* */
-TEST( Pose3, LogmapDerivative1) {
+TEST(Pose3, ExpmapDerivative2) {
+  // Iserles05an (Lie-group Methods) says:
+  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
+  // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
+  // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3)
+  // and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3)
+  // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4)
+
+  // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors.
+  // xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t)
+
+  // Let's verify the above formula.
+
+  auto xi = [](double t) {
+    Vector6 v;
+    v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t;
+    return v;
+  };
+  auto xi_dot = [](double t) {
+    Vector6 v;
+    v << 2, cos(t), 8 * t, 2, cos(t), 8 * t;
+    return v;
+  };
+
+  // We define a function T
+  auto T = [xi](double t) { return Pose3::Expmap(xi(t)); };
+
+  for (double t = -2.0; t < 2.0; t += 0.3) {
+    const Matrix expected = numericalDerivative11<Pose3, double>(T, t);
+    const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t);
+    CHECK(assert_equal(expected, actual, 1e-7));
+  }
+}
+
+/* ************************************************************************* */
+TEST( Pose3, LogmapDerivative) {
   Matrix6 actualH;
   Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
   Pose3 p = Pose3::Expmap(w);

gtsam/geometry/tests/testRot3.cpp

@@ -244,44 +244,99 @@ TEST(Rot3, retract_localCoordinates2)
   EXPECT(assert_equal(t1, t2.retract(d21)));
 }
 /* ************************************************************************* */
-Vector w = Vector3(0.1, 0.27, -0.2);
+namespace exmap_derivative {
-
+static const Vector3 w(0.1, 0.27, -0.2);
-// Left trivialization Derivative of exp(w) wrpt w:
+}
+// Left trivialized Derivative of exp(w) wrpt w:
 // How does exp(w) change when w changes?
 // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
 // => y = log (exp(-w) * exp(w+dw))
 Vector3 testDexpL(const Vector3& dw) {
+  using exmap_derivative::w;
   return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
 }
 
 TEST( Rot3, ExpmapDerivative) {
-  Matrix actualDexpL = Rot3::ExpmapDerivative(w);
+  using exmap_derivative::w;
-  Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
+  const Matrix actualDexpL = Rot3::ExpmapDerivative(w);
+  const Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
       Vector3::Zero(), 1e-2);
   EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
 
-  Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
+  const Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
+  }
+
+/* ************************************************************************* */
+TEST( Rot3, ExpmapDerivative2)
+{
+  const Vector3 theta(0.1, 0, 0.1);
+  const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+      boost::bind(&Rot3::Expmap, _1, boost::none), theta);
+
+  CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
+  CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
 }
 
 /* ************************************************************************* */
-Vector3 thetahat(0.1, 0, 0.1);
+TEST( Rot3, ExpmapDerivative3)
-TEST( Rot3, ExpmapDerivative2)
 {
-  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+  const Vector3 theta(10, 20, 30);
-      boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
+  const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+      boost::bind(&Rot3::Expmap, _1, boost::none), theta);
 
-  Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
+  CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
-  CHECK(assert_equal(Jexpected, Jactual));
+  CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
+}
 
-  Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
+/* ************************************************************************* */
-  CHECK(assert_equal(Jexpected, Jactual2));
+TEST(Rot3, ExpmapDerivative4) {
+  // Iserles05an (Lie-group Methods) says:
+  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
+  // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
+  // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3)
+  // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3)
+  // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
+
+  // In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors.
+  // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t)
+
+  // Let's verify the above formula.
+
+  auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
+  auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
+
+  // We define a function R
+  auto R = [w](double t) { return Rot3::Expmap(w(t)); };
+
+  for (double t = -2.0; t < 2.0; t += 0.3) {
+    const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
+    const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
+    CHECK(assert_equal(expected, actual, 1e-7));
+  }
+}
+
+/* ************************************************************************* */
+TEST(Rot3, ExpmapDerivative5) {
+  auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
+  auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
+
+  // Same as above, but define R as mapping local coordinates to neighborhood aroud R0.
+  const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2);
+  auto R = [R0, w](double t) { return R0.expmap(w(t)); };
+
+  for (double t = -2.0; t < 2.0; t += 0.3) {
+    const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
+    const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
+    CHECK(assert_equal(expected, actual, 1e-7));
+  }
 }
 
 /* ************************************************************************* */
 TEST( Rot3, jacobianExpmap )
 {
-  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
+  const Vector3 thetahat(0.1, 0, 0.1);
+  const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
       &Rot3::Expmap, _1, boost::none), thetahat);
   Matrix3 Jactual;
   const Rot3 R = Rot3::Expmap(thetahat, Jactual);
@@ -291,18 +346,20 @@ TEST( Rot3, jacobianExpmap )
 /* ************************************************************************* */
 TEST( Rot3, LogmapDerivative )
 {
-  Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Vector3 thetahat(0.1, 0, 0.1);
-  Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
+  const Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
       &Rot3::Logmap, _1, boost::none), R);
-  Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
+  const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
 /* ************************************************************************* */
-TEST( Rot3, jacobianLogmap )
+TEST( Rot3, JacobianLogmap )
 {
-  Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Vector3 thetahat(0.1, 0, 0.1);
-  Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
+  const Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
       &Rot3::Logmap, _1, boost::none), R);
   Matrix3 Jactual;
   Rot3::Logmap(R, Jactual);

gtsam/linear/GaussianBayesNet.cpp

@@ -141,7 +141,20 @@ namespace gtsam {
   /* ************************************************************************* */  
   pair<Matrix,Vector> GaussianBayesNet::matrix() const
   {
-    return GaussianFactorGraph(*this).jacobian();
+    GaussianFactorGraph factorGraph(*this);
+    KeySet keys = factorGraph.keys();
+    // add frontal keys in order
+    Ordering ordering;
+    BOOST_FOREACH (const sharedConditional& cg, *this)
+      BOOST_FOREACH (Key key, cg->frontals()) {
+        ordering.push_back(key);
+        keys.erase(key);
+      }
+    // add remaining keys in case Bayes net is incomplete
+    BOOST_FOREACH (Key key, keys)
+      ordering.push_back(key);
+    // return matrix and RHS
+    return factorGraph.jacobian(ordering);
   }
 
   ///* ************************************************************************* */