From 2c3fe398fca3ff05dc59d8e38dd67c28ab6523af Mon Sep 17 00:00:00 2001
From: Aaron Miller <acmiller273@gmail.com>
Date: Mon, 10 Apr 2023 06:23:41 +0000
Subject: [PATCH] [SymForce] Speed up values.cc compilation

BY_TYPE_HELPER is taking all the time, specifically all the matrix
overloads.  Split them into their own code path, compiles way faster
this way.

Topic: sym-values-compile-fast
---
 gen/cpp/sym/util/type_ops.h                   | 445 +++++++++++-------
 .../backends/cpp/templates/type_ops.h.jinja   |  63 ++-
 symforce/opt/values.cc                        |  48 +-
 symforce/pybind/cc_values.cc                  |  16 +-
 4 files changed, 387 insertions(+), 185 deletions(-)

diff --git a/gen/cpp/sym/util/type_ops.h b/gen/cpp/sym/util/type_ops.h
index cf8650fc6..0a4e6c821 100644
--- a/gen/cpp/sym/util/type_ops.h
+++ b/gen/cpp/sym/util/type_ops.h
@@ -26,6 +26,7 @@
 #include <sym/spherical_camera_cal.h>
 #include <sym/unit3.h>
 #include <sym/util/typedefs.h>
+#include <symforce/opt/assert.h>
 
 namespace sym {
 
@@ -36,6 +37,283 @@ template <typename T>
 static constexpr const bool kIsSparseEigenType =
     std::is_base_of<Eigen::SparseMatrix<typename T::Scalar>, T>::value;
 
+inline bool IsEigenType(const type_t type) {
+  switch (type.value) {
+    case type_t::VECTOR1:
+    case type_t::VECTOR2:
+    case type_t::VECTOR3:
+    case type_t::VECTOR4:
+    case type_t::VECTOR5:
+    case type_t::VECTOR6:
+    case type_t::VECTOR7:
+    case type_t::VECTOR8:
+    case type_t::VECTOR9:
+    case type_t::VECTORX:
+    case type_t::MATRIX12:
+    case type_t::MATRIX13:
+    case type_t::MATRIX14:
+    case type_t::MATRIX15:
+    case type_t::MATRIX16:
+    case type_t::MATRIX17:
+    case type_t::MATRIX18:
+    case type_t::MATRIX19:
+    case type_t::MATRIX22:
+    case type_t::MATRIX23:
+    case type_t::MATRIX24:
+    case type_t::MATRIX25:
+    case type_t::MATRIX26:
+    case type_t::MATRIX27:
+    case type_t::MATRIX28:
+    case type_t::MATRIX29:
+    case type_t::MATRIX32:
+    case type_t::MATRIX33:
+    case type_t::MATRIX34:
+    case type_t::MATRIX35:
+    case type_t::MATRIX36:
+    case type_t::MATRIX37:
+    case type_t::MATRIX38:
+    case type_t::MATRIX39:
+    case type_t::MATRIX42:
+    case type_t::MATRIX43:
+    case type_t::MATRIX44:
+    case type_t::MATRIX45:
+    case type_t::MATRIX46:
+    case type_t::MATRIX47:
+    case type_t::MATRIX48:
+    case type_t::MATRIX49:
+    case type_t::MATRIX52:
+    case type_t::MATRIX53:
+    case type_t::MATRIX54:
+    case type_t::MATRIX55:
+    case type_t::MATRIX56:
+    case type_t::MATRIX57:
+    case type_t::MATRIX58:
+    case type_t::MATRIX59:
+    case type_t::MATRIX62:
+    case type_t::MATRIX63:
+    case type_t::MATRIX64:
+    case type_t::MATRIX65:
+    case type_t::MATRIX66:
+    case type_t::MATRIX67:
+    case type_t::MATRIX68:
+    case type_t::MATRIX69:
+    case type_t::MATRIX72:
+    case type_t::MATRIX73:
+    case type_t::MATRIX74:
+    case type_t::MATRIX75:
+    case type_t::MATRIX76:
+    case type_t::MATRIX77:
+    case type_t::MATRIX78:
+    case type_t::MATRIX79:
+    case type_t::MATRIX82:
+    case type_t::MATRIX83:
+    case type_t::MATRIX84:
+    case type_t::MATRIX85:
+    case type_t::MATRIX86:
+    case type_t::MATRIX87:
+    case type_t::MATRIX88:
+    case type_t::MATRIX89:
+    case type_t::MATRIX92:
+    case type_t::MATRIX93:
+    case type_t::MATRIX94:
+    case type_t::MATRIX95:
+    case type_t::MATRIX96:
+    case type_t::MATRIX97:
+    case type_t::MATRIX98:
+    case type_t::MATRIX99:
+      return true;
+    case type_t::ROT2:
+    case type_t::ROT3:
+    case type_t::POSE2:
+    case type_t::POSE3:
+    case type_t::UNIT3:
+    case type_t::SCALAR:
+    case type_t::ATAN_CAMERA_CAL:
+    case type_t::DOUBLE_SPHERE_CAMERA_CAL:
+    case type_t::EQUIRECTANGULAR_CAMERA_CAL:
+    case type_t::LINEAR_CAMERA_CAL:
+    case type_t::POLYNOMIAL_CAMERA_CAL:
+    case type_t::SPHERICAL_CAMERA_CAL:
+    case type_t::DATABUFFER:
+      return false;
+    case type_t::INVALID:
+      SYM_ASSERT(false, "Invalid type");
+  }
+  SYM_ASSERT(false, "Unreachable");  // gcc (at least >=5,<=11) does not prove this is unreachable
+}
+
+/**
+ *  Returns the shape of an Eigen type as a pair of ints (rows, cols)
+ */
+inline std::pair<int, int> EigenTypeShape(const type_t type) {
+  switch (type.value) {
+    case type_t::VECTOR1:
+      return {1, 1};
+    case type_t::VECTOR2:
+      return {2, 1};
+    case type_t::VECTOR3:
+      return {3, 1};
+    case type_t::VECTOR4:
+      return {4, 1};
+    case type_t::VECTOR5:
+      return {5, 1};
+    case type_t::VECTOR6:
+      return {6, 1};
+    case type_t::VECTOR7:
+      return {7, 1};
+    case type_t::VECTOR8:
+      return {8, 1};
+    case type_t::VECTOR9:
+      return {9, 1};
+    case type_t::MATRIX12:
+      return {1, 2};
+    case type_t::MATRIX13:
+      return {1, 3};
+    case type_t::MATRIX14:
+      return {1, 4};
+    case type_t::MATRIX15:
+      return {1, 5};
+    case type_t::MATRIX16:
+      return {1, 6};
+    case type_t::MATRIX17:
+      return {1, 7};
+    case type_t::MATRIX18:
+      return {1, 8};
+    case type_t::MATRIX19:
+      return {1, 9};
+    case type_t::MATRIX22:
+      return {2, 2};
+    case type_t::MATRIX23:
+      return {2, 3};
+    case type_t::MATRIX24:
+      return {2, 4};
+    case type_t::MATRIX25:
+      return {2, 5};
+    case type_t::MATRIX26:
+      return {2, 6};
+    case type_t::MATRIX27:
+      return {2, 7};
+    case type_t::MATRIX28:
+      return {2, 8};
+    case type_t::MATRIX29:
+      return {2, 9};
+    case type_t::MATRIX32:
+      return {3, 2};
+    case type_t::MATRIX33:
+      return {3, 3};
+    case type_t::MATRIX34:
+      return {3, 4};
+    case type_t::MATRIX35:
+      return {3, 5};
+    case type_t::MATRIX36:
+      return {3, 6};
+    case type_t::MATRIX37:
+      return {3, 7};
+    case type_t::MATRIX38:
+      return {3, 8};
+    case type_t::MATRIX39:
+      return {3, 9};
+    case type_t::MATRIX42:
+      return {4, 2};
+    case type_t::MATRIX43:
+      return {4, 3};
+    case type_t::MATRIX44:
+      return {4, 4};
+    case type_t::MATRIX45:
+      return {4, 5};
+    case type_t::MATRIX46:
+      return {4, 6};
+    case type_t::MATRIX47:
+      return {4, 7};
+    case type_t::MATRIX48:
+      return {4, 8};
+    case type_t::MATRIX49:
+      return {4, 9};
+    case type_t::MATRIX52:
+      return {5, 2};
+    case type_t::MATRIX53:
+      return {5, 3};
+    case type_t::MATRIX54:
+      return {5, 4};
+    case type_t::MATRIX55:
+      return {5, 5};
+    case type_t::MATRIX56:
+      return {5, 6};
+    case type_t::MATRIX57:
+      return {5, 7};
+    case type_t::MATRIX58:
+      return {5, 8};
+    case type_t::MATRIX59:
+      return {5, 9};
+    case type_t::MATRIX62:
+      return {6, 2};
+    case type_t::MATRIX63:
+      return {6, 3};
+    case type_t::MATRIX64:
+      return {6, 4};
+    case type_t::MATRIX65:
+      return {6, 5};
+    case type_t::MATRIX66:
+      return {6, 6};
+    case type_t::MATRIX67:
+      return {6, 7};
+    case type_t::MATRIX68:
+      return {6, 8};
+    case type_t::MATRIX69:
+      return {6, 9};
+    case type_t::MATRIX72:
+      return {7, 2};
+    case type_t::MATRIX73:
+      return {7, 3};
+    case type_t::MATRIX74:
+      return {7, 4};
+    case type_t::MATRIX75:
+      return {7, 5};
+    case type_t::MATRIX76:
+      return {7, 6};
+    case type_t::MATRIX77:
+      return {7, 7};
+    case type_t::MATRIX78:
+      return {7, 8};
+    case type_t::MATRIX79:
+      return {7, 9};
+    case type_t::MATRIX82:
+      return {8, 2};
+    case type_t::MATRIX83:
+      return {8, 3};
+    case type_t::MATRIX84:
+      return {8, 4};
+    case type_t::MATRIX85:
+      return {8, 5};
+    case type_t::MATRIX86:
+      return {8, 6};
+    case type_t::MATRIX87:
+      return {8, 7};
+    case type_t::MATRIX88:
+      return {8, 8};
+    case type_t::MATRIX89:
+      return {8, 9};
+    case type_t::MATRIX92:
+      return {9, 2};
+    case type_t::MATRIX93:
+      return {9, 3};
+    case type_t::MATRIX94:
+      return {9, 4};
+    case type_t::MATRIX95:
+      return {9, 5};
+    case type_t::MATRIX96:
+      return {9, 6};
+    case type_t::MATRIX97:
+      return {9, 7};
+    case type_t::MATRIX98:
+      return {9, 8};
+    case type_t::MATRIX99:
+      return {9, 9};
+    default:
+      SYM_ASSERT(false);
+  }
+}
+
 /**
  * Helper to handle polymorphism by creating a switch from a runtime type enum to dispatch
  * to the templated method func. Used to perform type-aware operations.
@@ -44,9 +322,12 @@ static constexpr const bool kIsSparseEigenType =
  *   name: Name of the output function (ex: FormatByType)
  *   func: Name of a function template (ex: FormatHelper)
  */
-#define BY_TYPE_HELPER(name, func)                                   \
+#define BY_TYPE_HELPER(name, func, matrix_func)                      \
   template <typename Scalar, typename... Args>                       \
   auto name(const type_t type, Args&&... args) {                     \
+    if (IsEigenType(type)) {                                         \
+      return matrix_func<Scalar>(args...);                           \
+    }                                                                \
     switch (type.value) {                                            \
       case type_t::ROT2:                                             \
         return func<sym::Rot2<Scalar>>(args...);                     \
@@ -60,168 +341,6 @@ static constexpr const bool kIsSparseEigenType =
         return func<sym::Unit3<Scalar>>(args...);                    \
       case type_t::SCALAR:                                           \
         return func<Scalar>(args...);                                \
-      case type_t::VECTOR1:                                          \
-        return func<Eigen::Matrix<Scalar, 1, 1>>(args...);           \
-      case type_t::VECTOR2:                                          \
-        return func<Eigen::Matrix<Scalar, 2, 1>>(args...);           \
-      case type_t::VECTOR3:                                          \
-        return func<Eigen::Matrix<Scalar, 3, 1>>(args...);           \
-      case type_t::VECTOR4:                                          \
-        return func<Eigen::Matrix<Scalar, 4, 1>>(args...);           \
-      case type_t::VECTOR5:                                          \
-        return func<Eigen::Matrix<Scalar, 5, 1>>(args...);           \
-      case type_t::VECTOR6:                                          \
-        return func<Eigen::Matrix<Scalar, 6, 1>>(args...);           \
-      case type_t::VECTOR7:                                          \
-        return func<Eigen::Matrix<Scalar, 7, 1>>(args...);           \
-      case type_t::VECTOR8:                                          \
-        return func<Eigen::Matrix<Scalar, 8, 1>>(args...);           \
-      case type_t::VECTOR9:                                          \
-        return func<Eigen::Matrix<Scalar, 9, 1>>(args...);           \
-      case type_t::MATRIX12:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 2>>(args...);           \
-      case type_t::MATRIX13:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 3>>(args...);           \
-      case type_t::MATRIX14:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 4>>(args...);           \
-      case type_t::MATRIX15:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 5>>(args...);           \
-      case type_t::MATRIX16:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 6>>(args...);           \
-      case type_t::MATRIX17:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 7>>(args...);           \
-      case type_t::MATRIX18:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 8>>(args...);           \
-      case type_t::MATRIX19:                                         \
-        return func<Eigen::Matrix<Scalar, 1, 9>>(args...);           \
-      case type_t::MATRIX22:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 2>>(args...);           \
-      case type_t::MATRIX23:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 3>>(args...);           \
-      case type_t::MATRIX24:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 4>>(args...);           \
-      case type_t::MATRIX25:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 5>>(args...);           \
-      case type_t::MATRIX26:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 6>>(args...);           \
-      case type_t::MATRIX27:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 7>>(args...);           \
-      case type_t::MATRIX28:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 8>>(args...);           \
-      case type_t::MATRIX29:                                         \
-        return func<Eigen::Matrix<Scalar, 2, 9>>(args...);           \
-      case type_t::MATRIX32:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 2>>(args...);           \
-      case type_t::MATRIX33:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 3>>(args...);           \
-      case type_t::MATRIX34:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 4>>(args...);           \
-      case type_t::MATRIX35:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 5>>(args...);           \
-      case type_t::MATRIX36:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 6>>(args...);           \
-      case type_t::MATRIX37:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 7>>(args...);           \
-      case type_t::MATRIX38:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 8>>(args...);           \
-      case type_t::MATRIX39:                                         \
-        return func<Eigen::Matrix<Scalar, 3, 9>>(args...);           \
-      case type_t::MATRIX42:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 2>>(args...);           \
-      case type_t::MATRIX43:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 3>>(args...);           \
-      case type_t::MATRIX44:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 4>>(args...);           \
-      case type_t::MATRIX45:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 5>>(args...);           \
-      case type_t::MATRIX46:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 6>>(args...);           \
-      case type_t::MATRIX47:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 7>>(args...);           \
-      case type_t::MATRIX48:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 8>>(args...);           \
-      case type_t::MATRIX49:                                         \
-        return func<Eigen::Matrix<Scalar, 4, 9>>(args...);           \
-      case type_t::MATRIX52:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 2>>(args...);           \
-      case type_t::MATRIX53:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 3>>(args...);           \
-      case type_t::MATRIX54:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 4>>(args...);           \
-      case type_t::MATRIX55:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 5>>(args...);           \
-      case type_t::MATRIX56:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 6>>(args...);           \
-      case type_t::MATRIX57:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 7>>(args...);           \
-      case type_t::MATRIX58:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 8>>(args...);           \
-      case type_t::MATRIX59:                                         \
-        return func<Eigen::Matrix<Scalar, 5, 9>>(args...);           \
-      case type_t::MATRIX62:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 2>>(args...);           \
-      case type_t::MATRIX63:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 3>>(args...);           \
-      case type_t::MATRIX64:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 4>>(args...);           \
-      case type_t::MATRIX65:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 5>>(args...);           \
-      case type_t::MATRIX66:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 6>>(args...);           \
-      case type_t::MATRIX67:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 7>>(args...);           \
-      case type_t::MATRIX68:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 8>>(args...);           \
-      case type_t::MATRIX69:                                         \
-        return func<Eigen::Matrix<Scalar, 6, 9>>(args...);           \
-      case type_t::MATRIX72:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 2>>(args...);           \
-      case type_t::MATRIX73:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 3>>(args...);           \
-      case type_t::MATRIX74:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 4>>(args...);           \
-      case type_t::MATRIX75:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 5>>(args...);           \
-      case type_t::MATRIX76:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 6>>(args...);           \
-      case type_t::MATRIX77:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 7>>(args...);           \
-      case type_t::MATRIX78:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 8>>(args...);           \
-      case type_t::MATRIX79:                                         \
-        return func<Eigen::Matrix<Scalar, 7, 9>>(args...);           \
-      case type_t::MATRIX82:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 2>>(args...);           \
-      case type_t::MATRIX83:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 3>>(args...);           \
-      case type_t::MATRIX84:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 4>>(args...);           \
-      case type_t::MATRIX85:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 5>>(args...);           \
-      case type_t::MATRIX86:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 6>>(args...);           \
-      case type_t::MATRIX87:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 7>>(args...);           \
-      case type_t::MATRIX88:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 8>>(args...);           \
-      case type_t::MATRIX89:                                         \
-        return func<Eigen::Matrix<Scalar, 8, 9>>(args...);           \
-      case type_t::MATRIX92:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 2>>(args...);           \
-      case type_t::MATRIX93:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 3>>(args...);           \
-      case type_t::MATRIX94:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 4>>(args...);           \
-      case type_t::MATRIX95:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 5>>(args...);           \
-      case type_t::MATRIX96:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 6>>(args...);           \
-      case type_t::MATRIX97:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 7>>(args...);           \
-      case type_t::MATRIX98:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 8>>(args...);           \
-      case type_t::MATRIX99:                                         \
-        return func<Eigen::Matrix<Scalar, 9, 9>>(args...);           \
       case type_t::ATAN_CAMERA_CAL:                                  \
         return func<sym::ATANCameraCal<Scalar>>(args...);            \
       case type_t::DOUBLE_SPHERE_CAMERA_CAL:                         \
diff --git a/symforce/codegen/backends/cpp/templates/type_ops.h.jinja b/symforce/codegen/backends/cpp/templates/type_ops.h.jinja
index 0ffebc725..0f191f549 100644
--- a/symforce/codegen/backends/cpp/templates/type_ops.h.jinja
+++ b/symforce/codegen/backends/cpp/templates/type_ops.h.jinja
@@ -15,6 +15,7 @@
 #include <sym/{{ python_util.camelcase_to_snakecase(cls.__name__) }}.h>
 {% endfor %}
 #include <sym/util/typedefs.h>
+#include <symforce/opt/assert.h>
 
 #include <lcmtypes/sym/type_t.hpp>
 
@@ -27,6 +28,53 @@ template <typename T>
 static constexpr const bool kIsSparseEigenType =
     std::is_base_of<Eigen::SparseMatrix<typename T::Scalar>, T>::value;
 
+inline bool IsEigenType(const type_t type) {
+  switch (type.value) {
+    {% for i in range(1, 10) %}
+    case type_t::VECTOR{{ i }}:
+    {% endfor %}
+    case type_t::VECTORX:
+    {% for i in range(1, 10) %}
+    {% for j in range(2, 10) %}
+    case type_t::MATRIX{{ i }}{{ j }}:
+    {% endfor %}
+    {% endfor %}
+      return true;
+    {% for cls in sf.GEO_TYPES %}
+      case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}:
+    {% endfor %}
+    case type_t::SCALAR:
+    {% for cls in sf.CAM_TYPES %}
+    case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}:
+    {% endfor %}
+    case type_t::DATABUFFER:
+      return false;
+    case type_t::INVALID:
+      SYM_ASSERT(false, "Invalid type");
+  }
+  SYM_ASSERT(false, "Unreachable");  // gcc (at least >=5,<=11) does not prove this is unreachable
+}
+
+/**
+ *  Returns the shape of an Eigen type as a pair of ints (rows, cols)
+ */
+inline std::pair<int, int> EigenTypeShape(const type_t type) {
+  switch (type.value) {
+    {% for i in range(1, 10) %}
+    case type_t::VECTOR{{ i }}:
+      return { {{ i }}, 1 };
+    {% endfor %}
+    {% for i in range(1, 10) %}
+    {% for j in range(2, 10) %}
+    case type_t::MATRIX{{ i }}{{ j }}:
+      return { {{ i }}, {{ j }} };
+    {% endfor %}
+    {% endfor %}
+    default:
+      SYM_ASSERT(false);
+  }
+}
+
 /**
  * Helper to handle polymorphism by creating a switch from a runtime type enum to dispatch
  * to the templated method func. Used to perform type-aware operations.
@@ -35,9 +83,12 @@ static constexpr const bool kIsSparseEigenType =
  *   name: Name of the output function (ex: FormatByType)
  *   func: Name of a function template (ex: FormatHelper)
  */
-#define BY_TYPE_HELPER(name, func)                                     \
+#define BY_TYPE_HELPER(name, func, matrix_func)                        \
   template <typename Scalar, typename... Args>                         \
   auto name(const type_t type, Args&&... args) {                       \
+    if (IsEigenType(type)) {                                           \
+      return matrix_func<Scalar>(args...);                             \
+    }                                                                  \
     switch (type.value) {                                              \
       {% for cls in sf.GEO_TYPES %}
       case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}: \
@@ -45,16 +96,6 @@ static constexpr const bool kIsSparseEigenType =
       {% endfor %}
       case type_t::SCALAR:                                             \
         return func<Scalar>(args...);                                  \
-        {% for i in range(1, 10) %}
-      case type_t::VECTOR{{ i }}:                                      \
-        return func<Eigen::Matrix<Scalar, {{ i }}, 1>>(args...);       \
-        {% endfor %}
-        {% for i in range(1, 10) %}
-        {% for j in range(2, 10) %}
-      case type_t::MATRIX{{ i }}{{ j }}:                               \
-        return func<Eigen::Matrix<Scalar, {{ i }}, {{ j }}>>(args...); \
-        {% endfor %}
-        {% endfor %}
       {% for cls in sf.CAM_TYPES %}
       case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}: \
         return func<sym::{{ cls.__name__ }}<Scalar>>(args...);         \
diff --git a/symforce/opt/values.cc b/symforce/opt/values.cc
index 599decec5..a33a1e4d7 100644
--- a/symforce/opt/values.cc
+++ b/symforce/opt/values.cc
@@ -215,20 +215,31 @@ void Values<Scalar>::Update(const index_t& index_this, const index_t& index_othe
  * Polymorphic helper to apply a retraction.
  */
 template <typename T, typename Scalar = typename sym::StorageOps<T>::Scalar>
-void RetractHelper(const Scalar* tangent_data, const Scalar epsilon, Scalar* t_ptr) {
+void RetractHelper(const Scalar* tangent_data, const Scalar epsilon, Scalar* const t_ptr,
+                   const int32_t /* tangent_dim */) {
+  static_assert(!kIsEigenType<T>, "Eigen types not supported");
+
   const T t_in = sym::StorageOps<T>::FromStorage(t_ptr);
   const typename sym::LieGroupOps<T>::TangentVec tangent_vec(tangent_data);
   const T t_out = sym::LieGroupOps<T>::Retract(t_in, tangent_vec, epsilon);
   sym::StorageOps<T>::ToStorage(t_out, t_ptr);
 }
-BY_TYPE_HELPER(RetractByType, RetractHelper);
+
+template <typename Scalar>
+void MatrixRetractHelper(const Scalar* tangent_data, const Scalar /* epsilon */,
+                         Scalar* const t_ptr, const int32_t tangent_dim) {
+  for (int32_t i = 0; i < tangent_dim; ++i) {
+    t_ptr[i] += tangent_data[i];
+  }
+}
+BY_TYPE_HELPER(RetractByType, RetractHelper, MatrixRetractHelper);
 
 template <typename Scalar>
 void Values<Scalar>::Retract(const index_t& index, const Scalar* delta, const Scalar epsilon) {
   size_t tangent_inx = 0;
   for (const index_entry_t& entry : index.entries) {
     RetractByType<Scalar>(entry.type, /* tangent_data */ delta + tangent_inx, epsilon,
-                          /* t_ptr */ data_.data() + entry.offset);
+                          /* t_ptr */ data_.data() + entry.offset, entry.tangent_dim);
     tangent_inx += entry.tangent_dim;
   }
 }
@@ -237,8 +248,9 @@ void Values<Scalar>::Retract(const index_t& index, const Scalar* delta, const Sc
  * Polymorphic helper to compute local coordinates
  */
 template <typename T, typename Scalar = typename sym::StorageOps<T>::Scalar>
-void LocalCoordinatesHelper(const Scalar* storage_this, const Scalar* storage_others,
-                            Scalar* tangent_out, const Scalar epsilon) {
+void LocalCoordinatesHelper(const Scalar* const storage_this, const Scalar* const storage_others,
+                            Scalar* const tangent_out, const Scalar epsilon,
+                            const int32_t /* tangent_dim */) {
   const T t1 = sym::StorageOps<T>::FromStorage(storage_this);
   const T t2 = sym::StorageOps<T>::FromStorage(storage_others);
   const typename sym::LieGroupOps<T>::TangentVec tangent_vec =
@@ -246,7 +258,16 @@ void LocalCoordinatesHelper(const Scalar* storage_this, const Scalar* storage_ot
   // TODO(alvin): can we avoid this copy?
   std::copy_n(tangent_vec.data(), sym::LieGroupOps<T>::TangentDim(), tangent_out);
 }
-BY_TYPE_HELPER(LocalCoordinatesByType, LocalCoordinatesHelper);
+template <typename Scalar>
+void MatrixLocalCoordinatesHelper(const Scalar* const storage_this,
+                                  const Scalar* const storage_others, Scalar* const tangent_out,
+                                  const Scalar /* epsilon */, const int32_t tangent_dim) {
+  for (int32_t i = 0; i < tangent_dim; ++i) {
+    tangent_out[i] = storage_this[i] - storage_others[i];
+  }
+}
+
+BY_TYPE_HELPER(LocalCoordinatesByType, LocalCoordinatesHelper, MatrixLocalCoordinatesHelper);
 
 template <typename Scalar>
 VectorX<Scalar> Values<Scalar>::LocalCoordinates(const Values<Scalar>& others, const index_t& index,
@@ -257,7 +278,7 @@ VectorX<Scalar> Values<Scalar>::LocalCoordinates(const Values<Scalar>& others, c
   for (const index_entry_t& entry : index.entries) {
     LocalCoordinatesByType<Scalar>(entry.type, data_.data() + entry.offset,
                                    others.data_.data() + entry.offset,
-                                   tangent_vec.data() + tangent_inx, epsilon);
+                                   tangent_vec.data() + tangent_inx, epsilon, entry.tangent_dim);
     tangent_inx += entry.tangent_dim;
   }
 
@@ -267,10 +288,16 @@ VectorX<Scalar> Values<Scalar>::LocalCoordinates(const Values<Scalar>& others, c
 namespace {
 
 template <typename T>
-std::string FormatHelper(const typename StorageOps<T>::Scalar* data_ptr) {
+std::string FormatHelper(const type_t /* type */, const typename StorageOps<T>::Scalar* data_ptr,
+                         const int32_t /* storage_dim */) {
   return fmt::format("{}", StorageOps<T>::FromStorage(data_ptr));
 }
-BY_TYPE_HELPER(FormatByType, FormatHelper);
+template <typename Scalar>
+std::string MatrixFormatHelper(const type_t type, const Scalar* data_ptr,
+                               const int32_t storage_dim) {
+  return fmt::format("<{} {}>", type, Eigen::Map<const VectorX<Scalar>>(data_ptr, storage_dim));
+}
+BY_TYPE_HELPER(FormatByType, FormatHelper, MatrixFormatHelper);
 
 }  // namespace
 
@@ -292,7 +319,8 @@ std::ostream& operator<<(std::ostream& os, const Values<Scalar>& v) {
   for (const index_entry_t& entry : index.entries) {
     fmt::print(os, " {} [{}:{}] --> {}\n", Key(entry.key), entry.offset,
                entry.offset + entry.storage_dim,
-               FormatByType<Scalar>(entry.type, v.Data().data() + entry.offset));
+               FormatByType<Scalar>(entry.type, entry.type, v.Data().data() + entry.offset,
+                                    entry.storage_dim));
   }
 
   os << ">";
diff --git a/symforce/pybind/cc_values.cc b/symforce/pybind/cc_values.cc
index c746b39b9..f830d1159 100644
--- a/symforce/pybind/cc_values.cc
+++ b/symforce/pybind/cc_values.cc
@@ -61,6 +61,20 @@ py::object PyAt(const sym::Valuesd& v, const sym::index_entry_t& index_entry) {
   return py::cast(v.At<T>(index_entry));
 }
 
+template <typename Scalar>
+py::object PyAtMatrix(const sym::Valuesd& v, const sym::index_entry_t& index_entry) {
+  const auto shape = EigenTypeShape(index_entry.type);
+  if (shape.first == 1 || shape.second == 1) {
+    return py::cast(Eigen::Map<const Eigen::VectorXd>(v.Data().data() + index_entry.offset,
+                                                      shape.first * shape.second),
+                    py::return_value_policy::copy);
+  } else {
+    return py::cast(Eigen::Map<const Eigen::MatrixXd>(v.Data().data() + index_entry.offset,
+                                                      shape.first, shape.second),
+                    py::return_value_policy::copy);
+  }
+}
+
 /**
  * Has signature
  * template <typename Scalar>
@@ -71,7 +85,7 @@ py::object PyAt(const sym::Valuesd& v, const sym::index_entry_t& index_entry) {
  *
  * Precondition: type is a supported type_t
  */
-BY_TYPE_HELPER(DynamicPyAt, PyAt)
+BY_TYPE_HELPER(DynamicPyAt, PyAt, PyAtMatrix)
 
 /**
  * Dynamically identifies the type T stored in v at index_entry, then returns