diff --git a/gtsam/inference/VariableSlots.cpp b/gtsam/inference/VariableSlots.cpp index a06aad6cb..b1519c120 100644 --- a/gtsam/inference/VariableSlots.cpp +++ b/gtsam/inference/VariableSlots.cpp @@ -37,7 +37,7 @@ void VariableSlots::print(const std::string& str) const { for(size_t i=0; ibegin()->second.size(); ++i) { cout << " \t"; BOOST_FOREACH(const value_type& slot, *this) { - if(slot.second[i] == numeric_limits::max()) + if(slot.second[i] == numeric_limits::max()) cout << "x" << "\t"; else cout << slot.second[i] << "\t"; diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index e602736d4..ccef55296 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -51,11 +51,11 @@ namespace gtsam { * \nosubgrouping */ -class VariableSlots : public FastMap > { +class VariableSlots : public FastMap > { public: - typedef FastMap > Base; + typedef FastMap > Base; /// @name Standard Constructors /// @{ @@ -69,6 +69,7 @@ public: VariableSlots(const FG& factorGraph); /// @} + /// @name Testable /// @{ @@ -79,7 +80,6 @@ public: GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const; /// @} - }; /* ************************************************************************* */ @@ -96,7 +96,7 @@ VariableSlots::VariableSlots(const FG& factorGraph) { size_t jointFactorPos = 0; BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { assert(factor); - Index factorVarSlot = 0; + size_t factorVarSlot = 0; BOOST_FOREACH(const Index involvedVariable, *factor) { // Set the slot in this factor for this variable. If the // variable was not already discovered, create an array for it @@ -105,9 +105,9 @@ VariableSlots::VariableSlots(const FG& factorGraph) { // the array entry for each factor that will indicate the factor // does not involve the variable. iterator thisVarSlots; bool inserted; - boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector())); + boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector())); if(inserted) - thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits::max()); + thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits::max()); thisVarSlots->second[jointFactorPos] = factorVarSlot; if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; ++ factorVarSlot; diff --git a/gtsam/linear/GaussianConditionalUnordered.cpp b/gtsam/linear/GaussianConditionalUnordered.cpp index cb5cb34d4..512841e4d 100644 --- a/gtsam/linear/GaussianConditionalUnordered.cpp +++ b/gtsam/linear/GaussianConditionalUnordered.cpp @@ -37,19 +37,19 @@ namespace gtsam { /* ************************************************************************* */ GaussianConditionalUnordered::GaussianConditionalUnordered( - Index key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) : + Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) : BaseFactor(key, R, d, sigmas), BaseConditional(1) {} /* ************************************************************************* */ GaussianConditionalUnordered::GaussianConditionalUnordered( - Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, const SharedDiagonal& sigmas) : + Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, const SharedDiagonal& sigmas) : BaseFactor(key, R, name1, S, d, sigmas) {} /* ************************************************************************* */ GaussianConditionalUnordered::GaussianConditionalUnordered( - Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas) : + Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) : BaseFactor(key, R, name1, S, name2, T, d, sigmas) {} /* ************************************************************************* */ @@ -133,7 +133,7 @@ namespace gtsam { GaussianConditionalUnordered::const_iterator it; for (it = beginParents(); it!= endParents(); it++) { - const Index i = *it; + const Key i = *it; gtsam::transposeMultiplyAdd(-1.0, getA(it), frontalVec, gy[i]); } internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); diff --git a/gtsam/linear/GaussianConditionalUnordered.h b/gtsam/linear/GaussianConditionalUnordered.h index a7df7bfc1..0a01cf9b3 100644 --- a/gtsam/linear/GaussianConditionalUnordered.h +++ b/gtsam/linear/GaussianConditionalUnordered.h @@ -49,23 +49,23 @@ namespace gtsam { /** constructor with no parents * |Rx-d| */ - GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas); + GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas); /** constructor with only one parent * |Rx+Sy-d| */ - GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, const SharedDiagonal& sigmas); + GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, const SharedDiagonal& sigmas); /** constructor with two parents * |Rx+Sy+Tz-d| */ - GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, - Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas); + GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, + Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas); /** Constructor with number of arbitrary parents. \f$ |Rx+sum(Ai*xi)-d| \f$ * @tparam PARENTS A container whose value type is std::pair, specifying the * collection of parent keys and matrices. */ template - GaussianConditionalUnordered(Index key, const Vector& d, + GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas); /** Constructor with arbitrary number of frontals and parents. @@ -95,7 +95,7 @@ namespace gtsam { /** print */ void print(const std::string& = "GaussianConditional", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; /** equals function */ bool equals(const GaussianConditionalUnordered&cg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianFactorGraphUnordered.cpp b/gtsam/linear/GaussianFactorGraphUnordered.cpp index 6afeb8def..a515c0b96 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/GaussianFactorGraphUnordered.cpp @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const { - FastSet keys; + FastSet keys; BOOST_FOREACH(const sharedFactor& factor, *this) if (factor) keys.insert(factor->begin(), factor->end()); @@ -371,7 +371,7 @@ namespace gtsam { /* ************************************************************************* */ void residual(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) { - Index i = 0 ; + Key i = 0 ; BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); r[i] = Ai->getb(); @@ -385,7 +385,7 @@ namespace gtsam { /* ************************************************************************* */ void multiply(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) { r.setZero(); - Index i = 0; + Key i = 0; BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); Vector &y = r[i]; @@ -399,7 +399,7 @@ namespace gtsam { /* ************************************************************************* */ void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValues &r, VectorValues &x) { x.setZero(); - Index i = 0; + Key i = 0; BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp index b5693c99b..30ed6569d 100644 --- a/gtsam/linear/JacobianFactorUnordered.cpp +++ b/gtsam/linear/JacobianFactorUnordered.cpp @@ -74,14 +74,14 @@ namespace gtsam { } /* ************************************************************************* */ - JacobianFactorUnordered::JacobianFactorUnordered(Index i1, const Matrix& A1, + JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, const Vector& b, const SharedDiagonal& model) { fillTerms(cref_list_of<1>(make_pair(i1,A1)), b, model); } /* ************************************************************************* */ - JacobianFactorUnordered::JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, const Matrix& A2, + JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) { fillTerms(cref_list_of<2> @@ -90,8 +90,8 @@ namespace gtsam { } /* ************************************************************************* */ - JacobianFactorUnordered::JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, const Matrix& A2, - Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) + JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) { fillTerms(cref_list_of<3> (make_pair(i1,A1)) @@ -138,14 +138,14 @@ namespace gtsam { size_t m = 0; size_t n = 0; { - Index jointVarpos = 0; + size_t jointVarpos = 0; BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { assert(slots.second.size() == factors.size()); - Index sourceFactorI = 0; - BOOST_FOREACH(const Index sourceVarpos, slots.second) { - if(sourceVarpos < numeric_limits::max()) { + size_t sourceFactorI = 0; + BOOST_FOREACH(const size_t sourceVarpos, slots.second) { + if(sourceVarpos < numeric_limits::max()) { const JacobianFactorUnordered& sourceFactor = *factors[sourceFactorI]; size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS @@ -221,18 +221,18 @@ namespace gtsam { gttic(copy_blocks); // Loop over slots in combined factor - Index combinedSlot = 0; + size_t combinedSlot = 0; BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { JacobianFactorUnordered::ABlock destSlot(this->getA(this->begin()+combinedSlot)); // Loop over source jacobians size_t nextRow = 0; for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) { // Slot in source factor - const Index sourceSlot = varslot.second[factorI]; + const size_t sourceSlot = varslot.second[factorI]; const size_t sourceRows = jacobians[factorI]->rows(); JacobianFactorUnordered::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); // Copy if exists in source factor, otherwise set zero - if(sourceSlot != numeric_limits::max()) + if(sourceSlot != numeric_limits::max()) destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot); else destBlock.setZero(); diff --git a/gtsam/linear/JacobianFactorUnordered.h b/gtsam/linear/JacobianFactorUnordered.h index fca96f083..ccf2a6f0e 100644 --- a/gtsam/linear/JacobianFactorUnordered.h +++ b/gtsam/linear/JacobianFactorUnordered.h @@ -100,17 +100,17 @@ namespace gtsam { explicit JacobianFactorUnordered(const Vector& b_in); /** Construct unary factor */ - JacobianFactorUnordered(Index i1, const Matrix& A1, + JacobianFactorUnordered(Key i1, const Matrix& A1, const Vector& b, const SharedDiagonal& model); /** Construct binary factor */ - JacobianFactorUnordered(Index i1, const Matrix& A1, - Index i2, const Matrix& A2, + JacobianFactorUnordered(Key i1, const Matrix& A1, + Key i2, const Matrix& A2, const Vector& b, const SharedDiagonal& model); /** Construct ternary factor */ - JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, - const Matrix& A2, Index i3, const Matrix& A3, + JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model); /** Construct an n-ary factor