wrap sparseBA namespace for matlab and add an example

gtsam.h

@@ -458,7 +458,6 @@ class CalibratedCamera {
   double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
 };
 
-
 class SimpleCamera {
   // Standard Constructors and Named Constructors
 	SimpleCamera();
@@ -1358,3 +1357,83 @@ class LevenbergMarquardtOptimizer {
 };
 
 }///\namespace visualSLAM
+
+//************************************************************************
+// sparse BA
+//************************************************************************
+
+#include <gtsam/slam/sparseBA.h>
+namespace sparseBA {
+
+class Values {
+  Values();
+  Values(const sparseBA::Values& values);
+  size_t size() const;
+  void print(string s) const;
+  bool exists(size_t key);
+  gtsam::KeyVector keys() const;
+
+  // Access to cameras
+  sparseBA::Values allSimpleCameras() const ;
+  size_t  nrSimpleCameras() const ;
+  gtsam::KeyVector simpleCameraKeys() const ;
+  void insertSimpleCamera(size_t j, const gtsam::SimpleCamera& camera);
+  void updateSimpleCamera(size_t j, const gtsam::SimpleCamera& camera);
+  gtsam::SimpleCamera simpleCamera(size_t j) const;
+
+  // Access to points, inherited from visualSLAM
+  sparseBA::Values allPoints() const;
+  size_t nrPoints() const;
+  gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList
+  void insertPoint(size_t key, const gtsam::Point3& pose);
+  void updatePoint(size_t key, const gtsam::Point3& pose);
+  gtsam::Point3 point(size_t j);
+  Matrix points() const;
+};
+
+class Graph {
+  Graph();
+  Graph(const gtsam::NonlinearFactorGraph& graph);
+  Graph(const sparseBA::Graph& graph);
+
+  // Information
+  Matrix reprojectionErrors(const sparseBA::Values& values) const;
+
+  // inherited from FactorGraph
+  void print(string s) const;
+  bool equals(const sparseBA::Graph& fg, double tol) const;
+  size_t size() const;
+  bool empty() const;
+  void remove(size_t i);
+  size_t nrFactors() const;
+  gtsam::NonlinearFactor* at(size_t i) const;
+
+  double error(const sparseBA::Values& values) const;
+  gtsam::Ordering* orderingCOLAMD(const sparseBA::Values& values) const;
+  gtsam::GaussianFactorGraph* linearize(const sparseBA::Values& values, const gtsam::Ordering& ordering) const;
+
+  sparseBA::Values optimize(const sparseBA::Values& initialEstimate, size_t verbosity) const;
+  sparseBA::LevenbergMarquardtOptimizer optimizer(const sparseBA::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const;
+  gtsam::Marginals marginals(const sparseBA::Values& solution) const;
+
+  // inherited from visualSLAM
+  void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
+  void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model);
+
+  // add factors
+  void addSimpleCameraPrior(size_t cameraKey, const gtsam::SimpleCamera &camera, gtsam::noiseModel::Base* model);
+  void addSimpleCameraConstraint(size_t cameraKey, const gtsam::SimpleCamera &camera);
+  void addSimpleCameraMeasurement(const gtsam::Point2 &z, gtsam::noiseModel::Base* model, size_t cameraKey, size_t pointKey);
+};
+
+class LevenbergMarquardtOptimizer {
+  double lambda() const;
+  void iterate();
+  double error() const;
+  size_t iterations() const;
+  sparseBA::Values optimize();
+  sparseBA::Values optimizeSafely();
+  sparseBA::Values values() const;
+};
+}///\namespace sparseBA
+

gtsam/slam/sparseBA.cpp

@@ -0,0 +1,60 @@
+/**
+ * @file   sparseBA.cpp
+ * @brief  
+ * @date   Jul 6, 2012
+ * @author Yong-Dian Jian
+ */
+
+#include <gtsam/slam/sparseBA.h>
+
+namespace sparseBA {
+
+/* ************************************************************************* */
+void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) {
+  addCameraConstraint<SimpleCamera>(cameraKey, camera);
+}
+
+/* ************************************************************************* */
+void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) {
+  addCameraPrior<SimpleCamera>(cameraKey, camera, model);
+}
+
+/* ************************************************************************* */
+void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) {
+  addMeasurement<SimpleCamera>(z, model, cameraKey, pointKey);
+}
+
+/* ************************************************************************* */
+Matrix Graph::reprojectionErrors(const Values& values) const {
+
+  // TODO: support the other calibration objects. Now it only works for Cal3_S2.
+
+  typedef GeneralSFMFactor<SimpleCamera, Point3> SFMFactor;
+  typedef GeneralSFMFactor2<Cal3_S2> SFMFactor2;
+
+  // first count
+  size_t K = 0, k=0;
+  BOOST_FOREACH(const sharedFactor& f, *this)
+    if (boost::dynamic_pointer_cast<const SFMFactor>(f)) ++K;
+    else if (boost::dynamic_pointer_cast<const SFMFactor2>(f)) ++K;
+
+  // now fill
+  Matrix errors(2,K);
+  BOOST_FOREACH(const sharedFactor& f, *this) {
+    boost::shared_ptr<const SFMFactor> p = boost::dynamic_pointer_cast<const SFMFactor>(f);
+    if (p) {
+      errors.col(k++) = p->unwhitenedError(values);
+      continue;
+    }
+
+    boost::shared_ptr<const SFMFactor2> p2 = boost::dynamic_pointer_cast<const SFMFactor2>(f);
+    if (p2) {
+      errors.col(k++) = p2->unwhitenedError(values);
+    }
+  }
+  return errors;
+}
+/* ************************************************************************* */
+}
+
+

gtsam/slam/sparseBA.h

@@ -20,11 +20,45 @@
 
 #include <gtsam/slam/visualSLAM.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/geometry/SimpleCamera.h>
 
 namespace sparseBA {
 
   using namespace gtsam;
 
+  /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
+  struct Values: public visualSLAM::Values {
+
+    typedef boost::shared_ptr<Values> shared_ptr;
+    typedef gtsam::Values::ConstFiltered<SimpleCamera> SimpleCameraFiltered;
+    typedef gtsam::Values::ConstFiltered<Cal3_S2> Cal3_S2Filtered;
+
+    /// Default constructor
+    Values() {}
+
+    /// Copy constructor
+    Values(const gtsam::Values& values) : visualSLAM::Values(values) {}
+
+    /// Constructor from filtered values view of poses
+    Values(const SimpleCameraFiltered& view) : visualSLAM::Values(view) {}
+
+    /// Constructor from filtered values view of points
+    Values(const PointFiltered& view) : visualSLAM::Values(view) {}
+
+    SimpleCameraFiltered allSimpleCameras() const { return this->filter<SimpleCamera>(); }  ///< camera view
+    size_t  nrSimpleCameras()  const { return allSimpleCameras().size(); }                  ///< get number of poses
+    KeyList simpleCameraKeys() const { return allSimpleCameras().keys(); }                  ///< get keys to poses only
+
+    /// insert a camera
+    void insertSimpleCamera(Key j, const SimpleCamera& camera) { insert(j, camera); }
+
+    /// update a camera
+    void updateSimpleCamera(Key j, const SimpleCamera& camera) { update(j, camera); }
+
+    /// get a camera
+    SimpleCamera simpleCamera(Key j) const { return at<SimpleCamera>(j); }
+  };
+
   /**
    * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
    * @addtogroup SLAM
@@ -37,6 +71,34 @@ namespace sparseBA {
     /// Copy constructor given any other NonlinearFactorGraph
     Graph(const NonlinearFactorGraph& graph): visualSLAM::Graph(graph) {}
 
+    /// check if two graphs are equal
+    bool equals(const Graph& p, double tol = 1e-9) const {
+      return NonlinearFactorGraph::equals(p, tol);
+    }
+
+    /**
+      *  Add a prior on a pose
+      *  @param key variable key of the camera
+      *  @param p around which soft prior is defined
+      *  @param model uncertainty model of this prior
+      */
+    template <typename Camera>
+    void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) {
+      sharedFactor factor(new PriorFactor<Camera>(cameraKey, camera, model));
+      push_back(factor);
+    }
+
+    /**
+     *  Add a constraint on a camera
+     *  @param key variable key of the camera
+     *  @param p to which camera to constrain it to
+     */
+    template <typename Camera>
+    void addCameraConstraint(Key cameraKey, const Camera &camera) {
+      sharedFactor factor(new NonlinearEquality<Camera>(cameraKey, camera));
+      push_back(factor);
+    }
+
     /**
      *  Add a 2d projection measurement
      *  @param z the measurement
@@ -52,7 +114,7 @@ namespace sparseBA {
     }
 
     /**
-     *  Add a 2d projection measurement
+     *  Add a 2d projection measurement, but supports separated (or shared) pose and calibration object
      *  @param z the measurement
      *  @param model the noise model for the measurement
      *  @param poseKey variable key for the pose
@@ -65,6 +127,17 @@ namespace sparseBA {
       boost::shared_ptr<SFMFactor> factor(new SFMFactor(z, model, posekey, pointkey, calibkey));
       push_back(factor);
     }
+
+    /// Return a 2*K Matrix of reprojection errors
+    Matrix reprojectionErrors(const Values& values) const;
+
+    /**
+     * Matlab-specific wrappers
+     */
+
+    void addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model);
+    void addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) ;
+    void addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey);
   };
 
 }

matlab/examples/SBAExample.m

@@ -0,0 +1,85 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief An SFM example (adapted from SFMExample.m) optimizing calibration
+% @author Yong-Dian Jian
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Assumptions
+%  - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc...
+%  - Cameras are on a circle around the cube, pointing at the world origin
+%  - Each camera sees all landmarks. 
+%  - Visual measurements as 2D points are given, corrupted by Gaussian noise.
+
+% Data Options
+options.triangle = false;
+options.nrCameras = 10;
+options.showImages = false;
+
+%% Generate data
+[data,truth] = VisualISAMGenerateData(options);
+
+measurementNoiseSigma = 1.0;
+pointNoiseSigma = 0.1;
+cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ...
+                     0.001*ones(1,5)]';
+
+%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
+graph = sparseBAGraph;
+
+ 
+%% Add factors for all measurements
+measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma);
+for i=1:length(data.Z)
+    for k=1:length(data.Z{i})
+        j = data.J{i}{k};
+        graph.addSimpleCameraMeasurement(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j));
+    end
+end
+
+%% Add Gaussian priors for a pose and a landmark to constrain the system
+cameraPriorNoise  = gtsamnoiseModelDiagonal_Sigmas(cameraNoiseSigmas);
+firstCamera = gtsamSimpleCamera(truth.cameras{1}.pose, truth.K);
+graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise);
+
+pointPriorNoise  = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma);
+graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
+
+%% Print the graph
+graph.print(sprintf('\nFactor graph:\n'));
+
+
+%% Initialize cameras and points close to ground truth in this example
+initialEstimate = sparseBAValues;
+for i=1:size(truth.cameras,2)
+    pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
+    camera_i = gtsamSimpleCamera(pose_i, truth.K);
+    initialEstimate.insertSimpleCamera(symbol('c',i), camera_i);
+end
+for j=1:size(truth.points,2)
+    point_j = truth.points{j}.retract(0.1*randn(3,1));
+    initialEstimate.insertPoint(symbol('p',j), point_j);
+end
+initialEstimate.print(sprintf('\nInitial estimate:\n  '));
+
+%% Fine grain optimization, allowing user to iterate step by step
+
+parameters = gtsamLevenbergMarquardtParams;
+parameters.setlambdaInitial(1.0);
+parameters.setVerbosityLM('trylambda');
+
+optimizer = graph.optimizer(initialEstimate, parameters);
+
+for i=1:5
+    optimizer.iterate();
+end
+
+result = optimizer.values();
+result.print(sprintf('\nFinal result:\n  '));
+
+