MATLAB wrapping and Monocular VO example

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 getBasis() 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();
@@ -2180,6 +2223,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/EssentialMatrix.h

@@ -45,9 +45,31 @@ public:
 
   /// @}
 
-  /// @name Value
+  /// @name Testable
   /// @{
 
+  /// print with optional string
+  void print(const std::string& s = "") const {
+    std::cout << s;
+    aRb_.print("R:\n");
+    aTb_.print("d: ");
+  }
+
+  /// assert equality up to a tolerance
+  bool equals(const EssentialMatrix& other, double tol=1e-8) const {
+    return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
+  }
+
+  /// @}
+
+  /// @name Manifold
+  /// @{
+
+  /// Dimensionality of tangent space = 5 DOF
+  inline static size_t Dim() {
+    return 5;
+  }
+
   /// Return the dimensionality of the tangent space
   virtual size_t dim() const {
     return 5;
@@ -70,23 +92,6 @@ public:
 
   /// @}
 
-  /// @name Testable
-  /// @{
-
-  /// print with optional string
-  void print(const std::string& s) const {
-    std::cout << s;
-    aRb_.print("R:\n");
-    aTb_.print("d: ");
-  }
-
-  /// assert equality up to a tolerance
-  bool equals(const EssentialMatrix& other, double tol) const {
-    return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
-  }
-
-  /// @}
-
   /// @name Essential matrix methods
   /// @{
 
@@ -110,6 +115,7 @@ public:
       boost::optional<Matrix&> H = boost::none) const {
     if (H) {
       H->resize(1, 5);
+      // See math.lyx
       Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
       Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
           * aTb_.getBasis();

gtsam/geometry/Sphere2.cpp

@@ -13,12 +13,13 @@
  * @file Sphere2.h
  * @date Feb 02, 2011
  * @author Can Erdogan
+ * @author Frank Dellaert
  * @brief Develop a Sphere2 class - basically a point on a unit sphere
  */
 
 #include <gtsam/geometry/Sphere2.h>
 #include <gtsam/geometry/Point2.h>
-#include <cstdio>
+#include <iostream>
 
 using namespace std;
 
@@ -54,7 +55,7 @@ Matrix Sphere2::getBasis() const {
 /* ************************************************************************* */
 /// The print fuction
 void Sphere2::print(const std::string& s) const {
-  printf("(%.3lf, %.3lf, %.3lf)\n", s.c_str(), p_.x(), p_.y(), p_.z());
+  cout << s << ":" << p_ << endl;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Sphere2.h

@@ -13,17 +13,19 @@
  * @file Sphere2.h
  * @date Feb 02, 2011
  * @author Can Erdogan
+ * @author Frank Dellaert
  * @brief Develop a Sphere2 class - basically a point on a unit sphere
  */
 
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/base/DerivedValue.h>
 
 namespace gtsam {
 
 /// Represents a 3D point on a unit sphere.
-class Sphere2 {
+class Sphere2 : public DerivedValue<Sphere2> {
 
 private:
 

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -14,6 +14,9 @@
 using namespace std;
 using namespace gtsam;
 
+GTSAM_CONCEPT_TESTABLE_INST(EssentialMatrix)
+GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
+
 //*************************************************************************
 // Create two cameras and corresponding essential matrix E
 Rot3 aRb = Rot3::yaw(M_PI_2);

gtsam/geometry/tests/testSphere2.cpp

@@ -72,6 +72,46 @@ TEST(Sphere2, localCoordinates_retract) {
   }
 }
 
+///* ************************************************************************* */
+//TEST( Pose2, between )
+//{
+//  // <
+//  //
+//  //       ^
+//  //
+//  // *--0--*--*
+//  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
+//  Pose2 gT2(M_PI, Point2(-1,4));  // robot at (-1,4) loooking at negative x
+//
+//  Matrix actualH1,actualH2;
+//  Pose2 expected(M_PI/2.0, Point2(2,2));
+//  Pose2 actual1 = gT1.between(gT2);
+//  Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
+//  EXPECT(assert_equal(expected,actual1));
+//  EXPECT(assert_equal(expected,actual2));
+//
+//  Matrix expectedH1 = Matrix_(3,3,
+//      0.0,-1.0,-2.0,
+//      1.0, 0.0,-2.0,
+//      0.0, 0.0,-1.0
+//  );
+//  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
+//  EXPECT(assert_equal(expectedH1,actualH1));
+//  EXPECT(assert_equal(numericalH1,actualH1));
+//  // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
+//  EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
+//
+//  Matrix expectedH2 = Matrix_(3,3,
+//       1.0, 0.0, 0.0,
+//       0.0, 1.0, 0.0,
+//       0.0, 0.0, 1.0
+//  );
+//  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
+//  EXPECT(assert_equal(expectedH2,actualH2));
+//  EXPECT(assert_equal(numericalH2,actualH2));
+//
+//}
+//
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -42,7 +42,7 @@ it1 = std::transform(P, P + 5, pA.begin(),
 it2 = std::transform(P, P + 5, pB.begin(),
     boost::bind(&Cam::project, &cameraB, _1, boost::none, boost::none));
 
-// Converto to homogenous coordinates
+// Convert to homogeneous coordinates
 vector<Vector> vA(5), vB(5);
 vector<Vector>::iterator //
 it3 = std::transform(pA.begin(), pA.end(), vA.begin(),

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/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