From 12775256c1873e4b4fc67ecb82cfb6a1184a28b9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 24 Feb 2012 23:38:51 +0000 Subject: [PATCH] Fixed compile problem with gcc and bug fix in int key <-> Symbol encoding --- gtsam/base/FastList.h | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/nonlinear/ISAM2-impl-inl.h | 16 +++---- gtsam/nonlinear/Symbol.h | 34 +++++++++---- gtsam/nonlinear/Values.cpp | 42 ++++++++-------- gtsam/nonlinear/Values.h | 71 +++++++++++++++++++++------- gtsam/nonlinear/tests/testKey.cpp | 25 ++++++++++ gtsam/nonlinear/tests/testValues.cpp | 61 +++++++++++++++++------- gtsam/slam/dataset.cpp | 4 +- 9 files changed, 183 insertions(+), 74 deletions(-) diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 02f8734f6..60452357f 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -26,7 +26,7 @@ namespace gtsam { /** - * FastSet is a thin wrapper around std::set that uses the boost + * FastList is a thin wrapper around std::list that uses the boost * fast_pool_allocator instead of the default STL allocator. This is just a * convenience to avoid having lengthy types in the code. Through timing, * we've seen that the fast_pool_allocator can lead to speedups of several diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fd4bc08df..2d7e42363 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -62,7 +62,7 @@ namespace gtsam { Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} /** Construct from Pose2 */ - Pose3(const Pose2& pose2); + explicit Pose3(const Pose2& pose2); /** Constructor from 4*4 matrix */ Pose3(const Matrix &T) : diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index c2483488c..c7224884e 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -144,8 +144,8 @@ void ISAM2::Impl::AddVariables( Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { delta.permutation()[nextVar] = nextVar; - ordering.insert(key_value.first, nextVar); - if(debug) cout << "Adding variable " << keyFormatter(key_value.first) << " with order " << nextVar << endl; + ordering.insert(key_value.key, nextVar); + if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; ++ nextVar; } assert(delta.permutation().size() == delta.container().size()); @@ -234,19 +234,19 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permute Ordering::const_iterator key_index; for(key_value = values.begin(), key_index = ordering.begin(); key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) { - assert(key_value->first == key_index->first); + assert(key_value->key == key_index->first); const Index var = key_index->second; if(ISDEBUG("ISAM2 update verbose")) { if(mask[var]) - cout << "expmap " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + cout << "expmap " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; else - cout << " " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + cout << " " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; } - assert(delta[var].size() == (int)key_value->second.dim()); + assert(delta[var].size() == (int)key_value->value.dim()); assert(delta[var].unaryExpr(&isfinite).all()); if(mask[var]) { - Value* retracted = key_value->second.retract_(delta[var]); - key_value->second = *retracted; + Value* retracted = key_value->value.retract_(delta[var]); + key_value->value = *retracted; retracted->deallocate_(); if(invalidateIfDebug) (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index 79d6a049b..945044492 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -23,6 +23,10 @@ #include #include #include +#include +#include +#include +#include #include @@ -57,25 +61,25 @@ public: /** Constructor that decodes an integer Key */ Symbol(Key key) { - const size_t keyBytes = sizeof(Key); - const size_t chrBytes = sizeof(unsigned char); - const size_t indexBytes = keyBytes - chrBytes; - const Key chrMask = std::numeric_limits::max() << indexBytes; + const size_t keyBits = sizeof(Key) * 8; + const size_t chrBits = sizeof(unsigned char) * 8; + const size_t indexBits = keyBits - chrBits; + const Key chrMask = Key(std::numeric_limits::max()) << indexBits; const Key indexMask = ~chrMask; - c_ = (key & chrMask) >> indexBytes; + c_ = (unsigned char)((key & chrMask) >> indexBits); j_ = key & indexMask; } /** Cast to integer */ operator Key() const { - const size_t keyBytes = sizeof(Key); - const size_t chrBytes = sizeof(unsigned char); - const size_t indexBytes = keyBytes - chrBytes; - const Key chrMask = std::numeric_limits::max() << indexBytes; + const size_t keyBits = sizeof(Key) * 8; + const size_t chrBits = sizeof(unsigned char) * 8; + const size_t indexBits = keyBits - chrBits; + const Key chrMask = Key(std::numeric_limits::max()) << indexBits; const Key indexMask = ~chrMask; if(j_ > indexMask) throw std::invalid_argument("Symbol index is too large"); - Key key = (c_ << indexBytes) | j_; + Key key = (Key(c_) << indexBits) | j_; return key; } @@ -113,6 +117,16 @@ public: return comp.c_ != c_ || comp.j_ != j_; } + /** Return a filter function that returns true when evaluated on a Key whose + * character (when converted to a Symbol) matches \c c. Use this with the + * Values::filter() function to retrieve all key-value pairs with the + * requested character. + */ + static boost::function ChrTest(unsigned char c) { + namespace bl = boost::lambda; + return bl::bind(&Symbol::chr, bl::bind(bl::constructor(), bl::_1)) == c; + } + private: /** Serialization function */ diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 00b42730b..6fd99fc76 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -43,8 +43,8 @@ namespace gtsam { void Values::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str << "Values with " << size() << " values:\n" << endl; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - cout << " " << keyFormatter(key_value->first) << ": "; - key_value->second.print(""); + cout << " " << keyFormatter(key_value->key) << ": "; + key_value->value.print(""); } } @@ -53,11 +53,11 @@ namespace gtsam { if(this->size() != other.size()) return false; for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { - if(typeid(it1->second) != typeid(it2->second)) + if(typeid(it1->value) != typeid(it2->value)) return false; - if(it1->first != it2->first) + if(it1->key != it2->key) return false; - if(!it1->second.equals_(it2->second, tol)) + if(!it1->value.equals_(it2->value, tol)) return false; } return true; // We return false earlier if we find anything that does not match @@ -78,9 +78,9 @@ namespace gtsam { Values result; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value - Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument - Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract + const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value + Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument + Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract result.values_.insert(key, retractedValue); // Add retracted result directly to result values } @@ -99,10 +99,10 @@ namespace gtsam { if(this->size() != cp.size()) throw DynamicValuesMismatched(); for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { - if(it1->first != it2->first) + if(it1->key != it2->key) throw DynamicValuesMismatched(); // If keys do not match // Will throw a dynamic_cast exception if types do not match - result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second)); + result.insert(ordering[it1->key], it1->value.localCoordinates_(it2->value)); } } @@ -117,8 +117,8 @@ namespace gtsam { /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { - Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument - insert(key, key_value->second); + Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument + insert(key, key_value->value); } } @@ -139,7 +139,7 @@ namespace gtsam { /* ************************************************************************* */ void Values::update(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { - this->update(key_value->first, key_value->second); + this->update(key_value->key, key_value->value); } } @@ -152,10 +152,12 @@ namespace gtsam { } /* ************************************************************************* */ - FastList Values::keys() const { - FastList result; - for(const_iterator key_value = begin(); key_value != end(); ++key_value) - result.push_back(key_value->first); + std::list Values::keys() const { + std::list result; + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { + result.push_back(key_value->key); + cout << result.back() << endl; + } return result; } @@ -170,7 +172,7 @@ namespace gtsam { vector Values::dims(const Ordering& ordering) const { vector result(values_.size()); BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { - result[ordering[key_value.first]] = key_value.second.dim(); + result[ordering[key_value.key]] = key_value.value.dim(); } return result; } @@ -179,7 +181,7 @@ namespace gtsam { size_t Values::dim() const { size_t result = 0; BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { - result += key_value.second.dim(); + result += key_value.value.dim(); } return result; } @@ -188,7 +190,7 @@ namespace gtsam { Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { Ordering::shared_ptr ordering(new Ordering); for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - ordering->insert(key_value->first, firstVar++); + ordering->insert(key_value->key, firstVar++); } return ordering; } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 745498c29..abf0d3f8c 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -87,10 +87,21 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// A key-value pair, which you get by dereferencing iterators - typedef std::pair KeyValuePair; + struct KeyValuePair { + const Key key; ///< The key + Value& value; ///< The value + + KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {} + }; /// A key-value pair, which you get by dereferencing iterators - typedef std::pair ConstKeyValuePair; + struct ConstKeyValuePair { + const Key key; ///< The key + const Value& value; ///< The value + + ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {} + ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {} + }; /// Mutable forward iterator, with value type KeyValuePair typedef boost::transform_iterator< @@ -108,17 +119,42 @@ namespace gtsam { typedef boost::transform_iterator< boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + private: template + struct _KeyValuePair { + const Key key; ///< The key + ValueType& value; ///< The value + const Key& first; ///< For std::pair compatibility, the key + ValueType& second; ///< For std::pair compatibility, the value + + _KeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value), first(key), second(value) {} + }; + + template + struct _ConstKeyValuePair { + const Key key; ///< The key + const ValueType& value; ///< The value + const Key& first; ///< For std::pair compatibility, the key + const ValueType& second; ///< For std::pair compatibility, the value + + _ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value), first(key), second(value) {} + }; + + public: + template class Filtered : public boost::transformed_range< - std::pair(*)(KeyValuePair key_value), + _KeyValuePair(*)(Values::KeyValuePair key_value), const boost::filtered_range< boost::function, const boost::iterator_range > > { + public: + typedef _KeyValuePair KeyValuePair; + private: typedef boost::transformed_range< - std::pair(*)(KeyValuePair key_value), + KeyValuePair(*)(Values::KeyValuePair key_value), const boost::filtered_range< - boost::function, + boost::function, const boost::iterator_range > > Base; Filtered(const Base& base) : Base(base) {} @@ -126,17 +162,20 @@ namespace gtsam { friend class Values; }; - template + template class ConstFiltered : public boost::transformed_range< - std::pair(*)(ConstKeyValuePair key_value), + _ConstKeyValuePair(*)(Values::ConstKeyValuePair key_value), const boost::filtered_range< boost::function, const boost::iterator_range > > { + public: + typedef _ConstKeyValuePair KeyValuePair; + private: typedef boost::transformed_range< - std::pair(*)(ConstKeyValuePair key_value), + KeyValuePair(*)(Values::ConstKeyValuePair key_value), const boost::filtered_range< - boost::function, + boost::function, const boost::iterator_range > > Base; ConstFiltered(const Base& base) : Base(base) {} @@ -257,7 +296,7 @@ namespace gtsam { * Returns a set of keys in the config * Note: by construction, the list is ordered */ - FastList keys() const; + std::list keys() const; /** Replace all keys and variables */ Values& operator=(const Values& rhs); @@ -324,7 +363,7 @@ namespace gtsam { boost::make_iterator_range(begin(), end()), boost::function( boost::bind(&filterHelper, filterFcn, _1))), - &castHelper); + &castHelper, KeyValuePair>); } /** @@ -371,7 +410,7 @@ namespace gtsam { boost::make_iterator_range(begin(), end()), boost::function( boost::bind(&filterHelper, filterFcn, _1))), - &castHelper); + &castHelper, ConstKeyValuePair>); } private: @@ -380,14 +419,14 @@ namespace gtsam { template static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { // Filter and check the type - return filter(key_value.first) && (typeid(ValueType) == typeid(key_value.second) || typeid(ValueType) == typeid(Value)); + return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value)); } // Cast to the derived ValueType - template - static std::pair castHelper(KeyValuePairType key_value) { + template + static CastedKeyValuePairType castHelper(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - return std::pair(key_value.first, static_cast(key_value.second)); + return CastedKeyValuePairType(key_value.key, static_cast(key_value.value)); } /** Serialization function */ diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp index 78688192f..397bf9958 100644 --- a/gtsam/nonlinear/tests/testKey.cpp +++ b/gtsam/nonlinear/tests/testKey.cpp @@ -19,6 +19,7 @@ using namespace boost::assign; #include #include +#include #include using namespace std; @@ -33,6 +34,30 @@ TEST(Key, KeySymbolConversion) { EXPECT(assert_equal(expected, actual)) } +/* ************************************************************************* */ +TEST(Key, KeySymbolEncoding) { + + // Test encoding of Symbol <-> size_t <-> string + + if(sizeof(Key) == 8) { + Symbol symbol(0x61, 5); + Key key = 0x6100000000000005; + string str = "a5"; + + LONGS_EQUAL(key, (Key)symbol); + assert_equal(str, DefaultKeyFormatter(symbol)); + assert_equal(symbol, Symbol(key)); + } else if(sizeof(Key) == 4) { + Symbol symbol(0x61, 5); + Key key = 0x61000005; + string str = "a5"; + + LONGS_EQUAL(key, (Key)symbol); + assert_equal(str, DefaultKeyFormatter(symbol)); + assert_equal(symbol, Symbol(key)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index bdf3e1191..99973aa0f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -31,7 +31,7 @@ using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); -Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4)); +const Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4)); /* ************************************************************************* */ TEST( Values, equals1 ) @@ -218,14 +218,14 @@ TEST(Values, extract_keys) config.insert(key3, Pose2()); config.insert(key4, Pose2()); - FastList expected, actual; + std::list expected, actual; expected += key1, key2, key3, key4; actual = config.keys(); CHECK(actual.size() == expected.size()); - FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); + std::list::const_iterator itAct = actual.begin(), itExp = expected.begin(); for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { - LONGS_EQUAL(*itExp, *itAct); + EXPECT(*itExp == *itAct); } } @@ -274,15 +274,15 @@ TEST(Values, filter) { // Filter by key int i = 0; Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); - BOOST_FOREACH(const Values::Filtered::value_type& key_value, filtered) { + BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { if(i == 0) { - LONGS_EQUAL(2, key_value.first); - EXPECT(typeid(Pose2) == typeid(key_value.second)); - EXPECT(assert_equal(pose2, dynamic_cast(key_value.second))); + LONGS_EQUAL(2, key_value.key); + EXPECT(typeid(Pose2) == typeid(key_value.value)); + EXPECT(assert_equal(pose2, dynamic_cast(key_value.value))); } else if(i == 1) { - LONGS_EQUAL(3, key_value.first); - EXPECT(typeid(Pose3) == typeid(key_value.second)); - EXPECT(assert_equal(pose3, dynamic_cast(key_value.second))); + LONGS_EQUAL(3, key_value.key); + EXPECT(typeid(Pose3) == typeid(key_value.value)); + EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); } else { EXPECT(false); } @@ -292,13 +292,42 @@ TEST(Values, filter) { // Filter by type i = 0; - BOOST_FOREACH(const Values::Filtered::value_type& key_value, values.filter()) { + BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter()) { if(i == 0) { - LONGS_EQUAL(1, key_value.first); - EXPECT(assert_equal(pose1, key_value.second)); + LONGS_EQUAL(1, key_value.key); + EXPECT(assert_equal(pose1, key_value.value)); } else if(i == 1) { - LONGS_EQUAL(3, key_value.first); - EXPECT(assert_equal(pose3, key_value.second)); + LONGS_EQUAL(3, key_value.key); + EXPECT(assert_equal(pose3, key_value.value)); + } else { + EXPECT(false); + } + ++ i; + } + LONGS_EQUAL(2, i); +} + +/* ************************************************************************* */ +TEST(Values, Symbol_filter) { + Pose2 pose0(1.0, 2.0, 0.3); + Pose3 pose1(Pose2(0.1, 0.2, 0.3)); + Pose2 pose2(4.0, 5.0, 0.6); + Pose3 pose3(Pose2(0.3, 0.7, 0.9)); + + Values values; + values.insert(Symbol('x',0), pose0); + values.insert(Symbol('y',1), pose1); + values.insert(Symbol('x',2), pose2); + values.insert(Symbol('y',3), pose3); + + int i = 0; + BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { + if(i == 0) { + LONGS_EQUAL(Symbol('y',1), key_value.key); + EXPECT(assert_equal(pose1, dynamic_cast(key_value.value))); + } else if(i == 1) { + LONGS_EQUAL(Symbol('y',3), key_value.key); + EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); } else { EXPECT(false); } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index dae437b79..f45f560a7 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -155,8 +155,8 @@ void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDia // save poses BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { - const Pose2& pose = dynamic_cast(key_value.second); - stream << "VERTEX2 " << key_value.first << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + const Pose2& pose = dynamic_cast(key_value.value); + stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } // save edges