From 9ac20eea8a1508391889eb9d2d41071e5bd4eeb5 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 18 Jan 2011 02:40:22 +0000
Subject: [PATCH] Made LieValues allow for values to not be on the linear side
 during expmap

---
 gtsam/nonlinear/LieValues-inl.h | 7 +++++--
 gtsam/nonlinear/LieValues.h     | 8 +++++---
 gtsam/nonlinear/Ordering.h      | 8 ++++++++
 3 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/gtsam/nonlinear/LieValues-inl.h b/gtsam/nonlinear/LieValues-inl.h
index 0dab4f256..f2410b70a 100644
--- a/gtsam/nonlinear/LieValues-inl.h
+++ b/gtsam/nonlinear/LieValues-inl.h
@@ -148,8 +148,11 @@ namespace gtsam {
 		BOOST_FOREACH(const KeyValue& value, this->values_) {
 			const J& j = value.first;
 			const typename J::Value& pj = value.second;
-			const Vector& dj = delta[ordering[j]];
-			newValues.insert(j, pj.expmap(dj));
+			Index index;
+			if(ordering.tryAt(j,index)) {
+			  newValues.insert(j, pj.expmap(delta[index]));
+			} else
+			  newValues.insert(j, pj);
 		}
 		return newValues;
 	}
diff --git a/gtsam/nonlinear/LieValues.h b/gtsam/nonlinear/LieValues.h
index cd9689eec..7c6be82ce 100644
--- a/gtsam/nonlinear/LieValues.h
+++ b/gtsam/nonlinear/LieValues.h
@@ -206,9 +206,11 @@ namespace gtsam {
     std::vector<size_t> dimensions;
     _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
     template<typename I> void operator()(const I& key_value) {
-      Index var = ordering[key_value->first];
-      assert(var < dimensions.size());
-      dimensions[var] = key_value->second.dim();
+      Index var;
+      if(ordering.tryAt(key_value->first, var)) {
+        assert(var < dimensions.size());
+        dimensions[var] = key_value->second.dim();
+      }
     }
   };
 
diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h
index 66a046fe4..bfa14bb09 100644
--- a/gtsam/nonlinear/Ordering.h
+++ b/gtsam/nonlinear/Ordering.h
@@ -64,6 +64,14 @@ public:
 
   Index& at(const Symbol& key) { return operator[](key); }
   Index at(const Symbol& key) const { return operator[](key); }
+  bool tryAt(const Symbol& key, Index& index) const {
+    const_iterator i = order_.find(key);
+    if(i != order_.end()) {
+      index = i->second;
+      return true;
+    } else
+      return false;
+  }
   Index& operator[](const Symbol& key) {
     iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
   Index operator[](const Symbol& key) const {