diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt index 12c9f4db4..e54ccd359 100644 --- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt +++ b/examples/Data/dubrovnik-3-7-pre-rewritten.txt @@ -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 diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 12138d192..04d3fc676 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -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; diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index 00eb50702..3f86dc0c1 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -24,11 +24,6 @@ namespace gtsam { - /* ************************************************************************* */ - std::string _defaultIndexFormatter(Index j) { - return boost::lexical_cast(j); - } - /* ************************************************************************* */ std::string _defaultKeyFormatter(Key key) { const Symbol asSymbol(key); diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 935ab685e..e1fb0f33e 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -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 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; diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 6a4aacd37..9ed88bc3d 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -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 cs; // new cardinalities + map 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(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; diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 43eae0926..33d14e6dd 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -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 diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 7f5dde3e5..f21a705ff 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -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 cdf(nj); Values frontals; diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index ccc11f0b8..1ba97444f 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -42,7 +42,7 @@ public: /** A map from keys to values.. * TODO: Again, do we need this??? */ - typedef Assignment Values; + typedef Assignment Values; typedef boost::shared_ptr 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& parentsValues) const; + ADT choose(const Assignment& parentsValues) const; /** * solve a conditional diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index b59fe00d6..8351b310b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -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 Values; + typedef Assignment Values; typedef boost::shared_ptr 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); } diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 0285cd584..d2ccce055 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -39,8 +39,8 @@ namespace gtsam { } /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; + FastSet DiscreteFactorGraph::keys() const { + FastSet 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++) { diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 2230eb33b..cd186beaf 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -72,8 +72,8 @@ public: typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class /** A map from keys to values */ - typedef std::vector Indices; - typedef Assignment Values; + typedef std::vector Indices; + typedef Assignment Values; typedef boost::shared_ptr sharedValues; /** Default constructor */ @@ -120,7 +120,7 @@ public: } /** Return the set of variables involved in the factors (set union) */ - FastSet keys() const; + FastSet 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, diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index df9f5c3d9..828a93eec 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -32,15 +32,15 @@ namespace gtsam { } } - vector DiscreteKeys::indices() const { - vector < Index > js; + vector DiscreteKeys::indices() const { + vector < Key > js; BOOST_FOREACH(const DiscreteKey& key, *this) js.push_back(key.first); return js; } - map DiscreteKeys::cardinalities() const { - map cs; + map DiscreteKeys::cardinalities() const { + map cs; cs.insert(begin(),end()); // BOOST_FOREACH(const DiscreteKey& key, *this) // cs.insert(key); diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index 6451886c3..bc2bd862d 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -30,7 +30,7 @@ namespace gtsam { * Key type for discrete conditionals * Includes name and cardinality */ - typedef std::pair DiscreteKey; + typedef std::pair DiscreteKey; /// DiscreteKeys is a set of keys that can be assembled using the & operator struct DiscreteKeys: public std::vector { @@ -53,10 +53,10 @@ namespace gtsam { GTSAM_EXPORT DiscreteKeys(const std::vector& cs); /// Return a vector of indices - GTSAM_EXPORT std::vector indices() const; + GTSAM_EXPORT std::vector indices() const; /// Return a map from index to cardinality - GTSAM_EXPORT std::map cardinalities() const; + GTSAM_EXPORT std::map cardinalities() const; /// Add a key (non-const!) DiscreteKeys& operator&(const DiscreteKey& key) { diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index b25984cc4..2158973a9 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -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); diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index f222f6081..11dddee66 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -24,8 +24,8 @@ using namespace std; namespace gtsam { // explicit instantiation - template class DecisionTree ; - template class AlgebraicDecisionTree ; + template class DecisionTree ; + template class AlgebraicDecisionTree ; /* ************************************************************************* */ 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 ordering; +// map ordering; // // // Get the original keys from cardinalities_ // BOOST_FOREACH(const DiscreteKey& key, cardinalities_) @@ -78,7 +78,7 @@ namespace gtsam { // } // // // Change *this -// AlgebraicDecisionTree permuted((*this), ordering); +// AlgebraicDecisionTree permuted((*this), ordering); // *this = permuted; // cardinalities_ = keys.cardinalities(); // } diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 7d8aaa2ae..51475e07d 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -28,16 +28,16 @@ namespace gtsam { /** * A base class for both DiscreteFactor and DiscreteConditional */ - class Potentials: public AlgebraicDecisionTree { + class Potentials: public AlgebraicDecisionTree { public: - typedef AlgebraicDecisionTree ADT; + typedef AlgebraicDecisionTree ADT; protected: /// Cardinality for each key, used in combine - std::map cardinalities_; + std::map 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 diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index a281841a6..9e63b957d 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -131,8 +131,8 @@ namespace gtsam { return keys; } - vector Signature::indices() const { - vector js; + vector Signature::indices() const { + vector js; js.push_back(key_.first); BOOST_FOREACH(const DiscreteKey& key, parents_) js.push_back(key.first); diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h index 632869232..6d49c7e4c 100644 --- a/gtsam/discrete/Signature.h +++ b/gtsam/discrete/Signature.h @@ -90,7 +90,7 @@ namespace gtsam { DiscreteKeys discreteKeysParentsFirst() const; /** All key indices, with variable key first */ - std::vector indices() const; + std::vector indices() const; // the CPT as parsed, if successful const boost::optional& table() const { diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 61cedd720..cf938c901 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -38,7 +38,7 @@ using namespace std; using namespace gtsam; /* ******************************************************************************** */ -typedef AlgebraicDecisionTree ADT; +typedef AlgebraicDecisionTree ADT; #define DISABLE_DOT @@ -369,7 +369,7 @@ TEST(ADT, equality_parser) TEST(ADT, constructor) { DiscreteKey v0(0,2), v1(1,3); - Assignment x00, x01, x02, x10, x11, x12; + Assignment 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 assignment; + Assignment 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 x00, x01, x10, x11; + Assignment x00, x01, x10, x11; x00[0] = 0, x00[1] = 0; x01[0] = 0, x01[1] = 1; x10[0] = 1, x10[1] = 0; diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 313ccfcd9..c1acaf83d 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -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); //// } //// diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 9d631664d..cbeec97c5 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -121,7 +121,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTree::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 p_C1_B; { - FastVector C1_minus_B; { - FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { + FastVector C1_minus_B; { + FastSet 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 p_C2_B; { - FastVector C2_minus_B; { - FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); - BOOST_FOREACH(const Index j, *B->conditional()) { + FastVector C2_minus_B; { + FastSet 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()); } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 8071f7011..485f222a2 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -143,7 +143,7 @@ namespace gtsam { /** return root cliques */ const FastVector& 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 - Index findParentClique(const CONTAINER& parents) const; + Key findParentClique(const CONTAINER& parents) const; /** Remove all nodes */ void clear(); diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index ed7aaae55..237a31ded 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -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 { diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index e986b6194..a2ed9e5f1 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -38,7 +38,7 @@ namespace gtsam { * interleaved. * * VariableSlots describes the 2D block structure of the combined factor. It -* is a map >. The Index is the real +* is a map >. The Key is the real * variable index of the combined factor slot. The vector 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 > { +class VariableSlots : public FastMap > { public: - typedef FastMap > Base; + typedef FastMap > 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 diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 3900082fd..c6bd3967b 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -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) { diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 15db0ce35..4b4fc1665 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -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 diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 39c4009d2..1467d5783 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -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); diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index c37df5ad2..5bd375b20 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -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(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(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(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(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; diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index bcbea4394..9c2a50b46 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -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(); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 118c5effa..8c0de69a2 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -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]); } diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index b6c2a6911..034fdeeaf 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -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 ; diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 62f12cc2e..63bc61e80 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -93,11 +93,11 @@ namespace gtsam { described for Jacobian columns in the previous bullet. */ class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException { - 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(); }; diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index 52c85dbf6..c406cf571 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -242,7 +242,7 @@ TEST(HessianFactor, ConstructorNWay) (1, dx1) (2, dx2); - std::vector js; + std::vector js; js.push_back(0); js.push_back(1); js.push_back(2); std::vector 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 > meas; + vector > meas; meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(1, Al1x1)); JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index e3fe75d61..c5fe3ca51 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -371,7 +371,7 @@ TEST(JacobianFactor, eliminate2 ) b2(2) = 0.2; b2(3) = -0.1; - vector > meas; + vector > meas; meas.push_back(make_pair(2, Ax2)); meas.push_back(make_pair(11, Al1x1)); JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index cb7b71a8d..3859bcfa9 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -141,7 +141,7 @@ TEST (Serialization, linear_factors) { EXPECT(equalsXML(values)); EXPECT(equalsBinary(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)); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 2a3ff3672..60e034d9b 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -62,10 +62,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVect } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, +FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + FastSet relinKeys; if(const double* threshold = boost::get(&relinearizeThreshold)) { @@ -90,7 +90,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization @@ -112,7 +112,7 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { @@ -144,11 +144,11 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, +FastSet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + FastSet relinKeys; BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { if(relinearizeThreshold.type() == typeid(double)) CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); @@ -159,7 +159,7 @@ FastSet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& keys, const FastSet& markedMask) +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask) { static const bool debug = false; // does the separator contain any of the variables? diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index ee0723232..62755785f 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -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 > constrainedKeys; + boost::optional > 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 CheckRelinearizationFull(const VectorValues& delta, + static FastSet 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 CheckRelinearizationPartial(const FastVector& roots, + static FastSet CheckRelinearizationPartial(const FastVector& 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& keys, const FastSet& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask); /** * Apply expmap to the given values, but only for indices appearing in diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 7526b4772..3bb54d802 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -328,7 +328,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result if(affectedKeys.size() >= theta_.size() * batchThreshold) { @@ -408,11 +408,11 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& 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 unusedKeys; - FastSet unusedIndices; + FastSet 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 markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - FastSet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors + FastSet 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 observedKeys; observedKeys.reserve(markedKeys.size()); - BOOST_FOREACH(Index index, markedKeys) { + // vector(size_t count, Key value) instead of the iterator constructor. + FastVector 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 relinKeys; + FastSet 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 involvedRelinKeys; + FastSet 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& 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 marginalFactors; - map > marginalFactors; + map > marginalFactors; // Keep track of factors that get summarized by removing cliques FastSet factorIndicesToRemove; @@ -873,7 +873,7 @@ void ISAM2::marginalizeLeaves(const FastList& 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& leafKeysList, graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the conditional - const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); - FastVector cliqueFrontalsToEliminate; + const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + FastVector cliqueFrontalsToEliminate; std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::back_inserter(cliqueFrontalsToEliminate)); pair eliminationResult1 = diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 4b12eae6f..3329d0adf 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -125,7 +125,7 @@ boost::tuple 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); diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 78e4d807d..a8196b870 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -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 linearize(const Values& x) const { double u = x.at(meanKey_); double p = x.at(precisionKey_); - Index j1 = meanKey_; - Index j2 = precisionKey_; + Key j1 = meanKey_; + Key j2 = precisionKey_; return linearize(z_, u, p, j1, j2); } diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 76c7583a7..1d1009327 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -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 diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 537733286..610e98c58 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -82,7 +82,7 @@ TEST( SymbolicBayesNet, combine ) TEST(SymbolicBayesNet, saveGraph) { SymbolicBayesNet bn; bn += SymbolicConditional(_A_, _B_); - std::vector keys; + std::vector keys; keys.push_back(_B_); keys.push_back(_C_); keys.push_back(_D_); diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index a9f01e2d9..b87753efa 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -33,7 +33,7 @@ using namespace boost::assign; /* ************************************************************************* */ #ifdef TRACK_ELIMINATE TEST(SymbolicFactor, eliminate) { - vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; + vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; IndexFactor actual(keys.begin(), keys.end()); BayesNet fragment = *actual.eliminate(3); diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 5c6d2e518..b230aa570 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -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& 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(); diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index b95a29d05..3e7296593 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -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 cardinalities_; + std::map 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 { diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 4c75a59a6..1ae5a008a 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -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; } diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index ab3abffde..923365c40 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -22,8 +22,8 @@ namespace gtsam { public: /** A map from keys to values */ - typedef std::vector Indices; - typedef Assignment Values; + typedef std::vector Indices; + typedef Assignment Values; typedef boost::shared_ptr sharedValues; public: diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 76c7385ce..d8f4689a8 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -38,17 +38,17 @@ namespace gtsam { protected: /// Construct n-way factor - Constraint(const std::vector& js) : + Constraint(const std::vector& 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)) { } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 397c46626..94c977cb0 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -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 keys, vector& domains) { - Index j = keys_[0]; + bool Domain::checkAllDiff(const vector keys, vector& 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; diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 33e6a2709..812da9932 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -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 keys, std::vector& domains); + bool checkAllDiff(const std::vector keys, std::vector& domains); /// Partially apply known values virtual Constraint::shared_ptr partiallyApply( diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 34532e488..1f22fa0e6 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -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 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 { diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index 415a73dc1..e2115e8bc 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -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; diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 0cde752e0..9b8cc38f0 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -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(); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 54e8bbb97..9b2903a14 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -103,7 +103,7 @@ public: protected: /** A typedef defining an Key-Factor mapping **/ - typedef std::map > FactorIndex; + typedef std::map > FactorIndex; /** The L-M optimization parameters **/ LevenbergMarquardtParams parameters_; diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 4b95c65b4..526674a04 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -104,7 +104,7 @@ boost::shared_ptr LinearizedJacobianFactor::linearize(const Values& c) const { // Create the 'terms' data structure for the Jacobian constructor - std::vector > terms; + std::vector > terms; BOOST_FOREACH(Key key, keys()) { terms.push_back(std::make_pair(key, this->A(key))); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index b13667541..46d099738 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -579,11 +579,11 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + FastSet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { allkeys.erase(key_value.key); } - std::vector variables(allkeys.begin(), allkeys.end()); + std::vector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 5b776fc95..b21eaa8f0 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -581,10 +581,10 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + FastSet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) allkeys.erase(key_value.key); - std::vector variables(allkeys.begin(), allkeys.end()); + std::vector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 62ada7e41..1ce3a878d 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -47,7 +47,7 @@ DummyFactor::linearize(const Values& c) const { return boost::shared_ptr(); // Fill in terms with zero matrices - std::vector > terms(this->size()); + std::vector > terms(this->size()); for(size_t j=0; jsize(); ++j) { terms[j].first = keys()[j]; terms[j].second = zeros(rowDim_, dims_[j]); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 111b1a180..d9fb91d61 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -197,7 +197,7 @@ public: const X2& x2 = c.at(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(this->noiseModel_); if (constrained.get() != NULL) { @@ -333,7 +333,7 @@ public: const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); - const Index var1 = ordering[key()]; + const Key var1 = ordering[key()]; SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != NULL) { diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index be4799aa2..48c349877 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -186,12 +186,12 @@ done: // Permuted permuted(permutation, values); // // // After permutation, the indices above the threshold are 2 and 2 -// FastSet expected; +// FastSet expected; // expected.insert(2); // expected.insert(3); // // // Indices checked by CheckRelinearization -// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); +// FastSet 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& leafKeys) { Matrix expectedAugmentedHessian, expected3AugmentedHessian; - vector toKeep; + vector toKeep; BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys) if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end()) toKeep.push_back(j);