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

Changed linear config names to *Values, updated comments

release/4.3a0
Alex Cunningham 2010-10-09 03:09:58 +00:00
parent 6002931e12
commit 07bda5aa97
144 changed files with 1574 additions and 1578 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/PlanarSLAMExample.cpp
View File

@ -65,7 +65,7 @@ int main(int argc, char** argv) {
graph.print("Full Graph"); graph.print("Full Graph");
// initialize to noisy points // initialize to noisy points
Config initialEstimate; Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -75,7 +75,7 @@ int main(int argc, char** argv) {
initialEstimate.print("Initial Estimate"); initialEstimate.print("Initial Estimate");
// optimize using Levenburg-Marquadt optimization with an ordering from colamd // optimize using Levenburg-Marquadt optimization with an ordering from colamd
Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate); Optimizer::shared_values result = Optimizer::optimizeLM(graph, initialEstimate);
result->print("Final Result"); result->print("Final Result");

26
examples/PlanarSLAMSelfContained.cpp
View File

@ -31,11 +31,11 @@
// Main typedefs // Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included
typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included
typedef gtsam::LieValues<PoseKey> PoseConfig; // config type for poses typedef gtsam::LieValues<PoseKey> PoseValues; // config type for poses
typedef gtsam::LieValues<PointKey> PointConfig; // config type for points typedef gtsam::LieValues<PointKey> PointValues; // config type for points
typedef gtsam::TupleValues2<PoseConfig, PointConfig> Config; // main config with two variable classes typedef gtsam::TupleValues2<PoseValues, PointValues> Values; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Config> Graph; // graph structure typedef gtsam::NonlinearFactorGraph<Values> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Config> Optimizer; // optimization engine for this domain typedef gtsam::NonlinearOptimizer<Graph,Values> Optimizer; // optimization engine for this domain
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -61,7 +61,7 @@ int main(int argc, char** argv) {
// gaussian for prior // gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<Config, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor PriorFactor<Values, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
graph.add(posePrior); // add the factor to the graph graph.add(posePrior); // add the factor to the graph
/* add odometry */ /* add odometry */
@ -69,8 +69,8 @@ int main(int argc, char** argv) {
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry // create between factors to represent odometry
BetweenFactor<Config,PoseKey> odom12(x1, x2, odom_measurement, odom_model); BetweenFactor<Values,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Config,PoseKey> odom23(x2, x3, odom_measurement, odom_model); BetweenFactor<Values,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
graph.add(odom12); // add both to graph graph.add(odom12); // add both to graph
graph.add(odom23); graph.add(odom23);
@ -87,9 +87,9 @@ int main(int argc, char** argv) {
range32 = 2.0; range32 = 2.0;
// create bearing/range factors // create bearing/range factors
BearingRangeFactor<Config, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model); BearingRangeFactor<Values, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Config, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model); BearingRangeFactor<Values, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Config, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model); BearingRangeFactor<Values, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors // add the factors
graph.add(meas11); graph.add(meas11);
@ -99,7 +99,7 @@ int main(int argc, char** argv) {
graph.print("Full Graph"); graph.print("Full Graph");
// initialize to noisy points // initialize to noisy points
Config initialEstimate; Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -109,7 +109,7 @@ int main(int argc, char** argv) {
initialEstimate.print("Initial Estimate"); initialEstimate.print("Initial Estimate");
// optimize using Levenburg-Marquadt optimization with an ordering from colamd // optimize using Levenburg-Marquadt optimization with an ordering from colamd
Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate); Optimizer::shared_values result = Optimizer::optimizeLM(graph, initialEstimate);
result->print("Final Result"); result->print("Final Result");

16
examples/SimpleRotation.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
/* /*
* TODO: make factors independent of Config * TODO: make factors independent of Values
* TODO: get rid of excessive shared pointer stuff: mostly gone * TODO: get rid of excessive shared pointer stuff: mostly gone
* TODO: make toplevel documentation * TODO: make toplevel documentation
* TODO: investigate whether we can just use ints as keys: will occur for linear, not nonlinear * TODO: investigate whether we can just use ints as keys: will occur for linear, not nonlinear
@ -31,10 +31,10 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef TypedSymbol<Rot2, 'x'> Key; typedef TypedSymbol<Rot2, 'x'> Key;
typedef LieValues<Key> Config; typedef LieValues<Key> Values;
typedef NonlinearFactorGraph<Config> Graph; typedef NonlinearFactorGraph<Values> Graph;
typedef Factorization<Graph,Config> Solver; typedef Factorization<Graph,Values> Solver;
typedef NonlinearOptimizer<Graph,Config> Optimizer; typedef NonlinearOptimizer<Graph,Values> Optimizer;
const double degree = M_PI / 180; const double degree = M_PI / 180;
@ -47,19 +47,19 @@ int main() {
prior1.print("Goal Angle"); prior1.print("Goal Angle");
SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree); SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key1(1); Key key1(1);
PriorFactor<Config, Key> factor1(key1, prior1, model1); PriorFactor<Values, Key> factor1(key1, prior1, model1);
// Create a factor graph // Create a factor graph
Graph graph; Graph graph;
graph.add(factor1); graph.add(factor1);
// and an initial estimate // and an initial estimate
Config initialEstimate; Values initialEstimate;
initialEstimate.insert(key1, Rot2::fromAngle(20 * degree)); initialEstimate.insert(key1, Rot2::fromAngle(20 * degree));
initialEstimate.print("Initialization"); initialEstimate.print("Initialization");
// create an ordering // create an ordering
Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate, Optimizer::LAMBDA); Optimizer::shared_values result = Optimizer::optimizeLM(graph, initialEstimate, Optimizer::LAMBDA);
result->print("Final config"); result->print("Final config");
return 0; return 0;

12
gtsam-broken.h
View File

@ -1,9 +1,9 @@
// These are currently broken // These are currently broken
// Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class // Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class
// We also have to solve the shared pointer mess to avoid duplicate methods // We also have to solve the shared pointer mess to avoid duplicate methods
class Pose2Config{ class Pose2Values{
Pose2Config(); Pose2Values();
Pose2 get(string key) const; Pose2 get(string key) const;
void insert(string name, const Pose2& val); void insert(string name, const Pose2& val);
void print(string s) const; void print(string s) const;
@ -15,15 +15,15 @@ class Pose2Factor {
Pose2Factor(string key1, string key2, Pose2Factor(string key1, string key2,
const Pose2& measured, Matrix measurement_covariance); const Pose2& measured, Matrix measurement_covariance);
void print(string name) const; void print(string name) const;
double error(const Pose2Config& c) const; double error(const Pose2Values& c) const;
size_t size() const; size_t size() const;
GaussianFactor* linearize(const Pose2Config& config) const; GaussianFactor* linearize(const Pose2Values& config) const;
}; };
class Pose2Graph{ class Pose2Graph{
Pose2Graph(); Pose2Graph();
void print(string s) const; void print(string s) const;
GaussianFactorGraph* linearize_(const Pose2Config& config) const; GaussianFactorGraph* linearize_(const Pose2Values& config) const;
void push_back(Pose2Factor* factor); void push_back(Pose2Factor* factor);
}; };

52
gtsam.h
View File

@ -23,10 +23,10 @@ class SymbolicFactor{
void print(string s) const; void print(string s) const;
}; };
class VectorConfig { class VectorValues {
VectorConfig(); VectorValues();
void print(string s) const; void print(string s) const;
bool equals(const VectorConfig& expected, double tol) const; bool equals(const VectorValues& expected, double tol) const;
void insert(string name, Vector val); void insert(string name, Vector val);
Vector get(string name) const; Vector get(string name) const;
bool contains(string name) const; bool contains(string name) const;
@ -57,7 +57,7 @@ class GaussianFactor {
bool empty() const; bool empty() const;
Vector get_b() const; Vector get_b() const;
Matrix get_A(string key) const; Matrix get_A(string key) const;
double error(const VectorConfig& c) const; double error(const VectorValues& c) const;
bool involves(string key) const; bool involves(string key) const;
pair<Matrix,Vector> matrix(const Ordering& ordering) const; pair<Matrix,Vector> matrix(const Ordering& ordering) const;
pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const; pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
@ -91,7 +91,7 @@ class GaussianConditional {
void print(string s) const; void print(string s) const;
bool equals(const GaussianConditional &cg, double tol) const; bool equals(const GaussianConditional &cg, double tol) const;
void add(string key, Matrix S); void add(string key, Matrix S);
Vector solve(const VectorConfig& x); Vector solve(const VectorValues& x);
}; };
class GaussianBayesNet { class GaussianBayesNet {
@ -109,17 +109,17 @@ class GaussianFactorGraph {
size_t size() const; size_t size() const;
void push_back(GaussianFactor* ptr_f); void push_back(GaussianFactor* ptr_f);
double error(const VectorConfig& c) const; double error(const VectorValues& c) const;
double probPrime(const VectorConfig& c) const; double probPrime(const VectorValues& c) const;
void combine(const GaussianFactorGraph& lfg); void combine(const GaussianFactorGraph& lfg);
GaussianConditional* eliminateOne(string key); GaussianConditional* eliminateOne(string key);
GaussianBayesNet* eliminate_(const Ordering& ordering); GaussianBayesNet* eliminate_(const Ordering& ordering);
VectorConfig* optimize_(const Ordering& ordering); VectorValues* optimize_(const Ordering& ordering);
pair<Matrix,Vector> matrix(const Ordering& ordering) const; pair<Matrix,Vector> matrix(const Ordering& ordering) const;
Matrix sparse(const Ordering& ordering) const; Matrix sparse(const Ordering& ordering) const;
VectorConfig* steepestDescent_(const VectorConfig& x0) const; VectorValues* steepestDescent_(const VectorValues& x0) const;
VectorConfig* conjugateGradientDescent_(const VectorConfig& x0) const; VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
}; };
class Point2 { class Point2 {
@ -159,8 +159,8 @@ class Pose2 {
Rot2 r() const; Rot2 r() const;
}; };
class Simulated2DConfig { class Simulated2DValues {
Simulated2DConfig(); Simulated2DValues();
void print(string s) const; void print(string s) const;
void insertPose(int i, const Point2& p); void insertPose(int i, const Point2& p);
void insertPoint(int j, const Point2& p); void insertPoint(int j, const Point2& p);
@ -170,8 +170,8 @@ class Simulated2DConfig {
Point2* point(int j); Point2* point(int j);
}; };
class Simulated2DOrientedConfig { class Simulated2DOrientedValues {
Simulated2DOrientedConfig(); Simulated2DOrientedValues();
void print(string s) const; void print(string s) const;
void insertPose(int i, const Pose2& p); void insertPose(int i, const Pose2& p);
void insertPoint(int j, const Point2& p); void insertPoint(int j, const Point2& p);
@ -184,43 +184,43 @@ class Simulated2DOrientedConfig {
class Simulated2DPosePrior { class Simulated2DPosePrior {
Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const; GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DConfig& c) const; double error(const Simulated2DValues& c) const;
}; };
class Simulated2DOrientedPosePrior { class Simulated2DOrientedPosePrior {
Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i); Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DOrientedConfig& config) const; GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
double error(const Simulated2DOrientedConfig& c) const; double error(const Simulated2DOrientedValues& c) const;
}; };
class Simulated2DPointPrior { class Simulated2DPointPrior {
Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const; GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DConfig& c) const; double error(const Simulated2DValues& c) const;
}; };
class Simulated2DOdometry { class Simulated2DOdometry {
Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2); Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const; GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DConfig& c) const; double error(const Simulated2DValues& c) const;
}; };
class Simulated2DOrientedOdometry { class Simulated2DOrientedOdometry {
Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2); Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DOrientedConfig& config) const; GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
double error(const Simulated2DOrientedConfig& c) const; double error(const Simulated2DOrientedValues& c) const;
}; };
class Simulated2DMeasurement { class Simulated2DMeasurement {
Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j); Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const; GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DConfig& c) const; double error(const Simulated2DValues& c) const;
}; };
class Pose2SLAMOptimizer { class Pose2SLAMOptimizer {

2
inference/Factor.h
View File

@ -31,7 +31,7 @@ class Conditional;
* conflicts with efficiency as well, esp. in the case of incomplete * conflicts with efficiency as well, esp. in the case of incomplete
* QR factorization. A solution is still being sought. * QR factorization. A solution is still being sought.
* *
* A Factor is templated on a Config, for example VectorConfig is a configuration of * A Factor is templated on a Values, for example VectorValues is a values structure of
* labeled vectors. This way, we can have factors that might be defined on discrete * labeled vectors. This way, we can have factors that might be defined on discrete
* variables, continuous ones, or a combination of both. It is up to the config to * variables, continuous ones, or a combination of both. It is up to the config to
* provide the appropriate values at the appropriate time. * provide the appropriate values at the appropriate time.

2
inference/FactorGraph.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
* A factor graph is a bipartite graph with factor nodes connected to variable nodes. * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around. * In this class, however, only factor nodes are kept around.
* *
* Templated on the type of factors and configuration. * Templated on the type of factors and values structure.
*/ */
template<class Factor> template<class Factor>
class FactorGraph: public Testable<FactorGraph<Factor> > { class FactorGraph: public Testable<FactorGraph<Factor> > {

84
inference/ISAM2-inl.h
View File

@ -15,7 +15,7 @@ using namespace boost::assign;
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/Conditional.h>
@ -33,12 +33,12 @@ using namespace std;
static const bool disableReordering = false; static const bool disableReordering = false;
/** Create an empty Bayes Tree */ /** Create an empty Bayes Tree */
template<class Conditional, class Config> template<class Conditional, class Values>
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>(), delta_(Permutation(), deltaUnpermuted_) {} ISAM2<Conditional, Values>::ISAM2() : BayesTree<Conditional>(), delta_(Permutation(), deltaUnpermuted_) {}
/** Create a Bayes Tree from a nonlinear factor graph */ /** Create a Bayes Tree from a nonlinear factor graph */
//template<class Conditional, class Config> //template<class Conditional, class Values>
//ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config) : //ISAM2<Conditional, Values>::ISAM2(const NonlinearFactorGraph<Values>& nlfg, const Ordering& ordering, const Values& config) :
//BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)), theta_(config), //BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)), theta_(config),
//variableIndex_(nlfg.symbolic(config, ordering), config.dims(ordering)), deltaUnpermuted_(variableIndex_.dims()), //variableIndex_(nlfg.symbolic(config, ordering), config.dims(ordering)), deltaUnpermuted_(variableIndex_.dims()),
//delta_(Permutation::Identity(variableIndex_.size())), nonlinearFactors_(nlfg), ordering_(ordering) { //delta_(Permutation::Identity(variableIndex_.size())), nonlinearFactors_(nlfg), ordering_(ordering) {
@ -48,14 +48,14 @@ ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>(), delta_(Permutati
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Values>
list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<varid_t>& keys) const { list<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const list<varid_t>& keys) const {
static const bool debug = false; static const bool debug = false;
if(debug) cout << "Getting affected factors for "; if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const varid_t key, keys) { cout << key << " "; } } if(debug) { BOOST_FOREACH(const varid_t key, keys) { cout << key << " "; } }
if(debug) cout << endl; if(debug) cout << endl;
FactorGraph<NonlinearFactor<Config> > allAffected; FactorGraph<NonlinearFactor<Values> > allAffected;
list<size_t> indices; list<size_t> indices;
BOOST_FOREACH(const varid_t key, keys) { BOOST_FOREACH(const varid_t key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key); // const list<size_t> l = nonlinearFactors_.factors(key);
@ -77,15 +77,15 @@ list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<varid_t>&
/* ************************************************************************* */ /* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables // retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors) // (note that the remaining stuff is summarized in the cached factors)
template<class Conditional, class Config> template<class Conditional, class Values>
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Values>::relinearizeAffectedFactors
(const list<varid_t>& affectedKeys) const { (const list<varid_t>& affectedKeys) const {
tic("8.2.2.1 getAffectedFactors"); tic("8.2.2.1 getAffectedFactors");
list<size_t> candidates = getAffectedFactors(affectedKeys); list<size_t> candidates = getAffectedFactors(affectedKeys);
toc("8.2.2.1 getAffectedFactors"); toc("8.2.2.1 getAffectedFactors");
NonlinearFactorGraph<Config> nonlinearAffectedFactors; NonlinearFactorGraph<Values> nonlinearAffectedFactors;
tic("8.2.2.2 affectedKeysSet"); tic("8.2.2.2 affectedKeysSet");
// for fast lookup below // for fast lookup below
@ -113,8 +113,8 @@ boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAf
/* ************************************************************************* */ /* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area // find intermediate (linearized) factors from cache that are passed into the affected area
template<class Conditional, class Config> template<class Conditional, class Values>
GaussianFactorGraph ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) { GaussianFactorGraph ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false; static const bool debug = false;
@ -139,8 +139,8 @@ GaussianFactorGraph ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional,class Config> template<class Conditional,class Values>
void reinsertCache(const typename ISAM2<Conditional,Config>::sharedClique& root, vector<GaussianFactor::shared_ptr>& cache, const Permutation& selector, const Permutation& selectorInverse) { void reinsertCache(const typename ISAM2<Conditional,Values>::sharedClique& root, vector<GaussianFactor::shared_ptr>& cache, const Permutation& selector, const Permutation& selectorInverse) {
static const bool debug = false; static const bool debug = false;
if(root) { if(root) {
if(root->size() > 0) { if(root->size() > 0) {
@ -162,15 +162,15 @@ void reinsertCache(const typename ISAM2<Conditional,Config>::sharedClique& root,
assert(!root->cachedFactor()); assert(!root->cachedFactor());
root->cachedFactor() = cachedFactor; root->cachedFactor() = cachedFactor;
} }
typedef ISAM2<Conditional,Config> This; typedef ISAM2<Conditional,Values> This;
BOOST_FOREACH(typename This::sharedClique& child, root->children()) { BOOST_FOREACH(typename This::sharedClique& child, root->children()) {
reinsertCache<Conditional,Config>(child, cache, selector, selectorInverse); reinsertCache<Conditional,Values>(child, cache, selector, selectorInverse);
} }
} }
} }
template<class Conditional, class Config> template<class Conditional, class Values>
boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Config>::recalculate(const set<varid_t>& markedKeys, const vector<varid_t>& newKeys, const GaussianFactorGraph* newFactors) { boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Values>::recalculate(const set<varid_t>& markedKeys, const vector<varid_t>& newKeys, const GaussianFactorGraph* newFactors) {
static const bool debug = false; static const bool debug = false;
static const bool useMultiFrontal = true; static const bool useMultiFrontal = true;
@ -522,7 +522,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Config>::recalculate(const s
if(bayesNet->size() == 0) if(bayesNet->size() == 0)
assert(newlyCached.size() == 0); assert(newlyCached.size() == 0);
else else
reinsertCache<Conditional,Config>(this->root(), newlyCached, affectedKeysSelector, affectedKeysSelectorInverse); reinsertCache<Conditional,Values>(this->root(), newlyCached, affectedKeysSelector, affectedKeysSelectorInverse);
toc("8.7.3 re-assemble: insert cache"); toc("8.7.3 re-assemble: insert cache");
lastNnzTop = 0; //calculate_nnz(this->root()); lastNnzTop = 0; //calculate_nnz(this->root());
@ -563,16 +563,16 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Config>::recalculate(const s
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//template<class Conditional, class Config> //template<class Conditional, class Values>
//void ISAM2<Conditional, Config>::linear_update(const GaussianFactorGraph& newFactors) { //void ISAM2<Conditional, Values>::linear_update(const GaussianFactorGraph& newFactors) {
// const list<varid_t> markedKeys = newFactors.keys(); // const list<varid_t> markedKeys = newFactors.keys();
// recalculate(markedKeys, &newFactors); // recalculate(markedKeys, &newFactors);
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
// find all variables that are directly connected by a measurement to one of the marked variables // find all variables that are directly connected by a measurement to one of the marked variables
template<class Conditional, class Config> template<class Conditional, class Values>
void ISAM2<Conditional, Config>::find_all(sharedClique clique, set<varid_t>& keys, const vector<bool>& markedMask) { void ISAM2<Conditional, Values>::find_all(sharedClique clique, set<varid_t>& keys, const vector<bool>& markedMask) {
// does the separator contain any of the variables? // does the separator contain any of the variables?
bool found = false; bool found = false;
BOOST_FOREACH(const varid_t& key, clique->separator_) { BOOST_FOREACH(const varid_t& key, clique->separator_) {
@ -591,10 +591,10 @@ void ISAM2<Conditional, Config>::find_all(sharedClique clique, set<varid_t>& key
/* ************************************************************************* */ /* ************************************************************************* */
struct _SelectiveExpmap { struct _SelectiveExpmap {
const Permuted<VectorConfig>& delta; const Permuted<VectorValues>& delta;
const Ordering& ordering; const Ordering& ordering;
const vector<bool>& mask; const vector<bool>& mask;
_SelectiveExpmap(const Permuted<VectorConfig>& _delta, const Ordering& _ordering, const vector<bool>& _mask) : _SelectiveExpmap(const Permuted<VectorValues>& _delta, const Ordering& _ordering, const vector<bool>& _mask) :
delta(_delta), ordering(_ordering), mask(_mask) {} delta(_delta), ordering(_ordering), mask(_mask) {}
template<typename I> template<typename I>
void operator()(I it_x) { void operator()(I it_x) {
@ -604,10 +604,10 @@ struct _SelectiveExpmap {
}; };
#ifndef NDEBUG #ifndef NDEBUG
struct _SelectiveExpmapAndClear { struct _SelectiveExpmapAndClear {
Permuted<VectorConfig>& delta; Permuted<VectorValues>& delta;
const Ordering& ordering; const Ordering& ordering;
const vector<bool>& mask; const vector<bool>& mask;
_SelectiveExpmapAndClear(Permuted<VectorConfig>& _delta, const Ordering& _ordering, const vector<bool>& _mask) : _SelectiveExpmapAndClear(Permuted<VectorValues>& _delta, const Ordering& _ordering, const vector<bool>& _mask) :
delta(_delta), ordering(_ordering), mask(_mask) {} delta(_delta), ordering(_ordering), mask(_mask) {}
template<typename I> template<typename I>
void operator()(I it_x) { void operator()(I it_x) {
@ -627,8 +627,8 @@ struct _SelectiveExpmapAndClear {
#endif #endif
struct _VariableAdder { struct _VariableAdder {
Ordering& ordering; Ordering& ordering;
Permuted<VectorConfig>& vconfig; Permuted<VectorValues>& vconfig;
_VariableAdder(Ordering& _ordering, Permuted<VectorConfig>& _vconfig) : ordering(_ordering), vconfig(_vconfig) {} _VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig) : ordering(_ordering), vconfig(_vconfig) {}
template<typename I> template<typename I>
void operator()(I xIt) { void operator()(I xIt) {
static const bool debug = false; static const bool debug = false;
@ -638,9 +638,9 @@ struct _VariableAdder {
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl; if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl;
} }
}; };
template<class Conditional, class Config> template<class Conditional, class Values>
void ISAM2<Conditional, Config>::update( void ISAM2<Conditional, Values>::update(
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta, const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta,
double wildfire_threshold, double relinearize_threshold, bool relinearize) { double wildfire_threshold, double relinearize_threshold, bool relinearize) {
static const bool debug = false; static const bool debug = false;
@ -690,7 +690,7 @@ void ISAM2<Conditional, Config>::update(
// 3. Mark linear update // 3. Mark linear update
set<varid_t> markedKeys; set<varid_t> markedKeys;
vector<varid_t> newKeys; newKeys.reserve(newFactors.size() * 6); vector<varid_t> newKeys; newKeys.reserve(newFactors.size() * 6);
BOOST_FOREACH(const typename NonlinearFactor<Config>::shared_ptr& factor, newFactors) { BOOST_FOREACH(const typename NonlinearFactor<Values>::shared_ptr& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) { BOOST_FOREACH(const Symbol& key, factor->keys()) {
markedKeys.insert(ordering_[key]); markedKeys.insert(ordering_[key]);
newKeys.push_back(ordering_[key]); newKeys.push_back(ordering_[key]);
@ -798,7 +798,7 @@ void ISAM2<Conditional, Config>::update(
tic("9 step9"); tic("9 step9");
// 9. Solve // 9. Solve
if (wildfire_threshold<=0.) { if (wildfire_threshold<=0.) {
VectorConfig newDelta(variableIndex_.dims()); VectorValues newDelta(variableIndex_.dims());
optimize2(this->root(), newDelta); optimize2(this->root(), newDelta);
assert(newDelta.size() == delta_.size()); assert(newDelta.size() == delta_.size());
delta_.permutation() = Permutation::Identity(delta_.size()); delta_.permutation() = Permutation::Identity(delta_.size());
@ -807,7 +807,7 @@ void ISAM2<Conditional, Config>::update(
// GaussianFactorGraph linearfull = *nonlinearFactors_.linearize(theta_, ordering_); // GaussianFactorGraph linearfull = *nonlinearFactors_.linearize(theta_, ordering_);
// GaussianBayesNet gbn = *Inference::Eliminate(linearfull); // GaussianBayesNet gbn = *Inference::Eliminate(linearfull);
// VectorConfig deltafull = optimize(gbn); // VectorValues deltafull = optimize(gbn);
// assert(assert_equal(deltafull, newDelta, 1e-3)); // assert(assert_equal(deltafull, newDelta, 1e-3));
} else { } else {
@ -822,9 +822,9 @@ void ISAM2<Conditional, Config>::update(
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Values>
Config ISAM2<Conditional, Config>::calculateEstimate() const { Values ISAM2<Conditional, Values>::calculateEstimate() const {
Config ret(theta_); Values ret(theta_);
vector<bool> mask(ordering_.nVars(), true); vector<bool> mask(ordering_.nVars(), true);
_SelectiveExpmap selectiveExpmap(delta_, ordering_, mask); _SelectiveExpmap selectiveExpmap(delta_, ordering_, mask);
ret.apply(selectiveExpmap); ret.apply(selectiveExpmap);
@ -832,9 +832,9 @@ Config ISAM2<Conditional, Config>::calculateEstimate() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Values>
Config ISAM2<Conditional, Config>::calculateBestEstimate() const { Values ISAM2<Conditional, Values>::calculateBestEstimate() const {
VectorConfig delta(variableIndex_.dims()); VectorValues delta(variableIndex_.dims());
optimize2(this->root(), delta); optimize2(this->root(), delta);
return theta_.expmap(delta, ordering_); return theta_.expmap(delta, ordering_);
} }

28
inference/ISAM2.h
View File

@ -28,29 +28,29 @@ namespace gtsam {
//typedef std::vector<GaussianFactor::shared_ptr> CachedFactors; //typedef std::vector<GaussianFactor::shared_ptr> CachedFactors;
template<class Conditional, class Config> template<class Conditional, class Values>
class ISAM2: public BayesTree<Conditional> { class ISAM2: public BayesTree<Conditional> {
protected: protected:
// current linearization point // current linearization point
Config theta_; Values theta_;
// VariableIndex lets us look up factors by involved variable and keeps track of dimensions // VariableIndex lets us look up factors by involved variable and keeps track of dimensions
typedef GaussianVariableIndex<VariableIndexStorage_deque> VariableIndexType; typedef GaussianVariableIndex<VariableIndexStorage_deque> VariableIndexType;
VariableIndexType variableIndex_; VariableIndexType variableIndex_;
// the linear solution, an update to the estimate in theta // the linear solution, an update to the estimate in theta
VectorConfig deltaUnpermuted_; VectorValues deltaUnpermuted_;
// The residual permutation through which the deltaUnpermuted_ is // The residual permutation through which the deltaUnpermuted_ is
// referenced. Permuting the VectorConfig is slow, so for performance the // referenced. Permuting the VectorValues is slow, so for performance the
// permutation is applied at access time instead of to the VectorConfig // permutation is applied at access time instead of to the VectorValues
// itself. // itself.
Permuted<VectorConfig> delta_; Permuted<VectorValues> delta_;
// for keeping all original nonlinear factors // for keeping all original nonlinear factors
NonlinearFactorGraph<Config> nonlinearFactors_; NonlinearFactorGraph<Values> nonlinearFactors_;
// The "ordering" allows converting Symbols to varid_t (integer) keys. We // The "ordering" allows converting Symbols to varid_t (integer) keys. We
// keep it up to date as we add and reorder variables. // keep it up to date as we add and reorder variables.
@ -69,7 +69,7 @@ public:
ISAM2(); ISAM2();
// /** Create a Bayes Tree from a Bayes Net */ // /** Create a Bayes Tree from a Bayes Net */
// ISAM2(const NonlinearFactorGraph<Config>& fg, const Ordering& ordering, const Config& config); // ISAM2(const NonlinearFactorGraph<Values>& fg, const Ordering& ordering, const Values& config);
/** Destructor */ /** Destructor */
virtual ~ISAM2() {} virtual ~ISAM2() {}
@ -81,21 +81,21 @@ public:
/** /**
* ISAM2. * ISAM2.
*/ */
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta, void update(const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta,
double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true); double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);
// needed to create initial estimates // needed to create initial estimates
const Config& getLinearizationPoint() const {return theta_;} const Values& getLinearizationPoint() const {return theta_;}
// estimate based on incomplete delta (threshold!) // estimate based on incomplete delta (threshold!)
Config calculateEstimate() const; Values calculateEstimate() const;
// estimate based on full delta (note that this is based on the current linearization point) // estimate based on full delta (note that this is based on the current linearization point)
Config calculateBestEstimate() const; Values calculateBestEstimate() const;
const Permuted<VectorConfig>& getDelta() const { return delta_; } const Permuted<VectorValues>& getDelta() const { return delta_; }
const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; } const NonlinearFactorGraph<Values>& getFactorsUnsafe() const { return nonlinearFactors_; }
const Ordering& getOrdering() const { return ordering_; } const Ordering& getOrdering() const { return ordering_; }

30
inference/graph-inl.h
View File

@ -126,19 +126,19 @@ predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class V,class Pose, class Config> template <class V,class Pose, class Values>
class compose_key_visitor : public boost::default_bfs_visitor { class compose_key_visitor : public boost::default_bfs_visitor {
private: private:
boost::shared_ptr<Config> config_; boost::shared_ptr<Values> config_;
public: public:
compose_key_visitor(boost::shared_ptr<Config> config_in) {config_ = config_in;} compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const { template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
typename Config::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); typename Values::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename Config::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); typename Values::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge); Pose relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, (*config_)[key_from].compose(relativePose)); config_->insert(key_to, (*config_)[key_from].compose(relativePose));
} }
@ -146,23 +146,23 @@ public:
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class Factor, class Pose, class Config> template<class G, class Factor, class Pose, class Values>
boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree, boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree,
const Pose& rootPose) { const Pose& rootPose) {
//TODO: change edge_weight_t to edge_pose_t //TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list< typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS, boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, typename Config::Key>, boost::property<boost::vertex_name_t, typename Values::Key>,
boost::property<boost::edge_weight_t, Pose> > PoseGraph; boost::property<boost::edge_weight_t, Pose> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex; typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge; typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g; PoseGraph g;
PoseVertex root; PoseVertex root;
map<typename Config::Key, PoseVertex> key2vertex; map<typename Values::Key, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) = boost::tie(g, root, key2vertex) =
predecessorMap2Graph<PoseGraph, PoseVertex, typename Config::Key>(tree); predecessorMap2Graph<PoseGraph, PoseVertex, typename Values::Key>(tree);
// attach the relative poses to the edges // attach the relative poses to the edges
PoseEdge edge12, edge21; PoseEdge edge12, edge21;
@ -176,8 +176,8 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor); boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue; if (!factor) continue;
typename Config::Key key1 = factor->key1(); typename Values::Key key1 = factor->key1();
typename Config::Key key2 = factor->key2(); typename Values::Key key2 = factor->key2();
PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second; PoseVertex v2 = key2vertex.find(key2)->second;
@ -194,10 +194,10 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
} }
// compose poses // compose poses
boost::shared_ptr<Config> config(new Config); boost::shared_ptr<Values> config(new Values);
typename Config::Key rootKey = boost::get(boost::vertex_name, g, root); typename Values::Key rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose); config->insert(rootKey, rootPose);
compose_key_visitor<PoseVertex, Pose, Config> vis(config); compose_key_visitor<PoseVertex, Pose, Values> vis(config);
boost::breadth_first_search(g, root, boost::visitor(vis)); boost::breadth_first_search(g, root, boost::visitor(vis));
return config; return config;

6
inference/graph.h
View File

@ -76,8 +76,8 @@ namespace gtsam {
/** /**
* Compose the poses by following the chain specified by the spanning tree * Compose the poses by following the chain specified by the spanning tree
*/ */
template<class G, class Factor, class Pose, class Config> template<class G, class Factor, class Pose, class Values>
boost::shared_ptr<Config> boost::shared_ptr<Values>
composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree, const Pose& rootPose); composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree, const Pose& rootPose);
} // namespace gtsam } // namespace gtsam

2
inference/inference-inl.h
View File

@ -438,7 +438,7 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
// if(factor->keys().size() != 1 || factor->keys().front() != key) // if(factor->keys().size() != 1 || factor->keys().front() != key)
// throw runtime_error("Didn't get the right marginal!"); // throw runtime_error("Didn't get the right marginal!");
// //
// VectorConfig mean_cfg(marginal.optimize(Ordering(key))); // VectorValues mean_cfg(marginal.optimize(Ordering(key)));
// Matrix A(factor->get_A(key)); // Matrix A(factor->get_A(key));
// //
// return make_pair(mean_cfg[key], inverse(prod(trans(A), A))); // return make_pair(mean_cfg[key], inverse(prod(trans(A), A)));

2
inference/tests/testBinaryBayesNet.cpp
View File

@ -62,7 +62,7 @@ TEST( BinaryBayesNet, constructor )
// single parent conditional for x // single parent conditional for x
vector<double> cpt; vector<double> cpt;
cpt += 0.3, 0.6 ; // array index corresponds to binary parent configuration cpt += 0.3, 0.6 ; // array index corresponds to binary parent values structure
boost::shared_ptr<BinaryConditional> px_y(new BinaryConditional("x","y",cpt)); boost::shared_ptr<BinaryConditional> px_y(new BinaryConditional("x","y",cpt));
DOUBLES_EQUAL(0.7,px_y->probability(config),0.01); DOUBLES_EQUAL(0.7,px_y->probability(config),0.01);

26
linear/BayesNetPreconditioner.cpp
View File

@ -17,26 +17,26 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// R*x = y by solving x=inv(R)*y // R*x = y by solving x=inv(R)*y
VectorConfig BayesNetPreconditioner::backSubstitute(const VectorConfig& y) const { VectorValues BayesNetPreconditioner::backSubstitute(const VectorValues& y) const {
return gtsam::backSubstitute(Rd_, y); return gtsam::backSubstitute(Rd_, y);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// gy=inv(L)*gx by solving L*gy=gx. // gy=inv(L)*gx by solving L*gy=gx.
VectorConfig BayesNetPreconditioner::backSubstituteTranspose( VectorValues BayesNetPreconditioner::backSubstituteTranspose(
const VectorConfig& gx) const { const VectorValues& gx) const {
return gtsam::backSubstituteTranspose(Rd_, gx); return gtsam::backSubstituteTranspose(Rd_, gx);
} }
/* ************************************************************************* */ /* ************************************************************************* */
double BayesNetPreconditioner::error(const VectorConfig& y) const { double BayesNetPreconditioner::error(const VectorValues& y) const {
return Ab_.error(x(y)); return Ab_.error(x(y));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// gradient is inv(R')*A'*(A*inv(R)*y-b), // gradient is inv(R')*A'*(A*inv(R)*y-b),
VectorConfig BayesNetPreconditioner::gradient(const VectorConfig& y) const { VectorValues BayesNetPreconditioner::gradient(const VectorValues& y) const {
VectorConfig gx = VectorConfig::zero(y); VectorValues gx = VectorValues::zero(y);
Errors e = Ab_.errors(x(y)); Errors e = Ab_.errors(x(y));
Ab_.transposeMultiplyAdd(1.0,e,gx); Ab_.transposeMultiplyAdd(1.0,e,gx);
return gtsam::backSubstituteTranspose(Rd_, gx); return gtsam::backSubstituteTranspose(Rd_, gx);
@ -44,31 +44,31 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator * // Apply operator *
Errors BayesNetPreconditioner::operator*(const VectorConfig& y) const { Errors BayesNetPreconditioner::operator*(const VectorValues& y) const {
return Ab_ * x(y); return Ab_ * x(y);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// In-place version that overwrites e // In-place version that overwrites e
// TODO: version that takes scratch space for x // TODO: version that takes scratch space for x
void BayesNetPreconditioner::multiplyInPlace(const VectorConfig& y, Errors& e) const { void BayesNetPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
VectorConfig x = y; VectorValues x = y;
backSubstituteInPlace(Rd_,x); backSubstituteInPlace(Rd_,x);
Ab_.multiplyInPlace(x,e); Ab_.multiplyInPlace(x,e);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator inv(R')*A'*e // Apply operator inv(R')*A'*e
VectorConfig BayesNetPreconditioner::operator^(const Errors& e) const { VectorValues BayesNetPreconditioner::operator^(const Errors& e) const {
VectorConfig x = Ab_ ^ e; // x = A'*e2 VectorValues x = Ab_ ^ e; // x = A'*e2
return gtsam::backSubstituteTranspose(Rd_, x); return gtsam::backSubstituteTranspose(Rd_, x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// y += alpha*inv(R')*A'*e // y += alpha*inv(R')*A'*e
void BayesNetPreconditioner::transposeMultiplyAdd(double alpha, void BayesNetPreconditioner::transposeMultiplyAdd(double alpha,
const Errors& e, VectorConfig& y) const { const Errors& e, VectorValues& y) const {
VectorConfig x = VectorConfig::zero(y); VectorValues x = VectorValues::zero(y);
Ab_.transposeMultiplyAdd(1.0,e,x); // x += A'*e Ab_.transposeMultiplyAdd(1.0,e,x); // x += A'*e
axpy(alpha, gtsam::backSubstituteTranspose(Rd_, x), y); // y += alpha*inv(R')*x axpy(alpha, gtsam::backSubstituteTranspose(Rd_, x), y); // y += alpha*inv(R')*x
} }

18
linear/BayesNetPreconditioner.h
View File

@ -34,33 +34,33 @@ namespace gtsam {
const GaussianBayesNet& Rd); const GaussianBayesNet& Rd);
// R*x = y by solving x=inv(R)*y // R*x = y by solving x=inv(R)*y
VectorConfig backSubstitute(const VectorConfig& y) const; VectorValues backSubstitute(const VectorValues& y) const;
// gy=inv(L)*gx by solving L*gy=gx. // gy=inv(L)*gx by solving L*gy=gx.
VectorConfig backSubstituteTranspose(const VectorConfig& gx) const; VectorValues backSubstituteTranspose(const VectorValues& gx) const;
/* x = inv(R)*y */ /* x = inv(R)*y */
inline VectorConfig x(const VectorConfig& y) const { inline VectorValues x(const VectorValues& y) const {
return backSubstitute(y); return backSubstitute(y);
} }
/* error, given y */ /* error, given y */
double error(const VectorConfig& y) const; double error(const VectorValues& y) const;
/** gradient */ /** gradient */
VectorConfig gradient(const VectorConfig& y) const; VectorValues gradient(const VectorValues& y) const;
/** Apply operator A */ /** Apply operator A */
Errors operator*(const VectorConfig& y) const; Errors operator*(const VectorValues& y) const;
/** In-place version that overwrites e */ /** In-place version that overwrites e */
void multiplyInPlace(const VectorConfig& y, Errors& e) const; void multiplyInPlace(const VectorValues& y, Errors& e) const;
/** Apply operator A' */ /** Apply operator A' */
VectorConfig operator^(const Errors& e) const; VectorValues operator^(const Errors& e) const;
/** BLAS level 2 equivalent y += alpha*inv(R')*A'*e */ /** BLAS level 2 equivalent y += alpha*inv(R')*A'*e */
void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& y) const; void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
}; };
} // namespace gtsam } // namespace gtsam

2
linear/Errors.cpp
View File

@ -1,6 +1,6 @@
/** /**
* @file Errors.cpp * @file Errors.cpp
* @brief Factor Graph Configuration * @brief Factor Graph Values
* @brief Errors * @brief Errors
* @author Carlos Nieto * @author Carlos Nieto
* @author Christian Potthast * @author Christian Potthast

10
linear/Factorization.h
View File

@ -20,7 +20,7 @@ namespace gtsam {
/** /**
* A linear system solver using factorization * A linear system solver using factorization
*/ */
template <class NonlinearGraph, class Config> template <class NonlinearGraph, class Values>
class Factorization { class Factorization {
private: private:
boost::shared_ptr<Ordering> ordering_; boost::shared_ptr<Ordering> ordering_;
@ -35,14 +35,14 @@ namespace gtsam {
* solve for the optimal displacement in the tangent space, and then solve * solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system * the resulted linear system
*/ */
VectorConfig optimize(GaussianFactorGraph& fg) const { VectorValues optimize(GaussianFactorGraph& fg) const {
return gtsam::optimize(*Inference::Eliminate(fg)); return gtsam::optimize(*Inference::Eliminate(fg));
} }
/** /**
* linearize the non-linear graph around the current config * linearize the non-linear graph around the current config
*/ */
boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Config& config) const { boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Values& config) const {
return g.linearize(config, *ordering_); return g.linearize(config, *ordering_);
} }
@ -53,8 +53,8 @@ namespace gtsam {
return boost::shared_ptr<Factorization>(new Factorization(*this)); return boost::shared_ptr<Factorization>(new Factorization(*this));
} }
/** expmap the Config given the stored Ordering */ /** expmap the Values given the stored Ordering */
Config expmap(const Config& config, const VectorConfig& delta) const { Values expmap(const Values& config, const VectorValues& delta) const {
return config.expmap(delta, *ordering_); return config.expmap(delta, *ordering_);
} }
}; };

30
linear/GaussianBayesNet.cpp
View File

@ -10,7 +10,7 @@
#include <gtsam/base/Matrix-inl.h> #include <gtsam/base/Matrix-inl.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -59,25 +59,25 @@ void push_front(GaussianBayesNet& bn, varid_t key, Vector d, Matrix R,
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<VectorConfig> allocateVectorConfig(const GaussianBayesNet& bn) { boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) {
vector<size_t> dimensions(bn.size()); vector<size_t> dimensions(bn.size());
varid_t var = 0; varid_t var = 0;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) { BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
dimensions[var++] = conditional->get_R().size1(); dimensions[var++] = conditional->get_R().size1();
} }
return boost::shared_ptr<VectorConfig>(new VectorConfig(dimensions)); return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig optimize(const GaussianBayesNet& bn) VectorValues optimize(const GaussianBayesNet& bn)
{ {
return *optimize_(bn); return *optimize_(bn);
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<VectorConfig> optimize_(const GaussianBayesNet& bn) boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn)
{ {
boost::shared_ptr<VectorConfig> result(allocateVectorConfig(bn)); boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
/** solve each node in turn in topological sort order (parents first)*/ /** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) { BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) {
@ -88,16 +88,16 @@ boost::shared_ptr<VectorConfig> optimize_(const GaussianBayesNet& bn)
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig backSubstitute(const GaussianBayesNet& bn, const VectorConfig& y) { VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y) {
VectorConfig x(y); VectorValues x(y);
backSubstituteInPlace(bn,x); backSubstituteInPlace(bn,x);
return x; return x;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y) { void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y) {
VectorConfig& x = y; VectorValues& x = y;
/** solve each node in turn in topological sort order (parents first)*/ /** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) { BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
// i^th part of R*x=y, x=inv(R)*y // i^th part of R*x=y, x=inv(R)*y
@ -116,12 +116,12 @@ void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y) {
// gy=inv(L)*gx by solving L*gy=gx. // gy=inv(L)*gx by solving L*gy=gx.
// gy=inv(R'*inv(Sigma))*gx // gy=inv(R'*inv(Sigma))*gx
// gz'*R'=gx', gy = gz.*sigmas // gz'*R'=gx', gy = gz.*sigmas
VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn, VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
const VectorConfig& gx) { const VectorValues& gx) {
// Initialize gy from gx // Initialize gy from gx
// TODO: used to insert zeros if gx did not have an entry for a variable in bn // TODO: used to insert zeros if gx did not have an entry for a variable in bn
VectorConfig gy = gx; VectorValues gy = gx;
// we loop from first-eliminated to last-eliminated // we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L // i^th part of L*gy=gx is done block-column by block-column of L
@ -195,8 +195,8 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig rhs(const GaussianBayesNet& bn) { VectorValues rhs(const GaussianBayesNet& bn) {
boost::shared_ptr<VectorConfig> result(allocateVectorConfig(bn)); boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn) { BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn) {
varid_t key = cg->key(); varid_t key = cg->key();
// get sigmas // get sigmas

18
linear/GaussianBayesNet.h
View File

@ -43,30 +43,30 @@ namespace gtsam {
varid_t name1, Matrix S, varid_t name2, Matrix T, Vector sigmas); varid_t name1, Matrix S, varid_t name2, Matrix T, Vector sigmas);
/** /**
* Allocate a VectorConfig for the variables in a BayesNet * Allocate a VectorValues for the variables in a BayesNet
*/ */
boost::shared_ptr<VectorConfig> allocateVectorConfig(const GaussianBayesNet& bn); boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn);
/** /**
* optimize, i.e. return x = inv(R)*d * optimize, i.e. return x = inv(R)*d
*/ */
VectorConfig optimize(const GaussianBayesNet&); VectorValues optimize(const GaussianBayesNet&);
/** /**
* shared pointer version * shared pointer version
*/ */
boost::shared_ptr<VectorConfig> optimize_(const GaussianBayesNet& bn); boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn);
/* /*
* Backsubstitute * Backsubstitute
* (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) * (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
*/ */
VectorConfig backSubstitute(const GaussianBayesNet& bn, const VectorConfig& y); VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y);
/* /*
* Backsubstitute in place, y is replaced with solution * Backsubstitute in place, y is replaced with solution
*/ */
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y); void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y);
/* /*
* Transpose Backsubstitute * Transpose Backsubstitute
@ -74,7 +74,7 @@ namespace gtsam {
* gy=inv(R'*inv(Sigma))*gx * gy=inv(R'*inv(Sigma))*gx
* gz'*R'=gx', gy = gz.*sigmas * gz'*R'=gx', gy = gz.*sigmas
*/ */
VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn, const VectorConfig& gx); VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, const VectorValues& gx);
/** /**
* Return (dense) upper-triangular matrix representation * Return (dense) upper-triangular matrix representation
@ -82,9 +82,9 @@ namespace gtsam {
std::pair<Matrix, Vector> matrix(const GaussianBayesNet&); std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);
/** /**
* Return RHS d as a VectorConfig * Return RHS d as a VectorValues
* Such that backSubstitute(bn,d) = optimize(bn) * Such that backSubstitute(bn,d) = optimize(bn)
*/ */
VectorConfig rhs(const GaussianBayesNet&); VectorValues rhs(const GaussianBayesNet&);
} /// namespace gtsam } /// namespace gtsam

4
linear/GaussianConditional.cpp
View File

@ -160,7 +160,7 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianConditional::solve(const VectorConfig& x) const { Vector GaussianConditional::solve(const VectorValues& x) const {
static const bool debug = false; static const bool debug = false;
if(debug) print("Solving conditional "); if(debug) print("Solving conditional ");
Vector rhs(get_d()); Vector rhs(get_d());
@ -179,7 +179,7 @@ Vector GaussianConditional::solve(const VectorConfig& x) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianConditional::solve(const Permuted<VectorConfig>& x) const { Vector GaussianConditional::solve(const Permuted<VectorValues>& x) const {
Vector rhs(get_d()); Vector rhs(get_d());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false); ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false);

10
linear/GaussianConditional.h
View File

@ -15,7 +15,7 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/Conditional.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h> #include <gtsam/base/blockMatrices.h>
@ -118,17 +118,17 @@ public:
/** /**
* solve a conditional Gaussian * solve a conditional Gaussian
* @param x configuration in which the parents values (y,z,...) are known * @param x values structure in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...) * @return solution x = R \ (d - Sy - Tz - ...)
*/ */
Vector solve(const VectorConfig& x) const; Vector solve(const VectorValues& x) const;
/** /**
* solve a conditional Gaussian * solve a conditional Gaussian
* @param x configuration in which the parents values (y,z,...) are known * @param x values structure in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...) * @return solution x = R \ (d - Sy - Tz - ...)
*/ */
Vector solve(const Permuted<VectorConfig>& x) const; Vector solve(const Permuted<VectorValues>& x) const;
protected: protected:
rsd_type::column_type get_d_() { return rsd_.column(1+nrParents(), 0); } rsd_type::column_type get_d_() { return rsd_.column(1+nrParents(), 0); }

18
linear/GaussianFactor.cpp
View File

@ -249,7 +249,7 @@ void GaussianFactor::permuteWithInverse(const Permutation& inversePermutation) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianFactor::unweighted_error(const VectorConfig& c) const { Vector GaussianFactor::unweighted_error(const VectorValues& c) const {
Vector e = -getb(); Vector e = -getb();
if (empty()) return e; if (empty()) return e;
for(size_t pos=0; pos<keys_.size(); ++pos) for(size_t pos=0; pos<keys_.size(); ++pos)
@ -258,13 +258,13 @@ Vector GaussianFactor::unweighted_error(const VectorConfig& c) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianFactor::error_vector(const VectorConfig& c) const { Vector GaussianFactor::error_vector(const VectorValues& c) const {
if (empty()) return model_->whiten(-getb()); if (empty()) return model_->whiten(-getb());
return model_->whiten(unweighted_error(c)); return model_->whiten(unweighted_error(c));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double GaussianFactor::error(const VectorConfig& c) const { double GaussianFactor::error(const VectorValues& c) const {
if (empty()) return 0; if (empty()) return 0;
Vector weighted = error_vector(c); Vector weighted = error_vector(c);
return 0.5 * inner_prod(weighted,weighted); return 0.5 * inner_prod(weighted,weighted);
@ -287,7 +287,7 @@ double GaussianFactor::error(const VectorConfig& c) const {
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianFactor::operator*(const VectorConfig& x) const { Vector GaussianFactor::operator*(const VectorValues& x) const {
Vector Ax = zero(Ab_.size1()); Vector Ax = zero(Ab_.size1());
if (empty()) return Ax; if (empty()) return Ax;
@ -301,10 +301,10 @@ Vector GaussianFactor::operator*(const VectorConfig& x) const {
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactor::operator^(const Vector& e) const { //VectorValues GaussianFactor::operator^(const Vector& e) const {
// Vector E = model_->whiten(e); // Vector E = model_->whiten(e);
// VectorConfig x; // VectorValues x;
// // Just iterate over all A matrices and insert Ai^e into VectorConfig // // Just iterate over all A matrices and insert Ai^e into VectorValues
// for(size_t pos=0; pos<keys_.size(); ++pos) // for(size_t pos=0; pos<keys_.size(); ++pos)
// x.insert(keys_[pos], ARange(pos)^E); // x.insert(keys_[pos], ARange(pos)^E);
//// BOOST_FOREACH(const NamedMatrix& jA, As_) //// BOOST_FOREACH(const NamedMatrix& jA, As_)
@ -314,9 +314,9 @@ Vector GaussianFactor::operator*(const VectorConfig& x) const {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactor::transposeMultiplyAdd(double alpha, const Vector& e, void GaussianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
VectorConfig& x) const { VectorValues& x) const {
Vector E = alpha * model_->whiten(e); Vector E = alpha * model_->whiten(e);
// Just iterate over all A matrices and insert Ai^e into VectorConfig // Just iterate over all A matrices and insert Ai^e into VectorValues
for(size_t pos=0; pos<keys_.size(); ++pos) for(size_t pos=0; pos<keys_.size(); ++pos)
gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]); gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]);
// BOOST_FOREACH(const NamedMatrix& jA, As_) // BOOST_FOREACH(const NamedMatrix& jA, As_)

14
linear/GaussianFactor.h
View File

@ -29,7 +29,7 @@
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
#include <gtsam/inference/VariableSlots.h> #include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/SharedDiagonal.h> #include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
@ -105,9 +105,9 @@ public:
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
bool equals(const GaussianFactor& lf, double tol = 1e-9) const; bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
Vector unweighted_error(const VectorConfig& c) const; /** (A*x-b) */ Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
Vector error_vector(const VectorConfig& c) const; /** (A*x-b)/sigma */ Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
/** Check if the factor contains no information, i.e. zero rows. This does /** Check if the factor contains no information, i.e. zero rows. This does
* not necessarily mean that the factor involves no variables (to check for * not necessarily mean that the factor involves no variables (to check for
@ -170,13 +170,13 @@ public:
size_t numberOfRows() const { return Ab_.size1(); } size_t numberOfRows() const { return Ab_.size1(); }
/** Return A*x */ /** Return A*x */
Vector operator*(const VectorConfig& x) const; Vector operator*(const VectorValues& x) const;
// /** Return A'*e */ // /** Return A'*e */
// VectorConfig operator^(const Vector& e) const; // VectorValues operator^(const Vector& e) const;
/** x += A'*e */ /** x += A'*e */
void transposeMultiplyAdd(double alpha, const Vector& e, VectorConfig& x) const; void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;
/** /**
* Return (dense) matrix associated with factor * Return (dense) matrix associated with factor

64
linear/GaussianFactorGraph.cpp
View File

@ -51,7 +51,7 @@ void GaussianFactorGraph::permuteWithInverse(const Permutation& inversePermutati
} }
/* ************************************************************************* */ /* ************************************************************************* */
double GaussianFactorGraph::error(const VectorConfig& x) const { double GaussianFactorGraph::error(const VectorValues& x) const {
double total_error = 0.; double total_error = 0.;
BOOST_FOREACH(sharedFactor factor,factors_) BOOST_FOREACH(sharedFactor factor,factors_)
total_error += factor->error(x); total_error += factor->error(x);
@ -59,12 +59,12 @@ double GaussianFactorGraph::error(const VectorConfig& x) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Errors GaussianFactorGraph::errors(const VectorConfig& x) const { Errors GaussianFactorGraph::errors(const VectorValues& x) const {
return *errors_(x); return *errors_(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<Errors> GaussianFactorGraph::errors_(const VectorConfig& x) const { boost::shared_ptr<Errors> GaussianFactorGraph::errors_(const VectorValues& x) const {
boost::shared_ptr<Errors> e(new Errors); boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const sharedFactor& factor,factors_) BOOST_FOREACH(const sharedFactor& factor,factors_)
e->push_back(factor->error_vector(x)); e->push_back(factor->error_vector(x));
@ -72,7 +72,7 @@ boost::shared_ptr<Errors> GaussianFactorGraph::errors_(const VectorConfig& x) co
} }
/* ************************************************************************* */ /* ************************************************************************* */
Errors GaussianFactorGraph::operator*(const VectorConfig& x) const { Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
Errors e; Errors e;
BOOST_FOREACH(const sharedFactor& Ai,factors_) BOOST_FOREACH(const sharedFactor& Ai,factors_)
e.push_back((*Ai)*x); e.push_back((*Ai)*x);
@ -80,12 +80,12 @@ Errors GaussianFactorGraph::operator*(const VectorConfig& x) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x, Errors& e) const { void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
multiplyInPlace(x,e.begin()); multiplyInPlace(x,e.begin());
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x, void GaussianFactorGraph::multiplyInPlace(const VectorValues& x,
const Errors::iterator& e) const { const Errors::iterator& e) const {
Errors::iterator ei = e; Errors::iterator ei = e;
BOOST_FOREACH(const sharedFactor& Ai,factors_) { BOOST_FOREACH(const sharedFactor& Ai,factors_) {
@ -95,12 +95,12 @@ void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x,
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::operator^(const Errors& e) const { //VectorValues GaussianFactorGraph::operator^(const Errors& e) const {
// VectorConfig x; // VectorValues x;
// // For each factor add the gradient contribution // // For each factor add the gradient contribution
// Errors::const_iterator it = e.begin(); // Errors::const_iterator it = e.begin();
// BOOST_FOREACH(const sharedFactor& Ai,factors_) { // BOOST_FOREACH(const sharedFactor& Ai,factors_) {
// VectorConfig xi = (*Ai)^(*(it++)); // VectorValues xi = (*Ai)^(*(it++));
// x.insertAdd(xi); // x.insertAdd(xi);
// } // }
// return x; // return x;
@ -109,7 +109,7 @@ void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x,
/* ************************************************************************* */ /* ************************************************************************* */
// x += alpha*A'*e // x += alpha*A'*e
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
VectorConfig& x) const { VectorValues& x) const {
// For each factor add the gradient contribution // For each factor add the gradient contribution
Errors::const_iterator ei = e.begin(); Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const sharedFactor& Ai,factors_) BOOST_FOREACH(const sharedFactor& Ai,factors_)
@ -117,9 +117,9 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const { //VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const {
// // It is crucial for performance to make a zero-valued clone of x // // It is crucial for performance to make a zero-valued clone of x
// VectorConfig g = VectorConfig::zero(x); // VectorValues g = VectorValues::zero(x);
// transposeMultiplyAdd(1.0, errors(x), g); // transposeMultiplyAdd(1.0, errors(x), g);
// return g; // return g;
//} //}
@ -279,23 +279,23 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old) //VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, bool old)
//{ //{
// // eliminate all nodes in the given ordering -> chordal Bayes net // // eliminate all nodes in the given ordering -> chordal Bayes net
// GaussianBayesNet chordalBayesNet = eliminate(ordering, old); // GaussianBayesNet chordalBayesNet = eliminate(ordering, old);
// //
// // calculate new configuration (using backsubstitution) // // calculate new values structure (using backsubstitution)
// VectorConfig delta = ::optimize(chordalBayesNet); // VectorValues delta = ::optimize(chordalBayesNet);
// return delta; // return delta;
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering) //VectorValues GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
//{ //{
// GaussianJunctionTree junctionTree(*this, ordering); // GaussianJunctionTree junctionTree(*this, ordering);
// //
// // calculate new configuration (using backsubstitution) // // calculate new values structure (using backsubstitution)
// VectorConfig delta = junctionTree.optimize(); // VectorValues delta = junctionTree.optimize();
// return delta; // return delta;
//} //}
@ -312,9 +312,9 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//boost::shared_ptr<VectorConfig> //boost::shared_ptr<VectorValues>
//GaussianFactorGraph::optimize_(const Ordering& ordering) { //GaussianFactorGraph::optimize_(const Ordering& ordering) {
// return boost::shared_ptr<VectorConfig>(new VectorConfig(optimize(ordering))); // return boost::shared_ptr<VectorValues>(new VectorValues(optimize(ordering)));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -402,9 +402,9 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
//} //}
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::assembleConfig(const Vector& vs, const Ordering& ordering) const { //VectorValues GaussianFactorGraph::assembleValues(const Vector& vs, const Ordering& ordering) const {
// Dimensions dims = dimensions(); // Dimensions dims = dimensions();
// VectorConfig config; // VectorValues config;
// Vector::const_iterator itSrc = vs.begin(); // Vector::const_iterator itSrc = vs.begin();
// Vector::iterator itDst; // Vector::iterator itDst;
// BOOST_FOREACH(varid_t key, ordering){ // BOOST_FOREACH(varid_t key, ordering){
@ -415,7 +415,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
// config.insert(key, v); // config.insert(key, v);
// } // }
// if (itSrc != vs.end()) // if (itSrc != vs.end())
// throw runtime_error("assembleConfig: input vector and ordering are not compatible with the graph"); // throw runtime_error("assembleValues: input vector and ordering are not compatible with the graph");
// return config; // return config;
//} //}
// //
@ -508,30 +508,30 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
//} //}
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::steepestDescent(const VectorConfig& x0, //VectorValues GaussianFactorGraph::steepestDescent(const VectorValues& x0,
// bool verbose, double epsilon, size_t maxIterations) const { // bool verbose, double epsilon, size_t maxIterations) const {
// return gtsam::steepestDescent(*this, x0, verbose, epsilon, maxIterations); // return gtsam::steepestDescent(*this, x0, verbose, epsilon, maxIterations);
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//boost::shared_ptr<VectorConfig> GaussianFactorGraph::steepestDescent_( //boost::shared_ptr<VectorValues> GaussianFactorGraph::steepestDescent_(
// const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const { // const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return boost::shared_ptr<VectorConfig>(new VectorConfig( // return boost::shared_ptr<VectorValues>(new VectorValues(
// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon, // gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
// maxIterations))); // maxIterations)));
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::conjugateGradientDescent( //VectorValues GaussianFactorGraph::conjugateGradientDescent(
// const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const { // const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon, // return gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
// maxIterations); // maxIterations);
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//boost::shared_ptr<VectorConfig> GaussianFactorGraph::conjugateGradientDescent_( //boost::shared_ptr<VectorValues> GaussianFactorGraph::conjugateGradientDescent_(
// const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const { // const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return boost::shared_ptr<VectorConfig>(new VectorConfig( // return boost::shared_ptr<VectorValues>(new VectorValues(
// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon, // gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
// maxIterations))); // maxIterations)));
//} //}

54
linear/GaussianFactorGraph.h
View File

@ -5,10 +5,6 @@
* @author Christian Potthast * @author Christian Potthast
* @author Alireza Fathi * @author Alireza Fathi
*/ */
// $Id: GaussianFactorGraph.h,v 1.24 2009/08/14 20:48:51 acunning Exp $
// \callgraph
#pragma once #pragma once
@ -24,7 +20,7 @@ namespace gtsam {
/** /**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == GaussianFactor * Factor == GaussianFactor
* VectorConfig = A configuration of vectors * VectorValues = A values structure of vectors
* Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph.
*/ */
class GaussianFactorGraph : public FactorGraph<GaussianFactor> { class GaussianFactorGraph : public FactorGraph<GaussianFactor> {
@ -86,38 +82,38 @@ namespace gtsam {
void permuteWithInverse(const Permutation& inversePermutation); void permuteWithInverse(const Permutation& inversePermutation);
/** return A*x-b */ /** return A*x-b */
Errors errors(const VectorConfig& x) const; Errors errors(const VectorValues& x) const;
/** shared pointer version */ /** shared pointer version */
boost::shared_ptr<Errors> errors_(const VectorConfig& x) const; boost::shared_ptr<Errors> errors_(const VectorValues& x) const;
/** unnormalized error */ /** unnormalized error */
double error(const VectorConfig& x) const; double error(const VectorValues& x) const;
/** return A*x */ /** return A*x */
Errors operator*(const VectorConfig& x) const; Errors operator*(const VectorValues& x) const;
/* In-place version e <- A*x that overwrites e. */ /* In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const VectorConfig& x, Errors& e) const; void multiplyInPlace(const VectorValues& x, Errors& e) const;
/* In-place version e <- A*x that takes an iterator. */ /* In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const VectorConfig& x, const Errors::iterator& e) const; void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
// /** return A^e */ // /** return A^e */
// VectorConfig operator^(const Errors& e) const; // VectorValues operator^(const Errors& e) const;
/** x += alpha*A'*e */ /** x += alpha*A'*e */
void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& x) const; void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
// /** // /**
// * Calculate Gradient of A^(A*x-b) for a given config // * Calculate Gradient of A^(A*x-b) for a given config
// * @param x: VectorConfig specifying where to calculate gradient // * @param x: VectorValues specifying where to calculate gradient
// * @return gradient, as a VectorConfig as well // * @return gradient, as a VectorValues as well
// */ // */
// VectorConfig gradient(const VectorConfig& x) const; // VectorValues gradient(const VectorValues& x) const;
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const VectorConfig& c) const { double probPrime(const VectorValues& c) const {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
@ -145,19 +141,19 @@ namespace gtsam {
// * @param enableJoinFactor uses the older joint factor combine process when true, // * @param enableJoinFactor uses the older joint factor combine process when true,
// * and when false uses the newer single matrix combine // * and when false uses the newer single matrix combine
// */ // */
// VectorConfig optimize(const Ordering& ordering, bool enableJoinFactor = true); // VectorValues optimize(const Ordering& ordering, bool enableJoinFactor = true);
// /** // /**
// * optimize a linear factor graph using a multi-frontal solver // * optimize a linear factor graph using a multi-frontal solver
// * @param ordering fg in order // * @param ordering fg in order
// */ // */
// VectorConfig optimizeMultiFrontals(const Ordering& ordering); // VectorValues optimizeMultiFrontals(const Ordering& ordering);
// /** // /**
// * shared pointer versions for MATLAB // * shared pointer versions for MATLAB
// */ // */
// boost::shared_ptr<GaussianBayesNet> eliminate_(const Ordering&); // boost::shared_ptr<GaussianBayesNet> eliminate_(const Ordering&);
// boost::shared_ptr<VectorConfig> optimize_(const Ordering&); // boost::shared_ptr<VectorValues> optimize_(const Ordering&);
/** /**
* static function that combines two factor graphs * static function that combines two factor graphs
@ -207,7 +203,7 @@ namespace gtsam {
// * @param v: the source vector // * @param v: the source vector
// * @param ordeirng: the ordering corresponding to the vector // * @param ordeirng: the ordering corresponding to the vector
// */ // */
// VectorConfig assembleConfig(const Vector& v, const Ordering& ordering) const; // VectorValues assembleValues(const Vector& v, const Ordering& ordering) const;
// //
// /** // /**
// * get the 1-based starting column indices for all variables // * get the 1-based starting column indices for all variables
@ -237,32 +233,32 @@ namespace gtsam {
// /** // /**
// * Find solution using gradient descent // * Find solution using gradient descent
// * @param x0: VectorConfig specifying initial estimate // * @param x0: VectorValues specifying initial estimate
// * @return solution // * @return solution
// */ // */
// VectorConfig steepestDescent(const VectorConfig& x0, bool verbose = false, // VectorValues steepestDescent(const VectorValues& x0, bool verbose = false,
// double epsilon = 1e-3, size_t maxIterations = 0) const; // double epsilon = 1e-3, size_t maxIterations = 0) const;
// //
// /** // /**
// * shared pointer versions for MATLAB // * shared pointer versions for MATLAB
// */ // */
// boost::shared_ptr<VectorConfig> // boost::shared_ptr<VectorValues>
// steepestDescent_(const VectorConfig& x0, bool verbose = false, // steepestDescent_(const VectorValues& x0, bool verbose = false,
// double epsilon = 1e-3, size_t maxIterations = 0) const; // double epsilon = 1e-3, size_t maxIterations = 0) const;
// //
// /** // /**
// * Find solution using conjugate gradient descent // * Find solution using conjugate gradient descent
// * @param x0: VectorConfig specifying initial estimate // * @param x0: VectorValues specifying initial estimate
// * @return solution // * @return solution
// */ // */
// VectorConfig conjugateGradientDescent(const VectorConfig& x0, bool verbose = // VectorValues conjugateGradientDescent(const VectorValues& x0, bool verbose =
// false, double epsilon = 1e-3, size_t maxIterations = 0) const; // false, double epsilon = 1e-3, size_t maxIterations = 0) const;
// //
// /** // /**
// * shared pointer versions for MATLAB // * shared pointer versions for MATLAB
// */ // */
// boost::shared_ptr<VectorConfig> conjugateGradientDescent_( // boost::shared_ptr<VectorValues> conjugateGradientDescent_(
// const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3, // const VectorValues& x0, bool verbose = false, double epsilon = 1e-3,
// size_t maxIterations = 0) const; // size_t maxIterations = 0) const;
}; };

6
linear/GaussianISAM.cpp
View File

@ -16,7 +16,7 @@ template class ISAM<GaussianConditional>;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result) { void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result // parents are assumed to already be solved and available in result
GaussianISAM::Clique::const_reverse_iterator it; GaussianISAM::Clique::const_reverse_iterator it;
for (it = clique->rbegin(); it!=clique->rend(); it++) { for (it = clique->rbegin(); it!=clique->rend(); it++) {
@ -32,8 +32,8 @@ void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig optimize(const GaussianISAM& bayesTree) { VectorValues optimize(const GaussianISAM& bayesTree) {
VectorConfig result(bayesTree.dims_); VectorValues result(bayesTree.dims_);
// starting from the root, call optimize on each conditional // starting from the root, call optimize on each conditional
optimize(bayesTree.root(), result); optimize(bayesTree.root(), result);
return result; return result;

6
linear/GaussianISAM.h
View File

@ -56,14 +56,14 @@ public:
dims_.clear(); dims_.clear();
} }
friend VectorConfig optimize(const GaussianISAM&); friend VectorValues optimize(const GaussianISAM&);
}; };
// recursively optimize this conditional and all subtrees // recursively optimize this conditional and all subtrees
void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result); void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result);
// optimize the BayesTree, starting from the root // optimize the BayesTree, starting from the root
VectorConfig optimize(const GaussianISAM& bayesTree); VectorValues optimize(const GaussianISAM& bayesTree);
}/// namespace gtsam }/// namespace gtsam

10
linear/GaussianJunctionTree.cpp
View File

@ -25,7 +25,7 @@ namespace gtsam {
/** /**
* GaussianJunctionTree * GaussianJunctionTree
*/ */
void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorConfig& config) const { void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const {
// solve the bayes net in the current node // solve the bayes net in the current node
GaussianBayesNet::const_reverse_iterator it = current->rbegin(); GaussianBayesNet::const_reverse_iterator it = current->rbegin();
for (; it!=current->rend(); ++it) { for (; it!=current->rend(); ++it) {
@ -52,18 +52,18 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig GaussianJunctionTree::optimize() const { VectorValues GaussianJunctionTree::optimize() const {
tic("GJT optimize 1: eliminate"); tic("GJT optimize 1: eliminate");
// eliminate from leaves to the root // eliminate from leaves to the root
boost::shared_ptr<const BayesTree::Clique> rootClique(this->eliminate()); boost::shared_ptr<const BayesTree::Clique> rootClique(this->eliminate());
toc("GJT optimize 1: eliminate"); toc("GJT optimize 1: eliminate");
// Allocate solution vector // Allocate solution vector
tic("GJT optimize 2: allocate VectorConfig"); tic("GJT optimize 2: allocate VectorValues");
vector<size_t> dims(rootClique->back()->key() + 1, 0); vector<size_t> dims(rootClique->back()->key() + 1, 0);
countDims(rootClique, dims); countDims(rootClique, dims);
VectorConfig result(dims); VectorValues result(dims);
toc("GJT optimize 2: allocate VectorConfig"); toc("GJT optimize 2: allocate VectorValues");
// back-substitution // back-substitution
tic("GJT optimize 3: back-substitute"); tic("GJT optimize 3: back-substitute");

4
linear/GaussianJunctionTree.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
protected: protected:
// back-substitute in topological sort order (parents first) // back-substitute in topological sort order (parents first)
void btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorConfig& config) const; void btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const;
public : public :
@ -35,7 +35,7 @@ namespace gtsam {
GaussianJunctionTree(const GaussianFactorGraph& fg) : Base(fg) {} GaussianJunctionTree(const GaussianFactorGraph& fg) : Base(fg) {}
// optimize the linear graph // optimize the linear graph
VectorConfig optimize() const; VectorValues optimize() const;
}; // GaussianJunctionTree }; // GaussianJunctionTree
} // namespace gtsam } // namespace gtsam

4
linear/Makefile.am
View File

@ -16,7 +16,7 @@ sources += NoiseModel.cpp Errors.cpp
check_PROGRAMS += tests/testNoiseModel tests/testErrors check_PROGRAMS += tests/testNoiseModel tests/testErrors
# Vector Configurations # Vector Configurations
headers += VectorConfig.h headers += VectorValues.h
#sources += VectorMap.cpp VectorBTree.cpp #sources += VectorMap.cpp VectorBTree.cpp
#check_PROGRAMS += tests/testVectorMap tests/testVectorBTree #check_PROGRAMS += tests/testVectorMap tests/testVectorBTree
@ -36,7 +36,7 @@ check_PROGRAMS += tests/testGaussianJunctionTree
# Timing tests # Timing tests
noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike
#noinst_PROGRAMS += tests/timeVectorConfig #noinst_PROGRAMS += tests/timeVectorValues
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed # Create a libtool library that is not installed

38
linear/SubgraphPreconditioner.cpp
View File

@ -13,25 +13,25 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2,
sharedBayesNet& Rc1, sharedConfig& xbar) : sharedBayesNet& Rc1, sharedValues& xbar) :
Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) { Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// x = xbar + inv(R1)*y // x = xbar + inv(R1)*y
VectorConfig SubgraphPreconditioner::x(const VectorConfig& y) const { VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
#ifdef VECTORBTREE #ifdef VECTORBTREE
if (!y.cloned(*xbar_)) throw if (!y.cloned(*xbar_)) throw
invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar"); invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar");
#endif #endif
VectorConfig x = y; VectorValues x = y;
backSubstituteInPlace(*Rc1_,x); backSubstituteInPlace(*Rc1_,x);
x += *xbar_; x += *xbar_;
return x; return x;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorConfig& y) const { double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e; Errors e;
@ -42,7 +42,7 @@ namespace gtsam {
} }
// Add A2 contribution // Add A2 contribution
VectorConfig x = this->x(y); VectorValues x = this->x(y);
Errors e2 = Ab2_->errors(x); Errors e2 = Ab2_->errors(x);
e.splice(e.end(), e2); e.splice(e.end(), e2);
@ -51,18 +51,18 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorConfig SubgraphPreconditioner::gradient(const VectorConfig& y) const { VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
VectorConfig x = this->x(y); // x = inv(R1)*y VectorValues x = this->x(y); // x = inv(R1)*y
Errors e2 = Ab2_->errors(x); Errors e2 = Ab2_->errors(x);
VectorConfig gx2 = VectorConfig::zero(y); VectorValues gx2 = VectorValues::zero(y);
Ab2_->transposeMultiplyAdd(1.0,e2,gx2); // A2'*e2; Ab2_->transposeMultiplyAdd(1.0,e2,gx2); // A2'*e2;
VectorConfig gy2 = gtsam::backSubstituteTranspose(*Rc1_, gx2); // inv(R1')*gx2 VectorValues gy2 = gtsam::backSubstituteTranspose(*Rc1_, gx2); // inv(R1')*gx2
return y + gy2; return y + gy2;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorConfig& y) const { Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e; Errors e;
@ -73,7 +73,7 @@ namespace gtsam {
} }
// Add A2 contribution // Add A2 contribution
VectorConfig x = y; // TODO avoid ? VectorValues x = y; // TODO avoid ?
gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y
Errors e2 = *Ab2_ * x; // A2*x Errors e2 = *Ab2_ * x; // A2*x
e.splice(e.end(), e2); e.splice(e.end(), e2);
@ -83,7 +83,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// In-place version that overwrites e // In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorConfig& y, Errors& e) const { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin(); Errors::iterator ei = e.begin();
@ -95,16 +95,16 @@ namespace gtsam {
} }
// Add A2 contribution // Add A2 contribution
VectorConfig x = y; // TODO avoid ? VectorValues x = y; // TODO avoid ?
gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y
Ab2_->multiplyInPlace(x,ei); // use iterator version Ab2_->multiplyInPlace(x,ei); // use iterator version
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorConfig SubgraphPreconditioner::operator^(const Errors& e) const { VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
VectorConfig y; VectorValues y;
// Use BayesNet order to remove y contributions in order // Use BayesNet order to remove y contributions in order
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
@ -123,7 +123,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// y += alpha*A'*e // y += alpha*A'*e
void SubgraphPreconditioner::transposeMultiplyAdd void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorConfig& y) const { (double alpha, const Errors& e, VectorValues& y) const {
// Use BayesNet order to remove y contributions in order // Use BayesNet order to remove y contributions in order
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
@ -140,7 +140,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2 // y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
Errors::const_iterator it, Errors::const_iterator end, VectorConfig& y) const { Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {
// create e2 with what's left of e // create e2 with what's left of e
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
@ -149,10 +149,10 @@ namespace gtsam {
e2.push_back(*(it++)); e2.push_back(*(it++));
// Old code: // Old code:
// VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2 // VectorValues x = *Ab2_ ^ e2; // x = A2'*e2
// y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; // y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x;
// New Code: // New Code:
VectorConfig x = VectorConfig::zero(y); // x = 0 VectorValues x = VectorValues::zero(y); // x = 0
Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
} }

28
linear/SubgraphPreconditioner.h
View File

@ -24,13 +24,13 @@ namespace gtsam {
public: public:
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet; typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG; typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
typedef boost::shared_ptr<const VectorConfig> sharedConfig; typedef boost::shared_ptr<const VectorValues> sharedValues;
typedef boost::shared_ptr<const Errors> sharedErrors; typedef boost::shared_ptr<const Errors> sharedErrors;
private: private:
sharedFG Ab1_, Ab2_; sharedFG Ab1_, Ab2_;
sharedBayesNet Rc1_; sharedBayesNet Rc1_;
sharedConfig xbar_; sharedValues xbar_;
sharedErrors b2bar_; /** b2 - A2*xbar */ sharedErrors b2bar_; /** b2 - A2*xbar */
public: public:
@ -42,7 +42,7 @@ namespace gtsam {
* @param Rc1: the Bayes Net R1*x=c1 * @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1 * @param xbar: the solution to R1*x=c1
*/ */
SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedConfig& xbar); SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedValues& xbar);
std::pair<Matrix,Vector> Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); } std::pair<Matrix,Vector> Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); }
std::pair<Matrix,Vector> Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); } std::pair<Matrix,Vector> Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); }
@ -50,34 +50,34 @@ namespace gtsam {
Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(Ab1_->columnIndices(ordering)); } Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(Ab1_->columnIndices(ordering)); }
Vector b1() const { return Ab1_->rhsVector(); } Vector b1() const { return Ab1_->rhsVector(); }
Vector b2() const { return Ab2_->rhsVector(); } Vector b2() const { return Ab2_->rhsVector(); }
VectorConfig assembleConfig(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleConfig(v, ordering); } VectorValues assembleValues(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleValues(v, ordering); }
/* x = xbar + inv(R1)*y */ /* x = xbar + inv(R1)*y */
VectorConfig x(const VectorConfig& y) const; VectorValues x(const VectorValues& y) const;
/* A zero VectorConfig with the structure of xbar */ /* A zero VectorValues with the structure of xbar */
VectorConfig zero() const { return VectorConfig::zero(*xbar_);} VectorValues zero() const { return VectorValues::zero(*xbar_);}
/* error, given y */ /* error, given y */
double error(const VectorConfig& y) const; double error(const VectorValues& y) const;
/** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */
VectorConfig gradient(const VectorConfig& y) const; VectorValues gradient(const VectorValues& y) const;
/** Apply operator A */ /** Apply operator A */
Errors operator*(const VectorConfig& y) const; Errors operator*(const VectorValues& y) const;
/** Apply operator A in place: needs e allocated already */ /** Apply operator A in place: needs e allocated already */
void multiplyInPlace(const VectorConfig& y, Errors& e) const; void multiplyInPlace(const VectorValues& y, Errors& e) const;
/** Apply operator A' */ /** Apply operator A' */
VectorConfig operator^(const Errors& e) const; VectorValues operator^(const Errors& e) const;
/** /**
* Add A'*e to y * Add A'*e to y
* y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
*/ */
void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& y) const; void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
/** /**
* Add constraint part of the error only, used in both calls above * Add constraint part of the error only, used in both calls above
@ -85,7 +85,7 @@ namespace gtsam {
* Takes a range indicating e2 !!!! * Takes a range indicating e2 !!!!
*/ */
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
Errors::const_iterator end, VectorConfig& y) const; Errors::const_iterator end, VectorValues& y) const;
/** print the object */ /** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const; void print(const std::string& s = "SubgraphPreconditioner") const;

28
linear/SubgraphSolver-inl.h
View File

@ -20,14 +20,14 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Config> template<class Graph, class Values>
SubgraphSolver<Graph, Config>::SubgraphSolver(const Graph& G, const Config& theta0) { SubgraphSolver<Graph, Values>::SubgraphSolver(const Graph& G, const Values& theta0) {
initialize(G,theta0); initialize(G,theta0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Config> template<class Graph, class Values>
void SubgraphSolver<Graph, Config>::initialize(const Graph& G, const Config& theta0) { void SubgraphSolver<Graph, Values>::initialize(const Graph& G, const Values& theta0) {
// generate spanning tree // generate spanning tree
PredecessorMap<Key> tree = G.template findMinimumSpanningTree<Key, Constraint>(); PredecessorMap<Key> tree = G.template findMinimumSpanningTree<Key, Constraint>();
@ -46,21 +46,21 @@ namespace gtsam {
// compose the approximate solution // compose the approximate solution
Key root = keys.back(); Key root = keys.back();
theta_bar_ = composePoses<Graph, Constraint, Pose, Config> (T_, tree, theta0[root]); theta_bar_ = composePoses<Graph, Constraint, Pose, Values> (T_, tree, theta0[root]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Config> template<class Graph, class Values>
boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<Graph, Config>::linearize(const Graph& G, const Config& theta_bar) const { boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<Graph, Values>::linearize(const Graph& G, const Values& theta_bar) const {
SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar); SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar);
SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar); SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar);
#ifdef TIMING #ifdef TIMING
SubgraphPreconditioner::sharedBayesNet Rc1; SubgraphPreconditioner::sharedBayesNet Rc1;
SubgraphPreconditioner::sharedConfig xbar; SubgraphPreconditioner::sharedValues xbar;
#else #else
GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!! GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_); SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_);
SubgraphPreconditioner::sharedConfig xbar = gtsam::optimize_(*Rc1); SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
#endif #endif
// TODO: there does not seem to be a good reason to have Ab1_ // TODO: there does not seem to be a good reason to have Ab1_
// It seems only be used to provide an ordering for creating sparse matrices // It seems only be used to provide an ordering for creating sparse matrices
@ -68,14 +68,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Graph, class Config> template<class Graph, class Values>
VectorConfig SubgraphSolver<Graph, Config>::optimize(SubgraphPreconditioner& system) const { VectorValues SubgraphSolver<Graph, Values>::optimize(SubgraphPreconditioner& system) const {
VectorConfig zeros = system.zero(); VectorValues zeros = system.zero();
// Solve the subgraph PCG // Solve the subgraph PCG
VectorConfig ybar = conjugateGradients<SubgraphPreconditioner, VectorConfig, VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
Errors> (system, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_); Errors> (system, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_);
VectorConfig xbar = system.x(ybar); VectorValues xbar = system.x(ybar);
return xbar; return xbar;
} }

26
linear/SubgraphSolver.h
View File

@ -17,13 +17,13 @@ namespace gtsam {
* A nonlinear system solver using subgraph preconditioning conjugate gradient * A nonlinear system solver using subgraph preconditioning conjugate gradient
* Concept NonLinearSolver<G,T,L> implements * Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L * linearize: G * T -> L
* solve : L -> VectorConfig * solve : L -> VectorValues
*/ */
template<class Graph, class Config> template<class Graph, class Values>
class SubgraphSolver { class SubgraphSolver {
private: private:
typedef typename Config::Key Key; typedef typename Values::Key Key;
typedef typename Graph::Constraint Constraint; typedef typename Graph::Constraint Constraint;
typedef typename Graph::Pose Pose; typedef typename Graph::Pose Pose;
@ -36,40 +36,40 @@ namespace gtsam {
boost::shared_ptr<Ordering> ordering_; boost::shared_ptr<Ordering> ordering_;
/* the solution computed from the first subgraph */ /* the solution computed from the first subgraph */
boost::shared_ptr<Config> theta_bar_; boost::shared_ptr<Values> theta_bar_;
Graph T_, C_; Graph T_, C_;
public: public:
SubgraphSolver() {} SubgraphSolver() {}
SubgraphSolver(const Graph& G, const Config& theta0); SubgraphSolver(const Graph& G, const Values& theta0);
void initialize(const Graph& G, const Config& theta0); void initialize(const Graph& G, const Values& theta0);
boost::shared_ptr<Ordering> ordering() const { return ordering_; } boost::shared_ptr<Ordering> ordering() const { return ordering_; }
boost::shared_ptr<Config> theta_bar() const { return theta_bar_; } boost::shared_ptr<Values> theta_bar() const { return theta_bar_; }
/** /**
* linearize the non-linear graph around the current config and build the subgraph preconditioner systme * linearize the non-linear graph around the current config and build the subgraph preconditioner systme
*/ */
boost::shared_ptr<SubgraphPreconditioner> linearize(const Graph& G, const Config& theta_bar) const; boost::shared_ptr<SubgraphPreconditioner> linearize(const Graph& G, const Values& theta_bar) const;
/** /**
* solve for the optimal displacement in the tangent space, and then solve * solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system * the resulted linear system
*/ */
VectorConfig optimize(SubgraphPreconditioner& system) const; VectorValues optimize(SubgraphPreconditioner& system) const;
boost::shared_ptr<SubgraphSolver> prepareLinear(const SubgraphPreconditioner& fg) const { boost::shared_ptr<SubgraphSolver> prepareLinear(const SubgraphPreconditioner& fg) const {
return boost::shared_ptr<SubgraphSolver>(new SubgraphSolver(*this)); return boost::shared_ptr<SubgraphSolver>(new SubgraphSolver(*this));
} }
}; };
template<class Graph, class Config> const size_t SubgraphSolver<Graph,Config>::maxIterations_; template<class Graph, class Values> const size_t SubgraphSolver<Graph,Values>::maxIterations_;
template<class Graph, class Config> const bool SubgraphSolver<Graph,Config>::verbose_; template<class Graph, class Values> const bool SubgraphSolver<Graph,Values>::verbose_;
template<class Graph, class Config> const double SubgraphSolver<Graph,Config>::epsilon_; template<class Graph, class Values> const double SubgraphSolver<Graph,Values>::epsilon_;
template<class Graph, class Config> const double SubgraphSolver<Graph,Config>::epsilon_abs_; template<class Graph, class Values> const double SubgraphSolver<Graph,Values>::epsilon_abs_;
} // nsamespace gtsam } // nsamespace gtsam

8
linear/VectorBTree.cpp
View File

@ -1,7 +1,7 @@
/** /**
* @file VectorConfig.cpp * @file VectorValues.cpp
* @brief Factor Graph Configuration * @brief Factor Graph Values
* @brief VectorConfig * @brief VectorValues
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -14,7 +14,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Check if two VectorConfigs are compatible, throw exception if not // Check if two VectorValuess are compatible, throw exception if not
static void check_compatible(const string& s, const VectorBTree& a, static void check_compatible(const string& s, const VectorBTree& a,
const VectorBTree& b) { const VectorBTree& b) {
if (!a.compatible(b)) if (!a.compatible(b))

6
linear/VectorBTree.h
View File

@ -1,6 +1,6 @@
/** /**
* @file VectorBTree.h * @file VectorBTree.h
* @brief Factor Graph Configuration * @brief Factor Graph Valuesuration
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -19,7 +19,7 @@
namespace gtsam { namespace gtsam {
/** Factor Graph Configuration */ /** Factor Graph Valuesuration */
class VectorBTree: public Testable<VectorBTree> { class VectorBTree: public Testable<VectorBTree> {
private: private:
@ -74,7 +74,7 @@ public:
/** equals, for unit testing */ /** equals, for unit testing */
bool equals(const VectorBTree& expected, double tol = 1e-9) const; bool equals(const VectorBTree& expected, double tol = 1e-9) const;
/** Insert a value into the configuration with a given index: O(n) */ /** Insert a value into the values structure with a given index: O(n) */
VectorBTree& insert(const Symbol& j, const Vector& v); VectorBTree& insert(const Symbol& j, const Vector& v);
/** Insert or add a value with given index: O(n) if does not exist */ /** Insert or add a value with given index: O(n) if does not exist */

16
linear/VectorMap.cpp
View File

@ -1,6 +1,6 @@
/** /**
* @file VectorMap.cpp * @file VectorMap.cpp
* @brief Factor Graph Configuration * @brief Factor Graph Values
* @brief VectorMap * @brief VectorMap
* @author Carlos Nieto * @author Carlos Nieto
* @author Christian Potthast * @author Christian Potthast
@ -148,33 +148,33 @@ Vector VectorMap::vector() const {
/* ************************************************************************* */ /* ************************************************************************* */
VectorMap expmap(const VectorMap& original, const VectorMap& delta) VectorMap expmap(const VectorMap& original, const VectorMap& delta)
{ {
VectorMap newConfig; VectorMap newValues;
varid_t j; Vector vj; // rtodo: copying vector? varid_t j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) { FOREACH_PAIR(j, vj, original.values) {
if (delta.contains(j)) { if (delta.contains(j)) {
const Vector& dj = delta[j]; const Vector& dj = delta[j];
check_size(j,vj,dj); check_size(j,vj,dj);
newConfig.insert(j, vj + dj); newValues.insert(j, vj + dj);
} else { } else {
newConfig.insert(j, vj); newValues.insert(j, vj);
} }
} }
return newConfig; return newValues;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorMap expmap(const VectorMap& original, const Vector& delta) VectorMap expmap(const VectorMap& original, const Vector& delta)
{ {
VectorMap newConfig; VectorMap newValues;
size_t i = 0; size_t i = 0;
varid_t j; Vector vj; // rtodo: copying vector? varid_t j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) { FOREACH_PAIR(j, vj, original.values) {
size_t mj = vj.size(); size_t mj = vj.size();
Vector dj = sub(delta, i, i+mj); Vector dj = sub(delta, i, i+mj);
newConfig.insert(j, vj + dj); newValues.insert(j, vj + dj);
i += mj; i += mj;
} }
return newConfig; return newValues;
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
linear/VectorMap.h
View File

@ -1,6 +1,6 @@
/** /**
* @file VectorMap.h * @file VectorMap.h
* @brief Factor Graph Configuration * @brief Factor Graph Valuesuration
* @author Carlos Nieto * @author Carlos Nieto
* @author Christian Potthast * @author Christian Potthast
*/ */
@ -18,7 +18,7 @@
namespace gtsam { namespace gtsam {
/** Factor Graph Configuration */ /** Factor Graph Valuesuration */
class VectorMap : public Testable<VectorMap> { class VectorMap : public Testable<VectorMap> {
protected: protected:
@ -41,7 +41,7 @@ namespace gtsam {
/** convert into a single large vector */ /** convert into a single large vector */
Vector vector() const; Vector vector() const;
/** Insert a value into the configuration with a given index */ /** Insert a value into the values structure with a given index */
VectorMap& insert(varid_t name, const Vector& v); VectorMap& insert(varid_t name, const Vector& v);
/** Insert or add a value with given index */ /** Insert or add a value with given index */

52
linear/VectorConfig.h → linear/VectorValues.h
View File

@ -1,6 +1,6 @@
/** /**
* @file VectorConfig.h * @file VectorValues.h
* @brief Factor Graph Configuration * @brief Factor Graph Valuesuration
* @author Richard Roberts * @author Richard Roberts
*/ */
@ -19,16 +19,16 @@
namespace gtsam { namespace gtsam {
class VectorConfig : public Testable<VectorConfig> { class VectorValues : public Testable<VectorValues> {
protected: protected:
Vector values_; Vector values_;
std::vector<size_t> varStarts_; std::vector<size_t> varStarts_;
public: public:
template<class C> class _impl_iterator; // Forward declaration of iterator implementation template<class C> class _impl_iterator; // Forward declaration of iterator implementation
typedef boost::shared_ptr<VectorConfig> shared_ptr; typedef boost::shared_ptr<VectorValues> shared_ptr;
typedef _impl_iterator<VectorConfig> iterator; typedef _impl_iterator<VectorValues> iterator;
typedef _impl_iterator<const VectorConfig> const_iterator; typedef _impl_iterator<const VectorValues> const_iterator;
typedef boost::numeric::ublas::vector_range<Vector> value_reference_type; typedef boost::numeric::ublas::vector_range<Vector> value_reference_type;
typedef boost::numeric::ublas::vector_range<const Vector> const_value_reference_type; typedef boost::numeric::ublas::vector_range<const Vector> const_value_reference_type;
typedef boost::numeric::ublas::vector_range<Vector> mapped_type; typedef boost::numeric::ublas::vector_range<Vector> mapped_type;
@ -38,23 +38,23 @@ public:
// * Constructor requires an existing GaussianVariableIndex to get variable // * Constructor requires an existing GaussianVariableIndex to get variable
// * dimensions. // * dimensions.
// */ // */
// VectorConfig(const GaussianVariableIndex& variableIndex); // VectorValues(const GaussianVariableIndex& variableIndex);
/** /**
* Default constructor creates an empty VectorConfig. reserve(...) must be * Default constructor creates an empty VectorValues. reserve(...) must be
* called to allocate space before any values can be added. This prevents * called to allocate space before any values can be added. This prevents
* slow reallocation of space at runtime. * slow reallocation of space at runtime.
*/ */
VectorConfig() : varStarts_(1,0) {} VectorValues() : varStarts_(1,0) {}
/** Construct from a container of variable dimensions (in variable order). */ /** Construct from a container of variable dimensions (in variable order). */
template<class Container> template<class Container>
VectorConfig(const Container& dimensions); VectorValues(const Container& dimensions);
/** Construct from a container of variable dimensions in variable order and /** Construct from a container of variable dimensions in variable order and
* a combined Vector of all of the variables in order. * a combined Vector of all of the variables in order.
*/ */
VectorConfig(const std::vector<size_t>& dimensions, const Vector& values); VectorValues(const std::vector<size_t>& dimensions, const Vector& values);
/** Element access */ /** Element access */
mapped_type operator[](varid_t variable); mapped_type operator[](varid_t variable);
@ -72,10 +72,10 @@ public:
size_t dimCapacity() const { return values_.size(); } size_t dimCapacity() const { return values_.size(); }
/** Iterator access */ /** Iterator access */
iterator begin() { return _impl_iterator<VectorConfig>(*this, 0); } iterator begin() { return _impl_iterator<VectorValues>(*this, 0); }
const_iterator begin() const { return _impl_iterator<const VectorConfig>(*this, 0); } const_iterator begin() const { return _impl_iterator<const VectorValues>(*this, 0); }
iterator end() { return _impl_iterator<VectorConfig>(*this, varStarts_.size()-1); } iterator end() { return _impl_iterator<VectorValues>(*this, varStarts_.size()-1); }
const_iterator end() const { return _impl_iterator<const VectorConfig>(*this, varStarts_.size()-1); } const_iterator end() const { return _impl_iterator<const VectorValues>(*this, varStarts_.size()-1); }
/** Reserve space for a total number of variables and dimensionality */ /** Reserve space for a total number of variables and dimensionality */
void reserve(varid_t nVars, size_t totalDims) { values_.resize(std::max(totalDims, values_.size())); varStarts_.reserve(nVars+1); } void reserve(varid_t nVars, size_t totalDims) { values_.resize(std::max(totalDims, values_.size())); varStarts_.reserve(nVars+1); }
@ -95,7 +95,7 @@ public:
void makeZero() { boost::numeric::ublas::noalias(values_) = boost::numeric::ublas::zero_vector<double>(values_.size()); } void makeZero() { boost::numeric::ublas::noalias(values_) = boost::numeric::ublas::zero_vector<double>(values_.size()); }
/** print required by Testable for unit testing */ /** print required by Testable for unit testing */
void print(const std::string& str = "VectorConfig: ") const { void print(const std::string& str = "VectorValues: ") const {
std::cout << str << ": " << varStarts_.size()-1 << " elements\n"; std::cout << str << ": " << varStarts_.size()-1 << " elements\n";
for(varid_t var=0; var<size(); ++var) { for(varid_t var=0; var<size(); ++var) {
std::cout << " " << var << " " << operator[](var) << "\n"; std::cout << " " << var << " " << operator[](var) << "\n";
@ -104,7 +104,7 @@ public:
} }
/** equals required by Testable for unit testing */ /** equals required by Testable for unit testing */
bool equals(const VectorConfig& expected, double tol=1e-9) const { bool equals(const VectorValues& expected, double tol=1e-9) const {
if(size() != expected.size()) return false; if(size() != expected.size()) return false;
// iterate over all elements // iterate over all elements
for(varid_t var=0; var<size(); ++var) for(varid_t var=0; var<size(); ++var)
@ -116,9 +116,9 @@ public:
/** + operator simply adds Vectors. This checks for structural equivalence /** + operator simply adds Vectors. This checks for structural equivalence
* when NDEBUG is not defined. * when NDEBUG is not defined.
*/ */
VectorConfig operator+(const VectorConfig& c) const { VectorValues operator+(const VectorValues& c) const {
assert(varStarts_ == c.varStarts_); assert(varStarts_ == c.varStarts_);
VectorConfig result; VectorValues result;
result.varStarts_ = varStarts_; result.varStarts_ = varStarts_;
result.values_ = boost::numeric::ublas::project(values_, boost::numeric::ublas::range(0, varStarts_.back())) + result.values_ = boost::numeric::ublas::project(values_, boost::numeric::ublas::range(0, varStarts_.back())) +
boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back())); boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
@ -137,10 +137,10 @@ public:
_impl_iterator(C& config, varid_t curVariable) : config_(config), curVariable_(curVariable) {} _impl_iterator(C& config, varid_t curVariable) : config_(config), curVariable_(curVariable) {}
void checkCompat(const _impl_iterator<C>& r) { assert(&config_ == &r.config_); } void checkCompat(const _impl_iterator<C>& r) { assert(&config_ == &r.config_); }
friend class VectorConfig; friend class VectorValues;
public: public:
typedef typename const_selector<C, VectorConfig, VectorConfig::mapped_type, VectorConfig::const_mapped_type>::type value_type; typedef typename const_selector<C, VectorValues, VectorValues::mapped_type, VectorValues::const_mapped_type>::type value_type;
_impl_iterator<C>& operator++() { ++curVariable_; return *this; } _impl_iterator<C>& operator++() { ++curVariable_; return *this; }
_impl_iterator<C>& operator--() { --curVariable_; return *this; } _impl_iterator<C>& operator--() { --curVariable_; return *this; }
_impl_iterator<C>& operator++(int) { throw std::runtime_error("Use prefix ++ operator"); } _impl_iterator<C>& operator++(int) { throw std::runtime_error("Use prefix ++ operator"); }
@ -158,7 +158,7 @@ protected:
}; };
//inline VectorConfig::VectorConfig(const GaussianVariableIndex& variableIndex) : varStarts_(variableIndex.size()+1) { //inline VectorValues::VectorValues(const GaussianVariableIndex& variableIndex) : varStarts_(variableIndex.size()+1) {
// size_t varStart = 0; // size_t varStart = 0;
// varStarts_[0] = 0; // varStarts_[0] = 0;
// for(varid_t var=0; var<variableIndex.size(); ++var) { // for(varid_t var=0; var<variableIndex.size(); ++var) {
@ -169,7 +169,7 @@ protected:
//} //}
template<class Container> template<class Container>
inline VectorConfig::VectorConfig(const Container& dimensions) : varStarts_(dimensions.size()+1) { inline VectorValues::VectorValues(const Container& dimensions) : varStarts_(dimensions.size()+1) {
varStarts_[0] = 0; varStarts_[0] = 0;
size_t varStart = 0; size_t varStart = 0;
varid_t var = 0; varid_t var = 0;
@ -179,7 +179,7 @@ inline VectorConfig::VectorConfig(const Container& dimensions) : varStarts_(dime
values_.resize(varStarts_.back(), false); values_.resize(varStarts_.back(), false);
} }
inline VectorConfig::VectorConfig(const std::vector<size_t>& dimensions, const Vector& values) : inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const Vector& values) :
values_(values), varStarts_(dimensions.size()+1) { values_(values), varStarts_(dimensions.size()+1) {
varStarts_[0] = 0; varStarts_[0] = 0;
size_t varStart = 0; size_t varStart = 0;
@ -190,13 +190,13 @@ inline VectorConfig::VectorConfig(const std::vector<size_t>& dimensions, const V
assert(varStarts_.back() == values.size()); assert(varStarts_.back() == values.size());
} }
inline VectorConfig::mapped_type VectorConfig::operator[](varid_t variable) { inline VectorValues::mapped_type VectorValues::operator[](varid_t variable) {
checkVariable(variable); checkVariable(variable);
return boost::numeric::ublas::project(values_, return boost::numeric::ublas::project(values_,
boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1])); boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));
} }
inline VectorConfig::const_mapped_type VectorConfig::operator[](varid_t variable) const { inline VectorValues::const_mapped_type VectorValues::operator[](varid_t variable) const {
checkVariable(variable); checkVariable(variable);
return boost::numeric::ublas::project(values_, return boost::numeric::ublas::project(values_,
boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1])); boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1]));

12
linear/iterative.cpp
View File

@ -51,15 +51,15 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig steepestDescent(const GaussianFactorGraph& fg, VectorValues steepestDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg, return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
x, verbose, epsilon, epsilon_abs, maxIterations, true); x, verbose, epsilon, epsilon_abs, maxIterations, true);
} }
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg, VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg, return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
x, verbose, epsilon, epsilon_abs, maxIterations); x, verbose, epsilon, epsilon_abs, maxIterations);
} }

10
linear/iterative.h
View File

@ -8,7 +8,7 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
namespace gtsam { namespace gtsam {
@ -107,15 +107,15 @@ namespace gtsam {
/** /**
* Method of steepest gradients, Gaussian Factor Graph version * Method of steepest gradients, Gaussian Factor Graph version
* */ * */
VectorConfig steepestDescent(const GaussianFactorGraph& fg, VectorValues steepestDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, bool verbose = false, double epsilon = 1e-3, const VectorValues& x, bool verbose = false, double epsilon = 1e-3,
double epsilon_abs = 1e-5, size_t maxIterations = 0); double epsilon_abs = 1e-5, size_t maxIterations = 0);
/** /**
* Method of conjugate gradients (CG), Gaussian Factor Graph version * Method of conjugate gradients (CG), Gaussian Factor Graph version
* */ * */
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg, VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, bool verbose = false, double epsilon = 1e-3, const VectorValues& x, bool verbose = false, double epsilon = 1e-3,
double epsilon_abs = 1e-5, size_t maxIterations = 0); double epsilon_abs = 1e-5, size_t maxIterations = 0);
} // namespace gtsam } // namespace gtsam

12
linear/tests/testBayesNetPreconditioner.cpp
View File

@ -40,23 +40,23 @@ TEST( BayesNetPreconditioner, operators )
BayesNetPreconditioner P(dummy,bn); BayesNetPreconditioner P(dummy,bn);
// inv(R1)*d should equal solution [1;-2;1] // inv(R1)*d should equal solution [1;-2;1]
VectorConfig D; VectorValues D;
D.insert("x", d); D.insert("x", d);
D.insert("y", Vector_(1, 1.0 / 0.1)); // corrected by sigma D.insert("y", Vector_(1, 1.0 / 0.1)); // corrected by sigma
VectorConfig expected1; VectorValues expected1;
expected1.insert("x", Vector_(2, 1.0, -2.0)); expected1.insert("x", Vector_(2, 1.0, -2.0));
expected1.insert("y", Vector_(1, 1.0)); expected1.insert("y", Vector_(1, 1.0));
VectorConfig actual1 = P.backSubstitute(D); VectorValues actual1 = P.backSubstitute(D);
CHECK(assert_equal(expected1,actual1)); CHECK(assert_equal(expected1,actual1));
// inv(R1')*ones should equal ? // inv(R1')*ones should equal ?
VectorConfig ones; VectorValues ones;
ones.insert("x", Vector_(2, 1.0, 1.0)); ones.insert("x", Vector_(2, 1.0, 1.0));
ones.insert("y", Vector_(1, 1.0)); ones.insert("y", Vector_(1, 1.0));
VectorConfig expected2; VectorValues expected2;
expected2.insert("x", Vector_(2, 0.1, -0.1)); expected2.insert("x", Vector_(2, 0.1, -0.1));
expected2.insert("y", Vector_(1, 0.0)); expected2.insert("y", Vector_(1, 0.0));
VectorConfig actual2 = P.backSubstituteTranspose(ones); VectorValues actual2 = P.backSubstituteTranspose(ones);
CHECK(assert_equal(expected2,actual2)); CHECK(assert_equal(expected2,actual2));
} }

2
linear/tests/testGaussianConditional.cpp
View File

@ -145,7 +145,7 @@ TEST( GaussianConditional, solve )
Vector sl1(2); Vector sl1(2);
sl1(0) = 1.0; sl1(1) = 1.0; sl1(0) = 1.0; sl1(1) = 1.0;
VectorConfig solution(vector<size_t>(3, 2)); VectorValues solution(vector<size_t>(3, 2));
solution[_x1_] = sx1; solution[_x1_] = sx1;
solution[_l1_] = sl1; solution[_l1_] = sl1;

10
linear/tests/testGaussianFactor.cpp
View File

@ -180,7 +180,7 @@ TEST(GaussianFactor, Combine2)
// Vector b = Vector_(2,0.2,-0.1); // Vector b = Vector_(2,0.2,-0.1);
// GaussianFactor lf(_x1_, -I, _x2_, I, b, sigma0_1); // GaussianFactor lf(_x1_, -I, _x2_, I, b, sigma0_1);
// //
// VectorConfig c; // VectorValues c;
// c.insert(_x1_,Vector_(2,10.,20.)); // c.insert(_x1_,Vector_(2,10.,20.));
// c.insert(_x2_,Vector_(2,30.,60.)); // c.insert(_x2_,Vector_(2,30.,60.));
// //
@ -189,16 +189,16 @@ TEST(GaussianFactor, Combine2)
// CHECK(assert_equal(expectedE,e)); // CHECK(assert_equal(expectedE,e));
// //
// // test A^e // // test A^e
// VectorConfig expectedX; // VectorValues expectedX;
// expectedX.insert(_x1_,Vector_(2,-2000.,-4000.)); // expectedX.insert(_x1_,Vector_(2,-2000.,-4000.));
// expectedX.insert(_x2_,Vector_(2, 2000., 4000.)); // expectedX.insert(_x2_,Vector_(2, 2000., 4000.));
// CHECK(assert_equal(expectedX,lf^e)); // CHECK(assert_equal(expectedX,lf^e));
// //
// // test transposeMultiplyAdd // // test transposeMultiplyAdd
// VectorConfig x; // VectorValues x;
// x.insert(_x1_,Vector_(2, 1.,2.)); // x.insert(_x1_,Vector_(2, 1.,2.));
// x.insert(_x2_,Vector_(2, 3.,4.)); // x.insert(_x2_,Vector_(2, 3.,4.));
// VectorConfig expectedX2 = x + 0.1 * (lf^e); // VectorValues expectedX2 = x + 0.1 * (lf^e);
// lf.transposeMultiplyAdd(0.1,e,x); // lf.transposeMultiplyAdd(0.1,e,x);
// CHECK(assert_equal(expectedX2,x)); // CHECK(assert_equal(expectedX2,x));
//} //}
@ -514,7 +514,7 @@ TEST( GaussianFactor, default_error )
{ {
GaussianFactor f; GaussianFactor f;
vector<size_t> dims; vector<size_t> dims;
VectorConfig c(dims); VectorValues c(dims);
double actual = f.error(c); double actual = f.error(c);
CHECK(actual==0.0); CHECK(actual==0.0);
} }

4
linear/tests/testGaussianJunctionTree.cpp
View File

@ -78,8 +78,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
GaussianFactorGraph fg = createChain(); GaussianFactorGraph fg = createChain();
GaussianJunctionTree tree(fg); GaussianJunctionTree tree(fg);
VectorConfig actual = tree.optimize(); VectorValues actual = tree.optimize();
VectorConfig expected(vector<size_t>(4,1)); VectorValues expected(vector<size_t>(4,1));
expected[x1] = Vector_(1, 0.); expected[x1] = Vector_(1, 0.);
expected[x2] = Vector_(1, 1.); expected[x2] = Vector_(1, 1.);
expected[x3] = Vector_(1, 0.); expected[x3] = Vector_(1, 0.);

4
linear/tests/testVectorBTree.cpp
View File

@ -1,6 +1,6 @@
/** /**
* @file testVectorBTree.cpp * @file testVectorBTree.cpp
* @brief Unit tests for Factor Graph Configuration * @brief Unit tests for Factor Graph Values
* @author Frank Dellaert * @author Frank Dellaert
**/ **/
@ -283,7 +283,7 @@ TEST( VectorBTree, serialize)
cout << "VectorBTree: Running Serialization Test" << endl; cout << "VectorBTree: Running Serialization Test" << endl;
//create an VectorBTree //create an VectorBTree
VectorBTree fg = createConfig(); VectorBTree fg = createValues();
//serialize the config //serialize the config
std::ostringstream in_archive_stream; std::ostringstream in_archive_stream;

26
linear/tests/testVectorConfig.cpp
View File

@ -1,5 +1,5 @@
/** /**
* @file testVectorConfig.cpp * @file testVectorValues.cpp
* @brief * @brief
* @author Richard Roberts * @author Richard Roberts
* @created Sep 16, 2010 * @created Sep 16, 2010
@ -7,7 +7,7 @@
#include <vector> #include <vector>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Permutation.h> #include <gtsam/inference/Permutation.h>
#include <gtsam/CppUnitLite/TestHarness.h> #include <gtsam/CppUnitLite/TestHarness.h>
@ -15,13 +15,13 @@
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
TEST(VectorConfig, standard) { TEST(VectorValues, standard) {
Vector v1 = Vector_(3, 1.0,2.0,3.0); Vector v1 = Vector_(3, 1.0,2.0,3.0);
Vector v2 = Vector_(2, 4.0,5.0); Vector v2 = Vector_(2, 4.0,5.0);
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0); Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4; vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
VectorConfig combined(dims); VectorValues combined(dims);
combined[0] = v1; combined[0] = v1;
combined[1] = v2; combined[1] = v2;
combined[2] = v3; combined[2] = v3;
@ -30,7 +30,7 @@ TEST(VectorConfig, standard) {
CHECK(assert_equal(combined[1], v2)) CHECK(assert_equal(combined[1], v2))
CHECK(assert_equal(combined[2], v3)) CHECK(assert_equal(combined[2], v3))
VectorConfig incremental; VectorValues incremental;
incremental.reserve(3, 9); incremental.reserve(3, 9);
incremental.push_back_preallocated(v1); incremental.push_back_preallocated(v1);
incremental.push_back_preallocated(v2); incremental.push_back_preallocated(v2);
@ -41,13 +41,13 @@ TEST(VectorConfig, standard) {
CHECK(assert_equal(incremental[2], v3)) CHECK(assert_equal(incremental[2], v3))
} }
TEST(VectorConfig, permuted_combined) { TEST(VectorValues, permuted_combined) {
Vector v1 = Vector_(3, 1.0,2.0,3.0); Vector v1 = Vector_(3, 1.0,2.0,3.0);
Vector v2 = Vector_(2, 4.0,5.0); Vector v2 = Vector_(2, 4.0,5.0);
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0); Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4; vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
VectorConfig combined(dims); VectorValues combined(dims);
combined[0] = v1; combined[0] = v1;
combined[1] = v2; combined[1] = v2;
combined[2] = v3; combined[2] = v3;
@ -62,7 +62,7 @@ TEST(VectorConfig, permuted_combined) {
perm2[1] = 2; perm2[1] = 2;
perm2[2] = 0; perm2[2] = 0;
Permuted<VectorConfig> permuted1(combined); Permuted<VectorValues> permuted1(combined);
CHECK(assert_equal(v1, permuted1[0])) CHECK(assert_equal(v1, permuted1[0]))
CHECK(assert_equal(v2, permuted1[1])) CHECK(assert_equal(v2, permuted1[1]))
CHECK(assert_equal(v3, permuted1[2])) CHECK(assert_equal(v3, permuted1[2]))
@ -77,7 +77,7 @@ TEST(VectorConfig, permuted_combined) {
CHECK(assert_equal(v2, permuted1[2])) CHECK(assert_equal(v2, permuted1[2]))
CHECK(assert_equal(v3, permuted1[0])) CHECK(assert_equal(v3, permuted1[0]))
Permuted<VectorConfig> permuted2(perm1, combined); Permuted<VectorValues> permuted2(perm1, combined);
CHECK(assert_equal(v1, permuted2[2])) CHECK(assert_equal(v1, permuted2[2]))
CHECK(assert_equal(v2, permuted2[0])) CHECK(assert_equal(v2, permuted2[0]))
CHECK(assert_equal(v3, permuted2[1])) CHECK(assert_equal(v3, permuted2[1]))
@ -89,12 +89,12 @@ TEST(VectorConfig, permuted_combined) {
} }
TEST(VectorConfig, permuted_incremental) { TEST(VectorValues, permuted_incremental) {
Vector v1 = Vector_(3, 1.0,2.0,3.0); Vector v1 = Vector_(3, 1.0,2.0,3.0);
Vector v2 = Vector_(2, 4.0,5.0); Vector v2 = Vector_(2, 4.0,5.0);
Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0); Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
VectorConfig incremental; VectorValues incremental;
incremental.reserve(3, 9); incremental.reserve(3, 9);
incremental.push_back_preallocated(v1); incremental.push_back_preallocated(v1);
incremental.push_back_preallocated(v2); incremental.push_back_preallocated(v2);
@ -110,7 +110,7 @@ TEST(VectorConfig, permuted_incremental) {
perm2[1] = 2; perm2[1] = 2;
perm2[2] = 0; perm2[2] = 0;
Permuted<VectorConfig> permuted1(incremental); Permuted<VectorValues> permuted1(incremental);
CHECK(assert_equal(v1, permuted1[0])) CHECK(assert_equal(v1, permuted1[0]))
CHECK(assert_equal(v2, permuted1[1])) CHECK(assert_equal(v2, permuted1[1]))
CHECK(assert_equal(v3, permuted1[2])) CHECK(assert_equal(v3, permuted1[2]))
@ -125,7 +125,7 @@ TEST(VectorConfig, permuted_incremental) {
CHECK(assert_equal(v2, permuted1[2])) CHECK(assert_equal(v2, permuted1[2]))
CHECK(assert_equal(v3, permuted1[0])) CHECK(assert_equal(v3, permuted1[0]))
Permuted<VectorConfig> permuted2(perm1, incremental); Permuted<VectorValues> permuted2(perm1, incremental);
CHECK(assert_equal(v1, permuted2[2])) CHECK(assert_equal(v1, permuted2[2]))
CHECK(assert_equal(v2, permuted2[0])) CHECK(assert_equal(v2, permuted2[0]))
CHECK(assert_equal(v3, permuted2[1])) CHECK(assert_equal(v3, permuted2[1]))

4
linear/tests/testVectorMap.cpp
View File

@ -1,6 +1,6 @@
/** /**
* @file testVectorMap.cpp * @file testVectorMap.cpp
* @brief Unit tests for Factor Graph Configuration * @brief Unit tests for Factor Graph Values
* @author Carlos Nieto * @author Carlos Nieto
**/ **/
@ -221,7 +221,7 @@ TEST( VectorMap, serialize)
cout << "VectorMap: Running Serialization Test" << endl; cout << "VectorMap: Running Serialization Test" << endl;
//create an VectorMap //create an VectorMap
VectorMap fg = createConfig(); VectorMap fg = createValues();
//serialize the config //serialize the config
std::ostringstream in_archive_stream; std::ostringstream in_archive_stream;

4
linear/tests/timeFactorOverhead.cpp
View File

@ -70,7 +70,7 @@ int main(int argc, char *argv[]) {
for(size_t trial=0; trial<nTrials; ++trial) { for(size_t trial=0; trial<nTrials; ++trial) {
// cout << "Trial " << trial << endl; // cout << "Trial " << trial << endl;
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial])); GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial]));
VectorConfig soln(optimize(*gbn)); VectorValues soln(optimize(*gbn));
} }
blocksolve = timer.elapsed(); blocksolve = timer.elapsed();
cout << blocksolve << " s" << endl; cout << blocksolve << " s" << endl;
@ -114,7 +114,7 @@ int main(int argc, char *argv[]) {
timer.restart(); timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) { for(size_t trial=0; trial<nTrials; ++trial) {
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(combGfgs[trial])); GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(combGfgs[trial]));
VectorConfig soln(optimize(*gbn)); VectorValues soln(optimize(*gbn));
} }
combsolve = timer.elapsed(); combsolve = timer.elapsed();
cout << combsolve << " s" << endl; cout << combsolve << " s" << endl;

2
linear/tests/timeSLAMlike.cpp
View File

@ -106,7 +106,7 @@ int main(int argc, char *argv[]) {
for(size_t trial=0; trial<nTrials; ++trial) { for(size_t trial=0; trial<nTrials; ++trial) {
// cout << "Trial " << trial << endl; // cout << "Trial " << trial << endl;
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial])); GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial]));
VectorConfig soln(optimize(*gbn)); VectorValues soln(optimize(*gbn));
} }
blocksolve = timer.elapsed(); blocksolve = timer.elapsed();
cout << blocksolve << " s" << endl; cout << blocksolve << " s" << endl;

4
linear/tests/timeVectorConfig.cpp → linear/tests/timeVectorValues.cpp
View File

@ -1,6 +1,6 @@
/* /*
* @file timeVectorConfig.cpp * @file timeVectorValues.cpp
* @brief Performs timing and profiling for VectorConfig operations * @brief Performs timing and profiling for VectorValues operations
* @author Frank Dellaert * @author Frank Dellaert
*/ */

2
nonlinear/Key.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
class TypedLabeledSymbol; class TypedLabeledSymbol;
/** /**
* Character and index key used in VectorConfig, GaussianFactorGraph, * Character and index key used in VectorValues, GaussianFactorGraph,
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
* keys when linearizing a nonlinear factor graph. This key is not type * keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes. * safe, so cannot be used with any Nonlinear* classes.

34
nonlinear/LieValues-inl.h
View File

@ -11,7 +11,7 @@
#include <iostream> #include <iostream>
#include <stdexcept> #include <stdexcept>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
#include <gtsam/base/Lie-inl.h> #include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
@ -20,9 +20,9 @@
#define INSTANTIATE_LIE_CONFIG(J) \ #define INSTANTIATE_LIE_CONFIG(J) \
/*INSTANTIATE_LIE(T);*/ \ /*INSTANTIATE_LIE(T);*/ \
/*template LieValues<J> expmap(const LieValues<J>&, const VectorConfig&);*/ \ /*template LieValues<J> expmap(const LieValues<J>&, const VectorValues&);*/ \
/*template LieValues<J> expmap(const LieValues<J>&, const Vector&);*/ \ /*template LieValues<J> expmap(const LieValues<J>&, const Vector&);*/ \
/*template VectorConfig logmap(const LieValues<J>&, const LieValues<J>&);*/ \ /*template VectorValues logmap(const LieValues<J>&, const LieValues<J>&);*/ \
template class LieValues<J>; template class LieValues<J>;
using namespace std; using namespace std;
@ -69,8 +69,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
VectorConfig LieValues<J>::zero(const Ordering& ordering) const { VectorValues LieValues<J>::zero(const Ordering& ordering) const {
VectorConfig z(this->dims(ordering)); VectorValues z(this->dims(ordering));
z.makeZero(); z.makeZero();
return z; return z;
} }
@ -131,22 +131,22 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
template<class J> template<class J>
LieValues<J> LieValues<J>::expmap(const VectorConfig& delta, const Ordering& ordering) const { LieValues<J> LieValues<J>::expmap(const VectorValues& delta, const Ordering& ordering) const {
LieValues<J> newConfig; LieValues<J> newValues;
typedef pair<J,typename J::Value_t> KeyValue; typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) { BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first; const J& j = value.first;
const typename J::Value_t& pj = value.second; const typename J::Value_t& pj = value.second;
const Vector& dj = delta[ordering[j]]; const Vector& dj = delta[ordering[j]];
newConfig.insert(j, pj.expmap(dj)); newValues.insert(j, pj.expmap(dj));
} }
return newConfig; return newValues;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
std::vector<size_t> LieValues<J>::dims(const Ordering& ordering) const { std::vector<size_t> LieValues<J>::dims(const Ordering& ordering) const {
_ConfigDimensionCollector dimCollector(ordering); _ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector); this->apply(dimCollector);
return dimCollector.dimensions; return dimCollector.dimensions;
} }
@ -155,7 +155,7 @@ namespace gtsam {
template<class J> template<class J>
Ordering::shared_ptr LieValues<J>::orderingArbitrary(varid_t firstVar) const { Ordering::shared_ptr LieValues<J>::orderingArbitrary(varid_t firstVar) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
_ConfigKeyOrderer keyOrderer(firstVar); _ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer); this->apply(keyOrderer);
return keyOrderer.ordering; return keyOrderer.ordering;
} }
@ -168,32 +168,32 @@ namespace gtsam {
// cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
// throw invalid_argument("Delta vector length does not match config dimensionality."); // throw invalid_argument("Delta vector length does not match config dimensionality.");
// } // }
// LieValues<J> newConfig; // LieValues<J> newValues;
// int delta_offset = 0; // int delta_offset = 0;
// typedef pair<J,typename J::Value_t> KeyValue; // typedef pair<J,typename J::Value_t> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) { // BOOST_FOREACH(const KeyValue& value, this->values_) {
// const J& j = value.first; // const J& j = value.first;
// const typename J::Value_t& pj = value.second; // const typename J::Value_t& pj = value.second;
// int cur_dim = pj.dim(); // int cur_dim = pj.dim();
// newConfig.insert(j,pj.expmap(sub(delta, delta_offset, delta_offset+cur_dim))); // newValues.insert(j,pj.expmap(sub(delta, delta_offset, delta_offset+cur_dim)));
// delta_offset += cur_dim; // delta_offset += cur_dim;
// } // }
// return newConfig; // return newValues;
// } // }
/* ************************************************************************* */ /* ************************************************************************* */
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs // todo: currently only logmaps elements in both configs
template<class J> template<class J>
inline VectorConfig LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering) const { inline VectorValues LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta); logmap(cp, ordering, delta);
return delta; return delta;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering, VectorConfig& delta) const { void LieValues<J>::logmap(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const {
typedef pair<J,typename J::Value_t> KeyValue; typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first)); assert(this->exists(value.first));

30
nonlinear/LieValues.h
View File

@ -5,8 +5,8 @@
* @brief A templated config for Lie-group elements * @brief A templated config for Lie-group elements
* *
* Detailed story: * Detailed story:
* A configuration is a map from keys to values. It is used to specify the value of a bunch * A values structure is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A LieValues is a configuration which can hold variables that * of variables in a factor graph. A LieValues is a values structure which can hold variables that
* are elements of Lie groups, not just vectors. It then, as a whole, implements a aggregate type * are elements of Lie groups, not just vectors. It then, as a whole, implements a aggregate type
* which is also a Lie group, and hence supports operations dim, expmap, and logmap. * which is also a Lie group, and hence supports operations dim, expmap, and logmap.
*/ */
@ -23,14 +23,14 @@
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
namespace boost { template<class T> class optional; } namespace boost { template<class T> class optional; }
namespace gtsam { class VectorConfig; class Ordering; } namespace gtsam { class VectorValues; class Ordering; }
namespace gtsam { namespace gtsam {
/** /**
* Lie type configuration * Lie type values structure
* Takes two template types * Takes two template types
* J: a key type to look up values in the configuration (need to be sortable) * J: a key type to look up values in the values structure (need to be sortable)
* *
* Key concept: * Key concept:
* The key will be assumed to be sortable, and must have a * The key will be assumed to be sortable, and must have a
@ -95,8 +95,8 @@ namespace gtsam {
/** The dimensionality of the tangent space */ /** The dimensionality of the tangent space */
size_t dim() const; size_t dim() const;
/** Get a zero VectorConfig of the correct structure */ /** Get a zero VectorValues of the correct structure */
VectorConfig zero(const Ordering& ordering) const; VectorValues zero(const Ordering& ordering) const;
const_iterator begin() const { return values_.begin(); } const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); } const_iterator end() const { return values_.end(); }
@ -106,16 +106,16 @@ namespace gtsam {
// Lie operations // Lie operations
/** Add a delta config to current config and returns a new config */ /** Add a delta config to current config and returns a new config */
LieValues expmap(const VectorConfig& delta, const Ordering& ordering) const; LieValues expmap(const VectorValues& delta, const Ordering& ordering) const;
// /** Add a delta vector to current config and returns a new config, uses iterator order */ // /** Add a delta vector to current config and returns a new config, uses iterator order */
// LieValues expmap(const Vector& delta) const; // LieValues expmap(const Vector& delta) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
VectorConfig logmap(const LieValues& cp, const Ordering& ordering) const; VectorValues logmap(const LieValues& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
void logmap(const LieValues& cp, const Ordering& ordering, VectorConfig& delta) const; void logmap(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const;
// imperative methods: // imperative methods:
@ -159,7 +159,7 @@ namespace gtsam {
/** /**
* Apply a class with an application operator() to a const_iterator over * Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an * every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Config, (i.e. through templating). * iterator for every type in the Values, (i.e. through templating).
*/ */
template<typename A> template<typename A>
void apply(A& operation) { void apply(A& operation) {
@ -193,10 +193,10 @@ namespace gtsam {
}; };
struct _ConfigDimensionCollector { struct _ValuesDimensionCollector {
const Ordering& ordering; const Ordering& ordering;
std::vector<size_t> dimensions; std::vector<size_t> dimensions;
_ConfigDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {} _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
template<typename I> void operator()(const I& key_value) { template<typename I> void operator()(const I& key_value) {
varid_t var = ordering[key_value->first]; varid_t var = ordering[key_value->first];
assert(var < dimensions.size()); assert(var < dimensions.size());
@ -205,10 +205,10 @@ namespace gtsam {
}; };
/* ************************************************************************* */ /* ************************************************************************* */
struct _ConfigKeyOrderer { struct _ValuesKeyOrderer {
varid_t var; varid_t var;
Ordering::shared_ptr ordering; Ordering::shared_ptr ordering;
_ConfigKeyOrderer(varid_t firstVar) : var(firstVar), ordering(new Ordering) {} _ValuesKeyOrderer(varid_t firstVar) : var(firstVar), ordering(new Ordering) {}
template<typename I> void operator()(const I& key_value) { template<typename I> void operator()(const I& key_value) {
ordering->insert(key_value->first, var); ordering->insert(key_value->first, var);
++ var; ++ var;

108
nonlinear/NonlinearConstraint.h
View File

@ -27,12 +27,12 @@ namespace gtsam {
* on the constraint function that should be made high enough to be * on the constraint function that should be made high enough to be
* significant * significant
*/ */
template <class Config> template <class Values>
class NonlinearConstraint : public NonlinearFactor<Config> { class NonlinearConstraint : public NonlinearFactor<Values> {
protected: protected:
typedef NonlinearConstraint<Config> This; typedef NonlinearConstraint<Values> This;
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Values> Base;
double mu_; /// gain for quadratic merit function double mu_; /// gain for quadratic merit function
size_t dim_; /// dimension of the constraint size_t dim_; /// dimension of the constraint
@ -57,14 +57,14 @@ public:
size_t dim() const { return dim_; } size_t dim() const { return dim_; }
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor<Config>& f, double tol=1e-9) const { virtual bool equals(const NonlinearFactor<Values>& f, double tol=1e-9) const {
const This* p = dynamic_cast<const This*> (&f); const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
return Base::equals(*p, tol) && (mu_ == p->mu_); return Base::equals(*p, tol) && (mu_ == p->mu_);
} }
/** error function - returns the quadratic merit function */ /** error function - returns the quadratic merit function */
virtual double error(const Config& c) const { virtual double error(const Values& c) const {
const Vector error_vector = unwhitenedError(c); const Vector error_vector = unwhitenedError(c);
if (active(c)) if (active(c))
return mu_ * inner_prod(error_vector, error_vector); return mu_ * inner_prod(error_vector, error_vector);
@ -72,7 +72,7 @@ public:
} }
/** Raw error vector function g(x) */ /** Raw error vector function g(x) */
virtual Vector unwhitenedError(const Config& c) const = 0; virtual Vector unwhitenedError(const Values& c) const = 0;
/** /**
* active set check, defines what type of constraint this is * active set check, defines what type of constraint this is
@ -81,29 +81,29 @@ public:
* when the constraint is *NOT* fulfilled. * when the constraint is *NOT* fulfilled.
* @return true if the constraint is active * @return true if the constraint is active
*/ */
virtual bool active(const Config& c) const=0; virtual bool active(const Values& c) const=0;
/** /**
* Linearizes around a given config * Linearizes around a given config
* @param config is the configuration * @param config is the values structure
* @return a combined linear factor containing both the constraint and the constraint factor * @return a combined linear factor containing both the constraint and the constraint factor
*/ */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const=0; virtual boost::shared_ptr<GaussianFactor> linearize(const Values& c) const=0;
}; };
/** /**
* A unary constraint that defaults to an equality constraint * A unary constraint that defaults to an equality constraint
*/ */
template <class Config, class Key> template <class Values, class Key>
class NonlinearConstraint1 : public NonlinearConstraint<Config> { class NonlinearConstraint1 : public NonlinearConstraint<Values> {
public: public:
typedef typename Key::Value_t X; typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearConstraint1<Config,Key> This; typedef NonlinearConstraint1<Values,Key> This;
typedef NonlinearConstraint<Config> Base; typedef NonlinearConstraint<Values> Base;
/** key for the constrained variable */ /** key for the constrained variable */
Key key_; Key key_;
@ -130,14 +130,14 @@ public:
} }
/** Check if two factors are equal. Note type is Factor and needs cast. */ /** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const NonlinearFactor<Config>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor<Values>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f); const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
return Base::equals(*p, tol) && (key_ == p->key_); return Base::equals(*p, tol) && (key_ == p->key_);
} }
/** error function g(x), switched depending on whether the constraint is active */ /** error function g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const Config& x) const { inline Vector unwhitenedError(const Values& x) const {
if (!active(x)) { if (!active(x)) {
return zero(this->dim()); return zero(this->dim());
} }
@ -147,7 +147,7 @@ public:
} }
/** Linearize from config */ /** Linearize from config */
boost::shared_ptr<GaussianFactor> linearize(const Config& c, const Ordering& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
if (!active(c)) { if (!active(c)) {
boost::shared_ptr<GaussianFactor> factor; boost::shared_ptr<GaussianFactor> factor;
return factor; return factor;
@ -168,15 +168,15 @@ public:
/** /**
* Unary Equality constraint - simply forces the value of active() to true * Unary Equality constraint - simply forces the value of active() to true
*/ */
template <class Config, class Key> template <class Values, class Key>
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key> { class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Values, Key> {
public: public:
typedef typename Key::Value_t X; typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint1<Config,Key> This; typedef NonlinearEqualityConstraint1<Values,Key> This;
typedef NonlinearConstraint1<Config,Key> Base; typedef NonlinearConstraint1<Values,Key> Base;
public: public:
NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0) NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0)
@ -184,22 +184,22 @@ public:
virtual ~NonlinearEqualityConstraint1() {} virtual ~NonlinearEqualityConstraint1() {}
/** Always active, so fixed value for active() */ /** Always active, so fixed value for active() */
virtual bool active(const Config& c) const { return true; } virtual bool active(const Values& c) const { return true; }
}; };
/** /**
* A binary constraint with arbitrary cost and jacobian functions * A binary constraint with arbitrary cost and jacobian functions
*/ */
template <class Config, class Key1, class Key2> template <class Values, class Key1, class Key2>
class NonlinearConstraint2 : public NonlinearConstraint<Config> { class NonlinearConstraint2 : public NonlinearConstraint<Values> {
public: public:
typedef typename Key1::Value_t X1; typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2; typedef typename Key2::Value_t X2;
protected: protected:
typedef NonlinearConstraint2<Config,Key1,Key2> This; typedef NonlinearConstraint2<Values,Key1,Key2> This;
typedef NonlinearConstraint<Config> Base; typedef NonlinearConstraint<Values> Base;
/** keys for the constrained variables */ /** keys for the constrained variables */
Key1 key1_; Key1 key1_;
@ -230,14 +230,14 @@ public:
} }
/** Check if two factors are equal. Note type is Factor and needs cast. */ /** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const NonlinearFactor<Config>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor<Values>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f); const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_); return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_);
} }
/** error function g(x), switched depending on whether the constraint is active */ /** error function g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const Config& x) const { inline Vector unwhitenedError(const Values& x) const {
if (!active(x)) { if (!active(x)) {
return zero(this->dim()); return zero(this->dim());
} }
@ -249,7 +249,7 @@ public:
} }
/** Linearize from config */ /** Linearize from config */
boost::shared_ptr<GaussianFactor> linearize(const Config& c, const Ordering& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
if (!active(c)) { if (!active(c)) {
boost::shared_ptr<GaussianFactor> factor; boost::shared_ptr<GaussianFactor> factor;
return factor; return factor;
@ -271,16 +271,16 @@ public:
/** /**
* Binary Equality constraint - simply forces the value of active() to true * Binary Equality constraint - simply forces the value of active() to true
*/ */
template <class Config, class Key1, class Key2> template <class Values, class Key1, class Key2>
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, Key2> { class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Values, Key1, Key2> {
public: public:
typedef typename Key1::Value_t X1; typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2; typedef typename Key2::Value_t X2;
protected: protected:
typedef NonlinearEqualityConstraint2<Config,Key1,Key2> This; typedef NonlinearEqualityConstraint2<Values,Key1,Key2> This;
typedef NonlinearConstraint2<Config,Key1,Key2> Base; typedef NonlinearConstraint2<Values,Key1,Key2> Base;
public: public:
NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0)
@ -289,14 +289,14 @@ public:
/** Always active, so fixed value for active() */ /** Always active, so fixed value for active() */
virtual bool active(const Config& c) const { return true; } virtual bool active(const Values& c) const { return true; }
}; };
/** /**
* A ternary constraint * A ternary constraint
*/ */
template <class Config, class Key1, class Key2, class Key3> template <class Values, class Key1, class Key2, class Key3>
class NonlinearConstraint3 : public NonlinearConstraint<Config> { class NonlinearConstraint3 : public NonlinearConstraint<Values> {
public: public:
typedef typename Key1::Value_t X1; typedef typename Key1::Value_t X1;
@ -304,8 +304,8 @@ public:
typedef typename Key3::Value_t X3; typedef typename Key3::Value_t X3;
protected: protected:
typedef NonlinearConstraint3<Config,Key1,Key2,Key3> This; typedef NonlinearConstraint3<Values,Key1,Key2,Key3> This;
typedef NonlinearConstraint<Config> Base; typedef NonlinearConstraint<Values> Base;
/** keys for the constrained variables */ /** keys for the constrained variables */
Key1 key1_; Key1 key1_;
@ -341,14 +341,14 @@ public:
} }
/** Check if two factors are equal. Note type is Factor and needs cast. */ /** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const NonlinearFactor<Config>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor<Values>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f); const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_) && (key3_ == p->key3_); return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_) && (key3_ == p->key3_);
} }
/** error function g(x), switched depending on whether the constraint is active */ /** error function g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const Config& x) const { inline Vector unwhitenedError(const Values& x) const {
if (!active(x)) { if (!active(x)) {
return zero(this->dim()); return zero(this->dim());
} }
@ -362,7 +362,7 @@ public:
} }
/** Linearize from config */ /** Linearize from config */
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const { boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
if (!active(c)) { if (!active(c)) {
boost::shared_ptr<GaussianFactor> factor; boost::shared_ptr<GaussianFactor> factor;
return factor; return factor;
@ -385,8 +385,8 @@ public:
/** /**
* Ternary Equality constraint - simply forces the value of active() to true * Ternary Equality constraint - simply forces the value of active() to true
*/ */
template <class Config, class Key1, class Key2, class Key3> template <class Values, class Key1, class Key2, class Key3>
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Config, Key1, Key2, Key3> { class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Values, Key1, Key2, Key3> {
public: public:
typedef typename Key1::Value_t X1; typedef typename Key1::Value_t X1;
@ -394,8 +394,8 @@ public:
typedef typename Key3::Value_t X3; typedef typename Key3::Value_t X3;
protected: protected:
typedef NonlinearEqualityConstraint3<Config,Key1,Key2,Key3> This; typedef NonlinearEqualityConstraint3<Values,Key1,Key2,Key3> This;
typedef NonlinearConstraint3<Config,Key1,Key2,Key3> Base; typedef NonlinearConstraint3<Values,Key1,Key2,Key3> Base;
public: public:
NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3, NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3,
@ -404,27 +404,27 @@ public:
virtual ~NonlinearEqualityConstraint3() {} virtual ~NonlinearEqualityConstraint3() {}
/** Always active, so fixed value for active() */ /** Always active, so fixed value for active() */
virtual bool active(const Config& c) const { return true; } virtual bool active(const Values& c) const { return true; }
}; };
/** /**
* Simple unary equality constraint - fixes a value for a variable * Simple unary equality constraint - fixes a value for a variable
*/ */
template<class Config, class Key> template<class Values, class Key>
class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key> { class NonlinearEquality1 : public NonlinearEqualityConstraint1<Values, Key> {
public: public:
typedef typename Key::Value_t X; typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint1<Config, Key> Base; typedef NonlinearEqualityConstraint1<Values, Key> Base;
X value_; /// fixed value for variable X value_; /// fixed value for variable
public: public:
typedef boost::shared_ptr<NonlinearEquality1<Config, Key> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality1<Values, Key> > shared_ptr;
NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0)
: Base(key1, X::Dim(), mu), value_(value) {} : Base(key1, X::Dim(), mu), value_(value) {}
@ -443,17 +443,17 @@ public:
* Simple binary equality constraint - this constraint forces two factors to * Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type * be the same. This constraint requires the underlying type to a Lie type
*/ */
template<class Config, class Key> template<class Values, class Key>
class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, Key> { class NonlinearEquality2 : public NonlinearEqualityConstraint2<Values, Key, Key> {
public: public:
typedef typename Key::Value_t X; typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint2<Config, Key, Key> Base; typedef NonlinearEqualityConstraint2<Values, Key, Key> Base;
public: public:
typedef boost::shared_ptr<NonlinearEquality2<Config, Key> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality2<Values, Key> > shared_ptr;
NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu) {} : Base(key1, key2, X::Dim(), mu) {}

12
nonlinear/NonlinearEquality.h
View File

@ -30,8 +30,8 @@ namespace gtsam {
* - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
*/ */
template<class Config, class Key> template<class Values, class Key>
class NonlinearEquality: public NonlinearFactor1<Config, Key> { class NonlinearEquality: public NonlinearFactor1<Values, Key> {
public: public:
typedef typename Key::Value_t T; typedef typename Key::Value_t T;
@ -54,7 +54,7 @@ namespace gtsam {
*/ */
bool (*compare_)(const T& a, const T& b); bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<Config, Key> Base; typedef NonlinearFactor1<Values, Key> Base;
/** /**
* Constructor - forces exact evaluation * Constructor - forces exact evaluation
@ -81,13 +81,13 @@ namespace gtsam {
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearEquality<Config,Key>& f, double tol = 1e-9) const { bool equals(const NonlinearEquality<Values,Key>& f, double tol = 1e-9) const {
if (!Base::equals(f)) return false; if (!Base::equals(f)) return false;
return compare_(feasible_, f.feasible_); return compare_(feasible_, f.feasible_);
} }
/** actual error function calculation */ /** actual error function calculation */
virtual double error(const Config& c) const { virtual double error(const Values& c) const {
const T& xj = c[this->key_]; const T& xj = c[this->key_];
Vector e = this->unwhitenedError(c); Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) { if (allow_error_ || !compare_(xj, feasible_)) {
@ -114,7 +114,7 @@ namespace gtsam {
} }
// Linearize is over-written, because base linearization tries to whiten // Linearize is over-written, because base linearization tries to whiten
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& x, const Ordering& ordering) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
const T& xj = x[this->key_]; const T& xj = x[this->key_];
Matrix A; Matrix A;
Vector b = evaluateError(xj, A); Vector b = evaluateError(xj, A);

66
nonlinear/NonlinearFactor.h
View File

@ -33,23 +33,23 @@ namespace gtsam {
* Nonlinear factor which assumes zero-mean Gaussian noise on the * Nonlinear factor which assumes zero-mean Gaussian noise on the
* on a measurement predicted by a non-linear function h. * on a measurement predicted by a non-linear function h.
* *
* Templated on a configuration type. The configurations are typically * Templated on a values structure type. The values structures are typically
* more general than just vectors, e.g., Rot3 or Pose3, * more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups). * which are objects in non-linear manifolds (Lie groups).
*/ */
template<class Config> template<class Values>
class NonlinearFactor: public Testable<NonlinearFactor<Config> > { class NonlinearFactor: public Testable<NonlinearFactor<Values> > {
protected: protected:
typedef NonlinearFactor<Config> This; typedef NonlinearFactor<Values> This;
SharedGaussian noiseModel_; /** Noise model */ SharedGaussian noiseModel_; /** Noise model */
std::list<Symbol> keys_; /** cached keys */ std::list<Symbol> keys_; /** cached keys */
public: public:
typedef boost::shared_ptr<NonlinearFactor<Config> > shared_ptr; typedef boost::shared_ptr<NonlinearFactor<Values> > shared_ptr;
/** Default constructor for I/O only */ /** Default constructor for I/O only */
NonlinearFactor() { NonlinearFactor() {
@ -70,7 +70,7 @@ namespace gtsam {
} }
/** Check if two NonlinearFactor objects are equal */ /** Check if two NonlinearFactor objects are equal */
bool equals(const NonlinearFactor<Config>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor<Values>& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol); return noiseModel_->equals(*f.noiseModel_, tol);
} }
@ -78,7 +78,7 @@ namespace gtsam {
* calculate the error of the factor * calculate the error of the factor
* Override for systems with unusual noise models * Override for systems with unusual noise models
*/ */
virtual double error(const Config& c) const { virtual double error(const Values& c) const {
return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c)); return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c));
} }
@ -109,16 +109,16 @@ namespace gtsam {
} }
/** Vector of errors, unwhitened ! */ /** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const Config& c) const = 0; virtual Vector unwhitenedError(const Values& c) const = 0;
/** Vector of errors, whitened ! */ /** Vector of errors, whitened ! */
Vector whitenedError(const Config& c) const { Vector whitenedError(const Values& c) const {
return noiseModel_->whiten(unwhitenedError(c)); return noiseModel_->whiten(unwhitenedError(c));
} }
/** linearize to a GaussianFactor */ /** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor> virtual boost::shared_ptr<GaussianFactor>
linearize(const Config& c, const Ordering& ordering) const = 0; linearize(const Values& c, const Ordering& ordering) const = 0;
/** /**
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
@ -141,13 +141,13 @@ namespace gtsam {
/** /**
* A Gaussian nonlinear factor that takes 1 parameter * A Gaussian nonlinear factor that takes 1 parameter
* implementing the density P(z|x) \propto exp -0.5*|z-h(x)|^2_C * implementing the density P(z|x) \propto exp -0.5*|z-h(x)|^2_C
* Templated on the parameter type X and the configuration Config * Templated on the parameter type X and the values structure Values
* There is no return type specified for h(x). Instead, we require * There is no return type specified for h(x). Instead, we require
* the derived class implements error_vector(c) = h(x)-z \approx Ax-b * the derived class implements error_vector(c) = h(x)-z \approx Ax-b
* This allows a graph to have factors with measurements of mixed type. * This allows a graph to have factors with measurements of mixed type.
*/ */
template<class Config, class Key> template<class Values, class Key>
class NonlinearFactor1: public NonlinearFactor<Config> { class NonlinearFactor1: public NonlinearFactor<Values> {
public: public:
@ -159,8 +159,8 @@ namespace gtsam {
// The value of the key. Not const to allow serialization // The value of the key. Not const to allow serialization
Key key_; Key key_;
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor1<Config, Key> This; typedef NonlinearFactor1<Values, Key> This;
public: public:
@ -175,7 +175,7 @@ namespace gtsam {
/** /**
* Constructor * Constructor
* @param z measurement * @param z measurement
* @param key by which to look up X value in Config * @param key by which to look up X value in Values
*/ */
NonlinearFactor1(const SharedGaussian& noiseModel, NonlinearFactor1(const SharedGaussian& noiseModel,
const Key& key1) : const Key& key1) :
@ -191,12 +191,12 @@ namespace gtsam {
} }
/** Check if two factors are equal. Note type is Factor and needs cast. */ /** Check if two factors are equal. Note type is Factor and needs cast. */
bool equals(const NonlinearFactor1<Config,Key>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor1<Values,Key>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_); return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_);
} }
/** error function h(x)-z, unwhitened !!! */ /** error function h(x)-z, unwhitened !!! */
inline Vector unwhitenedError(const Config& x) const { inline Vector unwhitenedError(const Values& x) const {
const Key& j = key_; const Key& j = key_;
const X& xj = x[j]; const X& xj = x[j];
return evaluateError(xj); return evaluateError(xj);
@ -207,7 +207,7 @@ namespace gtsam {
* Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
* Hence b = z - h(x0) = - error_vector(x) * Hence b = z - h(x0) = - error_vector(x)
*/ */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& x, const Ordering& ordering) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
const X& xj = x[key_]; const X& xj = x[key_];
Matrix A; Matrix A;
Vector b = - evaluateError(xj, A); Vector b = - evaluateError(xj, A);
@ -256,8 +256,8 @@ namespace gtsam {
/** /**
* A Gaussian nonlinear factor that takes 2 parameters * A Gaussian nonlinear factor that takes 2 parameters
*/ */
template<class Config, class Key1, class Key2> template<class Values, class Key1, class Key2>
class NonlinearFactor2: public NonlinearFactor<Config> { class NonlinearFactor2: public NonlinearFactor<Values> {
public: public:
@ -271,8 +271,8 @@ namespace gtsam {
Key1 key1_; Key1 key1_;
Key2 key2_; Key2 key2_;
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor2<Config, Key1, Key2> This; typedef NonlinearFactor2<Values, Key1, Key2> This;
public: public:
@ -303,13 +303,13 @@ namespace gtsam {
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor2<Config,Key1,Key2>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor2<Values,Key1,Key2>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_); && (key2_ == f.key2_);
} }
/** error function z-h(x1,x2) */ /** error function z-h(x1,x2) */
inline Vector unwhitenedError(const Config& x) const { inline Vector unwhitenedError(const Values& x) const {
const X1& x1 = x[key1_]; const X1& x1 = x[key1_];
const X2& x2 = x[key2_]; const X2& x2 = x[key2_];
return evaluateError(x1, x2); return evaluateError(x1, x2);
@ -320,7 +320,7 @@ namespace gtsam {
* Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
* Hence b = z - h(x1,x2) = - error_vector(x) * Hence b = z - h(x1,x2) = - error_vector(x)
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Config& c, const Ordering& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
const X1& x1 = c[key1_]; const X1& x1 = c[key1_];
const X2& x2 = c[key2_]; const X2& x2 = c[key2_];
Matrix A1, A2; Matrix A1, A2;
@ -392,8 +392,8 @@ namespace gtsam {
/** /**
* A Gaussian nonlinear factor that takes 3 parameters * A Gaussian nonlinear factor that takes 3 parameters
*/ */
template<class Config, class Key1, class Key2, class Key3> template<class Values, class Key1, class Key2, class Key3>
class NonlinearFactor3: public NonlinearFactor<Config> { class NonlinearFactor3: public NonlinearFactor<Values> {
public: public:
@ -409,8 +409,8 @@ namespace gtsam {
Key2 key2_; Key2 key2_;
Key3 key3_; Key3 key3_;
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor3<Config, Key1, Key2, Key3> This; typedef NonlinearFactor3<Values, Key1, Key2, Key3> This;
public: public:
@ -443,13 +443,13 @@ namespace gtsam {
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor3<Config,Key1,Key2,Key3>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor3<Values,Key1,Key2,Key3>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_) && (key3_ == f.key3_); && (key2_ == f.key2_) && (key3_ == f.key3_);
} }
/** error function z-h(x1,x2) */ /** error function z-h(x1,x2) */
inline Vector unwhitenedError(const Config& x) const { inline Vector unwhitenedError(const Values& x) const {
const X1& x1 = x[key1_]; const X1& x1 = x[key1_];
const X2& x2 = x[key2_]; const X2& x2 = x[key2_];
const X3& x3 = x[key3_]; const X3& x3 = x[key3_];
@ -461,7 +461,7 @@ namespace gtsam {
* Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z * Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z
* Hence b = z - h(x1,x2,x3) = - error_vector(x) * Hence b = z - h(x1,x2,x3) = - error_vector(x)
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Config& c, const Ordering& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
const X1& x1 = c[key1_]; const X1& x1 = c[key1_];
const X2& x2 = c[key2_]; const X2& x2 = c[key2_];
const X3& x3 = c[key3_]; const X3& x3 = c[key3_];

32
nonlinear/NonlinearFactorGraph-inl.h
View File

@ -23,14 +23,14 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Values>
void NonlinearFactorGraph<Config>::print(const std::string& str) const { void NonlinearFactorGraph<Values>::print(const std::string& str) const {
Base::print(str); Base::print(str);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Values>
Vector NonlinearFactorGraph<Config>::unwhitenedError(const Config& c) const { Vector NonlinearFactorGraph<Values>::unwhitenedError(const Values& c) const {
list<Vector> errors; list<Vector> errors;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_)
errors.push_back(factor->unwhitenedError(c)); errors.push_back(factor->unwhitenedError(c));
@ -38,8 +38,8 @@ void NonlinearFactorGraph<Config>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Values>
double NonlinearFactorGraph<Config>::error(const Config& c) const { double NonlinearFactorGraph<Values>::error(const Values& c) const {
double total_error = 0.; double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities // iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_)
@ -48,9 +48,9 @@ void NonlinearFactorGraph<Config>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Values>
pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr> pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr>
NonlinearFactorGraph<Config>::orderingCOLAMD(const Config& config) const { NonlinearFactorGraph<Values>::orderingCOLAMD(const Values& config) const {
// Create symbolic graph and initial (iterator) ordering // Create symbolic graph and initial (iterator) ordering
FactorGraph<Factor>::shared_ptr symbolic; FactorGraph<Factor>::shared_ptr symbolic;
@ -76,9 +76,9 @@ void NonlinearFactorGraph<Config>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Values>
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<Config>::symbolic( SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<Values>::symbolic(
const Config& config, const Ordering& ordering) const { const Values& config, const Ordering& ordering) const {
// Generate the symbolic factor graph // Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new FactorGraph<Factor>); SymbolicFactorGraph::shared_ptr symbolicfg(new FactorGraph<Factor>);
symbolicfg->reserve(this->size()); symbolicfg->reserve(this->size());
@ -89,18 +89,18 @@ void NonlinearFactorGraph<Config>::print(const std::string& str) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Values>
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
NonlinearFactorGraph<Config>::symbolic(const Config& config) const { NonlinearFactorGraph<Values>::symbolic(const Values& config) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary()); Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(config, *ordering), ordering); return make_pair(symbolic(config, *ordering), ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Values>
boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize( boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Values>::linearize(
const Config& config, const Ordering& ordering) const { const Values& config, const Ordering& ordering) const {
// create an empty linear FG // create an empty linear FG
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);

28
nonlinear/NonlinearFactorGraph.h
View File

@ -20,32 +20,32 @@
namespace gtsam { namespace gtsam {
/** /**
* A non-linear factor graph is templated on a configuration, but the factor type * A non-linear factor graph is templated on a values structure, but the factor type
* is fixed as a NonlinearFactor. The configurations are typically (in SAM) more general * is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
* Linearizing the non-linear factor graph creates a linear factor graph on the * Linearizing the non-linear factor graph creates a linear factor graph on the
* tangent vector space at the linearization point. Because the tangent space is a true * tangent vector space at the linearization point. Because the tangent space is a true
* vector space, the config type will be an VectorConfig in that linearized factor graph. * vector space, the config type will be an VectorValues in that linearized factor graph.
*/ */
template<class Config> template<class Values>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Config> > { class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Values> > {
public: public:
typedef FactorGraph<NonlinearFactor<Config> > Base; typedef FactorGraph<NonlinearFactor<Values> > Base;
typedef typename boost::shared_ptr<NonlinearFactor<Config> > sharedFactor; typedef typename boost::shared_ptr<NonlinearFactor<Values> > sharedFactor;
/** print just calls base class */ /** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const; void print(const std::string& str = "NonlinearFactorGraph: ") const;
/** unnormalized error */ /** unnormalized error */
double error(const Config& c) const; double error(const Values& c) const;
/** all individual errors */ /** all individual errors */
Vector unwhitenedError(const Config& c) const; Vector unwhitenedError(const Values& c) const;
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const Config& c) const { double probPrime(const Values& c) const {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
@ -57,7 +57,7 @@ namespace gtsam {
/** /**
* Create a symbolic factor graph using an existing ordering * Create a symbolic factor graph using an existing ordering
*/ */
SymbolicFactorGraph::shared_ptr symbolic(const Config& config, const Ordering& ordering) const; SymbolicFactorGraph::shared_ptr symbolic(const Values& config, const Ordering& ordering) const;
/** /**
* Create a symbolic factor graph and initial variable ordering that can * Create a symbolic factor graph and initial variable ordering that can
@ -66,7 +66,7 @@ namespace gtsam {
* ordering is found. * ordering is found.
*/ */
std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
symbolic(const Config& config) const; symbolic(const Values& config) const;
/** /**
* Compute a fill-reducing ordering using COLAMD. This returns the * Compute a fill-reducing ordering using COLAMD. This returns the
@ -74,13 +74,13 @@ namespace gtsam {
* computation. * computation.
*/ */
std::pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr> std::pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr>
orderingCOLAMD(const Config& config) const; orderingCOLAMD(const Values& config) const;
/** /**
* linearize a nonlinear factor graph * linearize a nonlinear factor graph
*/ */
boost::shared_ptr<GaussianFactorGraph> boost::shared_ptr<GaussianFactorGraph>
linearize(const Config& config, const Ordering& ordering) const; linearize(const Values& config, const Ordering& ordering) const;
}; };

26
nonlinear/NonlinearOptimizer-inl.h
View File

@ -63,7 +63,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph, NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_config config, shared_solver solver, double lambda) : shared_values config, shared_solver solver, double lambda) :
graph_(graph), config_(config), lambda_(lambda), solver_(solver) { graph_(graph), config_(config), lambda_(lambda), solver_(solver) {
if (!graph) throw std::invalid_argument( if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL"); "NonlinearOptimizer constructor: graph = NULL");
@ -78,7 +78,7 @@ namespace gtsam {
// linearize and optimize // linearize and optimize
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class C, class L, class S, class W>
VectorConfig NonlinearOptimizer<G, C, L, S, W>::linearizeAndOptimizeForDelta() const { VectorValues NonlinearOptimizer<G, C, L, S, W>::linearizeAndOptimizeForDelta() const {
boost::shared_ptr<L> linearized = solver_->linearize(*graph_, *config_); boost::shared_ptr<L> linearized = solver_->linearize(*graph_, *config_);
NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linearized), error_, lambda_); NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linearized), error_, lambda_);
return prepared.solver_->optimize(*linearized); return prepared.solver_->optimize(*linearized);
@ -91,20 +91,20 @@ namespace gtsam {
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate( NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate(
verbosityLevel verbosity) const { verbosityLevel verbosity) const {
// linearize and optimize // linearize and optimize
VectorConfig delta = linearizeAndOptimizeForDelta(); VectorValues delta = linearizeAndOptimizeForDelta();
// maybe show output // maybe show output
if (verbosity >= DELTA) if (verbosity >= DELTA)
delta.print("delta"); delta.print("delta");
// take old config and update it // take old config and update it
shared_config newConfig(new C(solver_->expmap(*config_, delta))); shared_values newValues(new C(solver_->expmap(*config_, delta)));
// maybe show output // maybe show output
if (verbosity >= CONFIG) if (verbosity >= CONFIG)
newConfig->print("newConfig"); newValues->print("newValues");
NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newConfig, solver_, lambda_); NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, solver_, lambda_);
if (verbosity >= ERROR) if (verbosity >= ERROR)
cout << "error: " << newOptimizer.error_ << endl; cout << "error: " << newOptimizer.error_ << endl;
@ -166,17 +166,17 @@ namespace gtsam {
damped.print("damped"); damped.print("damped");
// solve // solve
VectorConfig delta = solver_->optimize(damped); VectorValues delta = solver_->optimize(damped);
if (verbosity >= TRYDELTA) if (verbosity >= TRYDELTA)
delta.print("delta"); delta.print("delta");
// update config // update config
shared_config newConfig(new C(solver_->expmap(*config_, delta))); // TODO: updateConfig shared_values newValues(new C(solver_->expmap(*config_, delta))); // TODO: updateValues
// if (verbosity >= TRYCONFIG) // if (verbosity >= TRYCONFIG)
// newConfig->print("config"); // newValues->print("config");
// create new optimization state with more adventurous lambda // create new optimization state with more adventurous lambda
NonlinearOptimizer next(graph_, newConfig, solver_, lambda_ / factor); NonlinearOptimizer next(graph_, newValues, solver_, lambda_ / factor);
if (verbosity >= TRYLAMBDA) cout << "next error = " << next.error_ << endl; if (verbosity >= TRYLAMBDA) cout << "next error = " << next.error_ << endl;
if(lambdaMode >= CAUTIOUS) { if(lambdaMode >= CAUTIOUS) {
@ -188,7 +188,7 @@ namespace gtsam {
// If we're cautious, see if the current lambda is better // If we're cautious, see if the current lambda is better
// todo: include stopping criterion here? // todo: include stopping criterion here?
if(lambdaMode == CAUTIOUS) { if(lambdaMode == CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newConfig, solver_, lambda_); NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_);
if(sameLambda.error_ <= next.error_) if(sameLambda.error_ <= next.error_)
return sameLambda; return sameLambda;
} }
@ -201,7 +201,7 @@ namespace gtsam {
// A more adventerous lambda was worse. If we're cautious, try the same lambda. // A more adventerous lambda was worse. If we're cautious, try the same lambda.
if(lambdaMode == CAUTIOUS) { if(lambdaMode == CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newConfig, solver_, lambda_); NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_);
if(sameLambda.error_ <= error_) if(sameLambda.error_ <= error_)
return sameLambda; return sameLambda;
} }
@ -212,7 +212,7 @@ namespace gtsam {
// TODO: can we avoid copying the config ? // TODO: can we avoid copying the config ?
if(lambdaMode >= BOUNDED && lambda_ >= 1.0e5) { if(lambdaMode >= BOUNDED && lambda_ >= 1.0e5) {
return NonlinearOptimizer(graph_, newConfig, solver_, lambda_);; return NonlinearOptimizer(graph_, newValues, solver_, lambda_);;
} else { } else {
NonlinearOptimizer cautious(graph_, config_, solver_, lambda_ * factor); NonlinearOptimizer cautious(graph_, config_, solver_, lambda_ * factor);
return cautious.try_lambda(linear, verbosity, factor, lambdaMode); return cautious.try_lambda(linear, verbosity, factor, lambdaMode);

38
nonlinear/NonlinearOptimizer.h
View File

@ -11,7 +11,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/Factorization.h> #include <gtsam/linear/Factorization.h>
@ -29,13 +29,13 @@ namespace gtsam {
* and then one of the optimization routines is called. These recursively iterate * and then one of the optimization routines is called. These recursively iterate
* until convergence. All methods are functional and return a new state. * until convergence. All methods are functional and return a new state.
* *
* The class is parameterized by the Graph type $G$, Config class type $T$, * The class is parameterized by the Graph type $G$, Values class type $T$,
* linear system class $L$ and the non linear solver type $S$. * linear system class $L$ and the non linear solver type $S$.
* the config type is in order to be able to optimize over non-vector configurations. * the config type is in order to be able to optimize over non-vector values structures.
* To use in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file * To use in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
* *
* For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Config, * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values,
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Config>. * $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
* The solver class has two main functions: linearize and optimize. The first one * The solver class has two main functions: linearize and optimize. The first one
* linearizes the nonlinear cost function around the current estimate, and the second * linearizes the nonlinear cost function around the current estimate, and the second
* one optimizes the linearized system using various methods. * one optimizes the linearized system using various methods.
@ -45,7 +45,7 @@ namespace gtsam {
public: public:
// For performance reasons in recursion, we store configs in a shared_ptr // For performance reasons in recursion, we store configs in a shared_ptr
typedef boost::shared_ptr<const T> shared_config; typedef boost::shared_ptr<const T> shared_values;
typedef boost::shared_ptr<const G> shared_graph; typedef boost::shared_ptr<const G> shared_graph;
typedef boost::shared_ptr<const S> shared_solver; typedef boost::shared_ptr<const S> shared_solver;
typedef const S solver; typedef const S solver;
@ -99,9 +99,9 @@ namespace gtsam {
// These normally do not change // These normally do not change
const shared_graph graph_; const shared_graph graph_;
// keep a configuration and its error // keep a values structure and its error
// These typically change once per iteration (in a functional way) // These typically change once per iteration (in a functional way)
const shared_config config_; const shared_values config_;
double error_; // TODO FD: no more const because in constructor I need to set it after checking :-( double error_; // TODO FD: no more const because in constructor I need to set it after checking :-(
// keep current lambda for use within LM only // keep current lambda for use within LM only
@ -120,13 +120,13 @@ namespace gtsam {
/** /**
* Constructor that evaluates new error * Constructor that evaluates new error
*/ */
NonlinearOptimizer(shared_graph graph, shared_config config, shared_solver solver, NonlinearOptimizer(shared_graph graph, shared_values config, shared_solver solver,
const double lambda = 1e-5); const double lambda = 1e-5);
/** /**
* Constructor that does not do any computation * Constructor that does not do any computation
*/ */
NonlinearOptimizer(shared_graph graph, shared_config config, shared_solver solver, NonlinearOptimizer(shared_graph graph, shared_values config, shared_solver solver,
const double error, const double lambda): graph_(graph), config_(config), const double error, const double lambda): graph_(graph), config_(config),
error_(error), lambda_(lambda), solver_(solver) {} error_(error), lambda_(lambda), solver_(solver) {}
@ -154,15 +154,15 @@ namespace gtsam {
/** /**
* Return the config * Return the config
*/ */
shared_config config() const{ shared_values config() const{
return config_; return config_;
} }
/** /**
* linearize and optimize * linearize and optimize
* This returns an VectorConfig, i.e., vectors in tangent space of T * This returns an VectorValues, i.e., vectors in tangent space of T
*/ */
VectorConfig linearizeAndOptimizeForDelta() const; VectorValues linearizeAndOptimizeForDelta() const;
/** /**
* Do one Gauss-Newton iteration and return next state * Do one Gauss-Newton iteration and return next state
@ -213,9 +213,9 @@ namespace gtsam {
* @param graph Nonlinear factor graph to optimize * @param graph Nonlinear factor graph to optimize
* @param config Initial config * @param config Initial config
* @param verbosity Integer specifying how much output to provide * @param verbosity Integer specifying how much output to provide
* @return an optimized configuration * @return an optimized values structure
*/ */
static shared_config optimizeLM(shared_graph graph, shared_config config, static shared_values optimizeLM(shared_graph graph, shared_values config,
verbosityLevel verbosity = SILENT) { verbosityLevel verbosity = SILENT) {
// Use a variable ordering from COLAMD // Use a variable ordering from COLAMD
@ -238,7 +238,7 @@ namespace gtsam {
/** /**
* Static interface to LM optimization (no shared_ptr arguments) - see above * Static interface to LM optimization (no shared_ptr arguments) - see above
*/ */
inline static shared_config optimizeLM(const G& graph, const T& config, inline static shared_values optimizeLM(const G& graph, const T& config,
verbosityLevel verbosity = SILENT) { verbosityLevel verbosity = SILENT) {
return optimizeLM(boost::make_shared<const G>(graph), return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(config), verbosity); boost::make_shared<const T>(config), verbosity);
@ -249,9 +249,9 @@ namespace gtsam {
* @param graph Nonlinear factor graph to optimize * @param graph Nonlinear factor graph to optimize
* @param config Initial config * @param config Initial config
* @param verbosity Integer specifying how much output to provide * @param verbosity Integer specifying how much output to provide
* @return an optimized configuration * @return an optimized values structure
*/ */
static shared_config optimizeGN(shared_graph graph, shared_config config, static shared_values optimizeGN(shared_graph graph, shared_values config,
verbosityLevel verbosity = SILENT) { verbosityLevel verbosity = SILENT) {
Ordering::shared_ptr ordering; Ordering::shared_ptr ordering;
GaussianVariableIndex<>::shared_ptr variableIndex; GaussianVariableIndex<>::shared_ptr variableIndex;
@ -271,7 +271,7 @@ namespace gtsam {
/** /**
* Static interface to GN optimization (no shared_ptr arguments) - see above * Static interface to GN optimization (no shared_ptr arguments) - see above
*/ */
inline static shared_config optimizeGN(const G& graph, const T& config, inline static shared_values optimizeGN(const G& graph, const T& config,
verbosityLevel verbosity = SILENT) { verbosityLevel verbosity = SILENT) {
return optimizeGN(boost::make_shared<const G>(graph), return optimizeGN(boost::make_shared<const G>(graph),
boost::make_shared<const T>(config), verbosity); boost::make_shared<const T>(config), verbosity);

240
nonlinear/TupleValues-inl.h
View File

@ -11,23 +11,23 @@
#include <gtsam/nonlinear/TupleValues.h> #include <gtsam/nonlinear/TupleValues.h>
// TupleValues instantiations for N = 1-6 // TupleValues instantiations for N = 1-6
#define INSTANTIATE_TUPLE_CONFIG1(Config1) \ #define INSTANTIATE_TUPLE_CONFIG1(Values1) \
template class TupleValues1<Config1>; template class TupleValues1<Values1>;
#define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \ #define INSTANTIATE_TUPLE_CONFIG2(Values1, Values2) \
template class TupleValues2<Config1, Config2>; template class TupleValues2<Values1, Values2>;
#define INSTANTIATE_TUPLE_CONFIG3(Config1, Config2, Config3) \ #define INSTANTIATE_TUPLE_CONFIG3(Values1, Values2, Values3) \
template class TupleValues3<Config1, Config2, Config3>; template class TupleValues3<Values1, Values2, Values3>;
#define INSTANTIATE_TUPLE_CONFIG4(Config1, Config2, Config3, Config4) \ #define INSTANTIATE_TUPLE_CONFIG4(Values1, Values2, Values3, Values4) \
template class TupleValues4<Config1, Config2, Config3, Config4>; template class TupleValues4<Values1, Values2, Values3, Values4>;
#define INSTANTIATE_TUPLE_CONFIG5(Config1, Config2, Config3, Config4, Config5) \ #define INSTANTIATE_TUPLE_CONFIG5(Values1, Values2, Values3, Values4, Values5) \
template class TupleValues5<Config1, Config2, Config3, Config4, Config5>; template class TupleValues5<Values1, Values2, Values3, Values4, Values5>;
#define INSTANTIATE_TUPLE_CONFIG6(Config1, Config2, Config3, Config4, Config5, Config6) \ #define INSTANTIATE_TUPLE_CONFIG6(Values1, Values2, Values3, Values4, Values5, Values6) \
template class TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>; template class TupleValues6<Values1, Values2, Values3, Values4, Values5, Values6>;
namespace gtsam { namespace gtsam {
@ -38,140 +38,140 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
/** TupleValues 1 */ /** TupleValues 1 */
template<class Config1> template<class Values1>
TupleValues1<Config1>::TupleValues1(const TupleValues1<Config1>& config) : TupleValues1<Values1>::TupleValues1(const TupleValues1<Values1>& config) :
TupleValuesEnd<Config1> (config) {} TupleValuesEnd<Values1> (config) {}
template<class Config1> template<class Values1>
TupleValues1<Config1>::TupleValues1(const Config1& cfg1) : TupleValues1<Values1>::TupleValues1(const Values1& cfg1) :
TupleValuesEnd<Config1> (cfg1) {} TupleValuesEnd<Values1> (cfg1) {}
template<class Config1> template<class Values1>
TupleValues1<Config1>::TupleValues1(const TupleValuesEnd<Config1>& config) : TupleValues1<Values1>::TupleValues1(const TupleValuesEnd<Values1>& config) :
TupleValuesEnd<Config1>(config) {} TupleValuesEnd<Values1>(config) {}
/* ************************************************************************* */ /* ************************************************************************* */
/** TupleValues 2 */ /** TupleValues 2 */
template<class Config1, class Config2> template<class Values1, class Values2>
TupleValues2<Config1, Config2>::TupleValues2(const TupleValues2<Config1, Config2>& config) : TupleValues2<Values1, Values2>::TupleValues2(const TupleValues2<Values1, Values2>& config) :
TupleValues<Config1, TupleValuesEnd<Config2> >(config) {} TupleValues<Values1, TupleValuesEnd<Values2> >(config) {}
template<class Config1, class Config2> template<class Values1, class Values2>
TupleValues2<Config1, Config2>::TupleValues2(const Config1& cfg1, const Config2& cfg2) : TupleValues2<Values1, Values2>::TupleValues2(const Values1& cfg1, const Values2& cfg2) :
TupleValues<Config1, TupleValuesEnd<Config2> >( TupleValues<Values1, TupleValuesEnd<Values2> >(
cfg1, TupleValuesEnd<Config2>(cfg2)) {} cfg1, TupleValuesEnd<Values2>(cfg2)) {}
template<class Config1, class Config2> template<class Values1, class Values2>
TupleValues2<Config1, Config2>::TupleValues2(const TupleValues<Config1, TupleValuesEnd<Config2> >& config) : TupleValues2<Values1, Values2>::TupleValues2(const TupleValues<Values1, TupleValuesEnd<Values2> >& config) :
TupleValues<Config1, TupleValuesEnd<Config2> >(config) {} TupleValues<Values1, TupleValuesEnd<Values2> >(config) {}
/* ************************************************************************* */ /* ************************************************************************* */
/** TupleValues 3 */ /** TupleValues 3 */
template<class Config1, class Config2, class Config3> template<class Values1, class Values2, class Values3>
TupleValues3<Config1, Config2, Config3>::TupleValues3( TupleValues3<Values1, Values2, Values3>::TupleValues3(
const TupleValues3<Config1, Config2, Config3>& config) : const TupleValues3<Values1, Values2, Values3>& config) :
TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >(config) {} TupleValues<Values1, TupleValues<Values2, TupleValuesEnd<Values3> > >(config) {}
template<class Config1, class Config2, class Config3> template<class Values1, class Values2, class Values3>
TupleValues3<Config1, Config2, Config3>::TupleValues3( TupleValues3<Values1, Values2, Values3>::TupleValues3(
const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) : const Values1& cfg1, const Values2& cfg2, const Values3& cfg3) :
TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >( TupleValues<Values1, TupleValues<Values2, TupleValuesEnd<Values3> > >(
cfg1, TupleValues<Config2, TupleValuesEnd<Config3> >( cfg1, TupleValues<Values2, TupleValuesEnd<Values3> >(
cfg2, TupleValuesEnd<Config3>(cfg3))) {} cfg2, TupleValuesEnd<Values3>(cfg3))) {}
template<class Config1, class Config2, class Config3> template<class Values1, class Values2, class Values3>
TupleValues3<Config1, Config2, Config3>::TupleValues3( TupleValues3<Values1, Values2, Values3>::TupleValues3(
const TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >& config) : const TupleValues<Values1, TupleValues<Values2, TupleValuesEnd<Values3> > >& config) :
TupleValues<Config1, TupleValues<Config2, TupleValuesEnd<Config3> > >(config) {} TupleValues<Values1, TupleValues<Values2, TupleValuesEnd<Values3> > >(config) {}
/* ************************************************************************* */ /* ************************************************************************* */
/** TupleValues 4 */ /** TupleValues 4 */
template<class Config1, class Config2, class Config3, class Config4> template<class Values1, class Values2, class Values3, class Values4>
TupleValues4<Config1, Config2, Config3, Config4>::TupleValues4( TupleValues4<Values1, Values2, Values3, Values4>::TupleValues4(
const TupleValues4<Config1, Config2, Config3, Config4>& config) : const TupleValues4<Values1, Values2, Values3, Values4>& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Values1, TupleValues<Values2,
TupleValues<Config3, TupleValuesEnd<Config4> > > >(config) {} TupleValues<Values3, TupleValuesEnd<Values4> > > >(config) {}
template<class Config1, class Config2, class Config3, class Config4> template<class Values1, class Values2, class Values3, class Values4>
TupleValues4<Config1, Config2, Config3, Config4>::TupleValues4( TupleValues4<Values1, Values2, Values3, Values4>::TupleValues4(
const Config1& cfg1, const Config2& cfg2, const Values1& cfg1, const Values2& cfg2,
const Config3& cfg3,const Config4& cfg4) : const Values3& cfg3,const Values4& cfg4) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Values1, TupleValues<Values2,
TupleValues<Config3, TupleValuesEnd<Config4> > > >( TupleValues<Values3, TupleValuesEnd<Values4> > > >(
cfg1, TupleValues<Config2, TupleValues<Config3, TupleValuesEnd<Config4> > >( cfg1, TupleValues<Values2, TupleValues<Values3, TupleValuesEnd<Values4> > >(
cfg2, TupleValues<Config3, TupleValuesEnd<Config4> >( cfg2, TupleValues<Values3, TupleValuesEnd<Values4> >(
cfg3, TupleValuesEnd<Config4>(cfg4)))) {} cfg3, TupleValuesEnd<Values4>(cfg4)))) {}
template<class Config1, class Config2, class Config3, class Config4> template<class Values1, class Values2, class Values3, class Values4>
TupleValues4<Config1, Config2, Config3, Config4>::TupleValues4( TupleValues4<Values1, Values2, Values3, Values4>::TupleValues4(
const TupleValues<Config1, TupleValues<Config2, const TupleValues<Values1, TupleValues<Values2,
TupleValues<Config3, TupleValuesEnd<Config4> > > >& config) : TupleValues<Values3, TupleValuesEnd<Values4> > > >& config) :
TupleValues<Config1, TupleValues<Config2,TupleValues<Config3, TupleValues<Values1, TupleValues<Values2,TupleValues<Values3,
TupleValuesEnd<Config4> > > >(config) {} TupleValuesEnd<Values4> > > >(config) {}
/* ************************************************************************* */ /* ************************************************************************* */
/** TupleValues 5 */ /** TupleValues 5 */
template<class Config1, class Config2, class Config3, class Config4, class Config5> template<class Values1, class Values2, class Values3, class Values4, class Values5>
TupleValues5<Config1, Config2, Config3, Config4, Config5>::TupleValues5( TupleValues5<Values1, Values2, Values3, Values4, Values5>::TupleValues5(
const TupleValues5<Config1, Config2, Config3, Config4, Config5>& config) : const TupleValues5<Values1, Values2, Values3, Values4, Values5>& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3, TupleValues<Values1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValuesEnd<Config5> > > > >(config) {} TupleValues<Values4, TupleValuesEnd<Values5> > > > >(config) {}
template<class Config1, class Config2, class Config3, class Config4, class Config5> template<class Values1, class Values2, class Values3, class Values4, class Values5>
TupleValues5<Config1, Config2, Config3, Config4, Config5>::TupleValues5( TupleValues5<Values1, Values2, Values3, Values4, Values5>::TupleValues5(
const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Config4& cfg4, const Config5& cfg5) : const Values4& cfg4, const Values5& cfg5) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Values1, TupleValues<Values2,
TupleValues<Config3, TupleValues<Config4, TupleValues<Values3, TupleValues<Values4,
TupleValuesEnd<Config5> > > > >( TupleValuesEnd<Values5> > > > >(
cfg1, TupleValues<Config2, TupleValues<Config3, cfg1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValuesEnd<Config5> > > >( TupleValues<Values4, TupleValuesEnd<Values5> > > >(
cfg2, TupleValues<Config3, TupleValues<Config4, TupleValuesEnd<Config5> > >( cfg2, TupleValues<Values3, TupleValues<Values4, TupleValuesEnd<Values5> > >(
cfg3, TupleValues<Config4, TupleValuesEnd<Config5> >( cfg3, TupleValues<Values4, TupleValuesEnd<Values5> >(
cfg4, TupleValuesEnd<Config5>(cfg5))))) {} cfg4, TupleValuesEnd<Values5>(cfg5))))) {}
template<class Config1, class Config2, class Config3, class Config4, class Config5> template<class Values1, class Values2, class Values3, class Values4, class Values5>
TupleValues5<Config1, Config2, Config3, Config4, Config5>::TupleValues5( TupleValues5<Values1, Values2, Values3, Values4, Values5>::TupleValues5(
const TupleValues<Config1, TupleValues<Config2, TupleValues<Config3, const TupleValues<Values1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValuesEnd<Config5> > > > >& config) : TupleValues<Values4, TupleValuesEnd<Values5> > > > >& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3, TupleValues<Values1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValuesEnd<Config5> > > > >(config) {} TupleValues<Values4, TupleValuesEnd<Values5> > > > >(config) {}
/* ************************************************************************* */ /* ************************************************************************* */
/** TupleValues 6 */ /** TupleValues 6 */
template<class Config1, class Config2, class Config3, template<class Values1, class Values2, class Values3,
class Config4, class Config5, class Config6> class Values4, class Values5, class Values6>
TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleValues6( TupleValues6<Values1, Values2, Values3, Values4, Values5, Values6>::TupleValues6(
const TupleValues6<Config1, Config2, Config3, const TupleValues6<Values1, Values2, Values3,
Config4, Config5, Config6>& config) : Values4, Values5, Values6>& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3, TupleValues<Values1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValues<Config5, TupleValues<Values4, TupleValues<Values5,
TupleValuesEnd<Config6> > > > > >(config) {} TupleValuesEnd<Values6> > > > > >(config) {}
template<class Config1, class Config2, class Config3, template<class Values1, class Values2, class Values3,
class Config4, class Config5, class Config6> class Values4, class Values5, class Values6>
TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleValues6( TupleValues6<Values1, Values2, Values3, Values4, Values5, Values6>::TupleValues6(
const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6) : const Values4& cfg4, const Values5& cfg5, const Values6& cfg6) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3, TupleValues<Values1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValues<Config5, TupleValuesEnd<Config6> > > > > >( TupleValues<Values4, TupleValues<Values5, TupleValuesEnd<Values6> > > > > >(
cfg1, TupleValues<Config2, TupleValues<Config3, TupleValues<Config4, cfg1, TupleValues<Values2, TupleValues<Values3, TupleValues<Values4,
TupleValues<Config5, TupleValuesEnd<Config6> > > > >( TupleValues<Values5, TupleValuesEnd<Values6> > > > >(
cfg2, TupleValues<Config3, TupleValues<Config4, TupleValues<Config5, cfg2, TupleValues<Values3, TupleValues<Values4, TupleValues<Values5,
TupleValuesEnd<Config6> > > >( TupleValuesEnd<Values6> > > >(
cfg3, TupleValues<Config4, TupleValues<Config5, cfg3, TupleValues<Values4, TupleValues<Values5,
TupleValuesEnd<Config6> > >( TupleValuesEnd<Values6> > >(
cfg4, TupleValues<Config5, TupleValuesEnd<Config6> >( cfg4, TupleValues<Values5, TupleValuesEnd<Values6> >(
cfg5, TupleValuesEnd<Config6>(cfg6)))))) {} cfg5, TupleValuesEnd<Values6>(cfg6)))))) {}
template<class Config1, class Config2, class Config3, template<class Values1, class Values2, class Values3,
class Config4, class Config5, class Config6> class Values4, class Values5, class Values6>
TupleValues6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleValues6( TupleValues6<Values1, Values2, Values3, Values4, Values5, Values6>::TupleValues6(
const TupleValues<Config1, TupleValues<Config2, TupleValues<Config3, const TupleValues<Values1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValues<Config5, TupleValues<Values4, TupleValues<Values5,
TupleValuesEnd<Config6> > > > > >& config) : TupleValuesEnd<Values6> > > > > >& config) :
TupleValues<Config1, TupleValues<Config2, TupleValues<Config3, TupleValues<Values1, TupleValues<Values2, TupleValues<Values3,
TupleValues<Config4, TupleValues<Config5, TupleValues<Values4, TupleValues<Values5,
TupleValuesEnd<Config6> > > > > >(config) {} TupleValuesEnd<Values6> > > > > >(config) {}
} }

192
nonlinear/TupleValues.h
View File

@ -6,7 +6,7 @@
*/ */
#include <gtsam/nonlinear/LieValues.h> #include <gtsam/nonlinear/LieValues.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#pragma once #pragma once
@ -27,7 +27,7 @@ namespace gtsam {
* For an easy interface, there are TupleValuesN classes, which wrap * For an easy interface, there are TupleValuesN classes, which wrap
* the recursive TupleValuess together as a single class, so you can have * the recursive TupleValuess together as a single class, so you can have
* mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent * mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent
* to the previously used PairConfig. * to the previously used PairValues.
* *
* Design and extension note: * Design and extension note:
* To implement a recursively templated data structure, note that most operations * To implement a recursively templated data structure, note that most operations
@ -35,28 +35,28 @@ namespace gtsam {
* for the arguments to be passed to the next config, while the specialized one * for the arguments to be passed to the next config, while the specialized one
* operates on the "first" config. TupleValuesEnd contains only the specialized version. * operates on the "first" config. TupleValuesEnd contains only the specialized version.
*/ */
template<class Config1, class Config2> template<class Values1, class Values2>
class TupleValues : public Testable<TupleValues<Config1, Config2> > { class TupleValues : public Testable<TupleValues<Values1, Values2> > {
protected: protected:
// Data for internal configs // Data for internal configs
Config1 first_; /// Arbitrary config Values1 first_; /// Arbitrary config
Config2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary config Values2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary config
public: public:
// typedefs for config subtypes // typedefs for config subtypes
typedef class Config1::Key Key1; typedef class Values1::Key Key1;
typedef class Config1::Value Value1; typedef class Values1::Value Value1;
/** default constructor */ /** default constructor */
TupleValues() {} TupleValues() {}
/** Copy constructor */ /** Copy constructor */
TupleValues(const TupleValues<Config1, Config2>& config) : TupleValues(const TupleValues<Values1, Values2>& config) :
first_(config.first_), second_(config.second_) {} first_(config.first_), second_(config.second_) {}
/** Construct from configs */ /** Construct from configs */
TupleValues(const Config1& cfg1, const Config2& cfg2) : TupleValues(const Values1& cfg1, const Values2& cfg2) :
first_(cfg1), second_(cfg2) {} first_(cfg1), second_(cfg2) {}
/** Print */ /** Print */
@ -66,7 +66,7 @@ namespace gtsam {
} }
/** Equality with tolerance for keys and values */ /** Equality with tolerance for keys and values */
bool equals(const TupleValues<Config1, Config2>& c, double tol=1e-9) const { bool equals(const TupleValues<Values1, Values2>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
} }
@ -90,7 +90,7 @@ namespace gtsam {
*/ */
template<class Cfg1, class Cfg2> template<class Cfg1, class Cfg2>
void insert(const TupleValues<Cfg1, Cfg2>& config) { second_.insert(config); } void insert(const TupleValues<Cfg1, Cfg2>& config) { second_.insert(config); }
void insert(const TupleValues<Config1, Config2>& config) { void insert(const TupleValues<Values1, Values2>& config) {
first_.insert(config.first_); first_.insert(config.first_);
second_.insert(config.second_); second_.insert(config.second_);
} }
@ -101,7 +101,7 @@ namespace gtsam {
*/ */
template<class Cfg1, class Cfg2> template<class Cfg1, class Cfg2>
void update(const TupleValues<Cfg1, Cfg2>& config) { second_.update(config); } void update(const TupleValues<Cfg1, Cfg2>& config) { second_.update(config); }
void update(const TupleValues<Config1, Config2>& config) { void update(const TupleValues<Values1, Values2>& config) {
first_.update(config.first_); first_.update(config.first_);
second_.update(config.second_); second_.update(config.second_);
} }
@ -121,7 +121,7 @@ namespace gtsam {
*/ */
template<class Cfg> template<class Cfg>
void insertSub(const Cfg& config) { second_.insertSub(config); } void insertSub(const Cfg& config) { second_.insertSub(config); }
void insertSub(const Config1& config) { first_.insert(config); } void insertSub(const Values1& config) { first_.insert(config); }
/** erase an element by key */ /** erase an element by key */
template<class Key> template<class Key>
@ -152,12 +152,12 @@ namespace gtsam {
const Value1& at(const Key1& j) const { return first_.at(j); } const Value1& at(const Key1& j) const { return first_.at(j); }
/** direct config access */ /** direct config access */
const Config1& config() const { return first_; } const Values1& config() const { return first_; }
const Config2& rest() const { return second_; } const Values2& rest() const { return second_; }
/** zero: create VectorConfig of appropriate structure */ /** zero: create VectorValues of appropriate structure */
VectorConfig zero(const Ordering& ordering) const { VectorValues zero(const Ordering& ordering) const {
VectorConfig z(this->dims(ordering)); VectorValues z(this->dims(ordering));
z.makeZero(); z.makeZero();
return z; return z;
} }
@ -173,7 +173,7 @@ namespace gtsam {
/** Create an array of variable dimensions using the given ordering */ /** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const { std::vector<size_t> dims(const Ordering& ordering) const {
_ConfigDimensionCollector dimCollector(ordering); _ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector); this->apply(dimCollector);
return dimCollector.dimensions; return dimCollector.dimensions;
} }
@ -186,25 +186,25 @@ namespace gtsam {
*/ */
Ordering::shared_ptr orderingArbitrary(varid_t firstVar = 0) const { Ordering::shared_ptr orderingArbitrary(varid_t firstVar = 0) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
_ConfigKeyOrderer keyOrderer(firstVar); _ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer); this->apply(keyOrderer);
return keyOrderer.ordering; return keyOrderer.ordering;
} }
/** Expmap */ /** Expmap */
TupleValues<Config1, Config2> expmap(const VectorConfig& delta, const Ordering& ordering) const { TupleValues<Values1, Values2> expmap(const VectorValues& delta, const Ordering& ordering) const {
return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering)); return TupleValues(first_.expmap(delta, ordering), second_.expmap(delta, ordering));
} }
/** logmap each element */ /** logmap each element */
VectorConfig logmap(const TupleValues<Config1, Config2>& cp, const Ordering& ordering) const { VectorValues logmap(const TupleValues<Values1, Values2>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta); logmap(cp, ordering, delta);
return delta; return delta;
} }
/** logmap each element */ /** logmap each element */
void logmap(const TupleValues<Config1, Config2>& cp, const Ordering& ordering, VectorConfig& delta) const { void logmap(const TupleValues<Values1, Values2>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.logmap(cp.first_, ordering, delta); first_.logmap(cp.first_, ordering, delta);
second_.logmap(cp.second_, ordering, delta); second_.logmap(cp.second_, ordering, delta);
} }
@ -212,7 +212,7 @@ namespace gtsam {
/** /**
* Apply a class with an application operator() to a const_iterator over * Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an * every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Config, (i.e. through templating). * iterator for every type in the Values, (i.e. through templating).
*/ */
template<typename A> template<typename A>
void apply(A& operation) { void apply(A& operation) {
@ -242,48 +242,48 @@ namespace gtsam {
* Do not use this class directly - it should only be used as a part * Do not use this class directly - it should only be used as a part
* of a recursive structure * of a recursive structure
*/ */
template<class Config> template<class Values>
class TupleValuesEnd : public Testable<TupleValuesEnd<Config> > { class TupleValuesEnd : public Testable<TupleValuesEnd<Values> > {
protected: protected:
// Data for internal configs // Data for internal configs
Config first_; Values first_;
public: public:
// typedefs // typedefs
typedef class Config::Key Key1; typedef class Values::Key Key1;
typedef class Config::Value Value1; typedef class Values::Value Value1;
TupleValuesEnd() {} TupleValuesEnd() {}
TupleValuesEnd(const TupleValuesEnd<Config>& config) : TupleValuesEnd(const TupleValuesEnd<Values>& config) :
first_(config.first_) {} first_(config.first_) {}
TupleValuesEnd(const Config& cfg) : TupleValuesEnd(const Values& cfg) :
first_(cfg) {} first_(cfg) {}
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
first_.print(); first_.print();
} }
bool equals(const TupleValuesEnd<Config>& c, double tol=1e-9) const { bool equals(const TupleValuesEnd<Values>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol); return first_.equals(c.first_, tol);
} }
void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}
void insert(const TupleValuesEnd<Config>& config) {first_.insert(config.first_); } void insert(const TupleValuesEnd<Values>& config) {first_.insert(config.first_); }
void update(const TupleValuesEnd<Config>& config) {first_.update(config.first_); } void update(const TupleValuesEnd<Values>& config) {first_.update(config.first_); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); } void update(const Key1& key, const Value1& value) { first_.update(key, value); }
void insertSub(const Config& config) {first_.insert(config); } void insertSub(const Values& config) {first_.insert(config); }
const Value1& operator[](const Key1& j) const { return first_[j]; } const Value1& operator[](const Key1& j) const { return first_[j]; }
const Config& config() const { return first_; } const Values& config() const { return first_; }
void erase(const Key1& j) { first_.erase(j); } void erase(const Key1& j) { first_.erase(j); }
@ -297,8 +297,8 @@ namespace gtsam {
const Value1& at(const Key1& j) const { return first_.at(j); } const Value1& at(const Key1& j) const { return first_.at(j); }
VectorConfig zero(const Ordering& ordering) const { VectorValues zero(const Ordering& ordering) const {
VectorConfig z(this->dims(ordering)); VectorValues z(this->dims(ordering));
z.makeZero(); z.makeZero();
return z; return z;
} }
@ -307,24 +307,24 @@ namespace gtsam {
size_t dim() const { return first_.dim(); } size_t dim() const { return first_.dim(); }
TupleValuesEnd<Config> expmap(const VectorConfig& delta, const Ordering& ordering) const { TupleValuesEnd<Values> expmap(const VectorValues& delta, const Ordering& ordering) const {
return TupleValuesEnd(first_.expmap(delta, ordering)); return TupleValuesEnd(first_.expmap(delta, ordering));
} }
VectorConfig logmap(const TupleValuesEnd<Config>& cp, const Ordering& ordering) const { VectorValues logmap(const TupleValuesEnd<Values>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta); logmap(cp, ordering, delta);
return delta; return delta;
} }
void logmap(const TupleValuesEnd<Config>& cp, const Ordering& ordering, VectorConfig& delta) const { void logmap(const TupleValuesEnd<Values>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.logmap(cp.first_, ordering, delta); first_.logmap(cp.first_, ordering, delta);
} }
/** /**
* Apply a class with an application operator() to a const_iterator over * Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an * every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Config, (i.e. through templating). * iterator for every type in the Values, (i.e. through templating).
*/ */
template<typename A> template<typename A>
void apply(A& operation) { void apply(A& operation) {
@ -348,14 +348,14 @@ namespace gtsam {
* recursively, as they are TupleValuess, and are primarily a short form of the config * recursively, as they are TupleValuess, and are primarily a short form of the config
* structure to make use of the TupleValuess easier. * structure to make use of the TupleValuess easier.
* *
* The interface is designed to mimic PairConfig, but for 2-6 config types. * The interface is designed to mimic PairValues, but for 2-6 config types.
*/ */
template<class C1> template<class C1>
class TupleValues1 : public TupleValuesEnd<C1> { class TupleValues1 : public TupleValuesEnd<C1> {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Values1;
typedef TupleValuesEnd<C1> Base; typedef TupleValuesEnd<C1> Base;
typedef TupleValues1<C1> This; typedef TupleValues1<C1> This;
@ -363,18 +363,18 @@ namespace gtsam {
TupleValues1() {} TupleValues1() {}
TupleValues1(const This& config); TupleValues1(const This& config);
TupleValues1(const Base& config); TupleValues1(const Base& config);
TupleValues1(const Config1& cfg1); TupleValues1(const Values1& cfg1);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Values1& first() const { return this->config(); }
}; };
template<class C1, class C2> template<class C1, class C2>
class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > { class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Values1;
typedef C2 Config2; typedef C2 Values2;
typedef TupleValues<C1, TupleValuesEnd<C2> > Base; typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
typedef TupleValues2<C1, C2> This; typedef TupleValues2<C1, C2> This;
@ -382,40 +382,40 @@ namespace gtsam {
TupleValues2() {} TupleValues2() {}
TupleValues2(const This& config); TupleValues2(const This& config);
TupleValues2(const Base& config); TupleValues2(const Base& config);
TupleValues2(const Config1& cfg1, const Config2& cfg2); TupleValues2(const Values1& cfg1, const Values2& cfg2);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Values1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); } inline const Values2& second() const { return this->rest().config(); }
}; };
template<class C1, class C2, class C3> template<class C1, class C2, class C3>
class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > { class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Values1;
typedef C2 Config2; typedef C2 Values2;
typedef C3 Config3; typedef C3 Values3;
TupleValues3() {} TupleValues3() {}
TupleValues3(const TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > >& config); TupleValues3(const TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > >& config);
TupleValues3(const TupleValues3<C1, C2, C3>& config); TupleValues3(const TupleValues3<C1, C2, C3>& config);
TupleValues3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3); TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Values1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); } inline const Values2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); } inline const Values3& third() const { return this->rest().rest().config(); }
}; };
template<class C1, class C2, class C3, class C4> template<class C1, class C2, class C3, class C4>
class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > { class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Values1;
typedef C2 Config2; typedef C2 Values2;
typedef C3 Config3; typedef C3 Values3;
typedef C4 Config4; typedef C4 Values4;
typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base; typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
typedef TupleValues4<C1, C2, C3, C4> This; typedef TupleValues4<C1, C2, C3, C4> This;
@ -423,62 +423,62 @@ namespace gtsam {
TupleValues4() {} TupleValues4() {}
TupleValues4(const This& config); TupleValues4(const This& config);
TupleValues4(const Base& config); TupleValues4(const Base& config);
TupleValues4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4); TupleValues4(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,const Values4& cfg4);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Values1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); } inline const Values2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); } inline const Values3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); } inline const Values4& fourth() const { return this->rest().rest().rest().config(); }
}; };
template<class C1, class C2, class C3, class C4, class C5> template<class C1, class C2, class C3, class C4, class C5>
class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > { class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Values1;
typedef C2 Config2; typedef C2 Values2;
typedef C3 Config3; typedef C3 Values3;
typedef C4 Config4; typedef C4 Values4;
typedef C5 Config5; typedef C5 Values5;
TupleValues5() {} TupleValues5() {}
TupleValues5(const TupleValues5<C1, C2, C3, C4, C5>& config); TupleValues5(const TupleValues5<C1, C2, C3, C4, C5>& config);
TupleValues5(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > >& config); TupleValues5(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > >& config);
TupleValues5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Config4& cfg4, const Config5& cfg5); const Values4& cfg4, const Values5& cfg5);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Values1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); } inline const Values2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); } inline const Values3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); } inline const Values4& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); } inline const Values5& fifth() const { return this->rest().rest().rest().rest().config(); }
}; };
template<class C1, class C2, class C3, class C4, class C5, class C6> template<class C1, class C2, class C3, class C4, class C5, class C6>
class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > { class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
public: public:
// typedefs // typedefs
typedef C1 Config1; typedef C1 Values1;
typedef C2 Config2; typedef C2 Values2;
typedef C3 Config3; typedef C3 Values3;
typedef C4 Config4; typedef C4 Values4;
typedef C5 Config5; typedef C5 Values5;
typedef C6 Config6; typedef C6 Values6;
TupleValues6() {} TupleValues6() {}
TupleValues6(const TupleValues6<C1, C2, C3, C4, C5, C6>& config); TupleValues6(const TupleValues6<C1, C2, C3, C4, C5, C6>& config);
TupleValues6(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > >& config); TupleValues6(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > >& config);
TupleValues6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6); const Values4& cfg4, const Values5& cfg5, const Values6& cfg6);
// access functions // access functions
inline const Config1& first() const { return this->config(); } inline const Values1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); } inline const Values2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); } inline const Values3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); } inline const Values4& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); } inline const Values5& fifth() const { return this->rest().rest().rest().rest().config(); }
inline const Config6& sixth() const { return this->rest().rest().rest().rest().rest().config(); } inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().config(); }
}; };
} }

8
nonlinear/tests/testConstraintOptimizer.cpp
View File

@ -43,7 +43,7 @@ TEST (NonlinearConstraint, problem1_cholesky ) {
Symbol x1("x1"), y1("y1"), L1("L1"); Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda] // state structure: [x y \lambda]
VectorConfig init, state; VectorValues init, state;
init.insert(x1, Vector_(1, 1.0)); init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0)); init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0)); init.insert(L1, Vector_(1, 1.0));
@ -106,18 +106,18 @@ TEST (NonlinearConstraint, problem1_cholesky ) {
// solve // solve
Ordering ord; Ordering ord;
ord += x1, y1, L1; ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord).scale(-1.0); VectorValues delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
// update initial estimate // update initial estimate
VectorConfig newState = expmap(state, delta); VectorValues newState = expmap(state, delta);
state = newState; state = newState;
if (verbose) state.print("Updated State"); if (verbose) state.print("Updated State");
} }
// verify that it converges to the nearest optimal point // verify that it converges to the nearest optimal point
VectorConfig expected; VectorValues expected;
expected.insert(L1, Vector_(1, -1.0)); expected.insert(L1, Vector_(1, -1.0));
expected.insert(x1, Vector_(1, 2.12)); expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5)); expected.insert(y1, Vector_(1, -0.5));

54
nonlinear/tests/testLieValues.cpp
View File

@ -17,17 +17,17 @@ using namespace std;
static double inf = std::numeric_limits<double>::infinity(); static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey; typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef LieValues<VecKey> Config; typedef LieValues<VecKey> Values;
VecKey key1(1), key2(2), key3(3), key4(4); VecKey key1(1), key2(2), key3(3), key4(4);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, equals1 ) TEST( LieValues, equals1 )
{ {
Config expected; Values expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0); Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert(key1,v); expected.insert(key1,v);
Config actual; Values actual;
actual.insert(key1,v); actual.insert(key1,v);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
@ -35,7 +35,7 @@ TEST( LieValues, equals1 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, equals2 ) TEST( LieValues, equals2 )
{ {
Config cfg1, cfg2; Values cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0); Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
@ -47,7 +47,7 @@ TEST( LieValues, equals2 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, equals_nan ) TEST( LieValues, equals_nan )
{ {
Config cfg1, cfg2; Values cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf); Vector v2 = Vector_(3, inf, inf, inf);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
@ -59,7 +59,7 @@ TEST( LieValues, equals_nan )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, insert_config ) TEST( LieValues, insert_config )
{ {
Config cfg1, cfg2, expected; Values cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0); Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
@ -82,7 +82,7 @@ TEST( LieValues, insert_config )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, update_element ) TEST( LieValues, update_element )
{ {
Config cfg; Values cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
@ -98,12 +98,12 @@ TEST( LieValues, update_element )
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST(LieValues, dim_zero) //TEST(LieValues, dim_zero)
//{ //{
// Config config0; // Values config0;
// config0.insert(key1, Vector_(2, 2.0, 3.0)); // config0.insert(key1, Vector_(2, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5,config0.dim()); // LONGS_EQUAL(5,config0.dim());
// //
// VectorConfig expected; // VectorValues expected;
// expected.insert(key1, zero(2)); // expected.insert(key1, zero(2));
// expected.insert(key2, zero(3)); // expected.insert(key2, zero(3));
// CHECK(assert_equal(expected, config0.zero())); // CHECK(assert_equal(expected, config0.zero()));
@ -112,16 +112,16 @@ TEST( LieValues, update_element )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieValues, expmap_a) TEST(LieValues, expmap_a)
{ {
Config config0; Values config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
Ordering ordering(*config0.orderingArbitrary()); Ordering ordering(*config0.orderingArbitrary());
VectorConfig increment(config0.dims(ordering)); VectorValues increment(config0.dims(ordering));
increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2);
increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
Config expected; Values expected;
expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
@ -131,15 +131,15 @@ TEST(LieValues, expmap_a)
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST(LieValues, expmap_b) //TEST(LieValues, expmap_b)
//{ //{
// Config config0; // Values config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// //
// Ordering ordering(*config0.orderingArbitrary()); // Ordering ordering(*config0.orderingArbitrary());
// VectorConfig increment(config0.dims(ordering)); // VectorValues increment(config0.dims(ordering));
// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); // increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
// //
// Config expected; // Values expected;
// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// //
@ -149,7 +149,7 @@ TEST(LieValues, expmap_a)
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST(LieValues, expmap_c) //TEST(LieValues, expmap_c)
//{ //{
// Config config0; // Values config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// //
@ -157,7 +157,7 @@ TEST(LieValues, expmap_a)
// 1.0, 1.1, 1.2, // 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5); // 1.3, 1.4, 1.5);
// //
// Config expected; // Values expected;
// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); // expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// //
@ -167,7 +167,7 @@ TEST(LieValues, expmap_a)
/* ************************************************************************* */ /* ************************************************************************* */
/*TEST(LieValues, expmap_d) /*TEST(LieValues, expmap_d)
{ {
Config config0; Values config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
//config0.print("config0"); //config0.print("config0");
@ -207,7 +207,7 @@ TEST(LieValues, expmap_a)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieValues, exists_) TEST(LieValues, exists_)
{ {
Config config0; Values config0;
config0.insert(key1, Vector_(1, 1.)); config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.)); config0.insert(key2, Vector_(1, 2.));
@ -218,17 +218,17 @@ TEST(LieValues, exists_)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieValues, update) TEST(LieValues, update)
{ {
Config config0; Values config0;
config0.insert(key1, Vector_(1, 1.)); config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.)); config0.insert(key2, Vector_(1, 2.));
Config superset; Values superset;
superset.insert(key1, Vector_(1, -1.)); superset.insert(key1, Vector_(1, -1.));
superset.insert(key2, Vector_(1, -2.)); superset.insert(key2, Vector_(1, -2.));
superset.insert(key3, Vector_(1, -3.)); superset.insert(key3, Vector_(1, -3.));
config0.update(superset); config0.update(superset);
Config expected; Values expected;
expected.insert(key1, Vector_(1, -1.)); expected.insert(key1, Vector_(1, -1.));
expected.insert(key2, Vector_(1, -2.)); expected.insert(key2, Vector_(1, -2.));
CHECK(assert_equal(expected,config0)); CHECK(assert_equal(expected,config0));
@ -238,18 +238,18 @@ TEST(LieValues, update)
TEST(LieValues, dummy_initialization) TEST(LieValues, dummy_initialization)
{ {
typedef TypedSymbol<LieVector, 'z'> Key; typedef TypedSymbol<LieVector, 'z'> Key;
typedef LieValues<Key> Config1; typedef LieValues<Key> Values1;
Config1 init1; Values1 init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0)); init1.insert(Key(1), Vector_(2, 1.0, 2.0));
init1.insert(Key(2), Vector_(2, 4.0, 3.0)); init1.insert(Key(2), Vector_(2, 4.0, 3.0));
Config init2; Values init2;
init2.insert(key1, Vector_(2, 1.0, 2.0)); init2.insert(key1, Vector_(2, 1.0, 2.0));
init2.insert(key2, Vector_(2, 4.0, 3.0)); init2.insert(key2, Vector_(2, 4.0, 3.0));
Config1 actual1(init2); Values1 actual1(init2);
Config actual2(init1); Values actual2(init1);
EXPECT(actual1.empty()); EXPECT(actual1.empty());
EXPECT(actual2.empty()); EXPECT(actual2.empty());

6
slam/BearingFactor.h
View File

@ -15,13 +15,13 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class Config, class PoseKey, class PointKey> template<class Values, class PoseKey, class PointKey>
class BearingFactor: public NonlinearFactor2<Config, PoseKey, PointKey> { class BearingFactor: public NonlinearFactor2<Values, PoseKey, PointKey> {
private: private:
Rot2 z_; /** measurement */ Rot2 z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, PointKey> Base; typedef NonlinearFactor2<Values, PoseKey, PointKey> Base;
public: public:

6
slam/BearingRangeFactor.h
View File

@ -16,15 +16,15 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class Config, class PoseKey, class PointKey> template<class Values, class PoseKey, class PointKey>
class BearingRangeFactor: public NonlinearFactor2<Config, PoseKey, PointKey> { class BearingRangeFactor: public NonlinearFactor2<Values, PoseKey, PointKey> {
private: private:
// the measurement // the measurement
Rot2 bearing_; Rot2 bearing_;
double range_; double range_;
typedef NonlinearFactor2<Config, PoseKey, PointKey> Base; typedef NonlinearFactor2<Values, PoseKey, PointKey> Base;
public: public:

8
slam/BetweenConstraint.h
View File

@ -14,19 +14,19 @@ namespace gtsam {
* Binary between constraint - forces between to a given value * Binary between constraint - forces between to a given value
* This constraint requires the underlying type to a Lie type * This constraint requires the underlying type to a Lie type
*/ */
template<class Config, class Key> template<class Values, class Key>
class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, Key> { class BetweenConstraint : public NonlinearEqualityConstraint2<Values, Key, Key> {
public: public:
typedef typename Key::Value_t X; typedef typename Key::Value_t X;
protected: protected:
typedef NonlinearEqualityConstraint2<Config, Key, Key> Base; typedef NonlinearEqualityConstraint2<Values, Key, Key> Base;
X measured_; /// fixed between value X measured_; /// fixed between value
public: public:
typedef boost::shared_ptr<BetweenConstraint<Config, Key> > shared_ptr; typedef boost::shared_ptr<BetweenConstraint<Values, Key> > shared_ptr;
BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu), measured_(measured) {} : Base(key1, key2, X::Dim(), mu), measured_(measured) {}

14
slam/BetweenFactor.h
View File

@ -15,19 +15,19 @@ namespace gtsam {
/** /**
* A class for a measurement predicted by "between(config[key1],config[key2])" * A class for a measurement predicted by "between(config[key1],config[key2])"
* T is the Lie group type, Config where the T's are gotten from * T is the Lie group type, Values where the T's are gotten from
* *
* FIXME: This should only need one key type, as we can't have different types * FIXME: This should only need one key type, as we can't have different types
*/ */
template<class Config, class Key1, class Key2 = Key1> template<class Values, class Key1, class Key2 = Key1>
class BetweenFactor: public NonlinearFactor2<Config, Key1, Key2> { class BetweenFactor: public NonlinearFactor2<Values, Key1, Key2> {
public: public:
typedef typename Key1::Value_t T; typedef typename Key1::Value_t T;
private: private:
typedef NonlinearFactor2<Config, Key1, Key2> Base; typedef NonlinearFactor2<Values, Key1, Key2> Base;
T measured_; /** The measurement */ T measured_; /** The measurement */
@ -51,9 +51,9 @@ namespace gtsam {
} }
/** equals */ /** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const { bool equals(const NonlinearFactor<Values>& expected, double tol) const {
const BetweenFactor<Config, Key1, Key2> *e = const BetweenFactor<Values, Key1, Key2> *e =
dynamic_cast<const BetweenFactor<Config, Key1, Key2>*> (&expected); dynamic_cast<const BetweenFactor<Values, Key1, Key2>*> (&expected);
return e != NULL && Base::equals(expected, tol) && this->measured_.equals( return e != NULL && Base::equals(expected, tol) && this->measured_.equals(
e->measured_, tol); e->measured_, tol);
} }

16
slam/GaussianISAM2.cpp
View File

@ -12,15 +12,15 @@ using namespace gtsam;
// Explicitly instantiate so we don't have to include everywhere // Explicitly instantiate so we don't have to include everywhere
#include <gtsam/inference/ISAM2-inl.h> #include <gtsam/inference/ISAM2-inl.h>
template class ISAM2<GaussianConditional, simulated2D::Config>; template class ISAM2<GaussianConditional, simulated2D::Values>;
template class ISAM2<GaussianConditional, planarSLAM::Config>; template class ISAM2<GaussianConditional, planarSLAM::Values>;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorConfig>& delta, int& count) { vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
// if none of the variables in this clique (frontal and separator!) changed // if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the // significantly, then by the running intersection property, none of the
// cliques in the children need to be processed // cliques in the children need to be processed
@ -60,7 +60,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
if (!redo) { if (!redo) {
// change is measured against the previous delta! // change is measured against the previous delta!
// if (delta.contains(cg->key())) { // if (delta.contains(cg->key())) {
const VectorConfig::mapped_type d_old(delta[cg->key()]); const VectorValues::mapped_type d_old(delta[cg->key()]);
assert(d_old.size() == d.size()); assert(d_old.size() == d.size());
for(size_t i=0; i<d_old.size(); ++i) { for(size_t i=0; i<d_old.size(); ++i) {
if(fabs(d(i) - d_old(i)) >= threshold) { if(fabs(d(i) - d_old(i)) >= threshold) {
@ -97,7 +97,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
/* ************************************************************************* */ /* ************************************************************************* */
// fast full version without threshold // fast full version without threshold
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& delta) { void optimize2(const GaussianISAM2::sharedClique& clique, VectorValues& delta) {
// parents are assumed to already be solved and available in result // parents are assumed to already be solved and available in result
GaussianISAM2::Clique::const_reverse_iterator it; GaussianISAM2::Clique::const_reverse_iterator it;
for (it = clique->rbegin(); it!=clique->rend(); it++) { for (it = clique->rbegin(); it!=clique->rend(); it++) {
@ -112,8 +112,8 @@ void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& delta) {
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//boost::shared_ptr<VectorConfig> optimize2(const GaussianISAM2::sharedClique& root) { //boost::shared_ptr<VectorValues> optimize2(const GaussianISAM2::sharedClique& root) {
// boost::shared_ptr<VectorConfig> delta(new VectorConfig()); // boost::shared_ptr<VectorValues> delta(new VectorValues());
// set<Symbol> changed; // set<Symbol> changed;
// // starting from the root, call optimize on each conditional // // starting from the root, call optimize on each conditional
// optimize2(root, delta); // optimize2(root, delta);
@ -121,7 +121,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& delta) {
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
int optimize2(const GaussianISAM2::sharedClique& root, double threshold, const vector<bool>& keys, Permuted<VectorConfig>& delta) { int optimize2(const GaussianISAM2::sharedClique& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
vector<bool> changed(keys.size(), false); vector<bool> changed(keys.size(), false);
int count = 0; int count = 0;
// starting from the root, call optimize on each conditional // starting from the root, call optimize on each conditional

8
slam/GaussianISAM2.h
View File

@ -16,11 +16,11 @@
namespace gtsam { namespace gtsam {
typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2; typedef ISAM2<GaussianConditional, simulated2D::Values> GaussianISAM2;
typedef ISAM2<GaussianConditional, planarSLAM::Config> GaussianISAM2_P; typedef ISAM2<GaussianConditional, planarSLAM::Values> GaussianISAM2_P;
// optimize the BayesTree, starting from the root // optimize the BayesTree, starting from the root
void optimize2(const GaussianISAM2::sharedClique& root, VectorConfig& delta); void optimize2(const GaussianISAM2::sharedClique& root, VectorValues& delta);
// optimize the BayesTree, starting from the root; "replaced" needs to contain // optimize the BayesTree, starting from the root; "replaced" needs to contain
// all variables that are contained in the top of the Bayes tree that has been // all variables that are contained in the top of the Bayes tree that has been
@ -31,7 +31,7 @@ namespace gtsam {
// variables are contained in the subtree. // variables are contained in the subtree.
// returns the number of variables that were solved for // returns the number of variables that were solved for
int optimize2(const GaussianISAM2::sharedClique& root, int optimize2(const GaussianISAM2::sharedClique& root,
double threshold, const std::vector<bool>& replaced, Permuted<VectorConfig>& delta); double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) // calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
int calculate_nnz(const GaussianISAM2::sharedClique& clique); int calculate_nnz(const GaussianISAM2::sharedClique& clique);

20
slam/LinearApproxFactor-inl.h
View File

@ -14,8 +14,8 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config, class Key> template <class Values, class Key>
LinearApproxFactor<Config,Key>::LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Config& lin_points) LinearApproxFactor<Values,Key>::LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Values& lin_points)
: Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())), : Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())),
lin_factor_(lin_factor), lin_points_(lin_points) lin_factor_(lin_factor), lin_points_(lin_points)
{ {
@ -27,10 +27,10 @@ LinearApproxFactor<Config,Key>::LinearApproxFactor(GaussianFactor::shared_ptr li
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config, class Key> template <class Values, class Key>
Vector LinearApproxFactor<Config,Key>::unwhitenedError(const Config& c) const { Vector LinearApproxFactor<Values,Key>::unwhitenedError(const Values& c) const {
// extract the points in the new config // extract the points in the new config
VectorConfig delta; VectorValues delta;
BOOST_FOREACH(const Key& key, nonlinearKeys_) { BOOST_FOREACH(const Key& key, nonlinearKeys_) {
X newPt = c[key], linPt = lin_points_[key]; X newPt = c[key], linPt = lin_points_[key];
Vector d = linPt.logmap(newPt); Vector d = linPt.logmap(newPt);
@ -41,9 +41,9 @@ Vector LinearApproxFactor<Config,Key>::unwhitenedError(const Config& c) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config, class Key> template <class Values, class Key>
boost::shared_ptr<GaussianFactor> boost::shared_ptr<GaussianFactor>
LinearApproxFactor<Config,Key>::linearize(const Config& c) const { LinearApproxFactor<Values,Key>::linearize(const Values& c) const {
Vector b = lin_factor_->getb(); Vector b = lin_factor_->getb();
SharedDiagonal model = lin_factor_->get_model(); SharedDiagonal model = lin_factor_->get_model();
std::vector<std::pair<Symbol, Matrix> > terms; std::vector<std::pair<Symbol, Matrix> > terms;
@ -56,9 +56,9 @@ LinearApproxFactor<Config,Key>::linearize(const Config& c) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config, class Key> template <class Values, class Key>
void LinearApproxFactor<Config,Key>::print(const std::string& s) const { void LinearApproxFactor<Values,Key>::print(const std::string& s) const {
LinearApproxFactor<Config,Key>::Base::print(s); LinearApproxFactor<Values,Key>::Base::print(s);
lin_factor_->print(); lin_factor_->print();
} }

20
slam/LinearApproxFactor.h
View File

@ -8,7 +8,7 @@
#include <vector> #include <vector>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
namespace gtsam { namespace gtsam {
@ -17,18 +17,18 @@ namespace gtsam {
* A dummy factor that takes a linearized factor and inserts it into * A dummy factor that takes a linearized factor and inserts it into
* a nonlinear graph. This version uses exactly one type of variable. * a nonlinear graph. This version uses exactly one type of variable.
*/ */
template <class Config, class Key> template <class Values, class Key>
class LinearApproxFactor : public NonlinearFactor<Config> { class LinearApproxFactor : public NonlinearFactor<Values> {
public: public:
/** type for the variable */ /** type for the variable */
typedef typename Key::Value_t X; typedef typename Key::Value_t X;
/** base type */ /** base type */
typedef NonlinearFactor<Config> Base; typedef NonlinearFactor<Values> Base;
/** shared pointer for convenience */ /** shared pointer for convenience */
typedef boost::shared_ptr<LinearApproxFactor<Config,Key> > shared_ptr; typedef boost::shared_ptr<LinearApproxFactor<Values,Key> > shared_ptr;
/** typedefs for key vectors */ /** typedefs for key vectors */
typedef std::vector<Key> KeyVector; typedef std::vector<Key> KeyVector;
@ -38,7 +38,7 @@ protected:
GaussianFactor::shared_ptr lin_factor_; GaussianFactor::shared_ptr lin_factor_;
/** linearization points for error calculation */ /** linearization points for error calculation */
Config lin_points_; Values lin_points_;
/** keep keys for the factor */ /** keep keys for the factor */
KeyVector nonlinearKeys_; KeyVector nonlinearKeys_;
@ -46,18 +46,18 @@ protected:
/** /**
* use this for derived classes with keys that don't copy easily * use this for derived classes with keys that don't copy easily
*/ */
LinearApproxFactor(size_t dim, const Config& lin_points) LinearApproxFactor(size_t dim, const Values& lin_points)
: Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {} : Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {}
public: public:
/** use this constructor when starting with nonlinear keys */ /** use this constructor when starting with nonlinear keys */
LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Config& lin_points); LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Values& lin_points);
virtual ~LinearApproxFactor() {} virtual ~LinearApproxFactor() {}
/** Vector of errors, unwhitened ! */ /** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const Config& c) const; virtual Vector unwhitenedError(const Values& c) const;
/** /**
* linearize to a GaussianFactor * linearize to a GaussianFactor
@ -65,7 +65,7 @@ public:
* NOTE: copies to avoid actual factor getting destroyed * NOTE: copies to avoid actual factor getting destroyed
* during elimination * during elimination
*/ */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const; virtual boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
/** get access to nonlinear keys */ /** get access to nonlinear keys */
KeyVector nonlinearKeys() const { return nonlinearKeys_; } KeyVector nonlinearKeys() const { return nonlinearKeys_; }

10
slam/Makefile.am
View File

@ -11,14 +11,14 @@ sources =
check_PROGRAMS = check_PROGRAMS =
# simulated2D example # simulated2D example
headers += Simulated2DConfig.h headers += Simulated2DValues.h
headers += Simulated2DPosePrior.h Simulated2DPointPrior.h headers += Simulated2DPosePrior.h Simulated2DPointPrior.h
headers += Simulated2DOdometry.h Simulated2DMeasurement.h headers += Simulated2DOdometry.h Simulated2DMeasurement.h
sources += simulated2D.cpp smallExample.cpp sources += simulated2D.cpp smallExample.cpp
check_PROGRAMS += tests/testSimulated2D check_PROGRAMS += tests/testSimulated2D
# simulated2DOriented example # simulated2DOriented example
headers += Simulated2DOrientedConfig.h headers += Simulated2DOrientedValues.h
headers += Simulated2DOrientedPosePrior.h headers += Simulated2DOrientedPosePrior.h
headers += Simulated2DOrientedOdometry.h headers += Simulated2DOrientedOdometry.h
sources += simulated2DOriented.cpp sources += simulated2DOriented.cpp
@ -40,7 +40,7 @@ headers += LinearApproxFactor.h LinearApproxFactor-inl.h
# 2D Pose SLAM # 2D Pose SLAM
sources += pose2SLAM.cpp dataset.cpp sources += pose2SLAM.cpp dataset.cpp
#sources += Pose2SLAMOptimizer.cpp #sources += Pose2SLAMOptimizer.cpp
check_PROGRAMS += tests/testPose2Factor tests/testPose2Config tests/testPose2SLAM tests/testPose2Prior check_PROGRAMS += tests/testPose2Factor tests/testPose2Values tests/testPose2SLAM tests/testPose2Prior
# 2D SLAM using Bearing and Range # 2D SLAM using Bearing and Range
headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h
@ -49,11 +49,11 @@ check_PROGRAMS += tests/testPlanarSLAM
# 3D Pose constraints # 3D Pose constraints
sources += pose3SLAM.cpp sources += pose3SLAM.cpp
check_PROGRAMS += tests/testPose3Factor tests/testPose3Config tests/testPose3SLAM check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLAM
# Visual SLAM # Visual SLAM
sources += visualSLAM.cpp sources += visualSLAM.cpp
check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMConfig check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues
# GaussianISAM2 is fairly SLAM-specific # GaussianISAM2 is fairly SLAM-specific
sources += GaussianISAM2.cpp sources += GaussianISAM2.cpp

4
slam/Pose2SLAMOptimizer.cpp
View File

@ -48,7 +48,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void Pose2SLAMOptimizer::update(const Vector& x) { void Pose2SLAMOptimizer::update(const Vector& x) {
VectorConfig X = system_->assembleConfig(x, *solver_.ordering()); VectorValues X = system_->assembleValues(x, *solver_.ordering());
*theta_ = theta_->expmap(X); *theta_ = theta_->expmap(X);
linearize(); linearize();
} }
@ -61,7 +61,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose2SLAMOptimizer::optimize() const { Vector Pose2SLAMOptimizer::optimize() const {
VectorConfig X = solver_.optimize(*system_); VectorValues X = solver_.optimize(*system_);
std::list<Vector> vs; std::list<Vector> vs;
BOOST_FOREACH(const Symbol& key, *solver_.ordering()) BOOST_FOREACH(const Symbol& key, *solver_.ordering())
vs.push_back(X[key]); vs.push_back(X[key]);

8
slam/Pose2SLAMOptimizer.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
/** /**
* Optimizer class for use in MATLAB * Optimizer class for use in MATLAB
* Keeps a Pose2Config estimate * Keeps a Pose2Values estimate
* Returns all relevant matrices so MATLAB can optimize :-) * Returns all relevant matrices so MATLAB can optimize :-)
*/ */
class Pose2SLAMOptimizer { class Pose2SLAMOptimizer {
@ -28,10 +28,10 @@ namespace gtsam {
boost::shared_ptr<Pose2Graph> graph_; boost::shared_ptr<Pose2Graph> graph_;
/** Current non-linear estimate */ /** Current non-linear estimate */
boost::shared_ptr<Pose2Config> theta_; boost::shared_ptr<Pose2Values> theta_;
/** Non-linear solver */ /** Non-linear solver */
typedef SubgraphSolver<Pose2Graph, Pose2Config> SPCG_Solver; typedef SubgraphSolver<Pose2Graph, Pose2Values> SPCG_Solver;
SPCG_Solver solver_; SPCG_Solver solver_;
/** Linear Solver */ /** Linear Solver */
@ -65,7 +65,7 @@ namespace gtsam {
/** /**
* return the current linearization point * return the current linearization point
*/ */
boost::shared_ptr<const Pose2Config> theta() const { boost::shared_ptr<const Pose2Values> theta() const {
return theta_; return theta_;
} }

16
slam/PriorFactor.h
View File

@ -15,20 +15,20 @@ namespace gtsam {
* A class for a soft prior on any Lie type * A class for a soft prior on any Lie type
* It takes three template parameters: * It takes three template parameters:
* T is the Lie group type for which the prior is define * T is the Lie group type for which the prior is define
* Key (typically TypedSymbol) is used to look up T's in a Config * Key (typically TypedSymbol) is used to look up T's in a Values
* Config where the T's are stored, typically LieValues<Key,T> or a TupleValues<...> * Values where the T's are stored, typically LieValues<Key,T> or a TupleValues<...>
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work * a simple type like int will not work
*/ */
template<class Config, class Key> template<class Values, class Key>
class PriorFactor: public NonlinearFactor1<Config, Key> { class PriorFactor: public NonlinearFactor1<Values, Key> {
public: public:
typedef typename Key::Value_t T; typedef typename Key::Value_t T;
private: private:
typedef NonlinearFactor1<Config, Key> Base; typedef NonlinearFactor1<Values, Key> Base;
T prior_; /** The measurement */ T prior_; /** The measurement */
@ -52,9 +52,9 @@ namespace gtsam {
} }
/** equals */ /** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const { bool equals(const NonlinearFactor<Values>& expected, double tol) const {
const PriorFactor<Config, Key> *e = dynamic_cast<const PriorFactor< const PriorFactor<Values, Key> *e = dynamic_cast<const PriorFactor<
Config, Key>*> (&expected); Values, Key>*> (&expected);
return e != NULL && Base::equals(expected, tol) && this->prior_.equals( return e != NULL && Base::equals(expected, tol) && this->prior_.equals(
e->prior_, tol); e->prior_, tol);
} }

6
slam/RangeFactor.h
View File

@ -14,13 +14,13 @@ namespace gtsam {
/** /**
* Binary factor for a range measurement * Binary factor for a range measurement
*/ */
template<class Config, class PoseKey, class PointKey> template<class Values, class PoseKey, class PointKey>
class RangeFactor: public NonlinearFactor2<Config, PoseKey, PointKey> { class RangeFactor: public NonlinearFactor2<Values, PoseKey, PointKey> {
private: private:
double z_; /** measurement */ double z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, PointKey> Base; typedef NonlinearFactor2<Values, PoseKey, PointKey> Base;
public: public:

2
slam/Simulated2DMeasurement.h
View File

@ -8,7 +8,7 @@
#pragma once #pragma once
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/Simulated2DConfig.h> #include <gtsam/slam/Simulated2DValues.h>
namespace gtsam { namespace gtsam {

2
slam/Simulated2DOdometry.h
View File

@ -8,7 +8,7 @@
#pragma once #pragma once
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/Simulated2DConfig.h> #include <gtsam/slam/Simulated2DValues.h>
namespace gtsam { namespace gtsam {

2
slam/Simulated2DOrientedOdometry.h
View File

@ -8,7 +8,7 @@
#pragma once #pragma once
#include <gtsam/slam/simulated2DOriented.h> #include <gtsam/slam/simulated2DOriented.h>
#include <gtsam/slam/Simulated2DOrientedConfig.h> #include <gtsam/slam/Simulated2DOrientedValues.h>
namespace gtsam { namespace gtsam {

4
slam/Simulated2DOrientedPosePrior.h
View File

@ -8,12 +8,12 @@
#pragma once #pragma once
#include <gtsam/slam/simulated2DOriented.h> #include <gtsam/slam/simulated2DOriented.h>
#include <gtsam/slam/Simulated2DOrientedConfig.h> #include <gtsam/slam/Simulated2DOrientedValues.h>
namespace gtsam { namespace gtsam {
/** Create a prior on a pose Point2 with key 'x1' etc... */ /** Create a prior on a pose Point2 with key 'x1' etc... */
typedef simulated2DOriented::GenericPosePrior<Simulated2DOrientedConfig, simulated2DOriented::PoseKey> Simulated2DOrientedPosePrior; typedef simulated2DOriented::GenericPosePrior<Simulated2DOrientedValues, simulated2DOriented::PoseKey> Simulated2DOrientedPosePrior;
} }

6
slam/Simulated2DOrientedConfig.h → slam/Simulated2DOrientedValues.h
View File

@ -1,5 +1,5 @@
/* /*
* Simulated2DConfig.h * Simulated2DValues.h
* *
* Re-created on Feb 22, 2010 for compatibility with MATLAB * Re-created on Feb 22, 2010 for compatibility with MATLAB
* Author: Frank Dellaert * Author: Frank Dellaert
@ -11,12 +11,12 @@
namespace gtsam { namespace gtsam {
class Simulated2DOrientedConfig: public simulated2DOriented::Config { class Simulated2DOrientedValues: public simulated2DOriented::Values {
public: public:
typedef boost::shared_ptr<Point2> sharedPoint; typedef boost::shared_ptr<Point2> sharedPoint;
typedef boost::shared_ptr<Pose2> sharedPose; typedef boost::shared_ptr<Pose2> sharedPose;
Simulated2DOrientedConfig() { Simulated2DOrientedValues() {
} }
void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) { void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) {

4
slam/Simulated2DPointPrior.h
View File

@ -8,12 +8,12 @@
#pragma once #pragma once
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/Simulated2DConfig.h> #include <gtsam/slam/Simulated2DValues.h>
namespace gtsam { namespace gtsam {
/** Create a prior on a landmark Point2 with key 'l1' etc... */ /** Create a prior on a landmark Point2 with key 'l1' etc... */
typedef simulated2D::GenericPrior<Simulated2DConfig, simulated2D::PointKey> Simulated2DPointPrior; typedef simulated2D::GenericPrior<Simulated2DValues, simulated2D::PointKey> Simulated2DPointPrior;
} }

4
slam/Simulated2DPosePrior.h
View File

@ -8,12 +8,12 @@
#pragma once #pragma once
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/Simulated2DConfig.h> #include <gtsam/slam/Simulated2DValues.h>
namespace gtsam { namespace gtsam {
/** Create a prior on a pose Point2 with key 'x1' etc... */ /** Create a prior on a pose Point2 with key 'x1' etc... */
typedef simulated2D::GenericPrior<Simulated2DConfig, simulated2D::PoseKey> Simulated2DPosePrior; typedef simulated2D::GenericPrior<Simulated2DValues, simulated2D::PoseKey> Simulated2DPosePrior;
} }

6
slam/Simulated2DConfig.h → slam/Simulated2DValues.h
View File

@ -1,5 +1,5 @@
/* /*
* Simulated2DConfig.h * Simulated2DValues.h
* *
* Re-created on Feb 22, 2010 for compatibility with MATLAB * Re-created on Feb 22, 2010 for compatibility with MATLAB
* Author: Frank Dellaert * Author: Frank Dellaert
@ -11,11 +11,11 @@
namespace gtsam { namespace gtsam {
class Simulated2DConfig: public simulated2D::Config { class Simulated2DValues: public simulated2D::Values {
public: public:
typedef boost::shared_ptr<Point2> sharedPoint; typedef boost::shared_ptr<Point2> sharedPoint;
Simulated2DConfig() { Simulated2DValues() {
} }
void insertPose(const simulated2D::PoseKey& i, const Point2& p) { void insertPose(const simulated2D::PoseKey& i, const Point2& p) {

2
slam/Simulated3D.cpp
View File

@ -13,7 +13,7 @@ namespace gtsam {
using namespace simulated3D; using namespace simulated3D;
INSTANTIATE_LIE_CONFIG(PointKey) INSTANTIATE_LIE_CONFIG(PointKey)
INSTANTIATE_LIE_CONFIG(PoseKey) INSTANTIATE_LIE_CONFIG(PoseKey)
INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues)
namespace simulated3D { namespace simulated3D {

16
slam/Simulated3D.h
View File

@ -10,7 +10,7 @@
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/linear/VectorConfig.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/TupleValues.h> #include <gtsam/nonlinear/TupleValues.h>
@ -23,9 +23,9 @@ namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey; typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey; typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<PoseKey> PoseConfig; typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointConfig; typedef LieValues<PointKey> PointValues;
typedef TupleValues2<PoseConfig, PointConfig> Config; typedef TupleValues2<PoseValues, PointValues> Values;
/** /**
* Prior on a single pose * Prior on a single pose
@ -46,13 +46,13 @@ namespace simulated3D {
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none); boost::optional<Matrix&> H2 = boost::none);
struct PointPrior3D: public NonlinearFactor1<Config, PoseKey> { struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
Point3 z_; Point3 z_;
PointPrior3D(const Point3& z, PointPrior3D(const Point3& z,
const SharedGaussian& model, const PoseKey& j) : const SharedGaussian& model, const PoseKey& j) :
NonlinearFactor1<Config, PoseKey> (model, j), z_(z) { NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
} }
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H = Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
@ -61,14 +61,14 @@ namespace simulated3D {
} }
}; };
struct Simulated3DMeasurement: public NonlinearFactor2<Config, struct Simulated3DMeasurement: public NonlinearFactor2<Values,
PoseKey, PointKey> { PoseKey, PointKey> {
Point3 z_; Point3 z_;
Simulated3DMeasurement(const Point3& z, Simulated3DMeasurement(const Point3& z,
const SharedGaussian& model, PoseKey& j1, PointKey j2) : const SharedGaussian& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<Config, PoseKey, PointKey> ( NonlinearFactor2<Values, PoseKey, PointKey> (
model, j1, j2), z_(z) { model, j1, j2), z_(z) {
} }

10
slam/TransformConstraint.h
View File

@ -13,7 +13,7 @@ namespace gtsam {
/** /**
* A constraint between two landmarks in separate maps * A constraint between two landmarks in separate maps
* Templated on: * Templated on:
* Config : The overall config * Values : The overall config
* PKey : Key of landmark being constrained * PKey : Key of landmark being constrained
* Point : Type of landmark * Point : Type of landmark
* TKey : Key of transform used * TKey : Key of transform used
@ -25,15 +25,15 @@ namespace gtsam {
* This base class should be specialized to implement the cost function for * This base class should be specialized to implement the cost function for
* specific classes of landmarks * specific classes of landmarks
*/ */
template<class Config, class PKey, class TKey> template<class Values, class PKey, class TKey>
class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, TKey, PKey> { class TransformConstraint : public NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> {
public: public:
typedef typename PKey::Value_t Point; typedef typename PKey::Value_t Point;
typedef typename TKey::Value_t Transform; typedef typename TKey::Value_t Transform;
typedef NonlinearEqualityConstraint3<Config, PKey, TKey, PKey> Base; typedef NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> Base;
typedef TransformConstraint<Config, PKey, TKey> This; typedef TransformConstraint<Values, PKey, TKey> This;
/** /**
* General constructor with all of the keys to variables in the map * General constructor with all of the keys to variables in the map

16
slam/dataset.cpp
View File

@ -20,7 +20,7 @@ using namespace gtsam;
#define LINESIZE 81920 #define LINESIZE 81920
typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph; typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
typedef boost::shared_ptr<Pose2Config> sharedPose2Config; typedef boost::shared_ptr<Pose2Values> sharedPose2Values;
namespace gtsam { namespace gtsam {
@ -68,13 +68,13 @@ pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, c
/* ************************************************************************* */ /* ************************************************************************* */
pair<sharedPose2Graph, sharedPose2Config> load2D( pair<sharedPose2Graph, sharedPose2Values> load2D(
pair<string, boost::optional<SharedDiagonal> > dataset, pair<string, boost::optional<SharedDiagonal> > dataset,
int maxID, bool addNoise, bool smart) { int maxID, bool addNoise, bool smart) {
return load2D(dataset.first, dataset.second, maxID, addNoise, smart); return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
} }
pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename, pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
boost::optional<SharedDiagonal> model, int maxID, bool addNoise, bool smart) { boost::optional<SharedDiagonal> model, int maxID, bool addNoise, bool smart) {
cout << "Will try to read " << filename << endl; cout << "Will try to read " << filename << endl;
ifstream is(filename.c_str()); ifstream is(filename.c_str());
@ -83,7 +83,7 @@ pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
exit(-1); exit(-1);
} }
sharedPose2Config poses(new Pose2Config); sharedPose2Values poses(new Pose2Values);
sharedPose2Graph graph(new Pose2Graph); sharedPose2Graph graph(new Pose2Graph);
string tag; string tag;
@ -151,14 +151,14 @@ pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
} }
/* ************************************************************************* */ /* ************************************************************************* */
void save2D(const Pose2Graph& graph, const Pose2Config& config, const SharedDiagonal model, void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiagonal model,
const string& filename) { const string& filename) {
typedef Pose2Config::Key Key; typedef Pose2Values::Key Key;
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
Pose2Config::Key key; Pose2Values::Key key;
Pose2 pose; Pose2 pose;
BOOST_FOREACH(boost::tie(key, pose), config) BOOST_FOREACH(boost::tie(key, pose), config)
stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
@ -166,7 +166,7 @@ void save2D(const Pose2Graph& graph, const Pose2Config& config, const SharedDiag
// save edges // save edges
Matrix R = model->R(); Matrix R = model->R();
Matrix RR = prod(trans(R),R); Matrix RR = prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<Pose2Config> > factor_, graph) { BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<Pose2Values> > factor_, graph) {
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_); boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
if (!factor) continue; if (!factor) continue;

6
slam/dataset.h
View File

@ -32,16 +32,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
* @param maxID, if non-zero cut out vertices >= maxID * @param maxID, if non-zero cut out vertices >= maxID
* @param smart: try to reduce complexity of covariance to cheapest model * @param smart: try to reduce complexity of covariance to cheapest model
*/ */
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Config> > load2D( std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D(
std::pair<std::string, boost::optional<SharedDiagonal> > dataset, std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
int maxID = 0, bool addNoise=false, bool smart=true); int maxID = 0, bool addNoise=false, bool smart=true);
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Config> > load2D( std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D(
const std::string& filename, const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(), boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
int maxID = 0, bool addNoise=false, bool smart=true); int maxID = 0, bool addNoise=false, bool smart=true);
/** save 2d graph */ /** save 2d graph */
void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Config& config, const gtsam::SharedDiagonal model, void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Values& config, const gtsam::SharedDiagonal model,
const std::string& filename); const std::string& filename);
/** /**

10
slam/planarSLAM.cpp
View File

@ -14,14 +14,14 @@ namespace gtsam {
using namespace planarSLAM; using namespace planarSLAM;
INSTANTIATE_LIE_CONFIG(PointKey) INSTANTIATE_LIE_CONFIG(PointKey)
INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace planarSLAM { namespace planarSLAM {
Graph::Graph(const NonlinearFactorGraph<Config>& graph) : Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
NonlinearFactorGraph<Config>(graph) {} NonlinearFactorGraph<Values>(graph) {}
void Graph::addPrior(const PoseKey& i, const Pose2& p, void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedGaussian& model) { const SharedGaussian& model) {

26
slam/planarSLAM.h
View File

@ -20,25 +20,25 @@ namespace gtsam {
// Use planarSLAM namespace for specific SLAM instance // Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM { namespace planarSLAM {
// Keys and Config // Keys and Values
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieValues<PoseKey> PoseConfig; typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointConfig; typedef LieValues<PointKey> PointValues;
typedef TupleValues2<PoseConfig, PointConfig> Config; typedef TupleValues2<PoseValues, PointValues> Values;
// Factors // Factors
typedef NonlinearEquality<Config, PoseKey> Constraint; typedef NonlinearEquality<Values, PoseKey> Constraint;
typedef PriorFactor<Config, PoseKey> Prior; typedef PriorFactor<Values, PoseKey> Prior;
typedef BetweenFactor<Config, PoseKey> Odometry; typedef BetweenFactor<Values, PoseKey> Odometry;
typedef BearingFactor<Config, PoseKey, PointKey> Bearing; typedef BearingFactor<Values, PoseKey, PointKey> Bearing;
typedef RangeFactor<Config, PoseKey, PointKey> Range; typedef RangeFactor<Values, PoseKey, PointKey> Range;
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange; typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
// Graph // Graph
struct Graph: public NonlinearFactorGraph<Config> { struct Graph: public NonlinearFactorGraph<Values> {
Graph(){} Graph(){}
Graph(const NonlinearFactorGraph<Config>& graph); Graph(const NonlinearFactorGraph<Values>& graph);
void addPrior(const PoseKey& i, const Pose2& p, const SharedGaussian& model); void addPrior(const PoseKey& i, const Pose2& p, const SharedGaussian& model);
void addPoseConstraint(const PoseKey& i, const Pose2& p); void addPoseConstraint(const PoseKey& i, const Pose2& p);
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
@ -52,7 +52,7 @@ namespace gtsam {
}; };
// Optimizer // Optimizer
typedef NonlinearOptimizer<Graph, Config> Optimizer; typedef NonlinearOptimizer<Graph, Values> Optimizer;
} // planarSLAM } // planarSLAM

8
slam/pose2SLAM.cpp
View File

@ -14,14 +14,14 @@ namespace gtsam {
using namespace pose2SLAM; using namespace pose2SLAM;
INSTANTIATE_LIE_CONFIG(Key) INSTANTIATE_LIE_CONFIG(Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace pose2SLAM { namespace pose2SLAM {
/* ************************************************************************* */ /* ************************************************************************* */
Config circle(size_t n, double R) { Values circle(size_t n, double R) {
Config x; Values x;
double theta = 0, dtheta = 2 * M_PI / n; double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta) for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));

20
slam/pose2SLAM.h
View File

@ -21,9 +21,9 @@ namespace gtsam {
// Use pose2SLAM namespace for specific SLAM instance // Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM { namespace pose2SLAM {
// Keys and Config // Keys and Values
typedef TypedSymbol<Pose2, 'x'> Key; typedef TypedSymbol<Pose2, 'x'> Key;
typedef LieValues<Key> Config; typedef LieValues<Key> Values;
/** /**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
@ -32,16 +32,16 @@ namespace gtsam {
* @param c character to use for keys * @param c character to use for keys
* @return circle of n 2D poses * @return circle of n 2D poses
*/ */
Config circle(size_t n, double R); Values circle(size_t n, double R);
// Factors // Factors
typedef PriorFactor<Config, Key> Prior; typedef PriorFactor<Values, Key> Prior;
typedef BetweenFactor<Config, Key> Constraint; typedef BetweenFactor<Values, Key> Constraint;
typedef NonlinearEquality<Config, Key> HardConstraint; typedef NonlinearEquality<Values, Key> HardConstraint;
// Graph // Graph
struct Graph: public NonlinearFactorGraph<Config> { struct Graph: public NonlinearFactorGraph<Values> {
typedef BetweenFactor<Config, Key> Constraint; typedef BetweenFactor<Values, Key> Constraint;
typedef Pose2 Pose; typedef Pose2 Pose;
void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model); void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model);
void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model); void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model);
@ -49,14 +49,14 @@ namespace gtsam {
}; };
// Optimizer // Optimizer
typedef NonlinearOptimizer<Graph, Config> Optimizer; typedef NonlinearOptimizer<Graph, Values> Optimizer;
} // pose2SLAM } // pose2SLAM
/** /**
* Backwards compatibility * Backwards compatibility
*/ */
typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Values Pose2Values;
typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Prior Pose2Prior;
typedef pose2SLAM::Constraint Pose2Factor; typedef pose2SLAM::Constraint Pose2Factor;
typedef pose2SLAM::Graph Pose2Graph; typedef pose2SLAM::Graph Pose2Graph;

8
slam/pose3SLAM.cpp
View File

@ -14,14 +14,14 @@ namespace gtsam {
using namespace pose3SLAM; using namespace pose3SLAM;
INSTANTIATE_LIE_CONFIG(Key) INSTANTIATE_LIE_CONFIG(Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace pose3SLAM { namespace pose3SLAM {
/* ************************************************************************* */ /* ************************************************************************* */
Config circle(size_t n, double radius) { Values circle(size_t n, double radius) {
Config x; Values x;
double theta = 0, dtheta = 2 * M_PI / n; double theta = 0, dtheta = 2 * M_PI / n;
// We use aerospace/navlab convention, X forward, Y right, Z down // We use aerospace/navlab convention, X forward, Y right, Z down
// First pose will be at (R,0,0) // First pose will be at (R,0,0)

18
slam/pose3SLAM.h
View File

@ -20,9 +20,9 @@ namespace gtsam {
// Use pose3SLAM namespace for specific SLAM instance // Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM { namespace pose3SLAM {
// Keys and Config // Keys and Values
typedef TypedSymbol<Pose3, 'x'> Key; typedef TypedSymbol<Pose3, 'x'> Key;
typedef LieValues<Key> Config; typedef LieValues<Key> Values;
/** /**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
@ -31,15 +31,15 @@ namespace gtsam {
* @param c character to use for keys * @param c character to use for keys
* @return circle of n 3D poses * @return circle of n 3D poses
*/ */
Config circle(size_t n, double R); Values circle(size_t n, double R);
// Factors // Factors
typedef PriorFactor<Config, Key> Prior; typedef PriorFactor<Values, Key> Prior;
typedef BetweenFactor<Config, Key> Constraint; typedef BetweenFactor<Values, Key> Constraint;
typedef NonlinearEquality<Config, Key> HardConstraint; typedef NonlinearEquality<Values, Key> HardConstraint;
// Graph // Graph
struct Graph: public NonlinearFactorGraph<Config> { struct Graph: public NonlinearFactorGraph<Values> {
void addPrior(const Key& i, const Pose3& p, void addPrior(const Key& i, const Pose3& p,
const SharedGaussian& model); const SharedGaussian& model);
void addConstraint(const Key& i, const Key& j, const Pose3& z, void addConstraint(const Key& i, const Key& j, const Pose3& z,
@ -48,14 +48,14 @@ namespace gtsam {
}; };
// Optimizer // Optimizer
typedef NonlinearOptimizer<Graph, Config> Optimizer; typedef NonlinearOptimizer<Graph, Values> Optimizer;
} // pose3SLAM } // pose3SLAM
/** /**
* Backwards compatibility * Backwards compatibility
*/ */
typedef pose3SLAM::Config Pose3Config; typedef pose3SLAM::Values Pose3Values;
typedef pose3SLAM::Prior Pose3Prior; typedef pose3SLAM::Prior Pose3Prior;
typedef pose3SLAM::Constraint Pose3Factor; typedef pose3SLAM::Constraint Pose3Factor;
typedef pose3SLAM::Graph Pose3Graph; typedef pose3SLAM::Graph Pose3Graph;

2
slam/saveGraph.cpp
View File

@ -20,7 +20,7 @@ namespace gtsam {
INSTANTIATE_LIE_CONFIG(Symbol, Point2) INSTANTIATE_LIE_CONFIG(Symbol, Point2)
/* ************************************************************************* */ /* ************************************************************************* */
void saveGraph(const SymbolicFactorGraph& fg, const SymbolicConfig& config, const std::string& s) { void saveGraph(const SymbolicFactorGraph& fg, const SymbolicValues& config, const std::string& s) {
Symbol key; Symbol key;
Point2 pt; Point2 pt;

4
slam/saveGraph.h
View File

@ -17,9 +17,9 @@
namespace gtsam { namespace gtsam {
class Point2; class Point2;
typedef LieValues<Symbol, Point2> SymbolicConfig; typedef LieValues<Symbol, Point2> SymbolicValues;
// save graph to the graphviz format // save graph to the graphviz format
void saveGraph(const SymbolicFactorGraph& fg, const SymbolicConfig& config, const std::string& s); void saveGraph(const SymbolicFactorGraph& fg, const SymbolicValues& config, const std::string& s);
} // namespace gtsam } // namespace gtsam

6
slam/simulated2D.cpp
View File

@ -13,9 +13,9 @@ namespace gtsam {
using namespace simulated2D; using namespace simulated2D;
// INSTANTIATE_LIE_CONFIG(PointKey) // INSTANTIATE_LIE_CONFIG(PointKey)
INSTANTIATE_LIE_CONFIG(PoseKey) INSTANTIATE_LIE_CONFIG(PoseKey)
INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) // INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) // INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace simulated2D { namespace simulated2D {

Some files were not shown because too many files have changed in this diff Show More