From 18fe7b17d81f67ffd527aeffb14f4d483e1cb8a5 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Fri, 6 Jul 2012 17:38:32 +0000 Subject: [PATCH] wrap sparseBA namespace for matlab and add an example --- gtsam.h | 81 +++++++++++++++++++++++++++++++++- gtsam/slam/sparseBA.cpp | 60 +++++++++++++++++++++++++ gtsam/slam/sparseBA.h | 75 ++++++++++++++++++++++++++++++- matlab/examples/SBAExample.m | 85 ++++++++++++++++++++++++++++++++++++ 4 files changed, 299 insertions(+), 2 deletions(-) create mode 100644 gtsam/slam/sparseBA.cpp create mode 100644 matlab/examples/SBAExample.m diff --git a/gtsam.h b/gtsam.h index 5564d38a9..48199cbf3 100644 --- a/gtsam.h +++ b/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 +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 + diff --git a/gtsam/slam/sparseBA.cpp b/gtsam/slam/sparseBA.cpp new file mode 100644 index 000000000..0ff4e1c64 --- /dev/null +++ b/gtsam/slam/sparseBA.cpp @@ -0,0 +1,60 @@ +/** + * @file sparseBA.cpp + * @brief + * @date Jul 6, 2012 + * @author Yong-Dian Jian + */ + +#include + +namespace sparseBA { + +/* ************************************************************************* */ +void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) { + addCameraConstraint(cameraKey, camera); +} + +/* ************************************************************************* */ +void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) { + addCameraPrior(cameraKey, camera, model); +} + +/* ************************************************************************* */ +void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) { + addMeasurement(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 SFMFactor; + typedef GeneralSFMFactor2 SFMFactor2; + + // first count + size_t K = 0, k=0; + BOOST_FOREACH(const sharedFactor& f, *this) + if (boost::dynamic_pointer_cast(f)) ++K; + else if (boost::dynamic_pointer_cast(f)) ++K; + + // now fill + Matrix errors(2,K); + BOOST_FOREACH(const sharedFactor& f, *this) { + boost::shared_ptr p = boost::dynamic_pointer_cast(f); + if (p) { + errors.col(k++) = p->unwhitenedError(values); + continue; + } + + boost::shared_ptr p2 = boost::dynamic_pointer_cast(f); + if (p2) { + errors.col(k++) = p2->unwhitenedError(values); + } + } + return errors; +} +/* ************************************************************************* */ +} + + diff --git a/gtsam/slam/sparseBA.h b/gtsam/slam/sparseBA.h index dda1ec44d..07aaa5817 100644 --- a/gtsam/slam/sparseBA.h +++ b/gtsam/slam/sparseBA.h @@ -20,11 +20,45 @@ #include #include +#include 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 shared_ptr; + typedef gtsam::Values::ConstFiltered SimpleCameraFiltered; + typedef gtsam::Values::ConstFiltered 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(); } ///< 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(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 + void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) { + sharedFactor factor(new PriorFactor(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 + void addCameraConstraint(Key cameraKey, const Camera &camera) { + sharedFactor factor(new NonlinearEquality(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 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); }; } diff --git a/matlab/examples/SBAExample.m b/matlab/examples/SBAExample.m new file mode 100644 index 000000000..9050d011d --- /dev/null +++ b/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 ')); + +