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

Fixed size_t/Key/Index types

release/4.3a0
Richard Roberts 2013-07-01 20:19:31 +00:00
parent 878f87fd25
commit 5cc13abbf1
7 changed files with 40 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/inference/VariableSlots.cpp
View File

@ -37,7 +37,7 @@ void VariableSlots::print(const std::string& str) const {
for(size_t i=0; i<this->begin()->second.size(); ++i) { for(size_t i=0; i<this->begin()->second.size(); ++i) {
cout << " \t"; cout << " \t";
BOOST_FOREACH(const value_type& slot, *this) { BOOST_FOREACH(const value_type& slot, *this) {
if(slot.second[i] == numeric_limits<Index>::max()) if(slot.second[i] == numeric_limits<size_t>::max())
cout << "x" << "\t"; cout << "x" << "\t";
else else
cout << slot.second[i] << "\t"; cout << slot.second[i] << "\t";

12
gtsam/inference/VariableSlots.h
View File

@ -51,11 +51,11 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
class VariableSlots : public FastMap<Index, std::vector<Index> > { class VariableSlots : public FastMap<Index, std::vector<size_t> > {
public: public:
typedef FastMap<Index, std::vector<Index> > Base; typedef FastMap<Index, std::vector<size_t> > Base;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -69,6 +69,7 @@ public:
VariableSlots(const FG& factorGraph); VariableSlots(const FG& factorGraph);
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -79,7 +80,6 @@ public:
GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const; 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; size_t jointFactorPos = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
assert(factor); assert(factor);
Index factorVarSlot = 0; size_t factorVarSlot = 0;
BOOST_FOREACH(const Index involvedVariable, *factor) { BOOST_FOREACH(const Index involvedVariable, *factor) {
// Set the slot in this factor for this variable. If the // Set the slot in this factor for this variable. If the
// variable was not already discovered, create an array for it // 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 // the array entry for each factor that will indicate the factor
// does not involve the variable. // does not involve the variable.
iterator thisVarSlots; bool inserted; iterator thisVarSlots; bool inserted;
boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector<Index>())); boost::tie(thisVarSlots, inserted) = this->insert(make_pair(involvedVariable, std::vector<size_t>()));
if(inserted) if(inserted)
thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<Index>::max()); thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<size_t>::max());
thisVarSlots->second[jointFactorPos] = factorVarSlot; thisVarSlots->second[jointFactorPos] = factorVarSlot;
if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
++ factorVarSlot; ++ factorVarSlot;

12
gtsam/linear/GaussianConditionalUnordered.cpp
View File

@ -37,19 +37,19 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditionalUnordered::GaussianConditionalUnordered( 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) {} BaseFactor(key, R, d, sigmas), BaseConditional(1) {}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditionalUnordered::GaussianConditionalUnordered( GaussianConditionalUnordered::GaussianConditionalUnordered(
Index key, const Vector& d, const Matrix& R, Key key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const SharedDiagonal& sigmas) : Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
BaseFactor(key, R, name1, S, d, sigmas) {} BaseFactor(key, R, name1, S, d, sigmas) {}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditionalUnordered::GaussianConditionalUnordered( GaussianConditionalUnordered::GaussianConditionalUnordered(
Index key, const Vector& d, const Matrix& R, Key key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas) : Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
BaseFactor(key, R, name1, S, name2, T, d, sigmas) {} BaseFactor(key, R, name1, S, name2, T, d, sigmas) {}
/* ************************************************************************* */ /* ************************************************************************* */
@ -133,7 +133,7 @@ namespace gtsam {
GaussianConditionalUnordered::const_iterator it; GaussianConditionalUnordered::const_iterator it;
for (it = beginParents(); it!= endParents(); it++) { for (it = beginParents(); it!= endParents(); it++) {
const Index i = *it; const Key i = *it;
gtsam::transposeMultiplyAdd(-1.0, getA(it), frontalVec, gy[i]); gtsam::transposeMultiplyAdd(-1.0, getA(it), frontalVec, gy[i]);
} }
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());

14
gtsam/linear/GaussianConditionalUnordered.h
View File

@ -49,23 +49,23 @@ namespace gtsam {
/** constructor with no parents /** constructor with no parents
* |Rx-d| * |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 /** constructor with only one parent
* |Rx+Sy-d| */ * |Rx+Sy-d| */
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const SharedDiagonal& sigmas); Key name1, const Matrix& S, const SharedDiagonal& sigmas);
/** constructor with two parents /** constructor with two parents
* |Rx+Sy+Tz-d| */ * |Rx+Sy+Tz-d| */
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas); 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$ /** Constructor with number of arbitrary parents. \f$ |Rx+sum(Ai*xi)-d| \f$
* @tparam PARENTS A container whose value type is std::pair<Key, Matrix>, specifying the * @tparam PARENTS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of parent keys and matrices. */ * collection of parent keys and matrices. */
template<typename PARENTS> template<typename PARENTS>
GaussianConditionalUnordered(Index key, const Vector& d, GaussianConditionalUnordered(Key key, const Vector& d,
const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas); const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas);
/** Constructor with arbitrary number of frontals and parents. /** Constructor with arbitrary number of frontals and parents.
@ -95,7 +95,7 @@ namespace gtsam {
/** print */ /** print */
void print(const std::string& = "GaussianConditional", void print(const std::string& = "GaussianConditional",
const IndexFormatter& formatter = DefaultIndexFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** equals function */ /** equals function */
bool equals(const GaussianConditionalUnordered&cg, double tol = 1e-9) const; bool equals(const GaussianConditionalUnordered&cg, double tol = 1e-9) const;

8
gtsam/linear/GaussianFactorGraphUnordered.cpp
View File

@ -30,7 +30,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const { GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const {
FastSet<Index> keys; FastSet<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) if (factor)
keys.insert(factor->begin(), factor->end()); keys.insert(factor->begin(), factor->end());
@ -371,7 +371,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void residual(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) { 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) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
r[i] = Ai->getb(); r[i] = Ai->getb();
@ -385,7 +385,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void multiply(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) { void multiply(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) {
r.setZero(); r.setZero();
Index i = 0; Key i = 0;
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Vector &y = r[i]; Vector &y = r[i];
@ -399,7 +399,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValues &r, VectorValues &x) { void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValues &r, VectorValues &x) {
x.setZero(); x.setZero();
Index i = 0; Key i = 0;
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {

22
gtsam/linear/JacobianFactorUnordered.cpp
View File

@ -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) const Vector& b, const SharedDiagonal& model)
{ {
fillTerms(cref_list_of<1>(make_pair(i1,A1)), b, 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) const Vector& b, const SharedDiagonal& model)
{ {
fillTerms(cref_list_of<2> fillTerms(cref_list_of<2>
@ -90,8 +90,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactorUnordered::JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, const Matrix& A2, JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model)
{ {
fillTerms(cref_list_of<3> fillTerms(cref_list_of<3>
(make_pair(i1,A1)) (make_pair(i1,A1))
@ -138,14 +138,14 @@ namespace gtsam {
size_t m = 0; size_t m = 0;
size_t n = 0; size_t n = 0;
{ {
Index jointVarpos = 0; size_t jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size()); assert(slots.second.size() == factors.size());
Index sourceFactorI = 0; size_t sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) { BOOST_FOREACH(const size_t sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) { if(sourceVarpos < numeric_limits<size_t>::max()) {
const JacobianFactorUnordered& sourceFactor = *factors[sourceFactorI]; const JacobianFactorUnordered& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
@ -221,18 +221,18 @@ namespace gtsam {
gttic(copy_blocks); gttic(copy_blocks);
// Loop over slots in combined factor // Loop over slots in combined factor
Index combinedSlot = 0; size_t combinedSlot = 0;
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) { BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
JacobianFactorUnordered::ABlock destSlot(this->getA(this->begin()+combinedSlot)); JacobianFactorUnordered::ABlock destSlot(this->getA(this->begin()+combinedSlot));
// Loop over source jacobians // Loop over source jacobians
size_t nextRow = 0; size_t nextRow = 0;
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) { for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
// Slot in source factor // Slot in source factor
const Index sourceSlot = varslot.second[factorI]; const size_t sourceSlot = varslot.second[factorI];
const size_t sourceRows = jacobians[factorI]->rows(); const size_t sourceRows = jacobians[factorI]->rows();
JacobianFactorUnordered::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); JacobianFactorUnordered::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
// Copy if exists in source factor, otherwise set zero // Copy if exists in source factor, otherwise set zero
if(sourceSlot != numeric_limits<Index>::max()) if(sourceSlot != numeric_limits<size_t>::max())
destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot); destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot);
else else
destBlock.setZero(); destBlock.setZero();

10
gtsam/linear/JacobianFactorUnordered.h
View File

@ -100,17 +100,17 @@ namespace gtsam {
explicit JacobianFactorUnordered(const Vector& b_in); explicit JacobianFactorUnordered(const Vector& b_in);
/** Construct unary factor */ /** Construct unary factor */
JacobianFactorUnordered(Index i1, const Matrix& A1, JacobianFactorUnordered(Key i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model); const Vector& b, const SharedDiagonal& model);
/** Construct binary factor */ /** Construct binary factor */
JacobianFactorUnordered(Index i1, const Matrix& A1, JacobianFactorUnordered(Key i1, const Matrix& A1,
Index i2, const Matrix& A2, Key i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model); const Vector& b, const SharedDiagonal& model);
/** Construct ternary factor */ /** Construct ternary factor */
JacobianFactorUnordered(Index i1, const Matrix& A1, Index i2, JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2,
const Matrix& A2, Index i3, const Matrix& A3, const Matrix& A2, Key i3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model); const Vector& b, const SharedDiagonal& model);
/** Construct an n-ary factor /** Construct an n-ary factor