Fixed compile problem with gcc and bug fix in int key <-> Symbol encoding

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

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) :

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -144,8 +144,8 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
     Index nextVar = originalnVars;
     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
       delta.permutation()[nextVar] = nextVar;
-      ordering.insert(key_value.first, nextVar);
+      ordering.insert(key_value.key, nextVar);
-      if(debug) cout << "Adding variable " << keyFormatter(key_value.first) << " with order " << nextVar << endl;
+      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<CONDITIONAL, GRAPH>::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<double>).all());
     if(mask[var]) {
-      Value* retracted = key_value->second.retract_(delta[var]);
+      Value* retracted = key_value->value.retract_(delta[var]);
-      key_value->second = *retracted;
+      key_value->value = *retracted;
       retracted->deallocate_();
       if(invalidateIfDebug)
         (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)

gtsam/nonlinear/Symbol.h

@@ -23,6 +23,10 @@
 #include <boost/mpl/char.hpp>
 #include <boost/format.hpp>
 #include <boost/serialization/nvp.hpp>
+#include <boost/function.hpp>
+#include <boost/lambda/bind.hpp>
+#include <boost/lambda/construct.hpp>
+#include <boost/lambda/lambda.hpp>
 
 #include <gtsam/nonlinear/Key.h>
 
@@ -57,25 +61,25 @@ public:
 
   /** Constructor that decodes an integer Key */
   Symbol(Key key) {
-    const size_t keyBytes = sizeof(Key);
+    const size_t keyBits = sizeof(Key) * 8;
-    const size_t chrBytes = sizeof(unsigned char);
+    const size_t chrBits = sizeof(unsigned char) * 8;
-    const size_t indexBytes = keyBytes - chrBytes;
+    const size_t indexBits = keyBits - chrBits;
-    const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
+    const Key chrMask = Key(std::numeric_limits<unsigned char>::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 keyBits = sizeof(Key) * 8;
-    const size_t chrBytes = sizeof(unsigned char);
+    const size_t chrBits = sizeof(unsigned char) * 8;
-    const size_t indexBytes = keyBytes - chrBytes;
+    const size_t indexBits = keyBits - chrBits;
-    const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
+    const Key chrMask = Key(std::numeric_limits<unsigned char>::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<bool(Key)> ChrTest(unsigned char c) {
+    namespace bl = boost::lambda;
+    return bl::bind(&Symbol::chr, bl::bind(bl::constructor<Symbol>(), bl::_1)) == c;
+  }
+
 private:
 
   /** Serialization function */

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) << ": ";
+      cout << "  " << keyFormatter(key_value->key) << ": ";
-      key_value->second.print("");
+      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
+      const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
-      Key key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
+      Key key = key_value->key;  // Non-const duplicate to deal with non-const insert argument
-      Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
+      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
+      Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
-      insert(key, key_value->second);
+      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<Key> Values::keys() const {
+  std::list<Key> Values::keys() const {
-    FastList<Key> result;
+    std::list<Key> result;
-    for(const_iterator key_value = begin(); key_value != end(); ++key_value)
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      result.push_back(key_value->first);
+      result.push_back(key_value->key);
+      cout << result.back() << endl;
+    }
     return result;
   }
 
@@ -170,7 +172,7 @@ namespace gtsam {
   vector<size_t> Values::dims(const Ordering& ordering) const {
     vector<size_t> 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;
   }

gtsam/nonlinear/Values.h

@@ -87,10 +87,21 @@ namespace gtsam {
     typedef boost::shared_ptr<Values> shared_ptr;
 
     /// A key-value pair, which you get by dereferencing iterators
-    typedef std::pair<const Key, Value&> 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<const Key, const Value&> 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<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
 
+  private:
     template<class ValueType>
+    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<class ValueType>
+    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 ValueType = Value>
     class Filtered : public boost::transformed_range<
-    std::pair<const Key, ValueType&>(*)(KeyValuePair key_value),
+    _KeyValuePair<ValueType>(*)(Values::KeyValuePair key_value),
     const boost::filtered_range<
     boost::function<bool(const ConstKeyValuePair&)>,
     const boost::iterator_range<iterator> > > {
+    public:
+      typedef _KeyValuePair<ValueType> KeyValuePair;
+
     private:
       typedef boost::transformed_range<
-          std::pair<const Key, ValueType&>(*)(KeyValuePair key_value),
+          KeyValuePair(*)(Values::KeyValuePair key_value),
           const boost::filtered_range<
-          boost::function<bool(const ConstKeyValuePair&)>,
+          boost::function<bool(const Values::ConstKeyValuePair&)>,
           const boost::iterator_range<iterator> > > Base;
 
       Filtered(const Base& base) : Base(base) {}
@@ -126,17 +162,20 @@ namespace gtsam {
       friend class Values;
     };
 
-    template<class ValueType>
+    template<class ValueType = Value>
     class ConstFiltered : public boost::transformed_range<
-    std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair key_value),
+    _ConstKeyValuePair<ValueType>(*)(Values::ConstKeyValuePair key_value),
     const boost::filtered_range<
     boost::function<bool(const ConstKeyValuePair&)>,
     const boost::iterator_range<const_iterator> > > {
+    public:
+      typedef _ConstKeyValuePair<ValueType> KeyValuePair;
+
     private:
       typedef boost::transformed_range<
-          std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair key_value),
+          KeyValuePair(*)(Values::ConstKeyValuePair key_value),
           const boost::filtered_range<
-          boost::function<bool(const ConstKeyValuePair&)>,
+          boost::function<bool(const Values::ConstKeyValuePair&)>,
           const boost::iterator_range<const_iterator> > > 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<Key> keys() const;
+    std::list<Key> 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<bool(const ConstKeyValuePair&)>(
                   boost::bind(&filterHelper<ValueType>, filterFcn, _1))),
-          &castHelper<ValueType, KeyValuePair>);
+          &castHelper<ValueType, _KeyValuePair<ValueType>, KeyValuePair>);
     }
 
     /**
@@ -371,7 +410,7 @@ namespace gtsam {
               boost::make_iterator_range(begin(), end()),
               boost::function<bool(const ConstKeyValuePair&)>(
                   boost::bind(&filterHelper<ValueType>, filterFcn, _1))),
-          &castHelper<const ValueType, ConstKeyValuePair>);
+          &castHelper<const ValueType, _ConstKeyValuePair<ValueType>, ConstKeyValuePair>);
     }
 
   private:
@@ -380,14 +419,14 @@ namespace gtsam {
     template<class ValueType>
     static bool filterHelper(const boost::function<bool(Key)> 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<class ValueType, class KeyValuePairType>
+    template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
-    static std::pair<const Key, ValueType&> castHelper(KeyValuePairType key_value) {
+    static CastedKeyValuePairType castHelper(KeyValuePairType key_value) {
       // Static cast because we already checked the type during filtering
-      return std::pair<const Key, ValueType&>(key_value.first, static_cast<ValueType&>(key_value.second));
+      return CastedKeyValuePairType(key_value.key, static_cast<ValueType&>(key_value.value));
     }
 
     /** Serialization function */

gtsam/nonlinear/tests/testKey.cpp

@@ -19,6 +19,7 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/nonlinear/Symbol.h>
 	
 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); }
 /* ************************************************************************* */

gtsam/nonlinear/tests/testValues.cpp

@@ -31,7 +31,7 @@ using namespace gtsam;
 using namespace std;
 static double inf = std::numeric_limits<double>::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<Key> expected, actual;
+	std::list<Key> expected, actual;
 	expected += key1, key2, key3, key4;
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
-	FastList<Key>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	std::list<Key>::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<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
-  BOOST_FOREACH(const Values::Filtered<Value>::value_type& key_value, filtered) {
+  BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) {
     if(i == 0) {
-      LONGS_EQUAL(2, key_value.first);
+      LONGS_EQUAL(2, key_value.key);
-      EXPECT(typeid(Pose2) == typeid(key_value.second));
+      EXPECT(typeid(Pose2) == typeid(key_value.value));
-      EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(key_value.second)));
+      EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(key_value.value)));
     } else if(i == 1) {
-      LONGS_EQUAL(3, key_value.first);
+      LONGS_EQUAL(3, key_value.key);
-      EXPECT(typeid(Pose3) == typeid(key_value.second));
+      EXPECT(typeid(Pose3) == typeid(key_value.value));
-      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.second)));
+      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
     } else {
       EXPECT(false);
     }
@@ -292,13 +292,42 @@ TEST(Values, filter) {
 
   // Filter by type
   i = 0;
-  BOOST_FOREACH(const Values::Filtered<Pose3>::value_type& key_value, values.filter<Pose3>()) {
+  BOOST_FOREACH(const Values::Filtered<Pose3>::KeyValuePair& key_value, values.filter<Pose3>()) {
     if(i == 0) {
-      LONGS_EQUAL(1, key_value.first);
+      LONGS_EQUAL(1, key_value.key);
-      EXPECT(assert_equal(pose1, key_value.second));
+      EXPECT(assert_equal(pose1, key_value.value));
     } else if(i == 1) {
-      LONGS_EQUAL(3, key_value.first);
+      LONGS_EQUAL(3, key_value.key);
-      EXPECT(assert_equal(pose3, key_value.second));
+      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<Value>::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<const Pose3&>(key_value.value)));
+    } else if(i == 1) {
+      LONGS_EQUAL(Symbol('y',3), key_value.key);
+      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
     } else {
       EXPECT(false);
     }

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<const Pose2&>(key_value.second);
+	  const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
-		stream << "VERTEX2 " << key_value.first << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
+		stream << "VERTEX2 " << key_value.key << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
 	}
 
 	// save edges