diff --git a/.cproject b/.cproject index 7feaf903a..c593a801b 100644 --- a/.cproject +++ b/.cproject @@ -1947,10 +1947,10 @@ make - -j2 + -j5 check true - false + true true @@ -2244,46 +2244,6 @@ true true - - make - -j5 - check - true - true - true - - - make - -j5 - wrap_gtsam_distclean - true - true - true - - - make - -j5 - wrap_gtsam_unstable_distclean - true - true - true - - - make - -j5 - wrap_gtsam_clean - true - true - true - - - make - -j5 - wrap_gtsam_unstable_clean - true - true - true - make -j5 diff --git a/gtsam.h b/gtsam.h index 9609adb03..68ed6201d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -997,6 +997,8 @@ class Values { class Graph { Graph(); + Graph(const gtsam::NonlinearFactorGraph& graph); + Graph(const pose2SLAM::Graph& graph); // FactorGraph void print(string s) const; @@ -1017,7 +1019,7 @@ class Graph { // pose2SLAM-specific void addPoseConstraint(size_t key, const gtsam::Pose2& pose); void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel); + void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; gtsam::Marginals marginals(const pose2SLAM::Values& solution) const; @@ -1048,6 +1050,8 @@ class Values { class Graph { Graph(); + Graph(const gtsam::NonlinearFactorGraph& graph); + Graph(const pose3SLAM::Graph& graph); // FactorGraph void print(string s) const; @@ -1115,6 +1119,9 @@ class Values { class Graph { Graph(); + Graph(const gtsam::NonlinearFactorGraph& graph); + Graph(const pose2SLAM::Graph& graph); + Graph(const planarSLAM::Graph& graph); // FactorGraph void print(string s) const; @@ -1135,7 +1142,7 @@ class Graph { // pose2SLAM-inherited void addPoseConstraint(size_t key, const gtsam::Pose2& pose); void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel); + void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate, size_t verbosity) const; planarSLAM::Values optimizeSPCG(const planarSLAM::Values& initialEstimate, size_t verbosity) const; gtsam::Marginals marginals(const planarSLAM::Values& solution) const; @@ -1197,6 +1204,9 @@ class Values { class Graph { Graph(); + Graph(const gtsam::NonlinearFactorGraph& graph); + Graph(const pose3SLAM::Graph& graph); + Graph(const visualSLAM::Graph& graph); // FactorGraph void print(string s) const; @@ -1258,6 +1268,7 @@ class LevenbergMarquardtOptimizer { double lambda() const; void iterate(); visualSLAM::Values optimize(); + visualSLAM::Values optimizeSafely(); double error() const; size_t iterations() const; visualSLAM::Values values() const; diff --git a/gtsam/nonlinear/EasyFactorGraph.cpp b/gtsam/nonlinear/EasyFactorGraph.cpp new file mode 100644 index 000000000..4f3765dec --- /dev/null +++ b/gtsam/nonlinear/EasyFactorGraph.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * 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 EasyFactorGraph.cpp + * @brief Nonlinear Factor Graph class with methods defined for safe and easy access in MATLAB + * @date June 24, 2012 + * @author Frank Dellaert + **/ + +#include +#include + +namespace gtsam { + + EasyFactorGraph::EasyFactorGraph() { + } + + EasyFactorGraph::EasyFactorGraph(const NonlinearFactorGraph& graph) : + NonlinearFactorGraph(graph) { + } + + LevenbergMarquardtOptimizer EasyFactorGraph::optimizer( + const Values& initialEstimate, const LevenbergMarquardtParams& p) const { + return LevenbergMarquardtOptimizer((*this), initialEstimate, p); + } + + const Values& EasyFactorGraph::optimize(const Values& initialEstimate, + size_t verbosity) const { + LevenbergMarquardtParams p; + p.verbosity = (NonlinearOptimizerParams::Verbosity) verbosity; + return optimizer(initialEstimate, p).optimizeSafely(); + } + + const Values& EasyFactorGraph::optimizeSPCG(const Values& initialEstimate, + size_t verbosity) const { + LevenbergMarquardtParams p; + p.verbosity = (NonlinearOptimizerParams::Verbosity) verbosity; + p.linearSolverType = SuccessiveLinearizationParams::CG; + p.iterativeParams = boost::make_shared(); + return optimizer(initialEstimate, p).optimizeSafely(); + } + + Marginals EasyFactorGraph::marginals(const Values& solution) const { + return Marginals(*this, solution); + } + +} // namespace gtsam + diff --git a/gtsam/nonlinear/EasyFactorGraph.h b/gtsam/nonlinear/EasyFactorGraph.h new file mode 100644 index 000000000..131e6a06a --- /dev/null +++ b/gtsam/nonlinear/EasyFactorGraph.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * 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 EasyFactorGraph.h + * @brief Nonlinear Factor Graph class with methods defined for safe and easy access in MATLAB + * @date June 24, 2012 + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * Nonlinear Factor Graph class with methods defined for safe and easy access in MATLAB + */ + struct EasyFactorGraph: public NonlinearFactorGraph { + + /// Default constructor for a NonlinearFactorGraph + EasyFactorGraph(); + + /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + EasyFactorGraph(const NonlinearFactorGraph& graph); + + /** + * Setup and return a LevenbargMarquardtOptimizer + * @param initialEstimate initial estimate of all variables in the graph + * @param p optimizer's parameters + * @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process + */ + LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate, + const LevenbergMarquardtParams& p = LevenbergMarquardtParams()) const; + + /** + * Safely optimize the graph, using Levenberg-Marquardt + * @param initialEstimate initial estimate of all variables in the graph + * @param verbosity unsigned specifying verbosity level + */ + const Values& optimize(const Values& initialEstimate, size_t verbosity = 0) const; + + /** + * Safely optimize using subgraph preconditioning + * @param initialEstimate initial estimate of all variables in the graph + * @param verbosity unsigned specifying verbosity level + */ + const Values& optimizeSPCG(const Values& initialEstimate, size_t verbosity = 0) const; + + /** + * Return a Marginals object, so you can query marginals + */ + Marginals marginals(const Values& solution) const; + + }; + +} // namespace gtsam + diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 3ab841892..967d91581 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -23,9 +23,6 @@ #include #include #include -#include -#include -#include #include // Use planarSLAM namespace for specific SLAM instance @@ -84,10 +81,10 @@ namespace planarSLAM { */ struct Graph: public pose2SLAM::Graph { - /// Default constructor for a NonlinearFactorGraph + /// Default constructor Graph(){} - /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + /// Copy constructor given any other NonlinearFactorGraph Graph(const NonlinearFactorGraph& graph): pose2SLAM::Graph(graph) {} diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 9607448c7..cc549d84a 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -15,7 +15,6 @@ * @author Frank Dellaert **/ -#include #include // Use pose2SLAM namespace for specific SLAM instance @@ -63,22 +62,5 @@ namespace pose2SLAM { } /* ************************************************************************* */ - Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const { - LevenbergMarquardtParams params; - params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity; - LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params); - return optimizer.optimize(); - } - - /* ************************************************************************* */ - Values Graph::optimizeSPCG(const Values& initialEstimate, size_t verbosity) const { - LevenbergMarquardtParams params; - params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity; - params.linearSolverType = SuccessiveLinearizationParams::CG; - params.iterativeParams = boost::make_shared(); - return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize(); - } - - /* ************************************************************************* */ } // pose2SLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 6349af5d5..018090c6a 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -19,11 +19,8 @@ #include #include -#include +#include #include -#include -#include -#include #include // Use pose2SLAM namespace for specific SLAM instance @@ -77,16 +74,16 @@ namespace pose2SLAM { * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper * @addtogroup SLAM */ - struct Graph: public NonlinearFactorGraph { + struct Graph: public EasyFactorGraph { typedef boost::shared_ptr shared_ptr; - /// Default constructor for a NonlinearFactorGraph + /// Default constructor Graph(){} - /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + /// Copy constructor given any other NonlinearFactorGraph Graph(const NonlinearFactorGraph& graph): - NonlinearFactorGraph(graph) {} + EasyFactorGraph(graph) {} /// Creates a hard constraint for key i with the given Pose2 p. void addPoseConstraint(Key i, const Pose2& p); @@ -96,18 +93,6 @@ namespace pose2SLAM { /// Creates an relative pose factor between poses with keys i1 and i2 void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model); - - /// Optimize - Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const; - - /// Optimize using subgraph preconditioning - Values optimizeSPCG(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const; - - /// Return a Marginals object - Marginals marginals(const Values& solution) const { - return Marginals(*this,solution); - } - }; } // pose2SLAM diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 2a5f435fc..07a89e419 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -69,13 +69,5 @@ namespace pose3SLAM { } /* ************************************************************************* */ - Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const { - LevenbergMarquardtParams params; - params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity; - LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params); - return optimizer.optimize(); - } - - /* ************************************************************************* */ } // pose3SLAM diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 8e60265e8..20ea8b0d5 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -19,10 +19,8 @@ #include #include -#include +#include #include -#include -#include #include /// Use pose3SLAM namespace for specific SLAM instance @@ -63,9 +61,6 @@ namespace pose3SLAM { /// get a pose Pose3 pose(Key i) const { return at(i); } - /// check if value with specified key exists - bool exists(Key i) const { return gtsam::Values::exists(i); } - Matrix translations() const; ///< get all pose translations coordinates in a matrix }; @@ -73,7 +68,14 @@ namespace pose3SLAM { * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper * @addtogroup SLAM */ - struct Graph: public NonlinearFactorGraph { + struct Graph: public EasyFactorGraph { + + /// Default constructor + Graph(){} + + /// Copy constructor given any other NonlinearFactorGraph + Graph(const NonlinearFactorGraph& graph): + EasyFactorGraph(graph) {} /** * Add a prior on a pose @@ -98,32 +100,6 @@ namespace pose3SLAM { * @param model uncertainty model of this measurement */ void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model); - - /** - * Optimize the graph - * @param initialEstimate initial estimate of all variables in the graph - * @param pointKey variable key of the landmark - * @param range approximate range to landmark - * @param model uncertainty model of this prior - */ - Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const; - - /** - * Setup and return a LevenbargMarquardtOptimizer - * @param initialEstimate initial estimate of all variables in the graph - * @param parameters optimizer's parameters - * @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process - */ - LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate, - const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const { - return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters); - } - - /// Return a Marginals object - Marginals marginals(const Values& solution) const { - return Marginals(*this,solution); - } - }; } // pose3SLAM diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index da87622fd..7e670ccd4 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -24,7 +24,6 @@ #include #include #include -#include #include namespace visualSLAM { @@ -76,20 +75,17 @@ namespace visualSLAM { /** * Non-linear factor graph for vanilla visual SLAM */ - class Graph: public pose3SLAM::Graph { + struct Graph: public pose3SLAM::Graph { - public: /// shared pointer to this type of graph typedef boost::shared_ptr shared_graph; - /// default constructor is empty graph - Graph() { - } + /// Default constructor + Graph(){} - /// print out graph - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - NonlinearFactorGraph::print(s, keyFormatter); - } + /// Copy constructor given any other NonlinearFactorGraph + Graph(const NonlinearFactorGraph& graph): + pose3SLAM::Graph(graph) {} /// check if two graphs are equal bool equals(const Graph& p, double tol = 1e-9) const {