RotateFactor now takes full rotations, old one that takes directions is RotateDirectionsFactor (in same header)

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20421 898a188c-9671-0410-8e00-e3fd810bbb7f

gtsam/slam/RotateFactor.h

@@ -13,10 +13,53 @@
 namespace gtsam {
 
 /**
- * Factor that evaluates distance between two rotated directions
+ * Factor on unknown rotation iRC that relates two incremental rotations
+ *   c1Rc2 = iRc' * i1Ri2 * iRc
+ * Which we can write (see doc/math.lyx)
+ *   e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p])
+ * with z and p measured and predicted angular velocities, and hence
+ *   p = iRc * z
  */
 class RotateFactor: public NoiseModelFactor1<Rot3> {
 
+  Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
+
+  typedef NoiseModelFactor1<Rot3> Base;
+
+public:
+
+  /// Constructor
+  RotateFactor(Key key, const Rot3& P, const Rot3& Z,
+      const SharedNoiseModel& model) :
+      Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
+  }
+
+  /// print
+  virtual void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    Base::print(s);
+    std::cout << "RotateFactor:" << std::endl;
+    p_.print("p");
+    z_.print("z");
+  }
+
+  /// vector of errors returns 2D vector
+  Vector evaluateError(const Rot3& R,
+      boost::optional<Matrix&> H = boost::none) const {
+    // predict p_ as q = R*z_, derivative H will be filled if not none
+    Point3 q = R.rotate(z_,H);
+    // error is just difference, and note derivative of that wrpt q is I3
+    return Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z();
+  }
+
+};
+
+/**
+ * Factor on unknown rotation R that relates two directions p_i = iRc * z_c
+ * Directions provide less constraints than a full rotation
+ */
+class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
+
   Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z
 
   typedef NoiseModelFactor1<Rot3> Base;
@@ -24,7 +67,7 @@ class RotateFactor: public NoiseModelFactor1<Rot3> {
 public:
 
   /// Constructor
-  RotateFactor(Key key, const Sphere2& p, const Sphere2& z,
+  RotateDirectionsFactor(Key key, const Sphere2& p, const Sphere2& z,
       const SharedNoiseModel& model) :
       Base(model, key), p_(p), z_(z) {
   }
@@ -33,6 +76,7 @@ public:
   virtual void print(const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s);
+    std::cout << "RotateDirectionsFactor:" << std::endl;
     p_.print("p");
     z_.print("z");
   }
@@ -50,19 +94,6 @@ public:
     return e;
   }
 
-  /// Obsolete: vector of errors returns 1D vector
-  Vector evaluateError1(const Rot3& R,
-      boost::optional<Matrix&> H = boost::none) const {
-    Sphere2 q = R * z_;
-    double e = p_.distance(q, H);
-    if (H) {
-      Matrix DR;
-      R.rotate(z_, DR);
-      *H = (*H) * DR;
-    }
-    return (Vector(1) << e);
-  }
-
 };
 } // gtsam
 

gtsam/slam/tests/testRotateFactor.cpp

@@ -26,23 +26,42 @@ using namespace gtsam;
 // And camera is looking forward.
 Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
 Rot3 iRc(cameraX, cameraY, cameraZ);
+
 // Now, let's create some rotations around IMU frame
 Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
-Rot3 gRi1 = Rot3::Expmap(Vector3(1, 0, 0));
+Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), //
+i2Ri3 = Rot3::rodriguez(p2, 1), //
+i3Ri4 = Rot3::rodriguez(p3, 1);
+
 // The corresponding rotations in the camera frame
-Sphere2 z1 = iRc.inverse() * p1, z2 = iRc.inverse() * p2, //
+Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
+c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
+c3Zc4 = iRc.inverse() * i3Ri4 * iRc;
+
+// The corresponding rotated directions in the camera frame
+Sphere2 z1 = iRc.inverse() * p1, //
+z2 = iRc.inverse() * p2, //
 z3 = iRc.inverse() * p3;
 
-// noise model
+typedef noiseModel::Isotropic::shared_ptr Model;
-noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, 0.01);
+
+//*************************************************************************
+TEST (RotateFactor, checkMath) {
+  EXPECT(assert_equal(c1Zc2, Rot3::rodriguez(z1, 1)));
+  EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1)));
+  EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1)));
+}
 
 //*************************************************************************
 TEST (RotateFactor, test) {
-  RotateFactor f(1, p1, z1, model);
+  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
+  RotateFactor f(1, i1Ri2, c1Zc2, model);
+  EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8));
+
   Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
-  EXPECT(assert_equal((Vector(2)<<0,0), f.evaluateError(iRc), 1e-8));
+  Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867);
-  EXPECT(
+  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
-      assert_equal((Vector(2)<<-0.08867, -0.20197), f.evaluateError(R), 1e-5));
+
   Matrix actual, expected;
   // Use numerical derivatives to calculate the expected Jacobian
   {
@@ -63,9 +82,10 @@ TEST (RotateFactor, test) {
 TEST (RotateFactor, minimization) {
   // Let's try to recover the correct iRc by minimizing
   NonlinearFactorGraph graph;
-  graph.add(RotateFactor(1, p1, z1, model));
+  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
-  graph.add(RotateFactor(1, p2, z2, model));
+  graph.add(RotateFactor(1, i1Ri2, c1Zc2, model));
-  graph.add(RotateFactor(1, p3, z3, model));
+  graph.add(RotateFactor(1, i2Ri3, c2Zc3, model));
+  graph.add(RotateFactor(1, i3Ri4, c3Zc4, model));
 
   // Check error at ground truth
   Values truth;
@@ -74,8 +94,71 @@ TEST (RotateFactor, minimization) {
 
   // Check error at initial estimate
   Values initial;
-  double degree = M_PI/180;
+  double degree = M_PI / 180;
-  Rot3 initialE = iRc.retract(degree*(Vector(3) << 20, -20, 20));
+  Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
+  initial.insert(1, initialE);
+  EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1);
+
+  // Optimize
+  LevenbergMarquardtParams parameters;
+  //parameters.setVerbosity("ERROR");
+  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
+  Values result = optimizer.optimize();
+
+  // Check result
+  Rot3 actual = result.at<Rot3>(1);
+  EXPECT(assert_equal(iRc, actual,1e-1));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
+}
+
+//*************************************************************************
+TEST (RotateDirectionsFactor, test) {
+  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
+  RotateDirectionsFactor f(1, p1, z1, model);
+  EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8));
+
+  Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
+  Vector expectedE = (Vector(2) << -0.08867, -0.20197);
+  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
+
+  Matrix actual, expected;
+  // Use numerical derivatives to calculate the expected Jacobian
+  {
+    expected = numericalDerivative11<Rot3>(
+        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
+            boost::none), iRc);
+    f.evaluateError(iRc, actual);
+    EXPECT(assert_equal(expected, actual, 1e-9));
+  }
+  {
+    expected = numericalDerivative11<Rot3>(
+        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
+            boost::none), R);
+    f.evaluateError(R, actual);
+    EXPECT(assert_equal(expected, actual, 1e-9));
+  }
+}
+
+//*************************************************************************
+TEST (RotateDirectionsFactor, minimization) {
+  // Let's try to recover the correct iRc by minimizing
+  NonlinearFactorGraph graph;
+  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
+  graph.add(RotateDirectionsFactor(1, p1, z1, model));
+  graph.add(RotateDirectionsFactor(1, p2, z2, model));
+  graph.add(RotateDirectionsFactor(1, p3, z3, model));
+
+  // Check error at ground truth
+  Values truth;
+  truth.insert(1, iRc);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Check error at initial estimate
+  Values initial;
+  double degree = M_PI / 180;
+  Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
   initial.insert(1, initialE);
   EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1);