Merge branch 'support/2.4.0/mergeDevelop'

gtsam.h

@@ -563,6 +563,49 @@ virtual class Pose3 : gtsam::Value {
   void serialize() const;
 };
 
+#include <gtsam/geometry/Sphere2.h>
+virtual class Sphere2 : gtsam::Value {
+  // Standard Constructors
+  Sphere2();
+  Sphere2(const gtsam::Point3& pose);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Sphere2& pose, double tol) const;
+
+  // Other functionality
+  Matrix basis() const;
+  Matrix skew() const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Sphere2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Sphere2& s) const;
+};
+
+#include <gtsam/geometry/EssentialMatrix.h>
+virtual class EssentialMatrix : gtsam::Value {
+  // Standard Constructors
+  EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Sphere2& aTb);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::EssentialMatrix retract(Vector v) const;
+  Vector localCoordinates(const gtsam::EssentialMatrix& s) const;
+
+  // Other methods:
+  gtsam::Rot3 rotation() const;
+  gtsam::Sphere2 direction() const;
+  Matrix matrix() const;
+  double error(Vector vA, Vector vB);
+};
+
 virtual class Cal3_S2 : gtsam::Value {
   // Standard Constructors
   Cal3_S2();
@@ -2273,6 +2316,12 @@ virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
 typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
 typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
 
+#include <gtsam/slam/EssentialMatrixFactor.h>
+virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
+  EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB,
+      const gtsam::noiseModel::Base* noiseModel);
+};
+
 #include <gtsam/slam/dataset.h>
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);

gtsam/geometry/Rot3.cpp

@@ -81,6 +81,17 @@ Sphere2 Rot3::rotate(const Sphere2& p,
   return q;
 }
 
+/* ************************************************************************* */
+Sphere2 Rot3::unrotate(const Sphere2& p,
+    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
+  Sphere2 q = unrotate(p.point3(Hp));
+  if (Hp)
+    (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
+  if (HR)
+    (*HR) = q.basis().transpose() * q.skew();
+  return q;
+}
+
 /* ************************************************************************* */
 Sphere2 Rot3::operator*(const Sphere2& p) const {
   return rotate(p);

gtsam/geometry/Rot3.h

@@ -331,6 +331,10 @@ namespace gtsam {
     Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
         boost::optional<Matrix&> Hp = boost::none) const;
 
+    /// unrotate 3D direction from world frame to rotated coordinate frame
+    Sphere2 unrotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
+        boost::optional<Matrix&> Hp = boost::none) const;
+
     /// rotate 3D direction from rotated coordinate frame to world frame
     Sphere2 operator*(const Sphere2& p) const;
 

gtsam/geometry/Rot3Q.cpp

@@ -62,11 +62,6 @@ namespace gtsam {
   /* ************************************************************************* */
   Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
 
-  /* ************************************************************************* */
-  void Rot3::print(const std::string& s) const {
-    gtsam::print((Matrix)matrix(), s);
-  }
-
   /* ************************************************************************* */
   Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
 

gtsam/geometry/Sphere2.cpp

@@ -14,6 +14,7 @@
  * @date Feb 02, 2011
  * @author Can Erdogan
  * @author Frank Dellaert
+ * @author Alex Trevor
  * @brief The Sphere2 class - basically a point on a unit sphere
  */
 
@@ -113,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
 }
 
 /* ************************************************************************* */
-Sphere2 Sphere2::retract(const Vector& v) const {
+Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
 
   // Get the vector form of the point and the basis matrix
   Vector p = Point3::Logmap(p_);
@@ -121,18 +122,54 @@ Sphere2 Sphere2::retract(const Vector& v) const {
 
   // Compute the 3D xi_hat vector
   Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
-  Vector newPoint = p + xi_hat;
   
+  if (mode == Sphere2::EXPMAP) {
+    double xi_hat_norm = xi_hat.norm();
+
+    // Avoid nan
+    if (xi_hat_norm == 0.0) {
+      if (v.norm () == 0.0)
+        return Sphere2 (point3 ());
+      else
+        return Sphere2 (-point3 ());
+    }
+    
+    Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
+    return Sphere2(exp_p_xi_hat);
+  } else if (mode == Sphere2::RENORM) {
     // Project onto the manifold, i.e. the closest point on the circle to the new location;
     // same as putting it onto the unit circle
+    Vector newPoint = p + xi_hat;
     Vector projected = newPoint / newPoint.norm();
 
     return Sphere2(Point3::Expmap(projected));
+  } else {
+    assert (false);
+    exit (1);
+  }  
 }
 
 /* ************************************************************************* */
-Vector Sphere2::localCoordinates(const Sphere2& y) const {
+  Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const {
 
+  if (mode == Sphere2::EXPMAP) {
+    Matrix B = basis();
+    
+    Vector p = Point3::Logmap(p_);
+    Vector q = Point3::Logmap(y.p_);
+    double theta = acos(p.transpose() * q);
+
+    // the below will be nan if theta == 0.0
+    if (p == q)
+      return (Vector (2) << 0, 0);
+    else if (p == (Vector)-q)
+      return (Vector (2) << M_PI, 0);
+    
+    Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
+    Vector result = B.transpose() * result_hat;
+    
+    return result;
+  } else if (mode == Sphere2::RENORM) {
     // Make sure that the angle different between x and y is less than 90. Otherwise,
     // we can project x + xi_hat from the tangent space at x to y.
     assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
@@ -150,6 +187,10 @@ Vector Sphere2::localCoordinates(const Sphere2& y) const {
     Matrix coeffs = (B.transpose() * q) / alpha;
     Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
     return result;
+  } else {
+    assert (false);
+    exit (1);
+  }
 }
 /* ************************************************************************* */
 

gtsam/geometry/Sphere2.h

@@ -14,6 +14,7 @@
  * @date Feb 02, 2011
  * @author Can Erdogan
  * @author Frank Dellaert
+ * @author Alex Trevor
  * @brief Develop a Sphere2 class - basically a point on a unit sphere
  */
 
@@ -22,6 +23,10 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/DerivedValue.h>
 
+#ifndef SPHERE2_DEFAULT_COORDINATES_MODE
+  #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM
+#endif
+
 // (Cumbersome) forward declaration for random generator
 namespace boost {
 namespace random {
@@ -106,6 +111,13 @@ public:
     return p_;
   }
 
+  /// Return unit-norm Vector
+  Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
+    if (H)
+      *H = basis();
+    return (p_.vector ());
+  }
+
   /// Signed, vector-valued error between two directions
   Vector error(const Sphere2& q,
       boost::optional<Matrix&> H = boost::none) const;
@@ -129,11 +141,16 @@ public:
     return 2;
   }
 
+  enum CoordinatesMode {
+    EXPMAP, ///< Use the exponential map to retract
+    RENORM ///< Retract with vector addtion and renormalize.
+  };
+
   /// The retract function
-  Sphere2 retract(const Vector& v) const;
+  Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
 
   /// The local coordinates function
-  Vector localCoordinates(const Sphere2& s) const;
+  Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
 
   /// @}
 };

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -89,10 +89,7 @@ TEST (EssentialMatrix, rotate) {
 
   // Derivatives
   Matrix actH1, actH2;
-  try {
-    bodyE.rotate(cRb, actH1, actH2);
-  } catch (exception e) {
-  } // avoid exception
+  try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
   Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
   expH2 = numericalDerivative22(rotate_, bodyE, cRb);
   EXPECT(assert_equal(expH1, actH1, 1e-8));

gtsam/geometry/tests/testSphere2.cpp

@@ -13,6 +13,8 @@
  * @file testSphere2.cpp
  * @date Feb 03, 2012
  * @author Can Erdogan
+ * @author Frank Dellaert
+ * @author Alex Trevor
  * @brief Tests the Sphere2 class
  */
 
@@ -76,10 +78,35 @@ TEST(Sphere2, rotate) {
   }
 }
 
+//*******************************************************************************
+static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
+  return R.unrotate (p);
+}
+
+TEST(Sphere2, unrotate) {
+  Rot3 R = Rot3::yaw(-M_PI/4.0);
+  Sphere2 p(1, 0, 0);
+  Sphere2 expected = Sphere2(1, 1, 0);
+  Sphere2 actual = R.unrotate (p);
+  EXPECT(assert_equal(expected, actual, 1e-8));
+  Matrix actualH, expectedH;
+  // Use numerical derivatives to calculate the expected Jacobian
+  {
+    expectedH = numericalDerivative21(unrotate_, R, p);
+    R.unrotate(p, actualH, boost::none);
+    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+  }
+  {
+    expectedH = numericalDerivative22(unrotate_, R, p);
+    R.unrotate(p, boost::none, actualH);
+    EXPECT(assert_equal(expectedH, actualH, 1e-9));
+  }
+}
+
 //*******************************************************************************
 TEST(Sphere2, error) {
-  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
-  r = p.retract((Vector(2) << 0.8, 0));
+  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
+  r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
   EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
   EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
   EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
@@ -102,8 +129,8 @@ TEST(Sphere2, error) {
 
 //*******************************************************************************
 TEST(Sphere2, distance) {
-  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
-  r = p.retract((Vector(2) << 0.8, 0));
+  Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
+  r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
   EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
   EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
   EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
@@ -147,9 +174,20 @@ TEST(Sphere2, retract) {
   Vector v(2);
   v << 0.5, 0;
   Sphere2 expected(Point3(1, 0, 0.5));
-  Sphere2 actual = p.retract(v);
+  Sphere2 actual = p.retract(v, Sphere2::RENORM);
   EXPECT(assert_equal(expected, actual, 1e-8));
-  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
+  EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8));
+}
+
+//*******************************************************************************
+TEST(Sphere2, retract_expmap) {
+  Sphere2 p;
+  Vector v(2);
+  v << (M_PI/2.0), 0;
+  Sphere2 expected(Point3(0, 0, 1));
+  Sphere2 actual = p.retract(v, Sphere2::EXPMAP);
+  EXPECT(assert_equal(expected, actual, 1e-8));
+  EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8));
 }
 
 //*******************************************************************************
@@ -199,6 +237,39 @@ TEST(Sphere2, localCoordinates_retract) {
   }
 }
 
+//*******************************************************************************
+// Let x and y be two Sphere2's.
+// The equality x.localCoordinates(x.retract(v)) == v should hold.
+TEST(Sphere2, localCoordinates_retract_expmap) {
+  
+  size_t numIterations = 10000;
+  Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
+      Vector_(3, 1.0, 1.0, 1.0);
+  Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI);
+  for (size_t i = 0; i < numIterations; i++) {
+
+    // Sleep for the random number generator (TODO?: Better create all of them first).
+    sleep(0);
+
+    // Create the two Sphere2s.
+    // Unlike the above case, we can use any two sphers.
+    Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
+//      Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
+    Vector v12 = randomVector(minXiLimit, maxXiLimit);
+    
+    // Magnitude of the rotation can be at most pi
+    if (v12.norm () > M_PI)
+      v12 = v12 / M_PI;
+    Sphere2 s2 = s1.retract(v12);
+
+    // Check if the local coordinates and retract return the same results.
+    Vector actual_v12 = s1.localCoordinates(s2);
+    EXPECT(assert_equal(v12, actual_v12, 1e-3));
+    Sphere2 actual_s2 = s1.retract(actual_v12);
+    EXPECT(assert_equal(s2, actual_s2, 1e-3));
+  }
+}
+
 //*******************************************************************************
 //TEST( Pose2, between )
 //{

matlab/+gtsam/EXPECT.m

@@ -0,0 +1,10 @@
+function EXPECT(name,assertion)
+% EXPECT throw a warning if an assertion fails
+%
+% EXPECT(name,assertion)
+%  - name of test
+%  - assertion
+
+if (assertion~=1)
+    warning(['EXPECT ' name ' fails']);
+end

matlab/+gtsam/covarianceEllipse.m

@@ -1,16 +1,17 @@
-function covarianceEllipse(x,P,color)
+function covarianceEllipse(x,P,color, k)
 % covarianceEllipse plots a Gaussian as an uncertainty ellipse
 % Based on Maybeck Vol 1, page 366
 % k=2.296 corresponds to 1 std, 68.26% of all probability
 % k=11.82 corresponds to 3 std, 99.74% of all probability
 %
-% covarianceEllipse(x,P,color)
+% covarianceEllipse(x,P,color,k)
 % it is assumed x and y are the first two components of state x
+% k is scaling for std deviations, defaults to 1 std
 
 [e,s] = eig(P(1:2,1:2));
 s1 = s(1,1);
 s2 = s(2,2);
-k = 2.296;
+if nargin<4, k = 2.296; end;
 [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
 line(ex,ey,'color',color);
 

matlab/gtsam_examples/MonocularVOExample.m

@@ -0,0 +1,56 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010-2013, 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
+%
+% @brief Monocular VO Example with 5 landmarks and two cameras
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% import
+import gtsam.*
+
+%% Create two cameras and corresponding essential matrix E
+aRb = Rot3.Yaw(pi/2);
+aTb = Point3 (0.1, 0, 0);
+identity=Pose3;
+aPb = Pose3(aRb, aTb);
+cameraA = CalibratedCamera(identity);
+cameraB = CalibratedCamera(aPb);
+
+%% Create 5 points
+P = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
+
+%% Project points in both cameras
+for i=1:5
+  pA{i}= cameraA.project(P{i});
+  pB{i}= cameraB.project(P{i});
+end
+
+%% We start with a factor graph and add epipolar constraints to it
+% Noise sigma is 1cm, assuming metric measurements
+graph = NonlinearFactorGraph;
+model = noiseModel.Isotropic.Sigma(1,0.01);
+for i=1:5
+  graph.add(EssentialMatrixFactor(1, pA{i}, pB{i}, model));
+end
+
+%% Create initial estimate
+initial = Values;
+trueE = EssentialMatrix(aRb,Sphere2(aTb));
+initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]');
+initial.insert(1, initialE);
+
+%% Optimize
+parameters = LevenbergMarquardtParams;
+% parameters.setVerbosity('ERROR');
+optimizer = LevenbergMarquardtOptimizer(graph, initial, parameters);
+result = optimizer.optimize();
+
+%% Print result (as essentialMatrix) and error
+error = graph.error(result)
+E = result.at(1)
+

matlab/gtsam_examples/Pose2SLAMExample_graph.m

@@ -13,7 +13,7 @@
 import gtsam.*
 
 %% Find data file
-datafile = findExampleDataFile('w100-odom.graph');
+datafile = findExampleDataFile('w100.graph');
 
 %% Initialize graph, initial estimate, and odometry noise
 model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);

matlab/gtsam_examples/StereoVOExample.m

@@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose));
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
 K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
-stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
+stereo_model = noiseModel.Isotropic.Sigma(3,1);
 
 %% Add measurements
 % pose 1