TypedLabeledSymbols now convert properly to Symbols, so they can be used to add a runtime label to a TypedKey to express "Pose 1 of robot A"

cpp/Key.h

@@ -111,13 +111,13 @@ namespace gtsam {
 
 		/**
 		 * Encoding two numbers into a single size_t for conversion to Symbol
-		 * Stores the label in the upper bits of the index
+		 * Stores the label in the upper bytes of the index
-		 *
-		 * STILL IN TESTING - DO NOT USE!!
 		 */
-		size_t encode() {
+		size_t encode() const {
-			//short label = label_;
+			short label = (short) label_; //bound size of label to 2 bytes
-			return this->j_; // + (label << 32);
+			size_t shift = (sizeof(size_t)-sizeof(short)) * 8;
+			size_t modifier = ((size_t) label) << shift;
+			return this->j_ + modifier;
 		}
 
 

cpp/testKey.cpp

@@ -54,27 +54,29 @@ TEST ( TypedLabledSymbol, basic_operations ) {
 	CHECK(key5 < key6);
 }
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 TEST ( TypedLabledSymbol, encoding ) {
 	typedef TypedLabeledSymbol<Pose3, 'x', char> RobotKey;
 
-	cout << "short : " << sizeof(short) << "  size_t: " << sizeof(size_t) << endl;
-	cout << "unsigned int : " << sizeof(unsigned int) << endl;
-
 	RobotKey key1(37, 'A');
 
-	size_t index = key1.index();
+	// Note: calculations done in test due to possible differences between machines
-	size_t modifier = 65;
+	// take the upper two bytes for the label
-	modifier = modifier << sizeof(size_t) * 4;
+	short label = key1.label();
-	index += modifier;
-	cout << "index: " << index << "  modifier: " << modifier << endl;
 
+	// find the shift necessary
+	size_t shift = (sizeof(size_t)-sizeof(short)) * 8;
+	size_t modifier = label;
+	modifier = modifier << shift;
+	size_t index = key1.index() + modifier;
 
-//	short encoded = 65;
+	// check index encoding
-//	size_t modifier = encoded << 32;
+	Symbol act1(key1), exp('x', index);
-//	size_t index = 37 + encoded;
+	CHECK(assert_equal(exp, act1));
-//	Symbol act(key1), exp('x', index);
+
-//	CHECK(assert_equal(exp, act));
+	// check casting
+	Symbol act2 = (Symbol) key1;
+	CHECK(assert_equal(exp, act2));
 }