wrap sparseBA namespace for matlab and add an example
Yong-Dian Jian
parent
1abf81052f
commit
18fe7b17d8
81
gtsam.h
81
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 '));
|
||||
|
||||
|
||||
Loading…
Reference in New Issue