From 4542292af99a76898b0d0b7d6e97a37a361ae94a Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 13 Dec 2014 22:07:07 +0000
Subject: [PATCH] Fixed version of at

---
 gtsam.h                    |  8 +++++---
 gtsam/nonlinear/Values.cpp | 18 ++++++++++++++++++
 gtsam/nonlinear/Values.h   |  4 ++++
 3 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 37040234d..440fece31 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1733,9 +1733,6 @@ class Values {
   void insert(size_t j, Vector t);
   void insert(size_t j, Matrix t);
 
-  // Fixed size version
-  void insertFixed(size_t j, Vector t, size_t n);
-
   void update(size_t j, const gtsam::Point2& t);
   void update(size_t j, const gtsam::Point3& t);
   void update(size_t j, const gtsam::Rot2& t);
@@ -1752,6 +1749,11 @@ class Values {
 
   template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
   T at(size_t j);
+
+  // Fixed size versions, for n in 1..9
+  void insertFixed(size_t j, Vector t, size_t n);
+  Vector atFixed(size_t j, size_t n);
+
 };
 
 // Actually a FastList<Key>
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 3cad10f31..372bced8e 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -112,6 +112,24 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  Vector Values::atFixed(Key j,  size_t n) {
+    switch (n) {
+    case 1: return at<Vector1>(j);
+    case 2: return at<Vector2>(j);
+    case 3: return at<Vector3>(j);
+    case 4: return at<Vector4>(j);
+    case 5: return at<Vector5>(j);
+    case 6: return at<Vector6>(j);
+    case 7: return at<Vector7>(j);
+    case 8: return at<Vector8>(j);
+    case 9: return at<Vector9>(j);
+    default:
+      throw runtime_error(
+          "Values::at fixed size can only handle n in 1..9");
+    }
+  }
+
   /* ************************************************************************* */
   const Value& Values::at(Key j) const {
     // Find the item
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 9a4c7e798..4acf5ad17 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -180,6 +180,10 @@ namespace gtsam {
     template<typename ValueType>
     const ValueType& at(Key j) const;
 
+    /// Special version for small fixed size vectors, for matlab/python
+    /// throws truntime error if n<1 || n>9
+    Vector atFixed(Key j, size_t n);
+
     /** Retrieve a variable by key \c j.  This version returns a reference
      * to the base Value class, and needs to be casted before use.
      * @param j Retrieve the value associated with this key