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

Removed old Index typedef and IndexFormatter

release/4.3a0
Richard Roberts 2013-11-08 16:35:28 +00:00
parent a95ade1f88
commit ec78d54f37
62 changed files with 240 additions and 258 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
examples/Data/dubrovnik-3-7-pre-rewritten.txt
View File

@ -1,80 +1,80 @@
3 7 19
0 0 -385.989990234375 387.1199951171875
1 0 -38.439998626708984375 492.1199951171875
2 0 -667.91998291015625 123.1100006103515625
0 1 383.8800048828125 -15.299989700317382812
1 1 559.75 -106.15000152587890625
1 0 -38.439998626708984 492.1199951171875
2 0 -667.91998291015625 123.11000061035156
0 1 383.8800048828125 -15.299989700317383
1 1 559.75 -106.15000152587891
0 2 591.54998779296875 136.44000244140625
1 2 863.8599853515625 -23.469970703125
2 2 494.720001220703125 112.51999664306640625
2 2 494.72000122070312 112.51999664306641
0 3 592.5 125.75
1 3 861.08001708984375 -35.219970703125
2 3 498.540008544921875 101.55999755859375
0 4 348.720001220703125 558.3800048828125
1 4 776.030029296875 483.529998779296875
2 4 7.7800288200378417969 326.350006103515625
2 3 498.54000854492187 101.55999755859375
0 4 348.72000122070312 558.3800048828125
1 4 776.030029296875 483.52999877929687
2 4 7.7800288200378418 326.35000610351562
0 5 14.010009765625 96.420013427734375
1 5 207.1300048828125 118.3600006103515625
0 6 202.7599945068359375 340.989990234375
1 5 207.1300048828125 118.36000061035156
0 6 202.75999450683594 340.989990234375
1 6 543.18011474609375 294.80999755859375
2 6 -58.419979095458984375 110.8300018310546875
2 6 -58.419979095458984 110.83000183105469
0.29656188120312942935
-0.035318354384285870207
0.31252101755032046793
0.47230274932665988752
-0.3572340863744113415
-2.0517704282499575896
0.29656188120312943
-0.03531835438428587
0.31252101755032047
0.47230274932665989
-0.35723408637441134
-2.0517704282499576
1430.031982421875
-7.5572756941255647689e-08
3.2377570134516087119e-14
-7.5572756941255648e-008
3.2377570134516087e-014
0.28532097381985194184
-0.27699838370789808817
0.048601169984112867206
-1.2598695987143850861
-0.049063798188844320869
-1.9586867140445654023
0.28532097290364833
-0.27699838355720618
0.048601170006498565
-1.2598695987678452
-0.04906379852479037
-1.9586867140054638
1432.137451171875
-7.3171918302250560373e-08
3.1759419042137054801e-14
-7.317191830225056e-008
3.1759419042137055e-014
0.057491325683772541433
0.34853090049579965592
0.47985129303736057116
8.1963904289063389541
6.5146840788718787252
-3.8392804395897406344
0.057491325683772541
0.34853090049579966
0.47985129303736057
8.196390428906339
6.5146840788718787
-3.8392804395897406
1572.047119140625
-1.5962623223231275915e-08
-1.6507904730136101212e-14
-1.5962623223231276e-008
-1.6507904730136101e-014
-11.317351620610928364
3.3594874875767186673
-42.755222607849105998
-11.317351620610928
3.3594874875767187
-42.755222607849106
4.2648515634753199066
-8.4629358700849355301
-22.252086323427270997
4.2648515634753199
-8.4629358700849355
-22.252086323427271
10.996977688149536689
-9.2123370180278048025
-29.206739014051372294
10.996977688149537
-9.2123370180278048
-29.206739014051372
10.935342607054865383
-9.4338917557810741954
-29.112263909175499776
10.935342607054865
-9.4338917557810742
-29.1122639091755
15.714024935401759819
1.3745079651566265433
-59.286834979937104606
15.71402493540176
1.3745079651566265
-59.286834979937105
-1.3624227800805182031
-4.1979357415396094666
-21.034430148188398846
-1.3624227800805182
-4.1979357415396095
-21.034430148188399
6.7690173115899296974
-4.7352452433700786827
-53.605307875695892506
6.7690173115899297
-4.7352452433700787
-53.605307875695893

2
gtsam/base/TestableAssertions.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/**
* Equals testing for basic types
*/
inline bool assert_equal(const Index& expected, const Index& actual, double tol = 0.0) {
inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) {
if(expected != actual) {
std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
return false;

5
gtsam/base/types.cpp
View File

@ -24,11 +24,6 @@
namespace gtsam {
/* ************************************************************************* */
std::string _defaultIndexFormatter(Index j) {
return boost::lexical_cast<std::string>(j);
}
/* ************************************************************************* */
std::string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);

13
gtsam/base/types.h
View File

@ -36,19 +36,6 @@
namespace gtsam {
/// Integer variable index type
typedef size_t Index;
/** A function to convert indices to strings, for example by translating back
* to a nonlinear key and then to a Symbol. */
typedef boost::function<std::string(Index)> IndexFormatter;
GTSAM_EXPORT std::string _defaultIndexFormatter(Index j);
/** The default IndexFormatter outputs the index */
static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter;
/// Integer nonlinear key type
typedef size_t Key;

16
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -56,7 +56,7 @@ namespace gtsam {
/* ************************************************************************* */
void DecisionTreeFactor::print(const string& s,
const IndexFormatter& formatter) const {
const KeyFormatter& formatter) const {
cout << s;
Potentials::print("Potentials:",formatter);
}
@ -64,10 +64,10 @@ namespace gtsam {
/* ************************************************************************* */
DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
ADT::Binary op) const {
map<Index,size_t> cs; // new cardinalities
map<Key,size_t> cs; // new cardinalities
// make unique key-cardinality map
BOOST_FOREACH(Index j, keys()) cs[j] = cardinality(j);
BOOST_FOREACH(Index j, f.keys()) cs[j] = f.cardinality(j);
BOOST_FOREACH(Key j, keys()) cs[j] = cardinality(j);
BOOST_FOREACH(Key j, f.keys()) cs[j] = f.cardinality(j);
// Convert map into keys
DiscreteKeys keys;
BOOST_FOREACH(const DiscreteKey& key, cs)
@ -91,14 +91,14 @@ namespace gtsam {
size_t i;
ADT result(*this);
for (i = 0; i < nrFrontals; i++) {
Index j = keys()[i];
Key j = keys()[i];
result = result.combine(j, cardinality(j), op);
}
// create new factor, note we start keys after nrFrontals
DiscreteKeys dkeys;
for (; i < keys().size(); i++) {
Index j = keys()[i];
Key j = keys()[i];
dkeys.push_back(DiscreteKey(j,cardinality(j)));
}
return boost::make_shared<DecisionTreeFactor>(dkeys, result);
@ -118,7 +118,7 @@ namespace gtsam {
size_t i;
ADT result(*this);
for (i = 0; i < frontalKeys.size(); i++) {
Index j = frontalKeys[i];
Key j = frontalKeys[i];
result = result.combine(j, cardinality(j), op);
}
@ -126,7 +126,7 @@ namespace gtsam {
// TODO: why do we need this??? result should contain correct keys!!!
DiscreteKeys dkeys;
for (i = 0; i < keys().size(); i++) {
Index j = keys()[i];
Key j = keys()[i];
// TODO: inefficient!
if (std::find(frontalKeys.begin(), frontalKeys.end(), j) != frontalKeys.end())
continue;

2
gtsam/discrete/DecisionTreeFactor.h
View File

@ -74,7 +74,7 @@ namespace gtsam {
// print
virtual void print(const std::string& s = "DecisionTreeFactor:\n",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface

16
gtsam/discrete/DiscreteConditional.cpp
View File

@ -66,7 +66,7 @@ DiscreteConditional::DiscreteConditional(const Signature& signature) :
/* ******************************************************************************** */
void DiscreteConditional::print(const std::string& s,
const IndexFormatter& formatter) const {
const KeyFormatter& formatter) const {
std::cout << s << std::endl;
Potentials::print(s);
}
@ -86,8 +86,8 @@ bool DiscreteConditional::equals(const DiscreteFactor& other,
/* ******************************************************************************** */
Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
ADT pFS(*this);
Index j; size_t value;
BOOST_FOREACH(Index key, parents())
Key j; size_t value;
BOOST_FOREACH(Key key, parents())
try {
j = (key);
value = parentsValues.at(j);
@ -111,7 +111,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
double maxP = 0;
DiscreteKeys keys;
BOOST_FOREACH(Index idx, frontals()) {
BOOST_FOREACH(Key idx, frontals()) {
DiscreteKey dk(idx, cardinality(idx));
keys & dk;
}
@ -129,7 +129,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
}
//set values (inPlace) to mpe
BOOST_FOREACH(Index j, frontals()) {
BOOST_FOREACH(Key j, frontals()) {
values[j] = mpe[j];
}
}
@ -137,7 +137,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
/* ******************************************************************************** */
void DiscreteConditional::sampleInPlace(Values& values) const {
assert(nrFrontals() == 1);
Index j = (firstFrontalKey());
Key j = (firstFrontalKey());
size_t sampled = sample(values); // Sample variable
values[j] = sampled; // store result in partial solution
}
@ -154,7 +154,7 @@ size_t DiscreteConditional::solve(const Values& parentsValues) const {
Values frontals;
double maxP = 0;
assert(nrFrontals() == 1);
Index j = (firstFrontalKey());
Key j = (firstFrontalKey());
for (size_t value = 0; value < cardinality(j); value++) {
frontals[j] = value;
double pValueS = pFS(frontals); // P(F=value|S=parentsValues)
@ -183,7 +183,7 @@ size_t DiscreteConditional::sample(const Values& parentsValues) const {
// get cumulative distribution function (cdf)
// TODO, only works for one key now, seems horribly slow this way
assert(nrFrontals() == 1);
Index j = (firstFrontalKey());
Key j = (firstFrontalKey());
size_t nj = cardinality(j);
vector<double> cdf(nj);
Values frontals;

6
gtsam/discrete/DiscreteConditional.h
View File

@ -42,7 +42,7 @@ public:
/** A map from keys to values..
* TODO: Again, do we need this??? */
typedef Assignment<Index> Values;
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
/// @name Standard Constructors
@ -79,7 +79,7 @@ public:
/// GTSAM-style print
void print(const std::string& s = "Discrete Conditional: ",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// GTSAM-style equals
bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
@ -99,7 +99,7 @@ public:
}
/** Restrict to given parent values, returns AlgebraicDecisionDiagram */
ADT choose(const Assignment<Index>& parentsValues) const;
ADT choose(const Assignment<Key>& parentsValues) const;
/**
* solve a conditional

4
gtsam/discrete/DiscreteFactor.h
View File

@ -49,7 +49,7 @@ public:
* cardinality of a Discrete variable. It should be handled naturally in
* the new class DiscreteValue, as the varible's type (domain)
*/
typedef Assignment<Index> Values;
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
public:
@ -78,7 +78,7 @@ public:
// print
virtual void print(const std::string& s = "DiscreteFactor\n",
const IndexFormatter& formatter = DefaultIndexFormatter) const {
const KeyFormatter& formatter = DefaultKeyFormatter) const {
Factor::print(s, formatter);
}

6
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -39,8 +39,8 @@ namespace gtsam {
}
/* ************************************************************************* */
FastSet<Index> DiscreteFactorGraph::keys() const {
FastSet<Index> keys;
FastSet<Key> DiscreteFactorGraph::keys() const {
FastSet<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end());
return keys;
@ -65,7 +65,7 @@ namespace gtsam {
/* ************************************************************************* */
void DiscreteFactorGraph::print(const std::string& s,
const IndexFormatter& formatter) const {
const KeyFormatter& formatter) const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {

8
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -72,8 +72,8 @@ public:
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
/** A map from keys to values */
typedef std::vector<Index> Indices;
typedef Assignment<Index> Values;
typedef std::vector<Key> Indices;
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
/** Default constructor */
@ -120,7 +120,7 @@ public:
}
/** Return the set of variables involved in the factors (set union) */
FastSet<Index> keys() const;
FastSet<Key> keys() const;
/** return product of all factors as a single factor */
DecisionTreeFactor product() const;
@ -130,7 +130,7 @@ public:
/// print
void print(const std::string& s = "DiscreteFactorGraph",
const IndexFormatter& formatter =DefaultIndexFormatter) const;
const KeyFormatter& formatter =DefaultKeyFormatter) const;
/** Solve the factor graph by performing variable elimination in COLAMD order using
* the dense elimination function specified in \c function,

8
gtsam/discrete/DiscreteKey.cpp
View File

@ -32,15 +32,15 @@ namespace gtsam {
}
}
vector<Index> DiscreteKeys::indices() const {
vector < Index > js;
vector<Key> DiscreteKeys::indices() const {
vector < Key > js;
BOOST_FOREACH(const DiscreteKey& key, *this)
js.push_back(key.first);
return js;
}
map<Index,size_t> DiscreteKeys::cardinalities() const {
map<Index,size_t> cs;
map<Key,size_t> DiscreteKeys::cardinalities() const {
map<Key,size_t> cs;
cs.insert(begin(),end());
// BOOST_FOREACH(const DiscreteKey& key, *this)
// cs.insert(key);

6
gtsam/discrete/DiscreteKey.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* Key type for discrete conditionals
* Includes name and cardinality
*/
typedef std::pair<Index,size_t> DiscreteKey;
typedef std::pair<Key,size_t> DiscreteKey;
/// DiscreteKeys is a set of keys that can be assembled using the & operator
struct DiscreteKeys: public std::vector<DiscreteKey> {
@ -53,10 +53,10 @@ namespace gtsam {
GTSAM_EXPORT DiscreteKeys(const std::vector<int>& cs);
/// Return a vector of indices
GTSAM_EXPORT std::vector<Index> indices() const;
GTSAM_EXPORT std::vector<Key> indices() const;
/// Return a map from index to cardinality
GTSAM_EXPORT std::map<Index,size_t> cardinalities() const;
GTSAM_EXPORT std::map<Key,size_t> cardinalities() const;
/// Add a key (non-const!)
DiscreteKeys& operator&(const DiscreteKey& key) {

2
gtsam/discrete/DiscreteMarginals.h
View File

@ -45,7 +45,7 @@ namespace gtsam {
}
/** Compute the marginal of a single variable */
DiscreteFactor::shared_ptr operator()(Index variable) const {
DiscreteFactor::shared_ptr operator()(Key variable) const {
// Compute marginal
DiscreteFactor::shared_ptr marginalFactor;
marginalFactor = bayesTree_->marginalFactor(variable, &EliminateDiscrete);

10
gtsam/discrete/Potentials.cpp
View File

@ -24,8 +24,8 @@ using namespace std;
namespace gtsam {
// explicit instantiation
template class DecisionTree<Index, double> ;
template class AlgebraicDecisionTree<Index> ;
template class DecisionTree<Key, double> ;
template class AlgebraicDecisionTree<Key> ;
/* ************************************************************************* */
double Potentials::safe_div(const double& a, const double& b) {
@ -52,7 +52,7 @@ namespace gtsam {
/* ************************************************************************* */
void Potentials::print(const string& s,
const IndexFormatter& formatter) const {
const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: ";
BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
cout << formatter(key.first) << "=" << formatter(key.second) << " ";
@ -65,7 +65,7 @@ namespace gtsam {
// void Potentials::remapIndices(const P& remapping) {
// // Permute the _cardinalities (TODO: Inefficient Consider Improving)
// DiscreteKeys keys;
// map<Index, Index> ordering;
// map<Key, Key> ordering;
//
// // Get the original keys from cardinalities_
// BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
@ -78,7 +78,7 @@ namespace gtsam {
// }
//
// // Change *this
// AlgebraicDecisionTree<Index> permuted((*this), ordering);
// AlgebraicDecisionTree<Key> permuted((*this), ordering);
// *this = permuted;
// cardinalities_ = keys.cardinalities();
// }

10
gtsam/discrete/Potentials.h
View File

@ -28,16 +28,16 @@ namespace gtsam {
/**
* A base class for both DiscreteFactor and DiscreteConditional
*/
class Potentials: public AlgebraicDecisionTree<Index> {
class Potentials: public AlgebraicDecisionTree<Key> {
public:
typedef AlgebraicDecisionTree<Index> ADT;
typedef AlgebraicDecisionTree<Key> ADT;
protected:
/// Cardinality for each key, used in combine
std::map<Index,size_t> cardinalities_;
std::map<Key,size_t> cardinalities_;
/** Constructor from ColumnIndex, and ADT */
Potentials(const ADT& potentials) :
@ -68,9 +68,9 @@ namespace gtsam {
// Testable
GTSAM_EXPORT bool equals(const Potentials& other, double tol = 1e-9) const;
GTSAM_EXPORT void print(const std::string& s = "Potentials: ",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const;
size_t cardinality(Index j) const { return cardinalities_.at(j);}
size_t cardinality(Key j) const { return cardinalities_.at(j);}
// /**
// * @brief Permutes the keys in Potentials

4
gtsam/discrete/Signature.cpp
View File

@ -131,8 +131,8 @@ namespace gtsam {
return keys;
}
vector<Index> Signature::indices() const {
vector<Index> js;
vector<Key> Signature::indices() const {
vector<Key> js;
js.push_back(key_.first);
BOOST_FOREACH(const DiscreteKey& key, parents_)
js.push_back(key.first);

2
gtsam/discrete/Signature.h
View File

@ -90,7 +90,7 @@ namespace gtsam {
DiscreteKeys discreteKeysParentsFirst() const;
/** All key indices, with variable key first */
std::vector<Index> indices() const;
std::vector<Key> indices() const;
// the CPT as parsed, if successful
const boost::optional<Table>& table() const {

8
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -38,7 +38,7 @@ using namespace std;
using namespace gtsam;
/* ******************************************************************************** */
typedef AlgebraicDecisionTree<Index> ADT;
typedef AlgebraicDecisionTree<Key> ADT;
#define DISABLE_DOT
@ -369,7 +369,7 @@ TEST(ADT, equality_parser)
TEST(ADT, constructor)
{
DiscreteKey v0(0,2), v1(1,3);
Assignment<Index> x00, x01, x02, x10, x11, x12;
Assignment<Key> x00, x01, x02, x10, x11, x12;
x00[0] = 0, x00[1] = 0;
x01[0] = 0, x01[1] = 1;
x02[0] = 0, x02[1] = 2;
@ -399,7 +399,7 @@ TEST(ADT, constructor)
BOOST_FOREACH(double& t, table)
t = x++;
ADT f3(z0 & z1 & z2 & z3, table);
Assignment<Index> assignment;
Assignment<Key> assignment;
assignment[0] = 0;
assignment[1] = 0;
assignment[2] = 0;
@ -501,7 +501,7 @@ TEST(ADT, zero)
ADT notb(B, 1, 0);
ADT anotb = a * notb;
// GTSAM_PRINT(anotb);
Assignment<Index> x00, x01, x10, x11;
Assignment<Key> x00, x01, x10, x11;
x00[0] = 0, x00[1] = 0;
x01[0] = 0, x01[1] = 1;
x10[0] = 1, x10[1] = 0;

2
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -55,7 +55,7 @@
////
//// /// print index signature only
//// void printSignature(const std::string& s = "Clique: ",
//// const IndexFormatter& indexFormatter = DefaultIndexFormatter) const {
//// const KeyFormatter& indexFormatter = DefaultKeyFormatter) const {
//// ((IndexConditionalOrdered::shared_ptr) conditional_)->print(s, indexFormatter);
//// }
////

16
gtsam/inference/BayesTree-inst.h
View File

@ -121,7 +121,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
BOOST_FOREACH(Index j, clique->conditional()->frontals())
BOOST_FOREACH(Key j, clique->conditional()->frontals())
nodes_[j] = clique;
if (parent_clique != NULL) {
clique->parent_ = parent_clique;
@ -260,7 +260,7 @@ namespace gtsam {
{
gttic(BayesTree_marginalFactor);
// get clique containing Index j
// get clique containing Key j
sharedClique clique = this->clique(j);
// calculate or retrieve its marginal P(C) = P(F,S)
@ -343,9 +343,9 @@ namespace gtsam {
// Get the set of variables to eliminate, which is C1\B.
gttic(Full_root_factoring);
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
FastVector<Index> C1_minus_B; {
FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) {
FastVector<Key> C1_minus_B; {
FastSet<Key> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) {
C1_minus_B_set.erase(j); }
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
}
@ -355,9 +355,9 @@ namespace gtsam {
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
}
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
FastVector<Index> C2_minus_B; {
FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) {
FastVector<Key> C2_minus_B; {
FastSet<Key> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) {
C2_minus_B_set.erase(j); }
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
}

8
gtsam/inference/BayesTree.h
View File

@ -143,7 +143,7 @@ namespace gtsam {
/** return root cliques */
const FastVector<sharedClique>& roots() const { return roots_; }
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
/** alternate syntax for matlab: find the clique that contains the variable with Key j */
const sharedClique& clique(Key j) const {
typename Nodes::const_iterator c = nodes_.find(j);
if(c == nodes_.end())
@ -169,13 +169,13 @@ namespace gtsam {
* return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
sharedFactorGraph joint(Index j1, Index j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
/**
* return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
sharedBayesNet jointBayesNet(Index j1, Index j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
/**
* Read only with side effects
@ -193,7 +193,7 @@ namespace gtsam {
* return the one with the lowest index in the ordering.
*/
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
Key findParentClique(const CONTAINER& parents) const;
/** Remove all nodes */
void clear();

10
gtsam/inference/VariableIndex.h
View File

@ -81,7 +81,7 @@ public:
* The number of variable entries. This is one greater than the variable
* with the highest index.
*/
Index size() const { return index_.size(); }
Key size() const { return index_.size(); }
/** The number of factors in the original factor graph */
size_t nFactors() const { return nFactors_; }
@ -154,11 +154,11 @@ public:
const_iterator find(Key key) const { return index_.find(key); }
protected:
Factor_iterator factorsBegin(Index variable) { return internalAt(variable).begin(); }
Factor_iterator factorsEnd(Index variable) { return internalAt(variable).end(); }
Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); }
Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); }
Factor_const_iterator factorsBegin(Index variable) const { return internalAt(variable).begin(); }
Factor_const_iterator factorsEnd(Index variable) const { return internalAt(variable).end(); }
Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); }
Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); }
/// Internal version of 'at' that asserts existence
const Factors& internalAt(Key variable) const {

8
gtsam/inference/VariableSlots.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
* interleaved.
*
* VariableSlots describes the 2D block structure of the combined factor. It
* is a map<Index, vector<size_t> >. The Index is the real
* is a map<Key, vector<size_t> >. The Key is the real
* variable index of the combined factor slot. The vector<size_t> tells, for
* each row-block (factor), which column-block (variable slot) from the
* component factor appears in this block of the combined factor.
@ -50,11 +50,11 @@ namespace gtsam {
*
* \nosubgrouping */
class VariableSlots : public FastMap<Index, FastVector<size_t> > {
class VariableSlots : public FastMap<Key, FastVector<size_t> > {
public:
typedef FastMap<Index, FastVector<size_t> > Base;
typedef FastMap<Key, FastVector<size_t> > Base;
/// @name Standard Constructors
/// @{
@ -98,7 +98,7 @@ VariableSlots::VariableSlots(const FG& factorGraph)
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
assert(factor);
size_t factorVarSlot = 0;
BOOST_FOREACH(const Index involvedVariable, *factor) {
BOOST_FOREACH(const Key involvedVariable, *factor) {
// Set the slot in this factor for this variable. If the
// variable was not already discovered, create an array for it
// that we'll fill with the slot indices for each factor that

2
gtsam/linear/GaussianConditional.cpp
View File

@ -54,7 +54,7 @@ namespace gtsam {
BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
/* ************************************************************************* */
void GaussianConditional::print(const string &s, const IndexFormatter& formatter) const
void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const
{
cout << s << " Conditional density ";
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {

2
gtsam/linear/GaussianDensity.h
View File

@ -49,7 +49,7 @@ namespace gtsam {
}
/// constructor using d, R
GaussianDensity(Index key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
GaussianConditional(key, d, R, noiseModel) {}
/// Construct using a mean and variance

8
gtsam/linear/HessianFactor.h
View File

@ -142,12 +142,12 @@ namespace gtsam {
* error is:
* 0.5*(f - 2*x'*g + x'*G*x)
*/
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
HessianFactor(Key j, const Matrix& G, const Vector& g, double f);
/** Construct a unary factor, given a mean and covariance matrix.
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
*/
HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
HessianFactor(Key j, const Vector& mu, const Matrix& Sigma);
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
@ -164,7 +164,7 @@ namespace gtsam {
1*1 f = b'*M*b
\endcode
*/
HessianFactor(Index j1, Index j2,
HessianFactor(Key j1, Key j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f);
@ -172,7 +172,7 @@ namespace gtsam {
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
HessianFactor(Index j1, Index j2, Index j3,
HessianFactor(Key j1, Key j2, Key j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f);

10
gtsam/linear/KalmanFilter.cpp
View File

@ -104,7 +104,7 @@ namespace gtsam {
// The factor related to the motion model is defined as
// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
Index k = step(p);
Key k = step(p);
return fuse(p, boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model), factorization());
}
@ -129,7 +129,7 @@ namespace gtsam {
Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
double f = dot(b, g2);
Index k = step(p);
Key k = step(p);
return fuse(p, boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f), factorization());
}
@ -138,7 +138,7 @@ namespace gtsam {
const Matrix& A1, const Vector& b, const SharedDiagonal& model) {
// Nhe factor related to the motion model is defined as
// f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
Index k = step(p);
Key k = step(p);
return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model), factorization());
}
@ -148,14 +148,14 @@ namespace gtsam {
// The factor related to the measurements would be defined as
// f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
// = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
Index k = step(p);
Key k = step(p);
return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model), factorization());
}
/* ************************************************************************* */
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z,
const Matrix& Q) {
Index k = step(p);
Key k = step(p);
Matrix M = inverse(Q), Ht = trans(H);
Matrix G = Ht * M * H;
Vector g = Ht * M * z;

2
gtsam/linear/KalmanFilter.h
View File

@ -86,7 +86,7 @@ namespace gtsam {
void print(const std::string& s = "") const;
/** Return step index k, starts at 0, incremented at each predict. */
static Index step(const State& p) {
static Key step(const State& p) {
return p->firstFrontalKey();
}

6
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -83,7 +83,7 @@ namespace gtsam {
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin();
for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) {
*ei = y[i];
}
@ -98,7 +98,7 @@ namespace gtsam {
Errors::const_iterator it = e.begin();
VectorValues y = zero();
for ( Index i = 0 ; i < y.size() ; ++i, ++it )
for ( Key i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ;
transposeMultiplyAdd2(1.0,it,e.end(),y);
return y;
@ -110,7 +110,7 @@ namespace gtsam {
(double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin();
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
for ( Key i = 0 ; i < y.size() ; ++i, ++it ) {
const Vector& ei = *it;
axpy(alpha, ei, y[i]);
}

2
gtsam/linear/SubgraphSolver.cpp
View File

@ -129,7 +129,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
/* check whether this factor should be augmented to the "tree" graph */
if ( gf->keys().size() == 1 ) augment = true;
else {
const Index u = gf->keys()[0], v = gf->keys()[1],
const Key u = gf->keys()[0], v = gf->keys()[1],
u_root = D.findSet(u), v_root = D.findSet(v);
if ( u_root != v_root ) {
t++; augment = true ;

6
gtsam/linear/linearExceptions.h
View File

@ -93,11 +93,11 @@ namespace gtsam {
described for Jacobian columns in the previous bullet.
*/
class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
Index j_;
Key j_;
public:
IndeterminantLinearSystemException(Index j) throw() : j_(j) {}
IndeterminantLinearSystemException(Key j) throw() : j_(j) {}
virtual ~IndeterminantLinearSystemException() throw() {}
Index nearbyVariable() const { return j_; }
Key nearbyVariable() const { return j_; }
virtual const char* what() const throw();
};

4
gtsam/linear/tests/testHessianFactorUnordered.cpp
View File

@ -242,7 +242,7 @@ TEST(HessianFactor, ConstructorNWay)
(1, dx1)
(2, dx2);
std::vector<Index> js;
std::vector<Key> js;
js.push_back(0); js.push_back(1); js.push_back(2);
std::vector<Matrix> Gs;
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
@ -356,7 +356,7 @@ TEST(HessianFactor, eliminate2 )
b2(2) = 0.2;
b2(3) = -0.1;
vector<pair<Index, Matrix> > meas;
vector<pair<Key, Matrix> > meas;
meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1));
JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));

2
gtsam/linear/tests/testJacobianFactorUnordered.cpp
View File

@ -371,7 +371,7 @@ TEST(JacobianFactor, eliminate2 )
b2(2) = 0.2;
b2(3) = -0.1;
vector<pair<Index, Matrix> > meas;
vector<pair<Key, Matrix> > meas;
meas.push_back(make_pair(2, Ax2));
meas.push_back(make_pair(11, Al1x1));
JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));

2
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -141,7 +141,7 @@ TEST (Serialization, linear_factors) {
EXPECT(equalsXML<VectorValues>(values));
EXPECT(equalsBinary<VectorValues>(values));
Index i1 = 4, i2 = 7;
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 1.0, 2.0, 3.0));

14
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -62,10 +62,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVect
}
/* ************************************************************************* */
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
FastSet<Key> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{
FastSet<Index> relinKeys;
FastSet<Key> relinKeys;
if(const double* threshold = boost::get<double>(&relinearizeThreshold))
{
@ -90,7 +90,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold,
void CheckRelinearizationRecursiveDouble(FastSet<Key>& relinKeys, double threshold,
const VectorValues& delta, const ISAM2Clique::shared_ptr& clique)
{
// Check the current clique for relinearization
@ -112,7 +112,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds,
void CheckRelinearizationRecursiveMap(FastSet<Key>& relinKeys, const FastMap<char,Vector>& thresholds,
const VectorValues& delta,
const ISAM2Clique::shared_ptr& clique)
{
@ -144,11 +144,11 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
}
/* ************************************************************************* */
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
FastSet<Key> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{
FastSet<Index> relinKeys;
FastSet<Key> relinKeys;
BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
if(relinearizeThreshold.type() == typeid(double))
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
@ -159,7 +159,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::
}
/* ************************************************************************* */
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const FastSet<Key>& markedMask)
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask)
{
static const bool debug = false;
// does the separator contain any of the variables?

8
gtsam/nonlinear/ISAM2-impl.h
View File

@ -32,7 +32,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
size_t nFullSystemVars;
enum { /*AS_ADDED,*/ COLAMD } algorithm;
enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
boost::optional<FastMap<Index,int> > constrainedKeys;
boost::optional<FastMap<Key,int> > constrainedKeys;
};
/**
@ -65,7 +65,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearizationFull(const VectorValues& delta,
static FastSet<Key> CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/**
@ -79,7 +79,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
static FastSet<Key> CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/**
@ -97,7 +97,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
*
* Alternatively could we trace up towards the root for each variable here?
*/
static void FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const FastSet<Key>& markedMask);
static void FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask);
/**
* Apply expmap to the given values, but only for indices appearing in

28
gtsam/nonlinear/ISAM2.cpp
View File

@ -328,7 +328,7 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals());
gttoc(affectedKeys);
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Key>()); // Will return this result
boost::shared_ptr<FastSet<Key> > affectedKeysSet(new FastSet<Key>()); // Will return this result
if(affectedKeys.size() >= theta_.size() * batchThreshold)
{
@ -408,11 +408,11 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
if(debug) factors.print("Relinearized factors: ");
gttoc(relinearizeAffected);
if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Key key, affectedKeys) { cout << key << " "; } cout << endl; }
// Reeliminated keys for detailed results
if(params_.enableDetailedResults) {
BOOST_FOREACH(Index key, affectedAndNewKeys) {
BOOST_FOREACH(Key key, affectedAndNewKeys) {
result.detail->variableStatus[key].isReeliminated = true;
}
}
@ -573,7 +573,7 @@ ISAM2Result ISAM2::update(
// Compute unused keys and indices
FastSet<Key> unusedKeys;
FastSet<Index> unusedIndices;
FastSet<Key> unusedIndices;
{
// Get keys from removed factors and new factors, and compute unused keys,
// i.e., keys that are empty now and do not appear in the new factors.
@ -611,7 +611,7 @@ ISAM2Result ISAM2::update(
FastSet<Key> markedKeys = newFactors.keys(); // Get keys from new factors
// Also mark keys involved in removed factors
{
FastSet<Index> markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors
FastSet<Key> markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors
markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
}
// Also mark any provided extra re-eliminate keys
@ -629,16 +629,16 @@ ISAM2Result ISAM2::update(
}
// NOTE: we use assign instead of the iterator constructor here because this
// is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Index value) instead of the iterator constructor.
FastVector<Index> observedKeys; observedKeys.reserve(markedKeys.size());
BOOST_FOREACH(Index index, markedKeys) {
// vector(size_t count, Key value) instead of the iterator constructor.
FastVector<Key> observedKeys; observedKeys.reserve(markedKeys.size());
BOOST_FOREACH(Key index, markedKeys) {
if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused
observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them
}
gttoc(gather_involved_keys);
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
FastSet<Index> relinKeys;
FastSet<Key> relinKeys;
if (relinearizeThisStep) {
gttic(gather_relinearize_keys);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
@ -680,7 +680,7 @@ ISAM2Result ISAM2::update(
// Relin involved keys for detailed results
if(params_.enableDetailedResults) {
FastSet<Index> involvedRelinKeys;
FastSet<Key> involvedRelinKeys;
BOOST_FOREACH(const sharedClique& root, roots_)
Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
BOOST_FOREACH(Key key, involvedRelinKeys) {
@ -763,7 +763,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// Keep track of marginal factors - map from clique to the marginal factors
// that should be incorporated into it, passed up from it's children.
// multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;
map<Index, vector<GaussianFactor::shared_ptr> > marginalFactors;
map<Key, vector<GaussianFactor::shared_ptr> > marginalFactors;
// Keep track of factors that get summarized by removing cliques
FastSet<size_t> factorIndicesToRemove;
@ -873,7 +873,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); }
// Remove any factors in subtrees that we're removing at this step
BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) {
BOOST_FOREACH(Index indexInClique, removedChild->conditional()->frontals()) {
BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) {
BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) {
factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } }
// Create factor graph from factor indices
@ -881,8 +881,8 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
// Reeliminate the linear graph to get the marginal and discard the conditional
const FastSet<Index> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
FastVector<Index> cliqueFrontalsToEliminate;
const FastSet<Key> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
FastVector<Key> cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(),
std::back_inserter(cliqueFrontalsToEliminate));
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr> eliminationResult1 =

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -125,7 +125,7 @@ boost::tuple<V, size_t> nonlinearConjugateGradient(const S &system, const V &ini
// GTSAM_CONCEPT_MANIFOLD_TYPE(V);
Index iteration = 0;
Key iteration = 0;
// check if we're already close enough
double currentError = system.error(initial);

8
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
* So f = 2 f(x), g = -df(x), G = ddf(x)
*/
static HessianFactor::shared_ptr linearize(double z, double u, double p,
Index j1, Index j2) {
Key j1, Key j2) {
double e = u - z, e2 = e * e;
double c = 2 * logSqrt2PI - log(p) + e2 * p;
Vector g1 = (Vec(1) << -e * p);
@ -145,7 +145,7 @@ namespace gtsam {
* variable indices.
*/
// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
// const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
// return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
// }
@ -157,8 +157,8 @@ namespace gtsam {
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
double u = x.at<LieScalar>(meanKey_);
double p = x.at<LieScalar>(precisionKey_);
Index j1 = meanKey_;
Index j2 = precisionKey_;
Key j1 = meanKey_;
Key j2 = precisionKey_;
return linearize(z_, u, p, j1, j2);
}

8
gtsam/symbolic/SymbolicConditional.h
View File

@ -51,16 +51,16 @@ namespace gtsam {
SymbolicConditional() {}
/** No parents */
SymbolicConditional(Index j) : BaseFactor(j), BaseConditional(1) {}
SymbolicConditional(Key j) : BaseFactor(j), BaseConditional(1) {}
/** Single parent */
SymbolicConditional(Index j, Index parent) : BaseFactor(j, parent), BaseConditional(1) {}
SymbolicConditional(Key j, Key parent) : BaseFactor(j, parent), BaseConditional(1) {}
/** Two parents */
SymbolicConditional(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(1) {}
SymbolicConditional(Key j, Key parent1, Key parent2) : BaseFactor(j, parent1, parent2), BaseConditional(1) {}
/** Three parents */
SymbolicConditional(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(1) {}
SymbolicConditional(Key j, Key parent1, Key parent2, Key parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(1) {}
/** Named constructor from an arbitrary number of keys and frontals */
template<typename ITERATOR>

2
gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View File

@ -82,7 +82,7 @@ TEST( SymbolicBayesNet, combine )
TEST(SymbolicBayesNet, saveGraph) {
SymbolicBayesNet bn;
bn += SymbolicConditional(_A_, _B_);
std::vector<Index> keys;
std::vector<Key> keys;
keys.push_back(_B_);
keys.push_back(_C_);
keys.push_back(_D_);

2
gtsam/symbolic/tests/testSymbolicFactor.cpp
View File

@ -33,7 +33,7 @@ using namespace boost::assign;
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
TEST(SymbolicFactor, eliminate) {
vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
vector<Key> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
IndexFactor actual(keys.begin(), keys.end());
BayesNet<IndexConditional> fragment = *actual.eliminate(3);

12
gtsam_unstable/discrete/AllDiff.cpp
View File

@ -21,9 +21,9 @@ namespace gtsam {
/* ************************************************************************* */
void AllDiff::print(const std::string& s,
const IndexFormatter& formatter) const {
const KeyFormatter& formatter) const {
std::cout << s << "AllDiff on ";
BOOST_FOREACH (Index dkey, keys_)
BOOST_FOREACH (Key dkey, keys_)
std::cout << formatter(dkey) << " ";
std::cout << std::endl;
}
@ -31,7 +31,7 @@ namespace gtsam {
/* ************************************************************************* */
double AllDiff::operator()(const Values& values) const {
std::set < size_t > taken; // record values taken by keys
BOOST_FOREACH(Index dkey, keys_) {
BOOST_FOREACH(Key dkey, keys_) {
size_t value = values.at(dkey); // get the value for that key
if (taken.count(value)) return 0.0;// check if value alreday taken
taken.insert(value);// if not, record it as taken and keep checking
@ -70,7 +70,7 @@ namespace gtsam {
// Check all other domains for singletons and erase corresponding values
// This is the same as arc-consistency on the equivalent binary constraints
bool changed = false;
BOOST_FOREACH(Index k, keys_)
BOOST_FOREACH(Key k, keys_)
if (k != j) {
const Domain& Dk = domains[k];
if (Dk.isSingleton()) { // check if singleton
@ -88,7 +88,7 @@ namespace gtsam {
Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const {
DiscreteKeys newKeys;
// loop over keys and add them only if they do not appear in values
BOOST_FOREACH(Index k, keys_)
BOOST_FOREACH(Key k, keys_)
if (values.find(k) == values.end()) {
newKeys.push_back(DiscreteKey(k,cardinalities_.at(k)));
}
@ -99,7 +99,7 @@ namespace gtsam {
Constraint::shared_ptr AllDiff::partiallyApply(
const std::vector<Domain>& domains) const {
DiscreteFactor::Values known;
BOOST_FOREACH(Index k, keys_) {
BOOST_FOREACH(Key k, keys_) {
const Domain& Dk = domains[k];
if (Dk.isSingleton())
known[k] = Dk.firstValue();

8
gtsam_unstable/discrete/AllDiff.h
View File

@ -16,15 +16,15 @@ namespace gtsam {
* General AllDiff constraint
* Returns 1 if values for all keys are different, 0 otherwise
* DiscreteFactors are all awkward in that they have to store two types of keys:
* for each variable we have a Index and an Index. In this factor, we
* for each variable we have a Key and an Key. In this factor, we
* keep the Indices locally, and the Indices are stored in IndexFactor.
*/
class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint {
std::map<Index,size_t> cardinalities_;
std::map<Key,size_t> cardinalities_;
DiscreteKey discreteKey(size_t i) const {
Index j = keys_[i];
Key j = keys_[i];
return DiscreteKey(j,cardinalities_.at(j));
}
@ -35,7 +35,7 @@ namespace gtsam {
// print
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// equals
bool equals(const DiscreteFactor& other, double tol) const {

2
gtsam_unstable/discrete/BinaryAllDiff.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
// print
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const {
const KeyFormatter& formatter = DefaultKeyFormatter) const {
std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
<< formatter(keys_[1]) << std::endl;
}

4
gtsam_unstable/discrete/CSP.h
View File

@ -22,8 +22,8 @@ namespace gtsam {
public:
/** A map from keys to values */
typedef std::vector<Index> Indices;
typedef Assignment<Index> Values;
typedef std::vector<Key> Indices;
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
public:

6
gtsam_unstable/discrete/Constraint.h
View File

@ -38,17 +38,17 @@ namespace gtsam {
protected:
/// Construct n-way factor
Constraint(const std::vector<Index>& js) :
Constraint(const std::vector<Key>& js) :
DiscreteFactor(js) {
}
/// Construct unary factor
Constraint(Index j) :
Constraint(Key j) :
DiscreteFactor(boost::assign::cref_list_of<1>(j)) {
}
/// Construct binary factor
Constraint(Index j1, Index j2) :
Constraint(Key j1, Key j2) :
DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {
}

8
gtsam_unstable/discrete/Domain.cpp
View File

@ -16,7 +16,7 @@ namespace gtsam {
/* ************************************************************************* */
void Domain::print(const string& s,
const IndexFormatter& formatter) const {
const KeyFormatter& formatter) const {
// cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" <<
// formatter(keys_[0]) << ") with values";
// BOOST_FOREACH (size_t v,values_) cout << " " << v;
@ -57,12 +57,12 @@ namespace gtsam {
}
/* ************************************************************************* */
bool Domain::checkAllDiff(const vector<Index> keys, vector<Domain>& domains) {
Index j = keys_[0];
bool Domain::checkAllDiff(const vector<Key> keys, vector<Domain>& domains) {
Key j = keys_[0];
// for all values in this domain
BOOST_FOREACH(size_t value, values_) {
// for all connected domains
BOOST_FOREACH(Index k, keys)
BOOST_FOREACH(Key k, keys)
// if any domain contains the value we cannot make this domain singleton
if (k!=j && domains[k].contains(value))
goto found;

4
gtsam_unstable/discrete/Domain.h
View File

@ -67,7 +67,7 @@ namespace gtsam {
// print
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// equals
bool equals(const DiscreteFactor& other, double tol) const {
@ -104,7 +104,7 @@ namespace gtsam {
* If found, we make this a singleton... Called in AllDiff::ensureArcConsistency
* @param keys connected domains through alldiff
*/
bool checkAllDiff(const std::vector<Index> keys, std::vector<Domain>& domains);
bool checkAllDiff(const std::vector<Key> keys, std::vector<Domain>& domains);
/// Partially apply known values
virtual Constraint::shared_ptr partiallyApply(

10
gtsam_unstable/discrete/Scheduler.cpp
View File

@ -70,9 +70,9 @@ namespace gtsam {
student.name_ = studentName;
// We fix the ordering by assigning a higher index to the student
// and numbering the areas lower
Index j = 3*maxNrStudents_ + nrStudents();
Key j = 3*maxNrStudents_ + nrStudents();
student.key_ = DiscreteKey(j, nrTimeSlots());
Index base = 3*nrStudents();
Key base = 3*nrStudents();
student.keys_[0] = DiscreteKey(base+0, nrFaculty());
student.keys_[1] = DiscreteKey(base+1, nrFaculty());
student.keys_[2] = DiscreteKey(base+2, nrFaculty());
@ -218,10 +218,10 @@ namespace gtsam {
// Not intended to be general! Assumes very particular ordering !
cout << endl;
for (size_t s = 0; s < nrStudents(); s++) {
Index j = 3*maxNrStudents_ + s;
Key j = 3*maxNrStudents_ + s;
size_t slot = assignment->at(j);
cout << studentName(s) << " slot: " << slotName_[slot] << endl;
Index base = 3*s;
Key base = 3*s;
for (size_t area = 0; area < 3; area++) {
size_t faculty = assignment->at(base+area);
cout << setw(12) << studentArea(s,area) << ": " << facultyName_[faculty]
@ -245,7 +245,7 @@ namespace gtsam {
void Scheduler::accumulateStats(sharedValues assignment, vector<
size_t>& stats) const {
for (size_t s = 0; s < nrStudents(); s++) {
Index base = 3*s;
Key base = 3*s;
for (size_t area = 0; area < 3; area++) {
size_t f = assignment->at(base+area);
assert(f<stats.size());

2
gtsam_unstable/discrete/SingleValue.cpp
View File

@ -17,7 +17,7 @@ namespace gtsam {
/* ************************************************************************* */
void SingleValue::print(const string& s,
const IndexFormatter& formatter) const {
const KeyFormatter& formatter) const {
cout << s << "SingleValue on " << "j=" << formatter(keys_[0])
<< " with value " << value_ << endl;
}

4
gtsam_unstable/discrete/SingleValue.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
typedef boost::shared_ptr<SingleValue> shared_ptr;
/// Constructor
SingleValue(Index key, size_t n, size_t value) :
SingleValue(Key key, size_t n, size_t value) :
Constraint(key), cardinality_(n), value_(value) {
}
@ -43,7 +43,7 @@ namespace gtsam {
// print
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// equals
bool equals(const DiscreteFactor& other, double tol) const {

8
gtsam_unstable/discrete/tests/testSudoku.cpp
View File

@ -34,8 +34,8 @@ public:
return dkeys_.at(IJ(i, j));
}
/// return Index for cell(i,j)
Index key(size_t i, size_t j) const {
/// return Key for cell(i,j)
Key key(size_t i, size_t j) const {
return dkey(i, j).first;
}
@ -45,7 +45,7 @@ public:
// Create variables, ordering, and unary constraints
va_list ap;
va_start(ap, n);
Index k=0;
Key k=0;
for (size_t i = 0; i < n; ++i) {
for (size_t j = 0; j < n; ++j, ++k) {
// create the key
@ -97,7 +97,7 @@ public:
void printAssignment(DiscreteFactor::sharedValues assignment) const {
for (size_t i = 0; i < n_; i++) {
for (size_t j = 0; j < n_; j++) {
Index k = key(i, j);
Key k = key(i, j);
cout << 1 + assignment->at(k) << " ";
}
cout << endl;

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -112,7 +112,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
/* ************************************************************************* */
void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
Index index;
Key index;
// Insert the factor into an existing hole in the factor graph, if possible
if(availableSlots_.size() > 0) {
index = availableSlots_.front();

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -103,7 +103,7 @@ public:
protected:
/** A typedef defining an Key-Factor mapping **/
typedef std::map<Key, std::set<Index> > FactorIndex;
typedef std::map<Key, std::set<Key> > FactorIndex;
/** The L-M optimization parameters **/
LevenbergMarquardtParams parameters_;

2
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -104,7 +104,7 @@ boost::shared_ptr<GaussianFactor>
LinearizedJacobianFactor::linearize(const Values& c) const {
// Create the 'terms' data structure for the Jacobian constructor
std::vector<std::pair<Index, Matrix> > terms;
std::vector<std::pair<Key, Matrix> > terms;
BOOST_FOREACH(Key key, keys()) {
terms.push_back(std::make_pair(key, this->A(key)));
}

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -579,11 +579,11 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Index> allkeys = LinFactorGraph->keys();
FastSet<Key> allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
allkeys.erase(key_value.key);
}
std::vector<Index> variables(allkeys.begin(), allkeys.end());
std::vector<Key> variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0);

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -581,10 +581,10 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Index> allkeys = LinFactorGraph->keys();
FastSet<Key> allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues)
allkeys.erase(key_value.key);
std::vector<Index> variables(allkeys.begin(), allkeys.end());
std::vector<Key> variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0);

2
gtsam_unstable/slam/DummyFactor.cpp
View File

@ -47,7 +47,7 @@ DummyFactor::linearize(const Values& c) const {
return boost::shared_ptr<JacobianFactor>();
// Fill in terms with zero matrices
std::vector<std::pair<Index, Matrix> > terms(this->size());
std::vector<std::pair<Key, Matrix> > terms(this->size());
for(size_t j=0; j<this->size(); ++j) {
terms[j].first = keys()[j];
terms[j].second = zeros(rowDim_, dims_[j]);

4
tests/testExtendedKalmanFilter.cpp
View File

@ -197,7 +197,7 @@ public:
const X2& x2 = c.at<X2>(key2());
Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2);
const Index var1 = ordering[key1()], var2 = ordering[key2()];
const Key var1 = ordering[key1()], var2 = ordering[key2()];
SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
@ -333,7 +333,7 @@ public:
const X& x1 = c.at<X>(key());
Matrix A1;
Vector b = -evaluateError(x1, A1);
const Index var1 = ordering[key()];
const Key var1 = ordering[key()];
SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {

6
tests/testGaussianISAM2.cpp
View File

@ -186,12 +186,12 @@ done:
// Permuted<VectorValues> permuted(permutation, values);
//
// // After permutation, the indices above the threshold are 2 and 2
// FastSet<Index> expected;
// FastSet<Key> expected;
// expected.insert(2);
// expected.insert(3);
//
// // Indices checked by CheckRelinearization
// FastSet<Index> actual = Impl::CheckRelinearization(permuted, 0.1);
// FastSet<Key> actual = Impl::CheckRelinearization(permuted, 0.1);
//
// EXPECT(assert_equal(expected, actual));
//}
@ -624,7 +624,7 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
namespace {
bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
Matrix expectedAugmentedHessian, expected3AugmentedHessian;
vector<Index> toKeep;
vector<Key> toKeep;
BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys)
if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end())
toKeep.push_back(j);