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");
// initialize to noisy points
Config initialEstimate;
Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -75,7 +75,7 @@ int main(int argc, char** argv) {
initialEstimate.print("Initial Estimate");
// 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");

26
examples/PlanarSLAMSelfContained.cpp
View File

@ -31,11 +31,11 @@
// Main typedefs
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::LieValues<PoseKey> PoseConfig; // config type for poses
typedef gtsam::LieValues<PointKey> PointConfig; // config type for points
typedef gtsam::TupleValues2<PoseConfig, PointConfig> Config; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Config> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Config> Optimizer; // optimization engine for this domain
typedef gtsam::LieValues<PoseKey> PoseValues; // config type for poses
typedef gtsam::LieValues<PointKey> PointValues; // config type for points
typedef gtsam::TupleValues2<PoseValues, PointValues> Values; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Values> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Values> Optimizer; // optimization engine for this domain
using namespace std;
using namespace gtsam;
@ -61,7 +61,7 @@ int main(int argc, char** argv) {
// gaussian for prior
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
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
/* 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));
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
BetweenFactor<Config,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Config,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
BetweenFactor<Values,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Values,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
graph.add(odom12); // add both to graph
graph.add(odom23);
@ -87,9 +87,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// create bearing/range factors
BearingRangeFactor<Config, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Config, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Config, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
BearingRangeFactor<Values, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Values, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Values, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors
graph.add(meas11);
@ -99,7 +99,7 @@ int main(int argc, char** argv) {
graph.print("Full Graph");
// initialize to noisy points
Config initialEstimate;
Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -109,7 +109,7 @@ int main(int argc, char** argv) {
initialEstimate.print("Initial Estimate");
// 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");

16
examples/SimpleRotation.cpp
View File

@ -21,7 +21,7 @@
#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: make toplevel documentation
* 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;
typedef TypedSymbol<Rot2, 'x'> Key;
typedef LieValues<Key> Config;
typedef NonlinearFactorGraph<Config> Graph;
typedef Factorization<Graph,Config> Solver;
typedef NonlinearOptimizer<Graph,Config> Optimizer;
typedef LieValues<Key> Values;
typedef NonlinearFactorGraph<Values> Graph;
typedef Factorization<Graph,Values> Solver;
typedef NonlinearOptimizer<Graph,Values> Optimizer;
const double degree = M_PI / 180;
@ -47,19 +47,19 @@ int main() {
prior1.print("Goal Angle");
SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key1(1);
PriorFactor<Config, Key> factor1(key1, prior1, model1);
PriorFactor<Values, Key> factor1(key1, prior1, model1);
// Create a factor graph
Graph graph;
graph.add(factor1);
// and an initial estimate
Config initialEstimate;
Values initialEstimate;
initialEstimate.insert(key1, Rot2::fromAngle(20 * degree));
initialEstimate.print("Initialization");
// 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");
return 0;

12
gtsam-broken.h
View File

@ -1,9 +1,9 @@
// 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
class Pose2Config{
Pose2Config();
class Pose2Values{
Pose2Values();
Pose2 get(string key) const;
void insert(string name, const Pose2& val);
void print(string s) const;
@ -15,15 +15,15 @@ class Pose2Factor {
Pose2Factor(string key1, string key2,
const Pose2& measured, Matrix measurement_covariance);
void print(string name) const;
double error(const Pose2Config& c) const;
double error(const Pose2Values& c) const;
size_t size() const;
GaussianFactor* linearize(const Pose2Config& config) const;
GaussianFactor* linearize(const Pose2Values& config) const;
};
class Pose2Graph{
Pose2Graph();
void print(string s) const;
GaussianFactorGraph* linearize_(const Pose2Config& config) const;
GaussianFactorGraph* linearize_(const Pose2Values& config) const;
void push_back(Pose2Factor* factor);
};

52
gtsam.h
View File

@ -23,10 +23,10 @@ class SymbolicFactor{
void print(string s) const;
};
class VectorConfig {
VectorConfig();
class VectorValues {
VectorValues();
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);
Vector get(string name) const;
bool contains(string name) const;
@ -57,7 +57,7 @@ class GaussianFactor {
bool empty() const;
Vector get_b() const;
Matrix get_A(string key) const;
double error(const VectorConfig& c) const;
double error(const VectorValues& c) const;
bool involves(string key) const;
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
@ -91,7 +91,7 @@ class GaussianConditional {
void print(string s) const;
bool equals(const GaussianConditional &cg, double tol) const;
void add(string key, Matrix S);
Vector solve(const VectorConfig& x);
Vector solve(const VectorValues& x);
};
class GaussianBayesNet {
@ -109,17 +109,17 @@ class GaussianFactorGraph {
size_t size() const;
void push_back(GaussianFactor* ptr_f);
double error(const VectorConfig& c) const;
double probPrime(const VectorConfig& c) const;
double error(const VectorValues& c) const;
double probPrime(const VectorValues& c) const;
void combine(const GaussianFactorGraph& lfg);
GaussianConditional* eliminateOne(string key);
GaussianBayesNet* eliminate_(const Ordering& ordering);
VectorConfig* optimize_(const Ordering& ordering);
VectorValues* optimize_(const Ordering& ordering);
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
Matrix sparse(const Ordering& ordering) const;
VectorConfig* steepestDescent_(const VectorConfig& x0) const;
VectorConfig* conjugateGradientDescent_(const VectorConfig& x0) const;
VectorValues* steepestDescent_(const VectorValues& x0) const;
VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
};
class Point2 {
@ -159,8 +159,8 @@ class Pose2 {
Rot2 r() const;
};
class Simulated2DConfig {
Simulated2DConfig();
class Simulated2DValues {
Simulated2DValues();
void print(string s) const;
void insertPose(int i, const Point2& p);
void insertPoint(int j, const Point2& p);
@ -170,8 +170,8 @@ class Simulated2DConfig {
Point2* point(int j);
};
class Simulated2DOrientedConfig {
Simulated2DOrientedConfig();
class Simulated2DOrientedValues {
Simulated2DOrientedValues();
void print(string s) const;
void insertPose(int i, const Pose2& p);
void insertPoint(int j, const Point2& p);
@ -184,43 +184,43 @@ class Simulated2DOrientedConfig {
class Simulated2DPosePrior {
Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const;
double error(const Simulated2DConfig& c) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const;
};
class Simulated2DOrientedPosePrior {
Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
void print(string s) const;
GaussianFactor* linearize(const Simulated2DOrientedConfig& config) const;
double error(const Simulated2DOrientedConfig& c) const;
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
double error(const Simulated2DOrientedValues& c) const;
};
class Simulated2DPointPrior {
Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const;
double error(const Simulated2DConfig& c) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const;
};
class Simulated2DOdometry {
Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const;
double error(const Simulated2DConfig& c) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const;
};
class Simulated2DOrientedOdometry {
Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
void print(string s) const;
GaussianFactor* linearize(const Simulated2DOrientedConfig& config) const;
double error(const Simulated2DOrientedConfig& c) const;
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
double error(const Simulated2DOrientedValues& c) const;
};
class Simulated2DMeasurement {
Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
void print(string s) const;
GaussianFactor* linearize(const Simulated2DConfig& config) const;
double error(const Simulated2DConfig& c) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const;
};
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
* 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
* variables, continuous ones, or a combination of both. It is up to the config to
* 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.
* 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>
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/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/Conditional.h>
@ -33,12 +33,12 @@ using namespace std;
static const bool disableReordering = false;
/** Create an empty Bayes Tree */
template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>(), delta_(Permutation(), deltaUnpermuted_) {}
template<class Conditional, class Values>
ISAM2<Conditional, Values>::ISAM2() : BayesTree<Conditional>(), delta_(Permutation(), deltaUnpermuted_) {}
/** Create a Bayes Tree from a nonlinear factor graph */
//template<class Conditional, class Config>
//ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config) :
//template<class Conditional, class Values>
//ISAM2<Conditional, Values>::ISAM2(const NonlinearFactorGraph<Values>& nlfg, const Ordering& ordering, const Values& config) :
//BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)), theta_(config),
//variableIndex_(nlfg.symbolic(config, ordering), config.dims(ordering)), deltaUnpermuted_(variableIndex_.dims()),
//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>
list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<varid_t>& keys) const {
template<class Conditional, class Values>
list<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const list<varid_t>& keys) const {
static const bool debug = false;
if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const varid_t key, keys) { cout << key << " "; } }
if(debug) cout << endl;
FactorGraph<NonlinearFactor<Config> > allAffected;
FactorGraph<NonlinearFactor<Values> > allAffected;
list<size_t> indices;
BOOST_FOREACH(const varid_t key, keys) {
// 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
// (note that the remaining stuff is summarized in the cached factors)
template<class Conditional, class Config>
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors
template<class Conditional, class Values>
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Values>::relinearizeAffectedFactors
(const list<varid_t>& affectedKeys) const {
tic("8.2.2.1 getAffectedFactors");
list<size_t> candidates = getAffectedFactors(affectedKeys);
toc("8.2.2.1 getAffectedFactors");
NonlinearFactorGraph<Config> nonlinearAffectedFactors;
NonlinearFactorGraph<Values> nonlinearAffectedFactors;
tic("8.2.2.2 affectedKeysSet");
// 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
template<class Conditional, class Config>
GaussianFactorGraph ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
template<class Conditional, class Values>
GaussianFactorGraph ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false;
@ -139,8 +139,8 @@ GaussianFactorGraph ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques
}
/* ************************************************************************* */
template<class Conditional,class Config>
void reinsertCache(const typename ISAM2<Conditional,Config>::sharedClique& root, vector<GaussianFactor::shared_ptr>& cache, const Permutation& selector, const Permutation& selectorInverse) {
template<class Conditional,class Values>
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;
if(root) {
if(root->size() > 0) {
@ -162,15 +162,15 @@ void reinsertCache(const typename ISAM2<Conditional,Config>::sharedClique& root,
assert(!root->cachedFactor());
root->cachedFactor() = cachedFactor;
}
typedef ISAM2<Conditional,Config> This;
typedef ISAM2<Conditional,Values> This;
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>
boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Config>::recalculate(const set<varid_t>& markedKeys, const vector<varid_t>& newKeys, const GaussianFactorGraph* newFactors) {
template<class Conditional, class Values>
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 useMultiFrontal = true;
@ -522,7 +522,7 @@ boost::shared_ptr<set<varid_t> > ISAM2<Conditional, Config>::recalculate(const s
if(bayesNet->size() == 0)
assert(newlyCached.size() == 0);
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");
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>
//void ISAM2<Conditional, Config>::linear_update(const GaussianFactorGraph& newFactors) {
//template<class Conditional, class Values>
//void ISAM2<Conditional, Values>::linear_update(const GaussianFactorGraph& newFactors) {
// const list<varid_t> markedKeys = newFactors.keys();
// recalculate(markedKeys, &newFactors);
//}
/* ************************************************************************* */
// find all variables that are directly connected by a measurement to one of the marked variables
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::find_all(sharedClique clique, set<varid_t>& keys, const vector<bool>& markedMask) {
template<class Conditional, class Values>
void ISAM2<Conditional, Values>::find_all(sharedClique clique, set<varid_t>& keys, const vector<bool>& markedMask) {
// does the separator contain any of the variables?
bool found = false;
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 {
const Permuted<VectorConfig>& delta;
const Permuted<VectorValues>& delta;
const Ordering& ordering;
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) {}
template<typename I>
void operator()(I it_x) {
@ -604,10 +604,10 @@ struct _SelectiveExpmap {
};
#ifndef NDEBUG
struct _SelectiveExpmapAndClear {
Permuted<VectorConfig>& delta;
Permuted<VectorValues>& delta;
const Ordering& ordering;
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) {}
template<typename I>
void operator()(I it_x) {
@ -627,8 +627,8 @@ struct _SelectiveExpmapAndClear {
#endif
struct _VariableAdder {
Ordering& ordering;
Permuted<VectorConfig>& vconfig;
_VariableAdder(Ordering& _ordering, Permuted<VectorConfig>& _vconfig) : ordering(_ordering), vconfig(_vconfig) {}
Permuted<VectorValues>& vconfig;
_VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig) : ordering(_ordering), vconfig(_vconfig) {}
template<typename I>
void operator()(I xIt) {
static const bool debug = false;
@ -638,9 +638,9 @@ struct _VariableAdder {
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl;
}
};
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update(
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
template<class Conditional, class Values>
void ISAM2<Conditional, Values>::update(
const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta,
double wildfire_threshold, double relinearize_threshold, bool relinearize) {
static const bool debug = false;
@ -690,7 +690,7 @@ void ISAM2<Conditional, Config>::update(
// 3. Mark linear update
set<varid_t> markedKeys;
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()) {
markedKeys.insert(ordering_[key]);
newKeys.push_back(ordering_[key]);
@ -798,7 +798,7 @@ void ISAM2<Conditional, Config>::update(
tic("9 step9");
// 9. Solve
if (wildfire_threshold<=0.) {
VectorConfig newDelta(variableIndex_.dims());
VectorValues newDelta(variableIndex_.dims());
optimize2(this->root(), newDelta);
assert(newDelta.size() == delta_.size());
delta_.permutation() = Permutation::Identity(delta_.size());
@ -807,7 +807,7 @@ void ISAM2<Conditional, Config>::update(
// GaussianFactorGraph linearfull = *nonlinearFactors_.linearize(theta_, ordering_);
// GaussianBayesNet gbn = *Inference::Eliminate(linearfull);
// VectorConfig deltafull = optimize(gbn);
// VectorValues deltafull = optimize(gbn);
// assert(assert_equal(deltafull, newDelta, 1e-3));
} else {
@ -822,9 +822,9 @@ void ISAM2<Conditional, Config>::update(
}
/* ************************************************************************* */
template<class Conditional, class Config>
Config ISAM2<Conditional, Config>::calculateEstimate() const {
Config ret(theta_);
template<class Conditional, class Values>
Values ISAM2<Conditional, Values>::calculateEstimate() const {
Values ret(theta_);
vector<bool> mask(ordering_.nVars(), true);
_SelectiveExpmap selectiveExpmap(delta_, ordering_, mask);
ret.apply(selectiveExpmap);
@ -832,9 +832,9 @@ Config ISAM2<Conditional, Config>::calculateEstimate() const {
}
/* ************************************************************************* */
template<class Conditional, class Config>
Config ISAM2<Conditional, Config>::calculateBestEstimate() const {
VectorConfig delta(variableIndex_.dims());
template<class Conditional, class Values>
Values ISAM2<Conditional, Values>::calculateBestEstimate() const {
VectorValues delta(variableIndex_.dims());
optimize2(this->root(), delta);
return theta_.expmap(delta, ordering_);
}

28
inference/ISAM2.h
View File

@ -28,29 +28,29 @@ namespace gtsam {
//typedef std::vector<GaussianFactor::shared_ptr> CachedFactors;
template<class Conditional, class Config>
template<class Conditional, class Values>
class ISAM2: public BayesTree<Conditional> {
protected:
// current linearization point
Config theta_;
Values theta_;
// VariableIndex lets us look up factors by involved variable and keeps track of dimensions
typedef GaussianVariableIndex<VariableIndexStorage_deque> VariableIndexType;
VariableIndexType variableIndex_;
// the linear solution, an update to the estimate in theta
VectorConfig deltaUnpermuted_;
VectorValues deltaUnpermuted_;
// The residual permutation through which the deltaUnpermuted_ is
// referenced. Permuting the VectorConfig is slow, so for performance the
// permutation is applied at access time instead of to the VectorConfig
// referenced. Permuting the VectorValues is slow, so for performance the
// permutation is applied at access time instead of to the VectorValues
// itself.
Permuted<VectorConfig> delta_;
Permuted<VectorValues> delta_;
// for keeping all original nonlinear factors
NonlinearFactorGraph<Config> nonlinearFactors_;
NonlinearFactorGraph<Values> nonlinearFactors_;
// The "ordering" allows converting Symbols to varid_t (integer) keys. We
// keep it up to date as we add and reorder variables.
@ -69,7 +69,7 @@ public:
ISAM2();
// /** 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 */
virtual ~ISAM2() {}
@ -81,21 +81,21 @@ public:
/**
* 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);
// needed to create initial estimates
const Config& getLinearizationPoint() const {return theta_;}
const Values& getLinearizationPoint() const {return theta_;}
// 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)
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_; }

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 {
private:
boost::shared_ptr<Config> config_;
boost::shared_ptr<Values> config_;
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 {
typename Config::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_from = boost::get(boost::vertex_name, g, boost::source(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);
config_->insert(key_to, (*config_)[key_from].compose(relativePose));
}
@ -146,23 +146,23 @@ public:
};
/* ************************************************************************* */
template<class G, class Factor, class Pose, class Config>
boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree,
template<class G, class Factor, class Pose, class Values>
boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree,
const Pose& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list<
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;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g;
PoseVertex root;
map<typename Config::Key, PoseVertex> key2vertex;
map<typename Values::Key, PoseVertex> 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
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);
if (!factor) continue;
typename Config::Key key1 = factor->key1();
typename Config::Key key2 = factor->key2();
typename Values::Key key1 = factor->key1();
typename Values::Key key2 = factor->key2();
PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second;
@ -194,10 +194,10 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
}
// compose poses
boost::shared_ptr<Config> config(new Config);
typename Config::Key rootKey = boost::get(boost::vertex_name, g, root);
boost::shared_ptr<Values> config(new Values);
typename Values::Key rootKey = boost::get(boost::vertex_name, g, root);
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));
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
*/
template<class G, class Factor, class Pose, class Config>
boost::shared_ptr<Config>
composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree, const Pose& rootPose);
template<class G, class Factor, class Pose, class Values>
boost::shared_ptr<Values>
composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree, const Pose& rootPose);
} // 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)
// 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));
//
// 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
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));
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
VectorConfig BayesNetPreconditioner::backSubstitute(const VectorConfig& y) const {
VectorValues BayesNetPreconditioner::backSubstitute(const VectorValues& y) const {
return gtsam::backSubstitute(Rd_, y);
}
/* ************************************************************************* */
// gy=inv(L)*gx by solving L*gy=gx.
VectorConfig BayesNetPreconditioner::backSubstituteTranspose(
const VectorConfig& gx) const {
VectorValues BayesNetPreconditioner::backSubstituteTranspose(
const VectorValues& gx) const {
return gtsam::backSubstituteTranspose(Rd_, gx);
}
/* ************************************************************************* */
double BayesNetPreconditioner::error(const VectorConfig& y) const {
double BayesNetPreconditioner::error(const VectorValues& y) const {
return Ab_.error(x(y));
}
/* ************************************************************************* */
// gradient is inv(R')*A'*(A*inv(R)*y-b),
VectorConfig BayesNetPreconditioner::gradient(const VectorConfig& y) const {
VectorConfig gx = VectorConfig::zero(y);
VectorValues BayesNetPreconditioner::gradient(const VectorValues& y) const {
VectorValues gx = VectorValues::zero(y);
Errors e = Ab_.errors(x(y));
Ab_.transposeMultiplyAdd(1.0,e,gx);
return gtsam::backSubstituteTranspose(Rd_, gx);
@ -44,31 +44,31 @@ namespace gtsam {
/* ************************************************************************* */
// Apply operator *
Errors BayesNetPreconditioner::operator*(const VectorConfig& y) const {
Errors BayesNetPreconditioner::operator*(const VectorValues& y) const {
return Ab_ * x(y);
}
/* ************************************************************************* */
// In-place version that overwrites e
// TODO: version that takes scratch space for x
void BayesNetPreconditioner::multiplyInPlace(const VectorConfig& y, Errors& e) const {
VectorConfig x = y;
void BayesNetPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
VectorValues x = y;
backSubstituteInPlace(Rd_,x);
Ab_.multiplyInPlace(x,e);
}
/* ************************************************************************* */
// Apply operator inv(R')*A'*e
VectorConfig BayesNetPreconditioner::operator^(const Errors& e) const {
VectorConfig x = Ab_ ^ e; // x = A'*e2
VectorValues BayesNetPreconditioner::operator^(const Errors& e) const {
VectorValues x = Ab_ ^ e; // x = A'*e2
return gtsam::backSubstituteTranspose(Rd_, x);
}
/* ************************************************************************* */
// y += alpha*inv(R')*A'*e
void BayesNetPreconditioner::transposeMultiplyAdd(double alpha,
const Errors& e, VectorConfig& y) const {
VectorConfig x = VectorConfig::zero(y);
const Errors& e, VectorValues& y) const {
VectorValues x = VectorValues::zero(y);
Ab_.transposeMultiplyAdd(1.0,e,x); // x += A'*e
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);
// 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.
VectorConfig backSubstituteTranspose(const VectorConfig& gx) const;
VectorValues backSubstituteTranspose(const VectorValues& gx) const;
/* x = inv(R)*y */
inline VectorConfig x(const VectorConfig& y) const {
inline VectorValues x(const VectorValues& y) const {
return backSubstitute(y);
}
/* error, given y */
double error(const VectorConfig& y) const;
double error(const VectorValues& y) const;
/** gradient */
VectorConfig gradient(const VectorConfig& y) const;
VectorValues gradient(const VectorValues& y) const;
/** Apply operator A */
Errors operator*(const VectorConfig& y) const;
Errors operator*(const VectorValues& y) const;
/** 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' */
VectorConfig operator^(const Errors& e) const;
VectorValues operator^(const Errors& e) const;
/** 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

2
linear/Errors.cpp
View File

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

10
linear/Factorization.h
View File

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

30
linear/GaussianBayesNet.cpp
View File

@ -10,7 +10,7 @@
#include <gtsam/base/Matrix-inl.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
using namespace std;
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());
varid_t var = 0;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
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);
}
/* ************************************************************************* */
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)*/
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) {
VectorConfig x(y);
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y) {
VectorValues x(y);
backSubstituteInPlace(bn,x);
return x;
}
/* ************************************************************************* */
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y) {
VectorConfig& x = y;
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y) {
VectorValues& x = y;
/** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
// 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(R'*inv(Sigma))*gx
// gz'*R'=gx', gy = gz.*sigmas
VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
const VectorConfig& gx) {
VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
const VectorValues& gx) {
// Initialize gy from gx
// 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
// 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) {
boost::shared_ptr<VectorConfig> result(allocateVectorConfig(bn));
VectorValues rhs(const GaussianBayesNet& bn) {
boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn) {
varid_t key = cg->key();
// 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);
/**
* 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
*/
VectorConfig optimize(const GaussianBayesNet&);
VectorValues optimize(const GaussianBayesNet&);
/**
* shared pointer version
*/
boost::shared_ptr<VectorConfig> optimize_(const GaussianBayesNet& bn);
boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn);
/*
* Backsubstitute
* (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
*/
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y);
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y);
/*
* Transpose Backsubstitute
@ -74,7 +74,7 @@ namespace gtsam {
* gy=inv(R'*inv(Sigma))*gx
* 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
@ -82,9 +82,9 @@ namespace gtsam {
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)
*/
VectorConfig rhs(const GaussianBayesNet&);
VectorValues rhs(const GaussianBayesNet&);
} /// 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;
if(debug) print("Solving conditional ");
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());
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
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/inference/Conditional.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h>
@ -118,17 +118,17 @@ public:
/**
* 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 - ...)
*/
Vector solve(const VectorConfig& x) const;
Vector solve(const VectorValues& x) const;
/**
* 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 - ...)
*/
Vector solve(const Permuted<VectorConfig>& x) const;
Vector solve(const Permuted<VectorValues>& x) const;
protected:
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();
if (empty()) return e;
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());
return model_->whiten(unweighted_error(c));
}
/* ************************************************************************* */
double GaussianFactor::error(const VectorConfig& c) const {
double GaussianFactor::error(const VectorValues& c) const {
if (empty()) return 0;
Vector weighted = error_vector(c);
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());
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);
// VectorConfig x;
// // Just iterate over all A matrices and insert Ai^e into VectorConfig
// VectorValues x;
// // Just iterate over all A matrices and insert Ai^e into VectorValues
// for(size_t pos=0; pos<keys_.size(); ++pos)
// x.insert(keys_[pos], ARange(pos)^E);
//// 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,
VectorConfig& x) const {
VectorValues& x) const {
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)
gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]);
// 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/inference.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
@ -105,9 +105,9 @@ public:
void print(const std::string& s = "") const;
bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
Vector unweighted_error(const VectorConfig& c) const; /** (A*x-b) */
Vector error_vector(const VectorConfig& c) const; /** (A*x-b)/sigma */
double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
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
* not necessarily mean that the factor involves no variables (to check for
@ -170,13 +170,13 @@ public:
size_t numberOfRows() const { return Ab_.size1(); }
/** Return A*x */
Vector operator*(const VectorConfig& x) const;
Vector operator*(const VectorValues& x) const;
// /** Return A'*e */
// VectorConfig operator^(const Vector& e) const;
// VectorValues operator^(const Vector& e) const;
/** 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

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.;
BOOST_FOREACH(sharedFactor factor,factors_)
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);
}
/* ************************************************************************* */
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_FOREACH(const sharedFactor& factor,factors_)
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;
BOOST_FOREACH(const sharedFactor& Ai,factors_)
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());
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x,
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x,
const Errors::iterator& e) const {
Errors::iterator ei = e;
BOOST_FOREACH(const sharedFactor& Ai,factors_) {
@ -95,12 +95,12 @@ void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x,
}
///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::operator^(const Errors& e) const {
// VectorConfig x;
//VectorValues GaussianFactorGraph::operator^(const Errors& e) const {
// VectorValues x;
// // For each factor add the gradient contribution
// Errors::const_iterator it = e.begin();
// BOOST_FOREACH(const sharedFactor& Ai,factors_) {
// VectorConfig xi = (*Ai)^(*(it++));
// VectorValues xi = (*Ai)^(*(it++));
// x.insertAdd(xi);
// }
// return x;
@ -109,7 +109,7 @@ void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x,
/* ************************************************************************* */
// x += alpha*A'*e
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
VectorConfig& x) const {
VectorValues& x) const {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
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
// VectorConfig g = VectorConfig::zero(x);
// VectorValues g = VectorValues::zero(x);
// transposeMultiplyAdd(1.0, errors(x), 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
// GaussianBayesNet chordalBayesNet = eliminate(ordering, old);
//
// // calculate new configuration (using backsubstitution)
// VectorConfig delta = ::optimize(chordalBayesNet);
// // calculate new values structure (using backsubstitution)
// VectorValues delta = ::optimize(chordalBayesNet);
// return delta;
//}
//
///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
//VectorValues GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
//{
// GaussianJunctionTree junctionTree(*this, ordering);
//
// // calculate new configuration (using backsubstitution)
// VectorConfig delta = junctionTree.optimize();
// // calculate new values structure (using backsubstitution)
// VectorValues delta = junctionTree.optimize();
// 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) {
// 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();
// VectorConfig config;
// VectorValues config;
// Vector::const_iterator itSrc = vs.begin();
// Vector::iterator itDst;
// BOOST_FOREACH(varid_t key, ordering){
@ -415,7 +415,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
// config.insert(key, v);
// }
// 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;
//}
//
@ -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 {
// return gtsam::steepestDescent(*this, x0, verbose, epsilon, maxIterations);
//}
//
///* ************************************************************************* */
//boost::shared_ptr<VectorConfig> GaussianFactorGraph::steepestDescent_(
// const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return boost::shared_ptr<VectorConfig>(new VectorConfig(
//boost::shared_ptr<VectorValues> GaussianFactorGraph::steepestDescent_(
// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return boost::shared_ptr<VectorValues>(new VectorValues(
// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
// maxIterations)));
//}
//
///* ************************************************************************* */
//VectorConfig GaussianFactorGraph::conjugateGradientDescent(
// const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const {
//VectorValues GaussianFactorGraph::conjugateGradientDescent(
// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
// maxIterations);
//}
//
///* ************************************************************************* */
//boost::shared_ptr<VectorConfig> GaussianFactorGraph::conjugateGradientDescent_(
// const VectorConfig& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return boost::shared_ptr<VectorConfig>(new VectorConfig(
//boost::shared_ptr<VectorValues> GaussianFactorGraph::conjugateGradientDescent_(
// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
// return boost::shared_ptr<VectorValues>(new VectorValues(
// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
// maxIterations)));
//}

54
linear/GaussianFactorGraph.h
View File

@ -5,10 +5,6 @@
* @author Christian Potthast
* @author Alireza Fathi
*/
// $Id: GaussianFactorGraph.h,v 1.24 2009/08/14 20:48:51 acunning Exp $
// \callgraph
#pragma once
@ -24,7 +20,7 @@ namespace gtsam {
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* 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.
*/
class GaussianFactorGraph : public FactorGraph<GaussianFactor> {
@ -86,38 +82,38 @@ namespace gtsam {
void permuteWithInverse(const Permutation& inversePermutation);
/** return A*x-b */
Errors errors(const VectorConfig& x) const;
Errors errors(const VectorValues& x) const;
/** shared pointer version */
boost::shared_ptr<Errors> errors_(const VectorConfig& x) const;
boost::shared_ptr<Errors> errors_(const VectorValues& x) const;
/** unnormalized error */
double error(const VectorConfig& x) const;
double error(const VectorValues& x) const;
/** return A*x */
Errors operator*(const VectorConfig& x) const;
Errors operator*(const VectorValues& x) const;
/* 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. */
void multiplyInPlace(const VectorConfig& x, const Errors::iterator& e) const;
void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
// /** return A^e */
// VectorConfig operator^(const Errors& e) const;
// VectorValues operator^(const Errors& e) const;
/** 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
// * @param x: VectorConfig specifying where to calculate gradient
// * @return gradient, as a VectorConfig as well
// * @param x: VectorValues specifying where to calculate gradient
// * @return gradient, as a VectorValues as well
// */
// VectorConfig gradient(const VectorConfig& x) const;
// VectorValues gradient(const VectorValues& x) const;
/** Unnormalized probability. O(n) */
double probPrime(const VectorConfig& c) const {
double probPrime(const VectorValues& c) const {
return exp(-0.5 * error(c));
}
@ -145,19 +141,19 @@ namespace gtsam {
// * @param enableJoinFactor uses the older joint factor combine process when true,
// * 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
// * @param ordering fg in order
// */
// VectorConfig optimizeMultiFrontals(const Ordering& ordering);
// VectorValues optimizeMultiFrontals(const Ordering& ordering);
// /**
// * shared pointer versions for MATLAB
// */
// 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
@ -207,7 +203,7 @@ namespace gtsam {
// * @param v: the source 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
@ -237,32 +233,32 @@ namespace gtsam {
// /**
// * Find solution using gradient descent
// * @param x0: VectorConfig specifying initial estimate
// * @param x0: VectorValues specifying initial estimate
// * @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;
//
// /**
// * shared pointer versions for MATLAB
// */
// boost::shared_ptr<VectorConfig>
// steepestDescent_(const VectorConfig& x0, bool verbose = false,
// boost::shared_ptr<VectorValues>
// steepestDescent_(const VectorValues& x0, bool verbose = false,
// double epsilon = 1e-3, size_t maxIterations = 0) const;
//
// /**
// * Find solution using conjugate gradient descent
// * @param x0: VectorConfig specifying initial estimate
// * @param x0: VectorValues specifying initial estimate
// * @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;
//
// /**
// * shared pointer versions for MATLAB
// */
// boost::shared_ptr<VectorConfig> conjugateGradientDescent_(
// const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3,
// boost::shared_ptr<VectorValues> conjugateGradientDescent_(
// const VectorValues& x0, bool verbose = false, double epsilon = 1e-3,
// size_t maxIterations = 0) const;
};

6
linear/GaussianISAM.cpp
View File

@ -16,7 +16,7 @@ template class ISAM<GaussianConditional>;
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
GaussianISAM::Clique::const_reverse_iterator 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) {
VectorConfig result(bayesTree.dims_);
VectorValues optimize(const GaussianISAM& bayesTree) {
VectorValues result(bayesTree.dims_);
// starting from the root, call optimize on each conditional
optimize(bayesTree.root(), result);
return result;

6
linear/GaussianISAM.h
View File

@ -56,14 +56,14 @@ public:
dims_.clear();
}
friend VectorConfig optimize(const GaussianISAM&);
friend VectorValues optimize(const GaussianISAM&);
};
// 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
VectorConfig optimize(const GaussianISAM& bayesTree);
VectorValues optimize(const GaussianISAM& bayesTree);
}/// namespace gtsam

10
linear/GaussianJunctionTree.cpp
View File

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

4
linear/GaussianJunctionTree.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
protected:
// 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 :
@ -35,7 +35,7 @@ namespace gtsam {
GaussianJunctionTree(const GaussianFactorGraph& fg) : Base(fg) {}
// optimize the linear graph
VectorConfig optimize() const;
VectorValues optimize() const;
}; // GaussianJunctionTree
} // namespace gtsam

4
linear/Makefile.am
View File

@ -16,7 +16,7 @@ sources += NoiseModel.cpp Errors.cpp
check_PROGRAMS += tests/testNoiseModel tests/testErrors
# Vector Configurations
headers += VectorConfig.h
headers += VectorValues.h
#sources += VectorMap.cpp VectorBTree.cpp
#check_PROGRAMS += tests/testVectorMap tests/testVectorBTree
@ -36,7 +36,7 @@ check_PROGRAMS += tests/testGaussianJunctionTree
# Timing tests
noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike
#noinst_PROGRAMS += tests/timeVectorConfig
#noinst_PROGRAMS += tests/timeVectorValues
#----------------------------------------------------------------------------------------------------
# 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,
sharedBayesNet& Rc1, sharedConfig& xbar) :
sharedBayesNet& Rc1, sharedValues& xbar) :
Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) {
}
/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorConfig SubgraphPreconditioner::x(const VectorConfig& y) const {
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
#ifdef VECTORBTREE
if (!y.cloned(*xbar_)) throw
invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar");
#endif
VectorConfig x = y;
VectorValues x = y;
backSubstituteInPlace(*Rc1_,x);
x += *xbar_;
return x;
}
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorConfig& y) const {
double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e;
@ -42,7 +42,7 @@ namespace gtsam {
}
// Add A2 contribution
VectorConfig x = this->x(y);
VectorValues x = this->x(y);
Errors e2 = Ab2_->errors(x);
e.splice(e.end(), e2);
@ -51,18 +51,18 @@ namespace gtsam {
/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorConfig SubgraphPreconditioner::gradient(const VectorConfig& y) const {
VectorConfig x = this->x(y); // x = inv(R1)*y
VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
VectorValues x = this->x(y); // x = inv(R1)*y
Errors e2 = Ab2_->errors(x);
VectorConfig gx2 = VectorConfig::zero(y);
VectorValues gx2 = VectorValues::zero(y);
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;
}
/* ************************************************************************* */
// 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;
@ -73,7 +73,7 @@ namespace gtsam {
}
// Add A2 contribution
VectorConfig x = y; // TODO avoid ?
VectorValues x = y; // TODO avoid ?
gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y
Errors e2 = *Ab2_ * x; // A2*x
e.splice(e.end(), e2);
@ -83,7 +83,7 @@ namespace gtsam {
/* ************************************************************************* */
// 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();
@ -95,16 +95,16 @@ namespace gtsam {
}
// Add A2 contribution
VectorConfig x = y; // TODO avoid ?
VectorValues x = y; // TODO avoid ?
gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y
Ab2_->multiplyInPlace(x,ei); // use iterator version
}
/* ************************************************************************* */
// 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
Errors::const_iterator it = e.begin();
@ -123,7 +123,7 @@ namespace gtsam {
/* ************************************************************************* */
// y += alpha*A'*e
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
Errors::const_iterator it = e.begin();
@ -140,7 +140,7 @@ namespace gtsam {
/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
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
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
@ -149,10 +149,10 @@ namespace gtsam {
e2.push_back(*(it++));
// 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;
// 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
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:
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
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;
private:
sharedFG Ab1_, Ab2_;
sharedBayesNet Rc1_;
sharedConfig xbar_;
sharedValues xbar_;
sharedErrors b2bar_; /** b2 - A2*xbar */
public:
@ -42,7 +42,7 @@ namespace gtsam {
* @param Rc1: the Bayes Net 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> 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)); }
Vector b1() const { return Ab1_->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 */
VectorConfig x(const VectorConfig& y) const;
VectorValues x(const VectorValues& y) const;
/* A zero VectorConfig with the structure of xbar */
VectorConfig zero() const { return VectorConfig::zero(*xbar_);}
/* A zero VectorValues with the structure of xbar */
VectorValues zero() const { return VectorValues::zero(*xbar_);}
/* 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) */
VectorConfig gradient(const VectorConfig& y) const;
VectorValues gradient(const VectorValues& y) const;
/** Apply operator A */
Errors operator*(const VectorConfig& y) const;
Errors operator*(const VectorValues& y) const;
/** 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' */
VectorConfig operator^(const Errors& e) const;
VectorValues operator^(const Errors& e) const;
/**
* Add A'*e to y
* 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
@ -85,7 +85,7 @@ namespace gtsam {
* Takes a range indicating e2 !!!!
*/
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 */
void print(const std::string& s = "SubgraphPreconditioner") const;

28
linear/SubgraphSolver-inl.h
View File

@ -20,14 +20,14 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class Graph, class Config>
SubgraphSolver<Graph, Config>::SubgraphSolver(const Graph& G, const Config& theta0) {
template<class Graph, class Values>
SubgraphSolver<Graph, Values>::SubgraphSolver(const Graph& G, const Values& theta0) {
initialize(G,theta0);
}
/* ************************************************************************* */
template<class Graph, class Config>
void SubgraphSolver<Graph, Config>::initialize(const Graph& G, const Config& theta0) {
template<class Graph, class Values>
void SubgraphSolver<Graph, Values>::initialize(const Graph& G, const Values& theta0) {
// generate spanning tree
PredecessorMap<Key> tree = G.template findMinimumSpanningTree<Key, Constraint>();
@ -46,21 +46,21 @@ namespace gtsam {
// compose the approximate solution
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>
boost::shared_ptr<SubgraphPreconditioner> SubgraphSolver<Graph, Config>::linearize(const Graph& G, const Config& theta_bar) const {
template<class Graph, class Values>
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 Ab2 = C_.linearize(theta_bar);
#ifdef TIMING
SubgraphPreconditioner::sharedBayesNet Rc1;
SubgraphPreconditioner::sharedConfig xbar;
SubgraphPreconditioner::sharedValues xbar;
#else
GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_);
SubgraphPreconditioner::sharedConfig xbar = gtsam::optimize_(*Rc1);
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
#endif
// 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
@ -68,14 +68,14 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class Graph, class Config>
VectorConfig SubgraphSolver<Graph, Config>::optimize(SubgraphPreconditioner& system) const {
VectorConfig zeros = system.zero();
template<class Graph, class Values>
VectorValues SubgraphSolver<Graph, Values>::optimize(SubgraphPreconditioner& system) const {
VectorValues zeros = system.zero();
// Solve the subgraph PCG
VectorConfig ybar = conjugateGradients<SubgraphPreconditioner, VectorConfig,
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
Errors> (system, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_);
VectorConfig xbar = system.x(ybar);
VectorValues xbar = system.x(ybar);
return xbar;
}

26
linear/SubgraphSolver.h
View File

@ -17,13 +17,13 @@ namespace gtsam {
* A nonlinear system solver using subgraph preconditioning conjugate gradient
* Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L
* solve : L -> VectorConfig
* solve : L -> VectorValues
*/
template<class Graph, class Config>
template<class Graph, class Values>
class SubgraphSolver {
private:
typedef typename Config::Key Key;
typedef typename Values::Key Key;
typedef typename Graph::Constraint Constraint;
typedef typename Graph::Pose Pose;
@ -36,40 +36,40 @@ namespace gtsam {
boost::shared_ptr<Ordering> ordering_;
/* the solution computed from the first subgraph */
boost::shared_ptr<Config> theta_bar_;
boost::shared_ptr<Values> theta_bar_;
Graph T_, C_;
public:
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<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
*/
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
* the resulted linear system
*/
VectorConfig optimize(SubgraphPreconditioner& system) const;
VectorValues optimize(SubgraphPreconditioner& system) const;
boost::shared_ptr<SubgraphSolver> prepareLinear(const SubgraphPreconditioner& fg) const {
return boost::shared_ptr<SubgraphSolver>(new SubgraphSolver(*this));
}
};
template<class Graph, class Config> const size_t SubgraphSolver<Graph,Config>::maxIterations_;
template<class Graph, class Config> const bool SubgraphSolver<Graph,Config>::verbose_;
template<class Graph, class Config> const double SubgraphSolver<Graph,Config>::epsilon_;
template<class Graph, class Config> const double SubgraphSolver<Graph,Config>::epsilon_abs_;
template<class Graph, class Values> const size_t SubgraphSolver<Graph,Values>::maxIterations_;
template<class Graph, class Values> const bool SubgraphSolver<Graph,Values>::verbose_;
template<class Graph, class Values> const double SubgraphSolver<Graph,Values>::epsilon_;
template<class Graph, class Values> const double SubgraphSolver<Graph,Values>::epsilon_abs_;
} // nsamespace gtsam

8
linear/VectorBTree.cpp
View File

@ -1,7 +1,7 @@
/**
* @file VectorConfig.cpp
* @brief Factor Graph Configuration
* @brief VectorConfig
* @file VectorValues.cpp
* @brief Factor Graph Values
* @brief VectorValues
* @author Frank Dellaert
*/
@ -14,7 +14,7 @@ using namespace std;
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,
const VectorBTree& b) {
if (!a.compatible(b))

6
linear/VectorBTree.h
View File

@ -1,6 +1,6 @@
/**
* @file VectorBTree.h
* @brief Factor Graph Configuration
* @brief Factor Graph Valuesuration
* @author Frank Dellaert
*/
@ -19,7 +19,7 @@
namespace gtsam {
/** Factor Graph Configuration */
/** Factor Graph Valuesuration */
class VectorBTree: public Testable<VectorBTree> {
private:
@ -74,7 +74,7 @@ public:
/** equals, for unit testing */
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);
/** 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
* @brief Factor Graph Configuration
* @brief Factor Graph Values
* @brief VectorMap
* @author Carlos Nieto
* @author Christian Potthast
@ -148,33 +148,33 @@ Vector VectorMap::vector() const {
/* ************************************************************************* */
VectorMap expmap(const VectorMap& original, const VectorMap& delta)
{
VectorMap newConfig;
VectorMap newValues;
varid_t j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) {
if (delta.contains(j)) {
const Vector& dj = delta[j];
check_size(j,vj,dj);
newConfig.insert(j, vj + dj);
newValues.insert(j, vj + dj);
} else {
newConfig.insert(j, vj);
newValues.insert(j, vj);
}
}
return newConfig;
return newValues;
}
/* ************************************************************************* */
VectorMap expmap(const VectorMap& original, const Vector& delta)
{
VectorMap newConfig;
VectorMap newValues;
size_t i = 0;
varid_t j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) {
size_t mj = vj.size();
Vector dj = sub(delta, i, i+mj);
newConfig.insert(j, vj + dj);
newValues.insert(j, vj + dj);
i += mj;
}
return newConfig;
return newValues;
}
/* ************************************************************************* */

6
linear/VectorMap.h
View File

@ -1,6 +1,6 @@
/**
* @file VectorMap.h
* @brief Factor Graph Configuration
* @brief Factor Graph Valuesuration
* @author Carlos Nieto
* @author Christian Potthast
*/
@ -18,7 +18,7 @@
namespace gtsam {
/** Factor Graph Configuration */
/** Factor Graph Valuesuration */
class VectorMap : public Testable<VectorMap> {
protected:
@ -41,7 +41,7 @@ namespace gtsam {
/** convert into a single large vector */
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);
/** Insert or add a value with given index */

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

@ -1,6 +1,6 @@
/**
* @file VectorConfig.h
* @brief Factor Graph Configuration
* @file VectorValues.h
* @brief Factor Graph Valuesuration
* @author Richard Roberts
*/
@ -19,16 +19,16 @@
namespace gtsam {
class VectorConfig : public Testable<VectorConfig> {
class VectorValues : public Testable<VectorValues> {
protected:
Vector values_;
std::vector<size_t> varStarts_;
public:
template<class C> class _impl_iterator; // Forward declaration of iterator implementation
typedef boost::shared_ptr<VectorConfig> shared_ptr;
typedef _impl_iterator<VectorConfig> iterator;
typedef _impl_iterator<const VectorConfig> const_iterator;
typedef boost::shared_ptr<VectorValues> shared_ptr;
typedef _impl_iterator<VectorValues> iterator;
typedef _impl_iterator<const VectorValues> const_iterator;
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<Vector> mapped_type;
@ -38,23 +38,23 @@ public:
// * Constructor requires an existing GaussianVariableIndex to get variable
// * 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
* slow reallocation of space at runtime.
*/
VectorConfig() : varStarts_(1,0) {}
VectorValues() : varStarts_(1,0) {}
/** Construct from a container of variable dimensions (in variable order). */
template<class Container>
VectorConfig(const Container& dimensions);
VectorValues(const Container& dimensions);
/** Construct from a container of variable dimensions in variable order and
* 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 */
mapped_type operator[](varid_t variable);
@ -72,10 +72,10 @@ public:
size_t dimCapacity() const { return values_.size(); }
/** Iterator access */
iterator begin() { return _impl_iterator<VectorConfig>(*this, 0); }
const_iterator begin() const { return _impl_iterator<const VectorConfig>(*this, 0); }
iterator end() { return _impl_iterator<VectorConfig>(*this, varStarts_.size()-1); }
const_iterator end() const { return _impl_iterator<const VectorConfig>(*this, varStarts_.size()-1); }
iterator begin() { return _impl_iterator<VectorValues>(*this, 0); }
const_iterator begin() const { return _impl_iterator<const VectorValues>(*this, 0); }
iterator end() { return _impl_iterator<VectorValues>(*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 */
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()); }
/** 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";
for(varid_t var=0; var<size(); ++var) {
std::cout << " " << var << " " << operator[](var) << "\n";
@ -104,7 +104,7 @@ public:
}
/** 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;
// iterate over all elements
for(varid_t var=0; var<size(); ++var)
@ -116,9 +116,9 @@ public:
/** + operator simply adds Vectors. This checks for structural equivalence
* when NDEBUG is not defined.
*/
VectorConfig operator+(const VectorConfig& c) const {
VectorValues operator+(const VectorValues& c) const {
assert(varStarts_ == c.varStarts_);
VectorConfig result;
VectorValues result;
result.varStarts_ = varStarts_;
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()));
@ -137,10 +137,10 @@ public:
_impl_iterator(C& config, varid_t curVariable) : config_(config), curVariable_(curVariable) {}
void checkCompat(const _impl_iterator<C>& r) { assert(&config_ == &r.config_); }
friend class VectorConfig;
friend class VectorValues;
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++(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;
// varStarts_[0] = 0;
// for(varid_t var=0; var<variableIndex.size(); ++var) {
@ -169,7 +169,7 @@ protected:
//}
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;
size_t varStart = 0;
varid_t var = 0;
@ -179,7 +179,7 @@ inline VectorConfig::VectorConfig(const Container& dimensions) : varStarts_(dime
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) {
varStarts_[0] = 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());
}
inline VectorConfig::mapped_type VectorConfig::operator[](varid_t variable) {
inline VectorValues::mapped_type VectorValues::operator[](varid_t variable) {
checkVariable(variable);
return boost::numeric::ublas::project(values_,
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);
return boost::numeric::ublas::project(values_,
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,
const VectorConfig& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg,
VectorValues steepestDescent(const GaussianFactorGraph& fg,
const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
x, verbose, epsilon, epsilon_abs, maxIterations, true);
}
VectorConfig conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorConfig& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorConfig, Errors> (fg,
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
x, verbose, epsilon, epsilon_abs, maxIterations);
}

10
linear/iterative.h
View File

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

12
linear/tests/testBayesNetPreconditioner.cpp
View File

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

2
linear/tests/testGaussianConditional.cpp
View File

@ -145,7 +145,7 @@ TEST( GaussianConditional, solve )
Vector sl1(2);
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[_l1_] = sl1;

10
linear/tests/testGaussianFactor.cpp
View File

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

4
linear/tests/testGaussianJunctionTree.cpp
View File

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

4
linear/tests/testVectorBTree.cpp
View File

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

26
linear/tests/testVectorConfig.cpp
View File

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

4
linear/tests/testVectorMap.cpp
View File

@ -1,6 +1,6 @@
/**
* @file testVectorMap.cpp
* @brief Unit tests for Factor Graph Configuration
* @brief Unit tests for Factor Graph Values
* @author Carlos Nieto
**/
@ -221,7 +221,7 @@ TEST( VectorMap, serialize)
cout << "VectorMap: Running Serialization Test" << endl;
//create an VectorMap
VectorMap fg = createConfig();
VectorMap fg = createValues();
//serialize the config
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) {
// cout << "Trial " << trial << endl;
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial]));
VectorConfig soln(optimize(*gbn));
VectorValues soln(optimize(*gbn));
}
blocksolve = timer.elapsed();
cout << blocksolve << " s" << endl;
@ -114,7 +114,7 @@ int main(int argc, char *argv[]) {
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(combGfgs[trial]));
VectorConfig soln(optimize(*gbn));
VectorValues soln(optimize(*gbn));
}
combsolve = timer.elapsed();
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) {
// cout << "Trial " << trial << endl;
GaussianBayesNet::shared_ptr gbn(Inference::Eliminate(blockGfgs[trial]));
VectorConfig soln(optimize(*gbn));
VectorValues soln(optimize(*gbn));
}
blocksolve = timer.elapsed();
cout << blocksolve << " s" << endl;

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

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

2
nonlinear/Key.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
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
* keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes.

34
nonlinear/LieValues-inl.h
View File

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

30
nonlinear/LieValues.h
View File

@ -5,8 +5,8 @@
* @brief A templated config for Lie-group elements
*
* Detailed story:
* A configuration 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
* 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 values structure which can hold variables that
* 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.
*/
@ -23,14 +23,14 @@
#include <gtsam/nonlinear/Ordering.h>
namespace boost { template<class T> class optional; }
namespace gtsam { class VectorConfig; class Ordering; }
namespace gtsam { class VectorValues; class Ordering; }
namespace gtsam {
/**
* Lie type configuration
* Lie type values structure
* 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:
* The key will be assumed to be sortable, and must have a
@ -95,8 +95,8 @@ namespace gtsam {
/** The dimensionality of the tangent space */
size_t dim() const;
/** Get a zero VectorConfig of the correct structure */
VectorConfig zero(const Ordering& ordering) const;
/** Get a zero VectorValues of the correct structure */
VectorValues zero(const Ordering& ordering) const;
const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); }
@ -106,16 +106,16 @@ namespace gtsam {
// Lie operations
/** 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 */
// LieValues expmap(const Vector& delta) const;
/** 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) */
void logmap(const LieValues& cp, const Ordering& ordering, VectorConfig& delta) const;
void logmap(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const;
// imperative methods:
@ -159,7 +159,7 @@ namespace gtsam {
/**
* 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
* 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>
void apply(A& operation) {
@ -193,10 +193,10 @@ namespace gtsam {
};
struct _ConfigDimensionCollector {
struct _ValuesDimensionCollector {
const Ordering& ordering;
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) {
varid_t var = ordering[key_value->first];
assert(var < dimensions.size());
@ -205,10 +205,10 @@ namespace gtsam {
};
/* ************************************************************************* */
struct _ConfigKeyOrderer {
struct _ValuesKeyOrderer {
varid_t var;
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) {
ordering->insert(key_value->first, 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
* significant
*/
template <class Config>
class NonlinearConstraint : public NonlinearFactor<Config> {
template <class Values>
class NonlinearConstraint : public NonlinearFactor<Values> {
protected:
typedef NonlinearConstraint<Config> This;
typedef NonlinearFactor<Config> Base;
typedef NonlinearConstraint<Values> This;
typedef NonlinearFactor<Values> Base;
double mu_; /// gain for quadratic merit function
size_t dim_; /// dimension of the constraint
@ -57,14 +57,14 @@ public:
size_t dim() const { return dim_; }
/** 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);
if (p == NULL) return false;
return Base::equals(*p, tol) && (mu_ == p->mu_);
}
/** 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);
if (active(c))
return mu_ * inner_prod(error_vector, error_vector);
@ -72,7 +72,7 @@ public:
}
/** 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
@ -81,29 +81,29 @@ public:
* when the constraint is *NOT* fulfilled.
* @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
* @param config is the configuration
* @param config is the values structure
* @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
*/
template <class Config, class Key>
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
template <class Values, class Key>
class NonlinearConstraint1 : public NonlinearConstraint<Values> {
public:
typedef typename Key::Value_t X;
protected:
typedef NonlinearConstraint1<Config,Key> This;
typedef NonlinearConstraint<Config> Base;
typedef NonlinearConstraint1<Values,Key> This;
typedef NonlinearConstraint<Values> Base;
/** key for the constrained variable */
Key key_;
@ -130,14 +130,14 @@ public:
}
/** 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);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key_ == p->key_);
}
/** 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)) {
return zero(this->dim());
}
@ -147,7 +147,7 @@ public:
}
/** 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)) {
boost::shared_ptr<GaussianFactor> factor;
return factor;
@ -168,15 +168,15 @@ public:
/**
* Unary Equality constraint - simply forces the value of active() to true
*/
template <class Config, class Key>
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key> {
template <class Values, class Key>
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Values, Key> {
public:
typedef typename Key::Value_t X;
protected:
typedef NonlinearEqualityConstraint1<Config,Key> This;
typedef NonlinearConstraint1<Config,Key> Base;
typedef NonlinearEqualityConstraint1<Values,Key> This;
typedef NonlinearConstraint1<Values,Key> Base;
public:
NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0)
@ -184,22 +184,22 @@ public:
virtual ~NonlinearEqualityConstraint1() {}
/** 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
*/
template <class Config, class Key1, class Key2>
class NonlinearConstraint2 : public NonlinearConstraint<Config> {
template <class Values, class Key1, class Key2>
class NonlinearConstraint2 : public NonlinearConstraint<Values> {
public:
typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2;
protected:
typedef NonlinearConstraint2<Config,Key1,Key2> This;
typedef NonlinearConstraint<Config> Base;
typedef NonlinearConstraint2<Values,Key1,Key2> This;
typedef NonlinearConstraint<Values> Base;
/** keys for the constrained variables */
Key1 key1_;
@ -230,14 +230,14 @@ public:
}
/** 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);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_);
}
/** 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)) {
return zero(this->dim());
}
@ -249,7 +249,7 @@ public:
}
/** 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)) {
boost::shared_ptr<GaussianFactor> factor;
return factor;
@ -271,16 +271,16 @@ public:
/**
* Binary Equality constraint - simply forces the value of active() to true
*/
template <class Config, class Key1, class Key2>
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, Key2> {
template <class Values, class Key1, class Key2>
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Values, Key1, Key2> {
public:
typedef typename Key1::Value_t X1;
typedef typename Key2::Value_t X2;
protected:
typedef NonlinearEqualityConstraint2<Config,Key1,Key2> This;
typedef NonlinearConstraint2<Config,Key1,Key2> Base;
typedef NonlinearEqualityConstraint2<Values,Key1,Key2> This;
typedef NonlinearConstraint2<Values,Key1,Key2> Base;
public:
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() */
virtual bool active(const Config& c) const { return true; }
virtual bool active(const Values& c) const { return true; }
};
/**
* A ternary constraint
*/
template <class Config, class Key1, class Key2, class Key3>
class NonlinearConstraint3 : public NonlinearConstraint<Config> {
template <class Values, class Key1, class Key2, class Key3>
class NonlinearConstraint3 : public NonlinearConstraint<Values> {
public:
typedef typename Key1::Value_t X1;
@ -304,8 +304,8 @@ public:
typedef typename Key3::Value_t X3;
protected:
typedef NonlinearConstraint3<Config,Key1,Key2,Key3> This;
typedef NonlinearConstraint<Config> Base;
typedef NonlinearConstraint3<Values,Key1,Key2,Key3> This;
typedef NonlinearConstraint<Values> Base;
/** keys for the constrained variables */
Key1 key1_;
@ -341,14 +341,14 @@ public:
}
/** 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);
if (p == NULL) return false;
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 */
inline Vector unwhitenedError(const Config& x) const {
inline Vector unwhitenedError(const Values& x) const {
if (!active(x)) {
return zero(this->dim());
}
@ -362,7 +362,7 @@ public:
}
/** Linearize from config */
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
if (!active(c)) {
boost::shared_ptr<GaussianFactor> factor;
return factor;
@ -385,8 +385,8 @@ public:
/**
* Ternary Equality constraint - simply forces the value of active() to true
*/
template <class Config, class Key1, class Key2, class Key3>
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Config, Key1, Key2, Key3> {
template <class Values, class Key1, class Key2, class Key3>
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<Values, Key1, Key2, Key3> {
public:
typedef typename Key1::Value_t X1;
@ -394,8 +394,8 @@ public:
typedef typename Key3::Value_t X3;
protected:
typedef NonlinearEqualityConstraint3<Config,Key1,Key2,Key3> This;
typedef NonlinearConstraint3<Config,Key1,Key2,Key3> Base;
typedef NonlinearEqualityConstraint3<Values,Key1,Key2,Key3> This;
typedef NonlinearConstraint3<Values,Key1,Key2,Key3> Base;
public:
NonlinearEqualityConstraint3(const Key1& key1, const Key2& key2, const Key3& key3,
@ -404,27 +404,27 @@ public:
virtual ~NonlinearEqualityConstraint3() {}
/** 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
*/
template<class Config, class Key>
class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key> {
template<class Values, class Key>
class NonlinearEquality1 : public NonlinearEqualityConstraint1<Values, Key> {
public:
typedef typename Key::Value_t X;
protected:
typedef NonlinearEqualityConstraint1<Config, Key> Base;
typedef NonlinearEqualityConstraint1<Values, Key> Base;
X value_; /// fixed value for variable
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)
: Base(key1, X::Dim(), mu), value_(value) {}
@ -443,17 +443,17 @@ public:
* Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type
*/
template<class Config, class Key>
class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, Key> {
template<class Values, class Key>
class NonlinearEquality2 : public NonlinearEqualityConstraint2<Values, Key, Key> {
public:
typedef typename Key::Value_t X;
protected:
typedef NonlinearEqualityConstraint2<Config, Key, Key> Base;
typedef NonlinearEqualityConstraint2<Values, Key, Key> Base;
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)
: 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
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
*/
template<class Config, class Key>
class NonlinearEquality: public NonlinearFactor1<Config, Key> {
template<class Values, class Key>
class NonlinearEquality: public NonlinearFactor1<Values, Key> {
public:
typedef typename Key::Value_t T;
@ -54,7 +54,7 @@ namespace gtsam {
*/
bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<Config, Key> Base;
typedef NonlinearFactor1<Values, Key> Base;
/**
* Constructor - forces exact evaluation
@ -81,13 +81,13 @@ namespace gtsam {
}
/** 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;
return compare_(feasible_, f.feasible_);
}
/** actual error function calculation */
virtual double error(const Config& c) const {
virtual double error(const Values& c) const {
const T& xj = c[this->key_];
Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) {
@ -114,7 +114,7 @@ namespace gtsam {
}
// 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_];
Matrix 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
* 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,
* which are objects in non-linear manifolds (Lie groups).
*/
template<class Config>
class NonlinearFactor: public Testable<NonlinearFactor<Config> > {
template<class Values>
class NonlinearFactor: public Testable<NonlinearFactor<Values> > {
protected:
typedef NonlinearFactor<Config> This;
typedef NonlinearFactor<Values> This;
SharedGaussian noiseModel_; /** Noise model */
std::list<Symbol> keys_; /** cached keys */
public:
typedef boost::shared_ptr<NonlinearFactor<Config> > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor<Values> > shared_ptr;
/** Default constructor for I/O only */
NonlinearFactor() {
@ -70,7 +70,7 @@ namespace gtsam {
}
/** 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);
}
@ -78,7 +78,7 @@ namespace gtsam {
* calculate the error of the factor
* 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));
}
@ -109,16 +109,16 @@ namespace gtsam {
}
/** 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 whitenedError(const Config& c) const {
Vector whitenedError(const Values& c) const {
return noiseModel_->whiten(unwhitenedError(c));
}
/** linearize to a 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
@ -141,13 +141,13 @@ namespace gtsam {
/**
* A Gaussian nonlinear factor that takes 1 parameter
* 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
* 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.
*/
template<class Config, class Key>
class NonlinearFactor1: public NonlinearFactor<Config> {
template<class Values, class Key>
class NonlinearFactor1: public NonlinearFactor<Values> {
public:
@ -159,8 +159,8 @@ namespace gtsam {
// The value of the key. Not const to allow serialization
Key key_;
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor1<Config, Key> This;
typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor1<Values, Key> This;
public:
@ -175,7 +175,7 @@ namespace gtsam {
/**
* Constructor
* @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,
const Key& key1) :
@ -191,12 +191,12 @@ namespace gtsam {
}
/** 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_);
}
/** 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 X& xj = x[j];
return evaluateError(xj);
@ -207,7 +207,7 @@ namespace gtsam {
* Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
* 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_];
Matrix A;
Vector b = - evaluateError(xj, A);
@ -256,8 +256,8 @@ namespace gtsam {
/**
* A Gaussian nonlinear factor that takes 2 parameters
*/
template<class Config, class Key1, class Key2>
class NonlinearFactor2: public NonlinearFactor<Config> {
template<class Values, class Key1, class Key2>
class NonlinearFactor2: public NonlinearFactor<Values> {
public:
@ -271,8 +271,8 @@ namespace gtsam {
Key1 key1_;
Key2 key2_;
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor2<Config, Key1, Key2> This;
typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor2<Values, Key1, Key2> This;
public:
@ -303,13 +303,13 @@ namespace gtsam {
}
/** 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_)
&& (key2_ == f.key2_);
}
/** 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 X2& x2 = x[key2_];
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
* 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 X2& x2 = c[key2_];
Matrix A1, A2;
@ -392,8 +392,8 @@ namespace gtsam {
/**
* A Gaussian nonlinear factor that takes 3 parameters
*/
template<class Config, class Key1, class Key2, class Key3>
class NonlinearFactor3: public NonlinearFactor<Config> {
template<class Values, class Key1, class Key2, class Key3>
class NonlinearFactor3: public NonlinearFactor<Values> {
public:
@ -409,8 +409,8 @@ namespace gtsam {
Key2 key2_;
Key3 key3_;
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor3<Config, Key1, Key2, Key3> This;
typedef NonlinearFactor<Values> Base;
typedef NonlinearFactor3<Values, Key1, Key2, Key3> This;
public:
@ -443,13 +443,13 @@ namespace gtsam {
}
/** 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_)
&& (key2_ == f.key2_) && (key3_ == f.key3_);
}
/** 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 X2& x2 = x[key2_];
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
* 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 X2& x2 = c[key2_];
const X3& x3 = c[key3_];

32
nonlinear/NonlinearFactorGraph-inl.h
View File

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

28
nonlinear/NonlinearFactorGraph.h
View File

@ -20,32 +20,32 @@
namespace gtsam {
/**
* A non-linear factor graph is templated on a configuration, but the factor type
* is fixed as a NonlinearFactor. The configurations are typically (in SAM) more general
* A non-linear factor graph is templated on a values structure, but the factor type
* 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.
* 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
* 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>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Config> > {
template<class Values>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Values> > {
public:
typedef FactorGraph<NonlinearFactor<Config> > Base;
typedef typename boost::shared_ptr<NonlinearFactor<Config> > sharedFactor;
typedef FactorGraph<NonlinearFactor<Values> > Base;
typedef typename boost::shared_ptr<NonlinearFactor<Values> > sharedFactor;
/** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const;
/** unnormalized error */
double error(const Config& c) const;
double error(const Values& c) const;
/** all individual errors */
Vector unwhitenedError(const Config& c) const;
Vector unwhitenedError(const Values& c) const;
/** Unnormalized probability. O(n) */
double probPrime(const Config& c) const {
double probPrime(const Values& c) const {
return exp(-0.5 * error(c));
}
@ -57,7 +57,7 @@ namespace gtsam {
/**
* 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
@ -66,7 +66,7 @@ namespace gtsam {
* ordering is found.
*/
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
@ -74,13 +74,13 @@ namespace gtsam {
* computation.
*/
std::pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr>
orderingCOLAMD(const Config& config) const;
orderingCOLAMD(const Values& config) const;
/**
* linearize a nonlinear factor graph
*/
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>
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) {
if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL");
@ -78,7 +78,7 @@ namespace gtsam {
// linearize and optimize
/* ************************************************************************* */
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_);
NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linearized), error_, lambda_);
return prepared.solver_->optimize(*linearized);
@ -91,20 +91,20 @@ namespace gtsam {
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate(
verbosityLevel verbosity) const {
// linearize and optimize
VectorConfig delta = linearizeAndOptimizeForDelta();
VectorValues delta = linearizeAndOptimizeForDelta();
// maybe show output
if (verbosity >= DELTA)
delta.print("delta");
// 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
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)
cout << "error: " << newOptimizer.error_ << endl;
@ -166,17 +166,17 @@ namespace gtsam {
damped.print("damped");
// solve
VectorConfig delta = solver_->optimize(damped);
VectorValues delta = solver_->optimize(damped);
if (verbosity >= TRYDELTA)
delta.print("delta");
// 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)
// newConfig->print("config");
// newValues->print("config");
// 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(lambdaMode >= CAUTIOUS) {
@ -188,7 +188,7 @@ namespace gtsam {
// If we're cautious, see if the current lambda is better
// todo: include stopping criterion here?
if(lambdaMode == CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newConfig, solver_, lambda_);
NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_);
if(sameLambda.error_ <= next.error_)
return sameLambda;
}
@ -201,7 +201,7 @@ namespace gtsam {
// A more adventerous lambda was worse. If we're cautious, try the same lambda.
if(lambdaMode == CAUTIOUS) {
NonlinearOptimizer sameLambda(graph_, newConfig, solver_, lambda_);
NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_);
if(sameLambda.error_ <= error_)
return sameLambda;
}
@ -212,7 +212,7 @@ namespace gtsam {
// TODO: can we avoid copying the config ?
if(lambdaMode >= BOUNDED && lambda_ >= 1.0e5) {
return NonlinearOptimizer(graph_, newConfig, solver_, lambda_);;
return NonlinearOptimizer(graph_, newValues, solver_, lambda_);;
} else {
NonlinearOptimizer cautious(graph_, config_, solver_, lambda_ * factor);
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/make_shared.hpp>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/Factorization.h>
@ -29,13 +29,13 @@ namespace gtsam {
* and then one of the optimization routines is called. These recursively iterate
* 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$.
* 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
*
* For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Config,
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, 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, Pose2Values>.
* 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
* one optimizes the linearized system using various methods.
@ -45,7 +45,7 @@ namespace gtsam {
public:
// 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 S> shared_solver;
typedef const S solver;
@ -99,9 +99,9 @@ namespace gtsam {
// These normally do not change
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)
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 :-(
// keep current lambda for use within LM only
@ -120,13 +120,13 @@ namespace gtsam {
/**
* 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);
/**
* 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),
error_(error), lambda_(lambda), solver_(solver) {}
@ -154,15 +154,15 @@ namespace gtsam {
/**
* Return the config
*/
shared_config config() const{
shared_values config() const{
return config_;
}
/**
* 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
@ -213,9 +213,9 @@ namespace gtsam {
* @param graph Nonlinear factor graph to optimize
* @param config Initial config
* @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) {
// Use a variable ordering from COLAMD
@ -238,7 +238,7 @@ namespace gtsam {
/**
* 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) {
return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(config), verbosity);
@ -249,9 +249,9 @@ namespace gtsam {
* @param graph Nonlinear factor graph to optimize
* @param config Initial config
* @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) {
Ordering::shared_ptr ordering;
GaussianVariableIndex<>::shared_ptr variableIndex;
@ -271,7 +271,7 @@ namespace gtsam {
/**
* 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) {
return optimizeGN(boost::make_shared<const G>(graph),
boost::make_shared<const T>(config), verbosity);

240
nonlinear/TupleValues-inl.h
View File

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

192
nonlinear/TupleValues.h
View File

@ -6,7 +6,7 @@
*/
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
#pragma once
@ -27,7 +27,7 @@ namespace gtsam {
* For an easy interface, there are TupleValuesN classes, which wrap
* 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
* to the previously used PairConfig.
* to the previously used PairValues.
*
* Design and extension note:
* 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
* operates on the "first" config. TupleValuesEnd contains only the specialized version.
*/
template<class Config1, class Config2>
class TupleValues : public Testable<TupleValues<Config1, Config2> > {
template<class Values1, class Values2>
class TupleValues : public Testable<TupleValues<Values1, Values2> > {
protected:
// Data for internal configs
Config1 first_; /// Arbitrary config
Config2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary config
Values1 first_; /// Arbitrary config
Values2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary config
public:
// typedefs for config subtypes
typedef class Config1::Key Key1;
typedef class Config1::Value Value1;
typedef class Values1::Key Key1;
typedef class Values1::Value Value1;
/** default constructor */
TupleValues() {}
/** Copy constructor */
TupleValues(const TupleValues<Config1, Config2>& config) :
TupleValues(const TupleValues<Values1, Values2>& config) :
first_(config.first_), second_(config.second_) {}
/** Construct from configs */
TupleValues(const Config1& cfg1, const Config2& cfg2) :
TupleValues(const Values1& cfg1, const Values2& cfg2) :
first_(cfg1), second_(cfg2) {}
/** Print */
@ -66,7 +66,7 @@ namespace gtsam {
}
/** 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);
}
@ -90,7 +90,7 @@ namespace gtsam {
*/
template<class Cfg1, class Cfg2>
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_);
second_.insert(config.second_);
}
@ -101,7 +101,7 @@ namespace gtsam {
*/
template<class Cfg1, class Cfg2>
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_);
second_.update(config.second_);
}
@ -121,7 +121,7 @@ namespace gtsam {
*/
template<class Cfg>
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 */
template<class Key>
@ -152,12 +152,12 @@ namespace gtsam {
const Value1& at(const Key1& j) const { return first_.at(j); }
/** direct config access */
const Config1& config() const { return first_; }
const Config2& rest() const { return second_; }
const Values1& config() const { return first_; }
const Values2& rest() const { return second_; }
/** zero: create VectorConfig of appropriate structure */
VectorConfig zero(const Ordering& ordering) const {
VectorConfig z(this->dims(ordering));
/** zero: create VectorValues of appropriate structure */
VectorValues zero(const Ordering& ordering) const {
VectorValues z(this->dims(ordering));
z.makeZero();
return z;
}
@ -173,7 +173,7 @@ namespace gtsam {
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const {
_ConfigDimensionCollector dimCollector(ordering);
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
}
@ -186,25 +186,25 @@ namespace gtsam {
*/
Ordering::shared_ptr orderingArbitrary(varid_t firstVar = 0) const {
// Generate an initial key ordering in iterator order
_ConfigKeyOrderer keyOrderer(firstVar);
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);
return keyOrderer.ordering;
}
/** 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));
}
/** logmap each element */
VectorConfig logmap(const TupleValues<Config1, Config2>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering));
VectorValues logmap(const TupleValues<Values1, Values2>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
logmap(cp, ordering, delta);
return delta;
}
/** 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);
second_.logmap(cp.second_, ordering, delta);
}
@ -212,7 +212,7 @@ namespace gtsam {
/**
* 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
* 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>
void apply(A& operation) {
@ -242,48 +242,48 @@ namespace gtsam {
* Do not use this class directly - it should only be used as a part
* of a recursive structure
*/
template<class Config>
class TupleValuesEnd : public Testable<TupleValuesEnd<Config> > {
template<class Values>
class TupleValuesEnd : public Testable<TupleValuesEnd<Values> > {
protected:
// Data for internal configs
Config first_;
Values first_;
public:
// typedefs
typedef class Config::Key Key1;
typedef class Config::Value Value1;
typedef class Values::Key Key1;
typedef class Values::Value Value1;
TupleValuesEnd() {}
TupleValuesEnd(const TupleValuesEnd<Config>& config) :
TupleValuesEnd(const TupleValuesEnd<Values>& config) :
first_(config.first_) {}
TupleValuesEnd(const Config& cfg) :
TupleValuesEnd(const Values& cfg) :
first_(cfg) {}
void print(const std::string& s = "") const {
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);
}
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(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 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 Config& config() const { return first_; }
const Values& config() const { return first_; }
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); }
VectorConfig zero(const Ordering& ordering) const {
VectorConfig z(this->dims(ordering));
VectorValues zero(const Ordering& ordering) const {
VectorValues z(this->dims(ordering));
z.makeZero();
return z;
}
@ -307,24 +307,24 @@ namespace gtsam {
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));
}
VectorConfig logmap(const TupleValuesEnd<Config>& cp, const Ordering& ordering) const {
VectorConfig delta(this->dims(ordering));
VectorValues logmap(const TupleValuesEnd<Values>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
logmap(cp, ordering, 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);
}
/**
* 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
* 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>
void apply(A& operation) {
@ -348,14 +348,14 @@ namespace gtsam {
* recursively, as they are TupleValuess, and are primarily a short form of the config
* 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>
class TupleValues1 : public TupleValuesEnd<C1> {
public:
// typedefs
typedef C1 Config1;
typedef C1 Values1;
typedef TupleValuesEnd<C1> Base;
typedef TupleValues1<C1> This;
@ -363,18 +363,18 @@ namespace gtsam {
TupleValues1() {}
TupleValues1(const This& config);
TupleValues1(const Base& config);
TupleValues1(const Config1& cfg1);
TupleValues1(const Values1& cfg1);
// access functions
inline const Config1& first() const { return this->config(); }
inline const Values1& first() const { return this->config(); }
};
template<class C1, class C2>
class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
public:
// typedefs
typedef C1 Config1;
typedef C2 Config2;
typedef C1 Values1;
typedef C2 Values2;
typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
typedef TupleValues2<C1, C2> This;
@ -382,40 +382,40 @@ namespace gtsam {
TupleValues2() {}
TupleValues2(const This& config);
TupleValues2(const Base& config);
TupleValues2(const Config1& cfg1, const Config2& cfg2);
TupleValues2(const Values1& cfg1, const Values2& cfg2);
// access functions
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Values1& first() const { return this->config(); }
inline const Values2& second() const { return this->rest().config(); }
};
template<class C1, class C2, class C3>
class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > {
public:
// typedefs
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
TupleValues3() {}
TupleValues3(const TupleValues<C1, TupleValues<C2, TupleValuesEnd<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
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
inline const Values1& first() const { return this->config(); }
inline const Values2& second() const { return this->rest().config(); }
inline const Values3& third() const { return this->rest().rest().config(); }
};
template<class C1, class C2, class C3, class C4>
class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > {
public:
// typedefs
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
typedef C4 Config4;
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
typedef TupleValues4<C1, C2, C3, C4> This;
@ -423,62 +423,62 @@ namespace gtsam {
TupleValues4() {}
TupleValues4(const This& 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
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
inline const Values1& first() const { return this->config(); }
inline const Values2& second() const { return this->rest().config(); }
inline const Values3& third() const { return this->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>
class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
public:
// typedefs
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
typedef C4 Config4;
typedef C5 Config5;
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef C5 Values5;
TupleValues5() {}
TupleValues5(const TupleValues5<C1, C2, C3, C4, 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,
const Config4& cfg4, const Config5& cfg5);
TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Values4& cfg4, const Values5& cfg5);
// access functions
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); }
inline const Values1& first() const { return this->config(); }
inline const Values2& second() const { return this->rest().config(); }
inline const Values3& third() const { return this->rest().rest().config(); }
inline const Values4& fourth() const { return this->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>
class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
public:
// typedefs
typedef C1 Config1;
typedef C2 Config2;
typedef C3 Config3;
typedef C4 Config4;
typedef C5 Config5;
typedef C6 Config6;
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef C5 Values5;
typedef C6 Values6;
TupleValues6() {}
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 Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
const Config4& cfg4, const Config5& cfg5, const Config6& cfg6);
TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Values4& cfg4, const Values5& cfg5, const Values6& cfg6);
// access functions
inline const Config1& first() const { return this->config(); }
inline const Config2& second() const { return this->rest().config(); }
inline const Config3& third() const { return this->rest().rest().config(); }
inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); }
inline const Config6& sixth() const { return this->rest().rest().rest().rest().rest().config(); }
inline const Values1& first() const { return this->config(); }
inline const Values2& second() const { return this->rest().config(); }
inline const Values3& third() const { return this->rest().rest().config(); }
inline const Values4& fourth() const { return this->rest().rest().rest().config(); }
inline const Values5& fifth() const { return this->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");
// state structure: [x y \lambda]
VectorConfig init, state;
VectorValues init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
@ -106,18 +106,18 @@ TEST (NonlinearConstraint, problem1_cholesky ) {
// solve
Ordering ord;
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");
// update initial estimate
VectorConfig newState = expmap(state, delta);
VectorValues newState = expmap(state, delta);
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
VectorValues expected;
expected.insert(L1, Vector_(1, -1.0));
expected.insert(x1, Vector_(1, 2.12));
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();
typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef LieValues<VecKey> Config;
typedef LieValues<VecKey> Values;
VecKey key1(1), key2(2), key3(3), key4(4);
/* ************************************************************************* */
TEST( LieValues, equals1 )
{
Config expected;
Values expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert(key1,v);
Config actual;
Values actual;
actual.insert(key1,v);
CHECK(assert_equal(expected,actual));
}
@ -35,7 +35,7 @@ TEST( LieValues, equals1 )
/* ************************************************************************* */
TEST( LieValues, equals2 )
{
Config cfg1, cfg2;
Values cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
cfg1.insert(key1, v1);
@ -47,7 +47,7 @@ TEST( LieValues, equals2 )
/* ************************************************************************* */
TEST( LieValues, equals_nan )
{
Config cfg1, cfg2;
Values cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf);
cfg1.insert(key1, v1);
@ -59,7 +59,7 @@ TEST( LieValues, equals_nan )
/* ************************************************************************* */
TEST( LieValues, insert_config )
{
Config cfg1, cfg2, expected;
Values cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
@ -82,7 +82,7 @@ TEST( LieValues, insert_config )
/* ************************************************************************* */
TEST( LieValues, update_element )
{
Config cfg;
Values cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
@ -98,12 +98,12 @@ TEST( LieValues, update_element )
///* ************************************************************************* */
//TEST(LieValues, dim_zero)
//{
// Config config0;
// Values config0;
// config0.insert(key1, Vector_(2, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5,config0.dim());
//
// VectorConfig expected;
// VectorValues expected;
// expected.insert(key1, zero(2));
// expected.insert(key2, zero(3));
// CHECK(assert_equal(expected, config0.zero()));
@ -112,16 +112,16 @@ TEST( LieValues, update_element )
/* ************************************************************************* */
TEST(LieValues, expmap_a)
{
Config config0;
Values config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
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[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(key2, Vector_(3, 6.3, 7.4, 8.5));
@ -131,15 +131,15 @@ TEST(LieValues, expmap_a)
///* ************************************************************************* */
//TEST(LieValues, expmap_b)
//{
// Config config0;
// Values config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
//
// 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);
//
// Config expected;
// Values expected;
// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
//
@ -149,7 +149,7 @@ TEST(LieValues, expmap_a)
///* ************************************************************************* */
//TEST(LieValues, expmap_c)
//{
// Config config0;
// Values config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.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.3, 1.4, 1.5);
//
// Config expected;
// Values expected;
// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
//
@ -167,7 +167,7 @@ TEST(LieValues, expmap_a)
/* ************************************************************************* */
/*TEST(LieValues, expmap_d)
{
Config config0;
Values config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
//config0.print("config0");
@ -207,7 +207,7 @@ TEST(LieValues, expmap_a)
/* ************************************************************************* */
TEST(LieValues, exists_)
{
Config config0;
Values config0;
config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.));
@ -218,17 +218,17 @@ TEST(LieValues, exists_)
/* ************************************************************************* */
TEST(LieValues, update)
{
Config config0;
Values config0;
config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.));
Config superset;
Values superset;
superset.insert(key1, Vector_(1, -1.));
superset.insert(key2, Vector_(1, -2.));
superset.insert(key3, Vector_(1, -3.));
config0.update(superset);
Config expected;
Values expected;
expected.insert(key1, Vector_(1, -1.));
expected.insert(key2, Vector_(1, -2.));
CHECK(assert_equal(expected,config0));
@ -238,18 +238,18 @@ TEST(LieValues, update)
TEST(LieValues, dummy_initialization)
{
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(2), Vector_(2, 4.0, 3.0));
Config init2;
Values init2;
init2.insert(key1, Vector_(2, 1.0, 2.0));
init2.insert(key2, Vector_(2, 4.0, 3.0));
Config1 actual1(init2);
Config actual2(init1);
Values1 actual1(init2);
Values actual2(init1);
EXPECT(actual1.empty());
EXPECT(actual2.empty());

6
slam/BearingFactor.h
View File

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

6
slam/BearingRangeFactor.h
View File

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

8
slam/BetweenConstraint.h
View File

@ -14,19 +14,19 @@ namespace gtsam {
* Binary between constraint - forces between to a given value
* This constraint requires the underlying type to a Lie type
*/
template<class Config, class Key>
class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, Key> {
template<class Values, class Key>
class BetweenConstraint : public NonlinearEqualityConstraint2<Values, Key, Key> {
public:
typedef typename Key::Value_t X;
protected:
typedef NonlinearEqualityConstraint2<Config, Key, Key> Base;
typedef NonlinearEqualityConstraint2<Values, Key, Key> Base;
X measured_; /// fixed between value
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)
: 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])"
* 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
*/
template<class Config, class Key1, class Key2 = Key1>
class BetweenFactor: public NonlinearFactor2<Config, Key1, Key2> {
template<class Values, class Key1, class Key2 = Key1>
class BetweenFactor: public NonlinearFactor2<Values, Key1, Key2> {
public:
typedef typename Key1::Value_t T;
private:
typedef NonlinearFactor2<Config, Key1, Key2> Base;
typedef NonlinearFactor2<Values, Key1, Key2> Base;
T measured_; /** The measurement */
@ -51,9 +51,9 @@ namespace gtsam {
}
/** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
const BetweenFactor<Config, Key1, Key2> *e =
dynamic_cast<const BetweenFactor<Config, Key1, Key2>*> (&expected);
bool equals(const NonlinearFactor<Values>& expected, double tol) const {
const BetweenFactor<Values, Key1, Key2> *e =
dynamic_cast<const BetweenFactor<Values, Key1, Key2>*> (&expected);
return e != NULL && Base::equals(expected, tol) && this->measured_.equals(
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
#include <gtsam/inference/ISAM2-inl.h>
template class ISAM2<GaussianConditional, simulated2D::Config>;
template class ISAM2<GaussianConditional, planarSLAM::Config>;
template class ISAM2<GaussianConditional, simulated2D::Values>;
template class ISAM2<GaussianConditional, planarSLAM::Values>;
namespace gtsam {
/* ************************************************************************* */
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
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
@ -60,7 +60,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
if (!redo) {
// change is measured against the previous delta!
// 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());
for(size_t i=0; i<d_old.size(); ++i) {
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
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
GaussianISAM2::Clique::const_reverse_iterator 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<VectorConfig> delta(new VectorConfig());
//boost::shared_ptr<VectorValues> optimize2(const GaussianISAM2::sharedClique& root) {
// boost::shared_ptr<VectorValues> delta(new VectorValues());
// set<Symbol> changed;
// // starting from the root, call optimize on each conditional
// 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);
int count = 0;
// starting from the root, call optimize on each conditional

8
slam/GaussianISAM2.h
View File

@ -16,11 +16,11 @@
namespace gtsam {
typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2;
typedef ISAM2<GaussianConditional, planarSLAM::Config> GaussianISAM2_P;
typedef ISAM2<GaussianConditional, simulated2D::Values> GaussianISAM2;
typedef ISAM2<GaussianConditional, planarSLAM::Values> GaussianISAM2_P;
// 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
// 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.
// returns the number of variables that were solved for
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)
int calculate_nnz(const GaussianISAM2::sharedClique& clique);

20
slam/LinearApproxFactor-inl.h
View File

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

20
slam/LinearApproxFactor.h
View File

@ -8,7 +8,7 @@
#include <vector>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
@ -17,18 +17,18 @@ namespace gtsam {
* A dummy factor that takes a linearized factor and inserts it into
* a nonlinear graph. This version uses exactly one type of variable.
*/
template <class Config, class Key>
class LinearApproxFactor : public NonlinearFactor<Config> {
template <class Values, class Key>
class LinearApproxFactor : public NonlinearFactor<Values> {
public:
/** type for the variable */
typedef typename Key::Value_t X;
/** base type */
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor<Values> Base;
/** 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 */
typedef std::vector<Key> KeyVector;
@ -38,7 +38,7 @@ protected:
GaussianFactor::shared_ptr lin_factor_;
/** linearization points for error calculation */
Config lin_points_;
Values lin_points_;
/** keep keys for the factor */
KeyVector nonlinearKeys_;
@ -46,18 +46,18 @@ protected:
/**
* 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) {}
public:
/** 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() {}
/** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const Config& c) const;
virtual Vector unwhitenedError(const Values& c) const;
/**
* linearize to a GaussianFactor
@ -65,7 +65,7 @@ public:
* NOTE: copies to avoid actual factor getting destroyed
* 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 */
KeyVector nonlinearKeys() const { return nonlinearKeys_; }

10
slam/Makefile.am
View File

@ -11,14 +11,14 @@ sources =
check_PROGRAMS =
# simulated2D example
headers += Simulated2DConfig.h
headers += Simulated2DValues.h
headers += Simulated2DPosePrior.h Simulated2DPointPrior.h
headers += Simulated2DOdometry.h Simulated2DMeasurement.h
sources += simulated2D.cpp smallExample.cpp
check_PROGRAMS += tests/testSimulated2D
# simulated2DOriented example
headers += Simulated2DOrientedConfig.h
headers += Simulated2DOrientedValues.h
headers += Simulated2DOrientedPosePrior.h
headers += Simulated2DOrientedOdometry.h
sources += simulated2DOriented.cpp
@ -40,7 +40,7 @@ headers += LinearApproxFactor.h LinearApproxFactor-inl.h
# 2D Pose SLAM
sources += pose2SLAM.cpp dataset.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
headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h
@ -49,11 +49,11 @@ check_PROGRAMS += tests/testPlanarSLAM
# 3D Pose constraints
sources += pose3SLAM.cpp
check_PROGRAMS += tests/testPose3Factor tests/testPose3Config tests/testPose3SLAM
check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLAM
# Visual SLAM
sources += visualSLAM.cpp
check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMConfig
check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues
# GaussianISAM2 is fairly SLAM-specific
sources += GaussianISAM2.cpp

4
slam/Pose2SLAMOptimizer.cpp
View File

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

8
slam/Pose2SLAMOptimizer.h
View File

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

16
slam/PriorFactor.h
View File

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

6
slam/RangeFactor.h
View File

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

2
slam/Simulated2DMeasurement.h
View File

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

2
slam/Simulated2DOdometry.h
View File

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

2
slam/Simulated2DOrientedOdometry.h
View File

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

4
slam/Simulated2DOrientedPosePrior.h
View File

@ -8,12 +8,12 @@
#pragma once
#include <gtsam/slam/simulated2DOriented.h>
#include <gtsam/slam/Simulated2DOrientedConfig.h>
#include <gtsam/slam/Simulated2DOrientedValues.h>
namespace gtsam {
/** 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
* Author: Frank Dellaert
@ -11,12 +11,12 @@
namespace gtsam {
class Simulated2DOrientedConfig: public simulated2DOriented::Config {
class Simulated2DOrientedValues: public simulated2DOriented::Values {
public:
typedef boost::shared_ptr<Point2> sharedPoint;
typedef boost::shared_ptr<Pose2> sharedPose;
Simulated2DOrientedConfig() {
Simulated2DOrientedValues() {
}
void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) {

4
slam/Simulated2DPointPrior.h
View File

@ -8,12 +8,12 @@
#pragma once
#include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/Simulated2DConfig.h>
#include <gtsam/slam/Simulated2DValues.h>
namespace gtsam {
/** 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
#include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/Simulated2DConfig.h>
#include <gtsam/slam/Simulated2DValues.h>
namespace gtsam {
/** 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
* Author: Frank Dellaert
@ -11,11 +11,11 @@
namespace gtsam {
class Simulated2DConfig: public simulated2D::Config {
class Simulated2DValues: public simulated2D::Values {
public:
typedef boost::shared_ptr<Point2> sharedPoint;
Simulated2DConfig() {
Simulated2DValues() {
}
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {

2
slam/Simulated3D.cpp
View File

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

16
slam/Simulated3D.h
View File

@ -10,7 +10,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/TupleValues.h>
@ -23,9 +23,9 @@ namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<PoseKey> PoseConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<PoseConfig, PointConfig> Config;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
/**
* Prior on a single pose
@ -46,13 +46,13 @@ namespace simulated3D {
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
struct PointPrior3D: public NonlinearFactor1<Config, PoseKey> {
struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
Point3 z_;
PointPrior3D(const Point3& z,
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 =
@ -61,14 +61,14 @@ namespace simulated3D {
}
};
struct Simulated3DMeasurement: public NonlinearFactor2<Config,
struct Simulated3DMeasurement: public NonlinearFactor2<Values,
PoseKey, PointKey> {
Point3 z_;
Simulated3DMeasurement(const Point3& z,
const SharedGaussian& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<Config, PoseKey, PointKey> (
NonlinearFactor2<Values, PoseKey, PointKey> (
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
* Templated on:
* Config : The overall config
* Values : The overall config
* PKey : Key of landmark being constrained
* Point : Type of landmark
* TKey : Key of transform used
@ -25,15 +25,15 @@ namespace gtsam {
* This base class should be specialized to implement the cost function for
* specific classes of landmarks
*/
template<class Config, class PKey, class TKey>
class TransformConstraint : public NonlinearEqualityConstraint3<Config, PKey, TKey, PKey> {
template<class Values, class PKey, class TKey>
class TransformConstraint : public NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> {
public:
typedef typename PKey::Value_t Point;
typedef typename TKey::Value_t Transform;
typedef NonlinearEqualityConstraint3<Config, PKey, TKey, PKey> Base;
typedef TransformConstraint<Config, PKey, TKey> This;
typedef NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> Base;
typedef TransformConstraint<Values, PKey, TKey> This;
/**
* 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
typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
typedef boost::shared_ptr<Pose2Config> sharedPose2Config;
typedef boost::shared_ptr<Pose2Values> sharedPose2Values;
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,
int maxID, bool addNoise, bool 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) {
cout << "Will try to read " << filename << endl;
ifstream is(filename.c_str());
@ -83,7 +83,7 @@ pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
exit(-1);
}
sharedPose2Config poses(new Pose2Config);
sharedPose2Values poses(new Pose2Values);
sharedPose2Graph graph(new Pose2Graph);
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) {
typedef Pose2Config::Key Key;
typedef Pose2Values::Key Key;
fstream stream(filename.c_str(), fstream::out);
// save poses
Pose2Config::Key key;
Pose2Values::Key key;
Pose2 pose;
BOOST_FOREACH(boost::tie(key, pose), config)
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
Matrix R = model->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_);
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 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,
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,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
int maxID = 0, bool addNoise=false, bool smart=true);
/** 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);
/**

10
slam/planarSLAM.cpp
View File

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

26
slam/planarSLAM.h
View File

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

8
slam/pose2SLAM.cpp
View File

@ -14,14 +14,14 @@ namespace gtsam {
using namespace pose2SLAM;
INSTANTIATE_LIE_CONFIG(Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace pose2SLAM {
/* ************************************************************************* */
Config circle(size_t n, double R) {
Config x;
Values circle(size_t n, double R) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)
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
namespace pose2SLAM {
// Keys and Config
// Keys and Values
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)
@ -32,16 +32,16 @@ namespace gtsam {
* @param c character to use for keys
* @return circle of n 2D poses
*/
Config circle(size_t n, double R);
Values circle(size_t n, double R);
// Factors
typedef PriorFactor<Config, Key> Prior;
typedef BetweenFactor<Config, Key> Constraint;
typedef NonlinearEquality<Config, Key> HardConstraint;
typedef PriorFactor<Values, Key> Prior;
typedef BetweenFactor<Values, Key> Constraint;
typedef NonlinearEquality<Values, Key> HardConstraint;
// Graph
struct Graph: public NonlinearFactorGraph<Config> {
typedef BetweenFactor<Config, Key> Constraint;
struct Graph: public NonlinearFactorGraph<Values> {
typedef BetweenFactor<Values, Key> Constraint;
typedef Pose2 Pose;
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);
@ -49,14 +49,14 @@ namespace gtsam {
};
// Optimizer
typedef NonlinearOptimizer<Graph, Config> Optimizer;
typedef NonlinearOptimizer<Graph, Values> Optimizer;
} // pose2SLAM
/**
* Backwards compatibility
*/
typedef pose2SLAM::Config Pose2Config;
typedef pose2SLAM::Values Pose2Values;
typedef pose2SLAM::Prior Pose2Prior;
typedef pose2SLAM::Constraint Pose2Factor;
typedef pose2SLAM::Graph Pose2Graph;

8
slam/pose3SLAM.cpp
View File

@ -14,14 +14,14 @@ namespace gtsam {
using namespace pose3SLAM;
INSTANTIATE_LIE_CONFIG(Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace pose3SLAM {
/* ************************************************************************* */
Config circle(size_t n, double radius) {
Config x;
Values circle(size_t n, double radius) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
// We use aerospace/navlab convention, X forward, Y right, Z down
// 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
namespace pose3SLAM {
// Keys and Config
// Keys and Values
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)
@ -31,15 +31,15 @@ namespace gtsam {
* @param c character to use for keys
* @return circle of n 3D poses
*/
Config circle(size_t n, double R);
Values circle(size_t n, double R);
// Factors
typedef PriorFactor<Config, Key> Prior;
typedef BetweenFactor<Config, Key> Constraint;
typedef NonlinearEquality<Config, Key> HardConstraint;
typedef PriorFactor<Values, Key> Prior;
typedef BetweenFactor<Values, Key> Constraint;
typedef NonlinearEquality<Values, Key> HardConstraint;
// Graph
struct Graph: public NonlinearFactorGraph<Config> {
struct Graph: public NonlinearFactorGraph<Values> {
void addPrior(const Key& i, const Pose3& p,
const SharedGaussian& model);
void addConstraint(const Key& i, const Key& j, const Pose3& z,
@ -48,14 +48,14 @@ namespace gtsam {
};
// Optimizer
typedef NonlinearOptimizer<Graph, Config> Optimizer;
typedef NonlinearOptimizer<Graph, Values> Optimizer;
} // pose3SLAM
/**
* Backwards compatibility
*/
typedef pose3SLAM::Config Pose3Config;
typedef pose3SLAM::Values Pose3Values;
typedef pose3SLAM::Prior Pose3Prior;
typedef pose3SLAM::Constraint Pose3Factor;
typedef pose3SLAM::Graph Pose3Graph;

2
slam/saveGraph.cpp
View File

@ -20,7 +20,7 @@ namespace gtsam {
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;
Point2 pt;

4
slam/saveGraph.h
View File

@ -17,9 +17,9 @@
namespace gtsam {
class Point2;
typedef LieValues<Symbol, Point2> SymbolicConfig;
typedef LieValues<Symbol, Point2> SymbolicValues;
// 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

6
slam/simulated2D.cpp
View File

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

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