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

All SLAM Graph classes now derive from a common base class "EasyFactorGraph", to avoid duplicating the common optimize convenience methods.

release/4.3a0
Frank Dellaert 2012-06-24 21:53:05 +00:00
parent 7c5e60edca
commit b1dbfab1b3
10 changed files with 163 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
.cproject
View File

@ -1947,10 +1947,10 @@
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildArguments>-j5</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -2244,46 +2244,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check j5" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_distclean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_unstable_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable_distclean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_unstable_clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable_clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

15
gtsam.h
View File

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

57
gtsam/nonlinear/EasyFactorGraph.cpp Normal file
View File

@ -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 <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam/linear/SimpleSPCGSolver.h>
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<SimpleSPCGSolverParameters>();
return optimizer(initialEstimate, p).optimizeSafely();
}
Marginals EasyFactorGraph::marginals(const Values& solution) const {
return Marginals(*this, solution);
}
} // namespace gtsam

69
gtsam/nonlinear/EasyFactorGraph.h Normal file
View File

@ -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 <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
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

7
gtsam/slam/planarSLAM.h
View File

@ -23,9 +23,6 @@
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose2.h>
// 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) {}

18
gtsam/slam/pose2SLAM.cpp
View File

@ -15,7 +15,6 @@
* @author Frank Dellaert
**/
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/slam/pose2SLAM.h>
// 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<SimpleSPCGSolverParameters>();
return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize();
}
/* ************************************************************************* */
} // pose2SLAM

25
gtsam/slam/pose2SLAM.h
View File

@ -19,11 +19,8 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose2.h>
// 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<Graph> 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

8
gtsam/slam/pose3SLAM.cpp
View File

@ -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

42
gtsam/slam/pose3SLAM.h
View File

@ -19,10 +19,8 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
/// Use pose3SLAM namespace for specific SLAM instance
@ -63,9 +61,6 @@ namespace pose3SLAM {
/// get a pose
Pose3 pose(Key i) const { return at<Pose3>(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

16
gtsam/slam/visualSLAM.h
View File

@ -24,7 +24,6 @@
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
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<Graph> 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 {