12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged from branch 'trunk'

release/4.3a0
Richard Roberts 2012-07-06 18:37:55 +00:00
parent b460f2a721 59c6234d4f
commit da5c924d58
36 changed files with 859 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.settings/org.eclipse.cdt.core.prefs
View File

@ -1,6 +1,16 @@
#Wed Oct 06 11:57:41 EDT 2010
eclipse.preferences.version=1
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/delimiter=\:
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/operation=append
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/value=/home/ydjian/matlab/R2012a/bin
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/appendContributed=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/delimiter=\:
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/operation=append
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/value=/home/ydjian/matlab/R2012a/bin
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/appendContributed=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\:
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=/home/ydjian/matlab/R2012a/bin
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true

1
CMakeLists.txt
View File

@ -50,7 +50,6 @@ else()
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON)
endif()
option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON)
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" OFF)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
if(MSVC)
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF)

122
gtsam.h
View File

@ -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();
@ -1011,8 +1010,25 @@ class Marginals {
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
class LevenbergMarquardtParams {
LevenbergMarquardtParams();
LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose);
void print(string s) const;
double getMaxIterations() const;
double getRelativeErrorTol() const;
double getAbsoluteErrorTol() const;
double getErrorTol() const;
string getVerbosity() const;
void setMaxIterations(double value);
void setRelativeErrorTol(double value);
void setAbsoluteErrorTol(double value);
void setErrorTol(double value);
void setVerbosity(string s);
bool isMultifrontal() const;
bool isSequential() const;
bool isCholmod() const;
bool isCG() const;
double getlambdaInitial() const ;
double getlambdaFactor() const ;
double getlambdaUpperBound() const;
@ -1035,6 +1051,7 @@ namespace pose2SLAM {
class Values {
Values();
Values(const pose2SLAM::Values& values);
size_t size() const;
void print(string s) const;
bool exists(size_t key);
@ -1088,6 +1105,7 @@ namespace pose3SLAM {
class Values {
Values();
Values(const pose3SLAM::Values& values);
size_t size() const;
void print(string s) const;
bool exists(size_t key);
@ -1141,6 +1159,7 @@ namespace planarSLAM {
class Values {
Values();
Values(const planarSLAM::Values& values);
size_t size() const;
void print(string s) const;
bool exists(size_t key);
@ -1226,6 +1245,7 @@ namespace visualSLAM {
class Values {
Values();
Values(const visualSLAM::Values& values);
size_t size() const;
void print(string s) const;
bool exists(size_t key);
@ -1251,6 +1271,8 @@ class Values {
void insertPoint(size_t key, const gtsam::Point3& pose);
void updatePoint(size_t key, const gtsam::Point3& pose);
gtsam::Point3 point(size_t j);
void insertBackprojections(const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
void perturbPoints(double sigma, size_t seed);
Matrix points() const;
};
@ -1288,10 +1310,18 @@ class Graph {
void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model);
// Measurements
void addMeasurement(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
void addMeasurement(const gtsam::Point2& measured,
const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
const gtsam::Cal3_S2* K);
void addMeasurements(size_t i, Vector J, Matrix Z,
const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void addStereoMeasurement(const gtsam::StereoPoint2& measured,
const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
const gtsam::Cal3_S2Stereo* K);
// Information
Matrix reprojectionErrors(const visualSLAM::Values& values) const;
};
class ISAM {
@ -1327,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

6
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -96,18 +96,16 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
VectorValues newDeltaNewton(dims);
VectorValues newDeltaGradSearch(dims);
std::vector<bool> newReplacedKeys(replacedKeys.size() - unusedIndices.size());
Base::Nodes newNodes(nodes.size()); // We still keep unused keys at the end until later in ISAM2::recalculate
Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size());
for(size_t j = 0; j < dims.size(); ++j) {
newDelta[j] = delta[unusedToEnd[j]];
newDeltaNewton[j] = deltaNewton[unusedToEnd[j]];
newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]];
newReplacedKeys[j] = replacedKeys[unusedToEnd[j]];
newNodes[j] = nodes[unusedToEnd[j]];
}
// Permute the nodes index so the unused variables are the end
unusedToEnd.applyToCollection(newNodes, nodes);
// Swap the new data structures with the outputs of this function
delta.swap(newDelta);
deltaNewton.swap(newDeltaNewton);

96
gtsam/nonlinear/ISAM2.cpp
View File

@ -203,8 +203,8 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
return cachedBoundary;
}
boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys,
boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& markedKeys,
const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices,
const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
// TODO: new factors are linearized twice, the newFactors passed in are not used.
@ -249,9 +249,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
this->removeTop(markedKeys, affectedBayesNet, orphans);
toc(1, "removetop");
// Now that the top is removed, correct the size of the Nodes index
this->nodes_.resize(delta_.size());
if(debug) affectedBayesNet.print("Removed top: ");
if(debug) orphans.print("Orphans: ");
@ -269,14 +266,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// ordering provides all keys in conditionals, there cannot be others because path to root included
tic(2,"affectedKeys");
FastList<Index> affectedKeys = affectedBayesNet.ordering();
// The removed top also contained removed variables, which will be ordered
// higher than the number of variables in the system since unused variables
// were already removed in ISAM2::update.
for(FastList<Index>::iterator key = affectedKeys.begin(); key != affectedKeys.end(); )
if(*key >= delta_.size())
affectedKeys.erase(key++);
else
++key;
toc(2,"affectedKeys");
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // Will return this result
@ -329,11 +318,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
toc(1,"reorder");
tic(2,"linearize");
linearFactors_ = *nonlinearFactors_.linearize(theta_, ordering_);
GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_);
if(params_.cacheLinearizedFactors)
linearFactors_ = linearized;
toc(2,"linearize");
tic(5,"eliminate");
JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearFactors_, variableIndex_);
JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearized, variableIndex_);
sharedClique newRoot;
if(params_.factorization == ISAM2Params::CHOLESKY)
newRoot = jt.eliminate(EliminatePreferCholesky);
@ -352,7 +343,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeysSet->size();
lastAffectedFactorCount = linearFactors_.size();
lastAffectedFactorCount = linearized.size();
// Reeliminated keys for detailed results
if(params_.enableDetailedResults) {
@ -428,8 +419,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
reorderingMode.constrainedKeys = FastMap<Index,int>();
BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
}
FastSet<Index> affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve
BOOST_FOREACH(Index unused, unusedIndices) {
affectedUsedKeys.erase(unused);
}
Impl::PartialSolveResult partialSolveResult =
Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR));
Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR));
toc(2,"PartialSolve");
// We now need to permute everything according this partial reordering: the
@ -557,33 +552,25 @@ ISAM2Result ISAM2::update(
// Remove removed factors from the variable index so we do not attempt to relinearize them
variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
// We now need to start keeping track of the marked keys involved in added or
// removed factors.
FastSet<Index> markedKeys;
// Remove unused keys and add keys from removed factors that are still used
// in other factors to markedKeys.
// Compute unused keys and indices
FastSet<Key> unusedKeys;
FastSet<Index> unusedIndices;
{
// Get keys from removed factors
FastSet<Key> removedFactorSymbKeys = removeFactors.keys();
// For each key, if still used in other factors, add to markedKeys to be
// recalculated, or if not used, add to unusedKeys to be removed from the
// system. Note that unusedKeys stores Key while markedKeys stores Index.
FastSet<Key> unusedKeys;
BOOST_FOREACH(Key key, removedFactorSymbKeys) {
Index index = ordering_[key];
if(variableIndex_[index].empty())
unusedKeys.insert(key);
else
markedKeys.insert(index);
// Get keys from removed factors and new factors, and compute unused keys,
// i.e., keys that are empty now and do not appear in the new factors.
FastSet<Key> removedAndEmpty;
BOOST_FOREACH(Key key, removeFactors.keys()) {
if(variableIndex_[ordering_[key]].empty())
removedAndEmpty.insert(removedAndEmpty.end(), key);
}
FastSet<Key> newFactorSymbKeys = newFactors.keys();
std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
// Remove unused keys. We must hold on to the new nodes index for now
// instead of placing it into the tree because removeTop will need to
// update it.
Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
// Get indices for unused keys
BOOST_FOREACH(Key key, unusedKeys) {
unusedIndices.insert(unusedIndices.end(), ordering_[key]);
}
}
toc(1,"push_back factors");
@ -603,10 +590,12 @@ ISAM2Result ISAM2::update(
tic(4,"gather involved keys");
// 3. Mark linear update
{
FastSet<Index> newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end());
}
FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
// Also mark keys involved in removed factors
{
FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
}
// Observed keys for detailed results
if(params_.enableDetailedResults) {
@ -617,7 +606,11 @@ ISAM2Result ISAM2::update(
// NOTE: we use assign instead of the iterator constructor here because this
// is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Index value) instead of the iterator constructor.
FastVector<Index> observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
FastVector<Index> observedKeys; observedKeys.reserve(markedKeys.size());
BOOST_FOREACH(Index index, markedKeys) {
if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused
observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them
}
toc(4,"gather involved keys");
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
@ -710,7 +703,7 @@ ISAM2Result ISAM2::update(
}
boost::shared_ptr<FastSet<Index> > replacedKeys;
if(!markedKeys.empty() || !observedKeys.empty())
replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result);
replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedIndices, result);
// Update replaced keys mask (accumulates until back-substitution takes place)
if(replacedKeys) {
@ -718,10 +711,11 @@ ISAM2Result ISAM2::update(
deltaReplacedMask_[var] = true; } }
toc(9,"recalculate");
//tic(9,"solve");
// 9. Solve
if(debug) delta_.print("delta_: ");
//toc(9,"solve");
// After the top of the tree has been redone and may have index gaps from
// unused keys, condense the indices to remove gaps by rearranging indices
// in all data structures.
Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
result.cliques = this->nodes().size();
deltaDoglegUptodate_ = false;

2
gtsam/nonlinear/ISAM2.h
View File

@ -516,7 +516,7 @@ private:
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
const FastVector<Index>& observedKeys, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
// void linear_update(const GaussianFactorGraph& newFactors);
void updateDelta(bool forceFullSolve = false) const;

8
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -46,13 +46,9 @@ public:
double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5)
VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity
LevenbergMarquardtParams() :
lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {}
LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose) :
lambdaInitial(initial), lambdaFactor(factor), lambdaUpperBound(bound), verbosityLM(VerbosityLM(verbose)) {}
LevenbergMarquardtParams() : lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {}
virtual ~LevenbergMarquardtParams() {}
virtual void print(const std::string& str = "") const;
inline double getlambdaInitial() const { return lambdaInitial; }

4
gtsam/slam/PriorFactor.h
View File

@ -65,8 +65,8 @@ namespace gtsam {
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "PriorFactor(" << keyFormatter(this->key()) << ")\n";
prior_.print(" prior");
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << ", prior value:\n";
prior_.print();
this->noiseModel_->print(" noise model: ");
}

60
gtsam/slam/sparseBA.cpp Normal file
View File

@ -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;
}
/* ************************************************************************* */
}

143
gtsam/slam/sparseBA.h Normal file
View File

@ -0,0 +1,143 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file sba.h
* @brief a suite for sparse bundle adjustment
* @date Jul 5, 2012
* @author ydjian
*/
#pragma once
#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
*/
struct Graph: public visualSLAM::Graph {
/// Default constructor
Graph(){}
/// 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
* @param model the noise model for the measurement
* @param cameraKey variable key for the pose+calibration
* @param pointKey variable key for the point
*/
template <typename Camera>
void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) {
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
boost::shared_ptr<SFMFactor> factor(new SFMFactor(z, model, cameraKey, pointKey));
push_back(factor);
}
/**
* 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
* @param pointKey variable key for the point
* @param calibKey variable key for the calibration
*/
template <typename Calib>
void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) {
typedef GeneralSFMFactor2<Calib> SFMFactor;
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);
};
}

193
gtsam/slam/tests/testSparseBA.cpp Normal file
View File

@ -0,0 +1,193 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file testsparseBA.cpp
* @brief
* @date Jul 5, 2012
* @author Yong-Dian Jian
*/
#include <gtsam/slam/sparseBA.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
using namespace std;
using namespace boost;
using namespace gtsam;
/* ************************************************************************* */
static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
// Convenience for named keys
using symbol_shorthand::X; /* pose3 */
using symbol_shorthand::K; /* calibration */
using symbol_shorthand::C; /* camera = [pose calibration] */
using symbol_shorthand::L; /* point3 */
static Point3 landmark1(-1.0,-1.0, 0.0);
static Point3 landmark2(-1.0, 1.0, 0.0);
static Point3 landmark3( 1.0, 1.0, 0.0);
static Point3 landmark4( 1.0,-1.0, 0.0);
static Pose3 pose1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.),
Point3(0,0,6.25));
static Pose3 pose2(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.),
Point3(0,0,5.00));
static Cal3_S2 calib1 (625, 625, 0, 0, 0);
static Cal3_S2 calib2 (625, 625, 0, 0, 0);
typedef PinholeCamera<Cal3_S2> Camera;
static Camera camera1(pose1, calib1);
static Camera camera2(pose2, calib2);
/* ************************************************************************* */
sparseBA::Graph testGraph1() {
Point2 z11(-100, 100);
Point2 z12(-100,-100);
Point2 z13( 100,-100);
Point2 z14( 100, 100);
Point2 z21(-125, 125);
Point2 z22(-125,-125);
Point2 z23( 125,-125);
Point2 z24( 125, 125);
sparseBA::Graph g;
g.addMeasurement<Camera>(z11, sigma, C(1), L(1));
g.addMeasurement<Camera>(z12, sigma, C(1), L(2));
g.addMeasurement<Camera>(z13, sigma, C(1), L(3));
g.addMeasurement<Camera>(z14, sigma, C(1), L(4));
g.addMeasurement<Camera>(z21, sigma, C(2), L(1));
g.addMeasurement<Camera>(z22, sigma, C(2), L(2));
g.addMeasurement<Camera>(z23, sigma, C(2), L(3));
g.addMeasurement<Camera>(z24, sigma, C(2), L(4));
return g;
}
sparseBA::Graph testGraph2() {
Point2 z11(-100, 100);
Point2 z12(-100,-100);
Point2 z13( 100,-100);
Point2 z14( 100, 100);
Point2 z21(-125, 125);
Point2 z22(-125,-125);
Point2 z23( 125,-125);
Point2 z24( 125, 125);
sparseBA::Graph g;
g.addMeasurement<Cal3_S2>(z11, sigma, X(1), L(1), K(1));
g.addMeasurement<Cal3_S2>(z12, sigma, X(1), L(2), K(1));
g.addMeasurement<Cal3_S2>(z13, sigma, X(1), L(3), K(1));
g.addMeasurement<Cal3_S2>(z14, sigma, X(1), L(4), K(1));
g.addMeasurement<Cal3_S2>(z21, sigma, X(2), L(1), K(1));
g.addMeasurement<Cal3_S2>(z22, sigma, X(2), L(2), K(1));
g.addMeasurement<Cal3_S2>(z23, sigma, X(2), L(3), K(1));
g.addMeasurement<Cal3_S2>(z24, sigma, X(2), L(4), K(1));
return g;
}
/* ************************************************************************* */
TEST( optimizeLM1, sparseBA )
{
// build a graph
sparseBA::Graph graph(testGraph1());
// add 3 landmark constraints
graph.addPointConstraint(L(1), landmark1);
graph.addPointConstraint(L(2), landmark2);
graph.addPointConstraint(L(3), landmark3);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(C(1), camera1);
initialEstimate.insert(C(2), camera2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
// Create an ordering of the variables
Ordering ordering;
ordering += L(1),L(2),L(3),L(4),C(1),C(2);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */
TEST( optimizeLM2, sparseBA )
{
// build a graph
sparseBA::Graph graph(testGraph2());
// add 3 landmark constraints
graph.addPointConstraint(L(1), landmark1);
graph.addPointConstraint(L(2), landmark2);
graph.addPointConstraint(L(3), landmark3);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
initialEstimate.insert(K(1), calib2);
// Create an ordering of the variables
Ordering ordering;
ordering += L(1),L(2),L(3),L(4),X(1),X(2),K(1);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

76
gtsam/slam/tests/testVisualSLAM.cpp
View File

@ -42,14 +42,14 @@ static Point3 landmark2(-1.0, 1.0, 0.0);
static Point3 landmark3( 1.0, 1.0, 0.0);
static Point3 landmark4( 1.0,-1.0, 0.0);
static Pose3 camera1(Matrix_(3,3,
static Pose3 pose1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,6.25));
static Pose3 camera2(Matrix_(3,3,
static Pose3 pose2(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
@ -92,8 +92,8 @@ TEST( VisualSLAM, optimizeLM)
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), camera1);
initialEstimate.insert(X(2), camera2);
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
@ -124,13 +124,13 @@ TEST( VisualSLAM, optimizeLM2)
// build a graph
visualSLAM::Graph graph(testGraph());
// add 2 camera constraints
graph.addPoseConstraint(X(1), camera1);
graph.addPoseConstraint(X(2), camera2);
graph.addPoseConstraint(X(1), pose1);
graph.addPoseConstraint(X(2), pose2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), camera1);
initialEstimate.insert(X(2), camera2);
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
@ -160,13 +160,13 @@ TEST( VisualSLAM, LMoptimizer)
// build a graph
visualSLAM::Graph graph(testGraph());
// add 2 camera constraints
graph.addPoseConstraint(X(1), camera1);
graph.addPoseConstraint(X(2), camera2);
graph.addPoseConstraint(X(1), pose1);
graph.addPoseConstraint(X(2), pose2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), camera1);
initialEstimate.insert(X(2), camera2);
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
@ -184,6 +184,10 @@ TEST( VisualSLAM, LMoptimizer)
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
// check errors
Matrix errors = graph.reprojectionErrors(optimizer.values());
CHECK(assert_equal(zeros(2,8), errors));
}
@ -193,13 +197,13 @@ TEST( VisualSLAM, CHECK_ORDERING)
// build a graph
visualSLAM::Graph graph = testGraph();
// add 2 camera constraints
graph.addPoseConstraint(X(1), camera1);
graph.addPoseConstraint(X(2), camera2);
graph.addPoseConstraint(X(1), pose1);
graph.addPoseConstraint(X(2), pose2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), camera1);
initialEstimate.insert(X(2), camera2);
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
@ -274,8 +278,8 @@ TEST( VisualSLAM, keys_and_view )
{
// create config
visualSLAM::Values c;
c.insert(X(1), camera1);
c.insert(X(2), camera2);
c.insert(X(1), pose1);
c.insert(X(2), pose2);
c.insert(L(2), landmark2);
LONGS_EQUAL(2,c.nrPoses());
LONGS_EQUAL(1,c.nrPoints());
@ -299,6 +303,42 @@ TEST( VisualSLAM, keys_and_view )
}
}
/* ************************************************************************* */
TEST( VisualSLAM, addMeasurements )
{
// create config
visualSLAM::Graph g;
Vector J = Vector_(3,1.0,2.0,3.0);
Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
g.addMeasurements(0,J,Z,sigma,sK);
EXPECT_LONGS_EQUAL(3,g.size());
}
/* ************************************************************************* */
TEST( VisualSLAM, insertBackProjections )
{
// create config
visualSLAM::Values c;
SimpleCamera camera(pose1);
Vector J = Vector_(3,1.0,2.0,3.0);
Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
c.insertBackprojections(camera,J,Z,1.0);
EXPECT_LONGS_EQUAL(3,c.nrPoints());
}
/* ************************************************************************* */
TEST( VisualSLAM, perturbPoints )
{
visualSLAM::Values c1,c2;
c1.insert(L(2), landmark2);
c1.perturbPoints(0.01,42u);
CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c1.point(L(2)),1e-6));
c2.insert(L(2), landmark2);
c2.perturbPoints(0.01,42u);
CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c2.point(L(2)),1e-6));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

57
gtsam/slam/visualSLAM.cpp
View File

@ -18,12 +18,36 @@
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/Sampler.h>
#include <boost/make_shared.hpp>
using boost::make_shared;
namespace visualSLAM {
using boost::make_shared;
/* ************************************************************************* */
void Values::insertBackprojections(const SimpleCamera& camera,
const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument(
"insertBackProjections: J and Z must have same number of entries");
for(size_t k=0;k<Z.cols();k++) {
Point2 p(Z(0,k),Z(1,k));
Point3 P = camera.backproject(p, depth);
insertPoint(J(k), P);
}
}
/* ************************************************************************* */
void Values::perturbPoints(double sigma, int32_t seed) {
ConstFiltered<Point3> points = allPoints();
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points) {
update(keyValue.key, keyValue.value.retract(sampler.sample()));
}
}
/* ************************************************************************* */
Matrix Values::points() const {
size_t j=0;
@ -47,7 +71,19 @@ namespace visualSLAM {
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) {
push_back(make_shared<GenericProjectionFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
push_back(
make_shared<GenericProjectionFactor<Pose3, Point3> >
(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addMeasurements(Key i, const Vector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K) {
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries");
for (size_t k = 0; k < Z.cols(); k++)
addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K);
}
/* ************************************************************************* */
@ -62,4 +98,19 @@ namespace visualSLAM {
}
/* ************************************************************************* */
Matrix Graph::reprojectionErrors(const Values& values) const {
// first count
size_t K = 0, k=0;
BOOST_FOREACH(const sharedFactor& f, *this)
if (boost::dynamic_pointer_cast<const ProjectionFactor>(f)) ++K;
// now fill
Matrix errors(2,K);
BOOST_FOREACH(const sharedFactor& f, *this) {
boost::shared_ptr<const ProjectionFactor> p =
boost::dynamic_pointer_cast<const ProjectionFactor>(f);
if (p) errors.col(k++) = p->unwhitenedError(values);
}
return errors;
}
/* ************************************************************************* */
}

40
gtsam/slam/visualSLAM.h
View File

@ -68,7 +68,15 @@ namespace visualSLAM {
/// get a point
Point3 point(Key j) const { return at<Point3>(j); }
Matrix points() const; ///< get all point coordinates in a matrix
/// insert a number of initial point values by backprojecting
void insertBackprojections(const SimpleCamera& c, const Vector& J,
const Matrix& Z, double depth);
/// perturb all points using normally distributed noise
void perturbPoints(double sigma, int32_t seed = 42u);
/// get all point coordinates in a matrix
Matrix points() const;
};
@ -94,14 +102,14 @@ namespace visualSLAM {
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the landmark
* @param key variable key of the point
* @param p point around which soft prior is defined
*/
void addPointConstraint(Key pointKey, const Point3& p = Point3());
/**
* Add a prior on a landmark
* @param key variable key of the landmark
* Add a prior on a point
* @param key variable key of the point
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
@ -109,10 +117,10 @@ namespace visualSLAM {
const SharedNoiseModel& model = noiseModel::Unit::Create(3));
/**
* Add a range prior to a landmark
* Add a range prior to a point
* @param poseKey variable key of the camera pose
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param pointKey variable key of the point
* @param range approximate range to point
* @param model uncertainty model of this prior
*/
void addRangeFactor(Key poseKey, Key pointKey, double range,
@ -123,23 +131,37 @@ namespace visualSLAM {
* @param measured the measurement
* @param model the noise model for the measurement
* @param poseKey variable key for the camera pose
* @param pointKey variable key for the landmark
* @param pointKey variable key for the point
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K);
/**
* Add a number of measurements at the same time
* @param i variable key for the camera pose
* @param J variable keys for the point, KeyVector of size K
* @param Z the 2*K measurements as a matrix
* @param model the noise model for the measurement
* @param K shared pointer to calibration object
*/
void addMeasurements(Key i, const Vector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K);
/**
* Add a stereo factor measurement
* @param measured the measurement
* @param model the noise model for the measurement
* @param poseKey variable key for the camera pose
* @param pointKey variable key for the landmark
* @param pointKey variable key for the point
* @param K shared pointer to stereo calibration object
*/
void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo K);
/// Return a 2*K Matrix of reprojection errors
Matrix reprojectionErrors(const Values& values) const;
}; // Graph
/**

22
gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp
View File

@ -1,22 +0,0 @@
/**
* @file testLinearContainerFactors.cpp
*
* @brief Tests the use of nonlinear containers for simple integration of linear factors into nonlinear graphs
*
* @date Jun 7, 2012
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
/* ************************************************************************* */
TEST( testLinearContainerFactors, jacobian_factor ) {
// LinearContainerFactor actualLinFactor();
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

85
matlab/examples/SBAExample.m Normal file
View File

@ -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 '));

2
matlab/examples/SFMExample.m
View File

@ -63,7 +63,7 @@ initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Fine grain optimization, allowing user to iterate step by step
parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 0);
parameters = gtsamLevenbergMarquardtParams;
parameters.setlambdaInitial(1.0);
parameters.setVerbosityLM('trylambda');

2
matlab/tests/testSFMExample.m
View File

@ -48,7 +48,7 @@ for j=1:size(truth.points,2)
end
%% Optimization
parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 0);
parameters = gtsamLevenbergMarquardtParams;
optimizer = graph.optimizer(initialEstimate, parameters);
for i=1:5
optimizer.iterate();

4
wrap/tests/expected/@Point2/Point2.m
View File

@ -7,12 +7,12 @@ classdef Point2 < handle
function obj = Point2(varargin)
if (nargin == 0), obj.self = new_Point2(0,0); end
if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Point2(0,1,varargin{1},varargin{2}); end
if nargin ==14, new_Point2_(varargin{1},0); end
if nargin ==14, new_Point2(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Point2 constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_Point2_(obj.self);
new_Point2(obj.self);
obj.self = 0;
end
end

4
wrap/tests/expected/@Point3/Point3.m
View File

@ -6,12 +6,12 @@ classdef Point3 < handle
methods
function obj = Point3(varargin)
if (nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')), obj.self = new_Point3(0,0,varargin{1},varargin{2},varargin{3}); end
if nargin ==14, new_Point3_(varargin{1},0); end
if nargin ==14, new_Point3(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Point3 constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_Point3_(obj.self);
new_Point3(obj.self);
obj.self = 0;
end
end

4
wrap/tests/expected/@Test/Test.m
View File

@ -7,12 +7,12 @@ classdef Test < handle
function obj = Test(varargin)
if (nargin == 0), obj.self = new_Test(0,0); end
if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Test(0,1,varargin{1},varargin{2}); end
if nargin ==14, new_Test_(varargin{1},0); end
if nargin ==14, new_Test(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Test constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_Test_(obj.self);
new_Test(obj.self);
obj.self = 0;
end
end

6
wrap/tests/expected/new_Point2.cpp
View File

@ -6,8 +6,10 @@ typedef boost::shared_ptr<Point2> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

6
wrap/tests/expected/new_Point3.cpp
View File

@ -7,8 +7,10 @@ typedef boost::shared_ptr<Point3> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

6
wrap/tests/expected/new_Test.cpp
View File

@ -7,8 +7,10 @@ typedef boost::shared_ptr<Test> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

4
wrap/tests/expected_namespaces/@ClassD/ClassD.m
View File

@ -6,12 +6,12 @@ classdef ClassD < handle
methods
function obj = ClassD(varargin)
if (nargin == 0), obj.self = new_ClassD(0,0); end
if nargin ==14, new_ClassD_(varargin{1},0); end
if nargin ==14, new_ClassD(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ClassD constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_ClassD_(obj.self);
new_ClassD(obj.self);
obj.self = 0;
end
end

4
wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m
View File

@ -6,12 +6,12 @@ classdef ns1ClassA < handle
methods
function obj = ns1ClassA(varargin)
if (nargin == 0), obj.self = new_ns1ClassA(0,0); end
if nargin ==14, new_ns1ClassA_(varargin{1},0); end
if nargin ==14, new_ns1ClassA(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns1ClassA constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_ns1ClassA_(obj.self);
new_ns1ClassA(obj.self);
obj.self = 0;
end
end

4
wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m
View File

@ -6,12 +6,12 @@ classdef ns1ClassB < handle
methods
function obj = ns1ClassB(varargin)
if (nargin == 0), obj.self = new_ns1ClassB(0,0); end
if nargin ==14, new_ns1ClassB_(varargin{1},0); end
if nargin ==14, new_ns1ClassB(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns1ClassB constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_ns1ClassB_(obj.self);
new_ns1ClassB(obj.self);
obj.self = 0;
end
end

4
wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m
View File

@ -6,12 +6,12 @@ classdef ns2ClassA < handle
methods
function obj = ns2ClassA(varargin)
if (nargin == 0), obj.self = new_ns2ClassA(0,0); end
if nargin ==14, new_ns2ClassA_(varargin{1},0); end
if nargin ==14, new_ns2ClassA(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ClassA constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_ns2ClassA_(obj.self);
new_ns2ClassA(obj.self);
obj.self = 0;
end
end

4
wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m
View File

@ -6,12 +6,12 @@ classdef ns2ClassC < handle
methods
function obj = ns2ClassC(varargin)
if (nargin == 0), obj.self = new_ns2ClassC(0,0); end
if nargin ==14, new_ns2ClassC_(varargin{1},0); end
if nargin ==14, new_ns2ClassC(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ClassC constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_ns2ClassC_(obj.self);
new_ns2ClassC(obj.self);
obj.self = 0;
end
end

4
wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m
View File

@ -6,12 +6,12 @@ classdef ns2ns3ClassB < handle
methods
function obj = ns2ns3ClassB(varargin)
if (nargin == 0), obj.self = new_ns2ns3ClassB(0,0); end
if nargin ==14, new_ns2ns3ClassB_(varargin{1},0); end
if nargin ==14, new_ns2ns3ClassB(varargin{1},0); end
if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ns3ClassB constructor failed'); end
end
function delete(obj)
if obj.self ~= 0
new_ns2ns3ClassB_(obj.self);
new_ns2ns3ClassB(obj.self);
obj.self = 0;
end
end

6
wrap/tests/expected_namespaces/new_ClassD.cpp
View File

@ -6,8 +6,10 @@ typedef boost::shared_ptr<ClassD> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

6
wrap/tests/expected_namespaces/new_ns1ClassA.cpp
View File

@ -6,8 +6,10 @@ typedef boost::shared_ptr<ns1::ClassA> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

6
wrap/tests/expected_namespaces/new_ns1ClassB.cpp
View File

@ -7,8 +7,10 @@ typedef boost::shared_ptr<ns1::ClassB> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

6
wrap/tests/expected_namespaces/new_ns2ClassA.cpp
View File

@ -7,8 +7,10 @@ typedef boost::shared_ptr<ns2::ClassA> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

6
wrap/tests/expected_namespaces/new_ns2ClassC.cpp
View File

@ -6,8 +6,10 @@ typedef boost::shared_ptr<ns2::ClassC> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{

6
wrap/tests/expected_namespaces/new_ns2ns3ClassB.cpp
View File

@ -7,8 +7,10 @@ typedef boost::shared_ptr<ns2::ns3::ClassB> Shared;
static std::set<Shared*> collector;
void cleanup(void) {
BOOST_FOREACH(Shared* p, collector)
collector.erase(p);
for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
delete *iter;
collector.erase(iter++);
}
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{