diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs index a915be654..3278fdadc 100644 --- a/.settings/org.eclipse.cdt.core.prefs +++ b/.settings/org.eclipse.cdt.core.prefs @@ -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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 675e7c80b..77b8b0c18 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/gtsam.h b/gtsam.h index 4b74f120d..f5d245111 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(); @@ -1011,8 +1010,25 @@ class Marginals { #include 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 +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/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 5e71ee22b..f3b27b138 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -96,18 +96,16 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli VectorValues newDeltaNewton(dims); VectorValues newDeltaGradSearch(dims); std::vector 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); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index e47127edd..a9bd195ba 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -203,8 +203,8 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { return cachedBoundary; } -boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& observedKeys, +boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, + const FastSet& relinKeys, const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -249,9 +249,6 @@ boost::shared_ptr > 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 > ISAM2::recalculate( // ordering provides all keys in conditionals, there cannot be others because path to root included tic(2,"affectedKeys"); FastList 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::iterator key = affectedKeys.begin(); key != affectedKeys.end(); ) - if(*key >= delta_.size()) - affectedKeys.erase(key++); - else - ++key; toc(2,"affectedKeys"); boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result @@ -329,11 +318,13 @@ boost::shared_ptr > 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 jt(linearFactors_, variableIndex_); + JunctionTree jt(linearized, variableIndex_); sharedClique newRoot; if(params_.factorization == ISAM2Params::CHOLESKY) newRoot = jt.eliminate(EliminatePreferCholesky); @@ -352,7 +343,7 @@ boost::shared_ptr > 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 > ISAM2::recalculate( reorderingMode.constrainedKeys = FastMap(); BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } } + FastSet 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 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 unusedKeys; + FastSet unusedIndices; { - // Get keys from removed factors - FastSet 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 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 removedAndEmpty; + BOOST_FOREACH(Key key, removeFactors.keys()) { + if(variableIndex_[ordering_[key]].empty()) + removedAndEmpty.insert(removedAndEmpty.end(), key); } + FastSet 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 newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors - markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end()); - } + FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + // Also mark keys involved in removed factors + { + FastSet 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 observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them + FastVector 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 > 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; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 1fa893819..34fc7b46f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -516,7 +516,7 @@ private: GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result); + const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index f5e4195da..5677f6c08 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -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; } diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 094065336..0d0497132 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -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: "); } 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 new file mode 100644 index 000000000..07aaa5817 --- /dev/null +++ b/gtsam/slam/sparseBA.h @@ -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 +#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 + */ + 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 + 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 + * @param model the noise model for the measurement + * @param cameraKey variable key for the pose+calibration + * @param pointKey variable key for the point + */ + template + void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) { + typedef GeneralSFMFactor SFMFactor; + boost::shared_ptr 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 + void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) { + typedef GeneralSFMFactor2 SFMFactor; + 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/gtsam/slam/tests/testSparseBA.cpp b/gtsam/slam/tests/testSparseBA.cpp new file mode 100644 index 000000000..cc6e11400 --- /dev/null +++ b/gtsam/slam/tests/testSparseBA.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include + +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 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(z11, sigma, C(1), L(1)); + g.addMeasurement(z12, sigma, C(1), L(2)); + g.addMeasurement(z13, sigma, C(1), L(3)); + g.addMeasurement(z14, sigma, C(1), L(4)); + g.addMeasurement(z21, sigma, C(2), L(1)); + g.addMeasurement(z22, sigma, C(2), L(2)); + g.addMeasurement(z23, sigma, C(2), L(3)); + g.addMeasurement(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(z11, sigma, X(1), L(1), K(1)); + g.addMeasurement(z12, sigma, X(1), L(2), K(1)); + g.addMeasurement(z13, sigma, X(1), L(3), K(1)); + g.addMeasurement(z14, sigma, X(1), L(4), K(1)); + g.addMeasurement(z21, sigma, X(2), L(1), K(1)); + g.addMeasurement(z22, sigma, X(2), L(2), K(1)); + g.addMeasurement(z23, sigma, X(2), L(3), K(1)); + g.addMeasurement(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); } +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp index faf733a22..65c31adf9 100644 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ b/gtsam/slam/tests/testVisualSLAM.cpp @@ -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); } /* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 6f31c345c..5b04b92c6 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -18,12 +18,36 @@ #include #include #include +#include #include -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 points = allPoints(); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const ConstFiltered::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 >(measured, model, poseKey, pointKey, K)); + push_back( + make_shared > + (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(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); + } + return errors; + } + /* ************************************************************************* */ } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 7e670ccd4..ae2956c5f 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -68,7 +68,15 @@ namespace visualSLAM { /// get a point Point3 point(Key j) const { return at(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 /** diff --git a/gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp b/gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp deleted file mode 100644 index 05fabc66a..000000000 --- a/gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp +++ /dev/null @@ -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 - - -/* ************************************************************************* */ -TEST( testLinearContainerFactors, jacobian_factor ) { - -// LinearContainerFactor actualLinFactor(); - -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ 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 ')); + + diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index 34682e1e6..277f31648 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/examples/SFMExample.m @@ -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'); diff --git a/matlab/tests/testSFMExample.m b/matlab/tests/testSFMExample.m index ebda9535e..ef27a7e4b 100644 --- a/matlab/tests/testSFMExample.m +++ b/matlab/tests/testSFMExample.m @@ -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(); diff --git a/wrap/tests/expected/@Point2/Point2.m b/wrap/tests/expected/@Point2/Point2.m index 31d54bb19..5cfd7fc61 100644 --- a/wrap/tests/expected/@Point2/Point2.m +++ b/wrap/tests/expected/@Point2/Point2.m @@ -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 diff --git a/wrap/tests/expected/@Point3/Point3.m b/wrap/tests/expected/@Point3/Point3.m index 6dd8e9ee5..81bd00a5a 100644 --- a/wrap/tests/expected/@Point3/Point3.m +++ b/wrap/tests/expected/@Point3/Point3.m @@ -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 diff --git a/wrap/tests/expected/@Test/Test.m b/wrap/tests/expected/@Test/Test.m index 60628b997..63914d2aa 100644 --- a/wrap/tests/expected/@Test/Test.m +++ b/wrap/tests/expected/@Test/Test.m @@ -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 diff --git a/wrap/tests/expected/new_Point2.cpp b/wrap/tests/expected/new_Point2.cpp index 30a707f88..b24642ad4 100644 --- a/wrap/tests/expected/new_Point2.cpp +++ b/wrap/tests/expected/new_Point2.cpp @@ -6,8 +6,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected/new_Point3.cpp b/wrap/tests/expected/new_Point3.cpp index c3d758266..2e715263b 100644 --- a/wrap/tests/expected/new_Point3.cpp +++ b/wrap/tests/expected/new_Point3.cpp @@ -7,8 +7,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected/new_Test.cpp b/wrap/tests/expected/new_Test.cpp index 050d179d3..57ac1a4ea 100644 --- a/wrap/tests/expected/new_Test.cpp +++ b/wrap/tests/expected/new_Test.cpp @@ -7,8 +7,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected_namespaces/@ClassD/ClassD.m b/wrap/tests/expected_namespaces/@ClassD/ClassD.m index 282b761ed..90fbf48a4 100644 --- a/wrap/tests/expected_namespaces/@ClassD/ClassD.m +++ b/wrap/tests/expected_namespaces/@ClassD/ClassD.m @@ -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 diff --git a/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m index b8f1b3535..727e020ae 100644 --- a/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m +++ b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m @@ -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 diff --git a/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m index 42add5e9c..19b0a73b9 100644 --- a/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m +++ b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m @@ -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 diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m index cbbbbbce2..27de74dc3 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m +++ b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m @@ -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 diff --git a/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m index 5bd68fee7..2425b59bd 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m +++ b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m @@ -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 diff --git a/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m index 936c7cdd1..de7f7a3ca 100644 --- a/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m +++ b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m @@ -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 diff --git a/wrap/tests/expected_namespaces/new_ClassD.cpp b/wrap/tests/expected_namespaces/new_ClassD.cpp index 13bbe9d5e..d7a7e4908 100644 --- a/wrap/tests/expected_namespaces/new_ClassD.cpp +++ b/wrap/tests/expected_namespaces/new_ClassD.cpp @@ -6,8 +6,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA.cpp b/wrap/tests/expected_namespaces/new_ns1ClassA.cpp index fc60abe6d..2554eafc1 100644 --- a/wrap/tests/expected_namespaces/new_ns1ClassA.cpp +++ b/wrap/tests/expected_namespaces/new_ns1ClassA.cpp @@ -6,8 +6,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB.cpp b/wrap/tests/expected_namespaces/new_ns1ClassB.cpp index 419453968..bc1dd313c 100644 --- a/wrap/tests/expected_namespaces/new_ns1ClassB.cpp +++ b/wrap/tests/expected_namespaces/new_ns1ClassB.cpp @@ -7,8 +7,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA.cpp b/wrap/tests/expected_namespaces/new_ns2ClassA.cpp index 31e3385d5..57faabc79 100644 --- a/wrap/tests/expected_namespaces/new_ns2ClassA.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ClassA.cpp @@ -7,8 +7,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC.cpp b/wrap/tests/expected_namespaces/new_ns2ClassC.cpp index c0446109a..e836a80fd 100644 --- a/wrap/tests/expected_namespaces/new_ns2ClassC.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ClassC.cpp @@ -6,8 +6,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB.cpp b/wrap/tests/expected_namespaces/new_ns2ns3ClassB.cpp index a952790b5..af1a25922 100644 --- a/wrap/tests/expected_namespaces/new_ns2ns3ClassB.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB.cpp @@ -7,8 +7,10 @@ typedef boost::shared_ptr Shared; static std::set collector; void cleanup(void) { - BOOST_FOREACH(Shared* p, collector) - collector.erase(p); + for(std::set::iterator iter = collector.begin(); iter != collector.end(); ) { + delete *iter; + collector.erase(iter++); + } } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {