dimensions implemented and tested

gtsam_unstable/nonlinear/Expression-inl.h

@@ -336,6 +336,12 @@ public:
     return keys;
   }
 
+  /// Return dimensions for each argument
+  virtual std::map<Key,size_t> dimensions() const {
+    std::map<Key,size_t> map;
+    return map;
+  }
+
   // Return size needed for memory buffer in traceExecution
   size_t traceSize() const {
     return traceSize_;
@@ -410,6 +416,13 @@ public:
     return keys;
   }
 
+  /// Return dimensions for each argument
+  virtual std::map<Key,size_t> dimensions() const {
+    std::map<Key,size_t> map;
+    map[key_] = T::dimension;
+    return map;
+  }
+
   /// Return value
   virtual T value(const Values& values) const {
     return values.at<T>(key_);
@@ -526,6 +539,14 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
     return keys;
   }
 
+  /// Return dimensions for each argument
+  virtual std::map<Key,size_t> dimensions() const {
+    std::map<Key,size_t> map = Base::dimensions();
+    std::map<Key,size_t> myMap = This::expression->dimensions();
+    map.insert(myMap.begin(), myMap.end());
+    return map;
+  }
+
   /// Recursive Record Class for Functional Expressions
   struct Record: JacobianTrace<T, A, N>, Base::Record {
 

gtsam_unstable/nonlinear/Expression.h

@@ -107,6 +107,11 @@ public:
     return root_->keys();
   }
 
+  /// Return dimensions for each argument, as a map (allows order to change later)
+  std::map<Key,size_t> dimensions() const {
+    return root_->dimensions();
+  }
+
   /// Return value and derivatives, forward AD version
   Augmented<T> forward(const Values& values) const {
     return root_->forward(values);

gtsam_unstable/nonlinear/tests/testExpression.cpp

@@ -26,9 +26,15 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+using boost::assign::map_list_of;
+
 using namespace std;
 using namespace gtsam;
 
+typedef pair<Key,size_t> Pair;
+
 /* ************************************************************************* */
 
 template<class CAL>
@@ -94,13 +100,18 @@ Expression<Point3> p_cam(x, &Pose3::transform_to, p);
 }
 /* ************************************************************************* */
 // keys
-TEST(Expression, keys_binary) {
+TEST(Expression, BinaryKeys) {
-
+  set<Key> expected = list_of(1)(2);
-  // Check keys
+  EXPECT(expected == binary::p_cam.keys());
-  set<Key> expectedKeys;
+}
-  expectedKeys.insert(1);
+/* ************************************************************************* */
-  expectedKeys.insert(2);
+// dimensions
-  EXPECT(expectedKeys == binary::p_cam.keys());
+TEST(Expression, BinaryDimensions) {
+  map<Key, size_t> expected = map_list_of(1, 6)(2, 3), //
+  actual = binary::p_cam.dimensions();
+  EXPECT_LONGS_EQUAL(expected.size(),actual.size());
+  BOOST_FOREACH(Pair pair, actual)
+    EXPECT_LONGS_EQUAL(expected[pair.first],pair.second);
 }
 /* ************************************************************************* */
 // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
@@ -115,14 +126,18 @@ Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
 }
 /* ************************************************************************* */
 // keys
-TEST(Expression, keys_tree) {
+TEST(Expression, TreeKeys) {
-
+  set<Key> expected = list_of(1)(2)(3);
-  // Check keys
+  EXPECT(expected == tree::uv_hat.keys());
-  set<Key> expectedKeys;
+}
-  expectedKeys.insert(1);
+/* ************************************************************************* */
-  expectedKeys.insert(2);
+// dimensions
-  expectedKeys.insert(3);
+TEST(Expression, TreeDimensions) {
-  EXPECT(expectedKeys == tree::uv_hat.keys());
+  map<Key, size_t> expected = map_list_of(1, 6)(2, 3)(3, 5), //
+  actual = tree::uv_hat.dimensions();
+  EXPECT_LONGS_EQUAL(expected.size(),actual.size());
+  BOOST_FOREACH(Pair pair, actual)
+    EXPECT_LONGS_EQUAL(expected[pair.first],pair.second);
 }
 /* ************************************************************************* */
 
@@ -133,10 +148,8 @@ TEST(Expression, compose1) {
   Expression<Rot3> R3 = R1 * R2;
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(1)(2);
-  expectedKeys.insert(1);
+  EXPECT(expected == R3.keys());
-  expectedKeys.insert(2);
-  EXPECT(expectedKeys == R3.keys());
 }
 
 /* ************************************************************************* */
@@ -148,9 +161,8 @@ TEST(Expression, compose2) {
   Expression<Rot3> R3 = R1 * R2;
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(1);
-  expectedKeys.insert(1);
+  EXPECT(expected == R3.keys());
-  EXPECT(expectedKeys == R3.keys());
 }
 
 /* ************************************************************************* */
@@ -162,9 +174,8 @@ TEST(Expression, compose3) {
   Expression<Rot3> R3 = R1 * R2;
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(3);
-  expectedKeys.insert(3);
+  EXPECT(expected == R3.keys());
-  EXPECT(expectedKeys == R3.keys());
 }
 
 /* ************************************************************************* */
@@ -189,11 +200,8 @@ TEST(Expression, ternary) {
   Expression<Rot3> ABC(composeThree, A, B, C);
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(1)(2)(3);
-  expectedKeys.insert(1);
+  EXPECT(expected == ABC.keys());
-  expectedKeys.insert(2);
-  expectedKeys.insert(3);
-  EXPECT(expectedKeys == ABC.keys());
 }
 
 /* ************************************************************************* */