Add support for maximum matrix sizes to TinySolver.

This change restructures the `TinySolver` template and its associated
adapters (`AutoDiff` and `CostFunction`) to make maximum sizing
attributes first-class parameters. This enables the entire `TinySolver`
stack to be used in restricted environments (e.g., small MCUs) without
dynamic memory allocation, even when the number of residuals or
parameters is only known at runtime (`Eigen::Dynamic`).

Specifically:
- Adds `kMaxResiduals` and `kMaxParameters` template parameters to
  `TinySolver`.
- Updated `TinySolverAutoDiffFunction` and
  `TinySolverCostFunctionAdapter` to support optional maximum size
  template parameters for their internal buffers.
- The new API maintains backward compatibility for existing users by
  defaulting to the sizes defined in the `Function`'s enums.
- This structure also supports reducing code bloat by allowing
  `TinySolver` to be instantiated with an abstract base class, using
  dynamic dispatch for cost function evaluation.

New test cases for `TinySolver` and its adapters verify the
zero-allocation behavior and the unified API flexibility.

Change-Id: Ic6f43984d384dbe71472b31c5ebd2b538d61f19d

include/ceres/tiny_solver.h

@@ -55,6 +55,7 @@
 #include <cassert>
 #include <cmath>
 
+#include "Eigen/Core"
 #include "Eigen/Dense"
 
 namespace ceres {
@@ -126,11 +127,15 @@ namespace ceres {
 //
 //   int NumParameters() const;
 //
-template <typename Function,
+template <typename Function, int kMaxResiduals = Function::NUM_RESIDUALS,
+          int kMaxParameters = Function::NUM_PARAMETERS,
           typename LinearSolver =
               Eigen::LDLT<Eigen::Matrix<typename Function::Scalar,  //
                                         Function::NUM_PARAMETERS,   //
-                                        Function::NUM_PARAMETERS>>>
+                                        Function::NUM_PARAMETERS,   //
+                                        0,                          //
+                                        kMaxParameters,             //
+                                        kMaxParameters>>>
 class TinySolver {
  public:
   // This class needs to have an Eigen aligned operator new as it contains
@@ -139,10 +144,27 @@ class TinySolver {
 
   enum {
     NUM_RESIDUALS = Function::NUM_RESIDUALS,
-    NUM_PARAMETERS = Function::NUM_PARAMETERS
+    NUM_PARAMETERS = Function::NUM_PARAMETERS,
+    MAX_NUM_RESIDUALS = kMaxResiduals,
+    MAX_NUM_PARAMETERS = kMaxParameters,
   };
   using Scalar = typename Function::Scalar;
-  using Parameters = typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1>;
+  using ParameterVector = typename Eigen::
+      Matrix<Scalar, NUM_PARAMETERS, 1, 0, MAX_NUM_PARAMETERS, 1>;
+  using ResidualVector =
+      typename Eigen::Matrix<Scalar, NUM_RESIDUALS, 1, 0, MAX_NUM_RESIDUALS, 1>;
+  using JacobianMatrix = typename Eigen::Matrix<Scalar,
+                                                NUM_RESIDUALS,
+                                                NUM_PARAMETERS,
+                                                0,
+                                                MAX_NUM_RESIDUALS,
+                                                MAX_NUM_PARAMETERS>;
+  using HessianMatrix = Eigen::Matrix<Scalar,
+                                      NUM_PARAMETERS,
+                                      NUM_PARAMETERS,
+                                      0,
+                                      MAX_NUM_PARAMETERS,
+                                      MAX_NUM_PARAMETERS>;
 
   enum Status {
     // max_norm |J'(x) * f(x)| < gradient_tolerance
@@ -188,7 +210,7 @@ class TinySolver {
     Status status = HIT_MAX_ITERATIONS;
   };
 
-  bool Update(const Function& function, const Parameters& x) {
+  bool Update(const Function& function, const ParameterVector& x) {
     if (!function(x.data(), residuals_.data(), jacobian_.data())) {
       return false;
     }
@@ -218,10 +240,10 @@ class TinySolver {
     return true;
   }
 
-  const Summary& Solve(const Function& function, Parameters* x_and_min) {
+  const Summary& Solve(const Function& function, ParameterVector* x_and_min) {
     Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function);
     assert(x_and_min);
-    Parameters& x = *x_and_min;
+    ParameterVector& x = *x_and_min;
     summary = Summary();
     summary.iterations = 0;
 
@@ -329,12 +351,13 @@ class TinySolver {
     return summary;
   }
 
-  Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> Residuals() const {
+  ResidualVector Residuals()
+      const {
     // Residual updates are stored with the opposite sign.
     return -residuals_;
   }
 
-  Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> Jacobian() const {
+  JacobianMatrix Jacobian() const {
     // Undo the scaling applied to the jacobian matrix during Update().
     return jacobian_ * jacobi_scaling_.cwiseInverse().asDiagonal();
   }
@@ -347,10 +370,10 @@ class TinySolver {
   // linear system. This allows reusing the intermediate storage across solves.
   LinearSolver linear_solver_;
   Scalar cost_;
-  Parameters dx_, x_new_, g_, jacobi_scaling_, lm_step_;
+  ParameterVector dx_, x_new_, g_, jacobi_scaling_, lm_step_;
-  Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> residuals_, f_x_new_;
+  ResidualVector residuals_, f_x_new_;
-  Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_;
+  JacobianMatrix jacobian_;
-  Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_;
+  HessianMatrix jtj_, jtj_regularized_;
 
   template <int R, int P>
   void Initialize(const Function& function) {

include/ceres/tiny_solver_autodiff_function.h

@@ -103,10 +103,8 @@ namespace ceres {
 //   solver.Solve(f, &x);
 //
 // WARNING: The cost function adapter is not thread safe.
-template <typename CostFunctor,
+template <typename CostFunctor, int kNumResiduals, int kNumParameters,
-          int kNumResiduals,
+          typename T = double, int kMaxResiduals = kNumResiduals>
-          int kNumParameters,
-          typename T = double>
 class TinySolverAutoDiffFunction {
  public:
   // This class needs to have an Eigen aligned operator new as it contains
@@ -118,11 +116,17 @@ class TinySolverAutoDiffFunction {
     Initialize<kNumResiduals>(cost_functor);
   }
 
-  using Scalar = T;
   enum {
     NUM_PARAMETERS = kNumParameters,
     NUM_RESIDUALS = kNumResiduals,
+    MAX_NUM_RESIDUALS = kMaxResiduals,
   };
+  using Scalar = T;
+  using JacobianMatrix = typename Eigen::Matrix<Scalar,
+                                                NUM_RESIDUALS,
+                                                NUM_PARAMETERS,
+                                                0,
+                                                MAX_NUM_RESIDUALS>;
 
   // This is similar to AutoDifferentiate(), but since there is only one
   // parameter block it is easier to inline to avoid overhead.
@@ -151,7 +155,7 @@ class TinySolverAutoDiffFunction {
     }
 
     // Copy the jacobian out of the derivative part of the residual jets.
-    Eigen::Map<Eigen::Matrix<T, kNumResiduals, kNumParameters>> jacobian_matrix(
+    Eigen::Map<JacobianMatrix> jacobian_matrix(
         jacobian, num_residuals_, kNumParameters);
     for (int r = 0; r < num_residuals_; ++r) {
       residuals[r] = jet_residuals_[r].a;
@@ -179,10 +183,14 @@ class TinySolverAutoDiffFunction {
   // and jet_residuals_ are where the final cost and derivatives end up.
   //
   // Since this buffer is used for evaluation, the adapter is not thread safe.
+  static_assert(kNumParameters != Eigen::Dynamic);
   using JetType = Jet<T, kNumParameters>;
+  using JetResidualVector =
+      Eigen::Matrix<JetType, kNumResiduals, 1, 0, kMaxResiduals, 1>;
   mutable JetType jet_parameters_[kNumParameters];
+
   // Eigen::Matrix serves as static or dynamic container.
-  mutable Eigen::Matrix<JetType, kNumResiduals, 1> jet_residuals_;
+  mutable JetResidualVector jet_residuals_;
 
   template <int R>
   void Initialize(const CostFunctor& function) {

include/ceres/tiny_solver_cost_function_adapter.h

@@ -71,15 +71,25 @@ namespace ceres {
 //
 //   TinySolverCostFunctionAdapter cost_function_adapter(*cost_function);
 //
-template <int kNumResiduals = Eigen::Dynamic,
+template <
-          int kNumParameters = Eigen::Dynamic>
+    int kNumResiduals = Eigen::Dynamic, int kNumParameters = Eigen::Dynamic,
+    int kMaxResiduals = kNumResiduals, int kMaxParameters = kNumParameters>
 class TinySolverCostFunctionAdapter {
  public:
   using Scalar = double;
   enum ComponentSizeType {
     NUM_PARAMETERS = kNumParameters,
-    NUM_RESIDUALS = kNumResiduals
+    NUM_RESIDUALS = kNumResiduals,
+    MAX_NUM_RESIDUALS = kMaxResiduals,
+    MAX_NUM_PARAMETERS = kMaxParameters,
   };
+  template <int Layout>  // Eigen::RowMajor or Eigen::ColMajor
+  using JacobianMatrix = typename Eigen::Matrix<Scalar,
+                                                NUM_RESIDUALS,
+                                                NUM_PARAMETERS,
+                                                Layout,
+                                                MAX_NUM_RESIDUALS,
+                                                MAX_NUM_PARAMETERS>;
 
   // This struct needs to have an Eigen aligned operator new as it contains
   // fixed-size Eigen types.
@@ -120,8 +130,8 @@ class TinySolverCostFunctionAdapter {
     // column-major layout, and the CostFunction objects use row-major
     // Jacobian matrices. So the following bit of code does the
     // conversion from row-major Jacobians to column-major Jacobians.
-    Eigen::Map<Eigen::Matrix<double, NUM_RESIDUALS, NUM_PARAMETERS>>
+    Eigen::Map<JacobianMatrix<Eigen::ColMajor>> col_major_jacobian(
-        col_major_jacobian(jacobian, NumResiduals(), NumParameters());
+        jacobian, NumResiduals(), NumParameters());
     col_major_jacobian = row_major_jacobian_;
     return true;
   }
@@ -133,8 +143,7 @@ class TinySolverCostFunctionAdapter {
 
  private:
   const CostFunction& cost_function_;
-  mutable Eigen::Matrix<double, NUM_RESIDUALS, NUM_PARAMETERS, Eigen::RowMajor>
+  mutable JacobianMatrix<Eigen::RowMajor> row_major_jacobian_;
-      row_major_jacobian_;
 };
 
 }  // namespace ceres

internal/ceres/tiny_solver_autodiff_function_test.cc

@@ -143,4 +143,21 @@ TEST(TinySolverAutoDiffFunction, ResidualsDynamicAutoDiff) {
   EXPECT_NEAR(0.0, solver.summary.final_cost, 1e-10);
 }
 
+// A test case for when the number of residuals is dynamic,
+// but the maximum is statically sized.
+TEST(TinySolverAutoDiffFunction, ResidualsDynamicWithMaxResiduals) {
+  Eigen::Vector3d x0(0.76026643, -30.01799744, 0.55192142);
+
+  DynamicResidualsFunctor f;
+  // kNumResiduals = Eigen::Dynamic, but kMaxResiduals = 5
+  using AutoDiffCostFunctor =
+      ceres::TinySolverAutoDiffFunction<DynamicResidualsFunctor, Eigen::Dynamic,
+                                        3, double, 5>;
+  AutoDiffCostFunctor f_autodiff(f);
+
+  TinySolver<AutoDiffCostFunctor> solver;
+  solver.Solve(f_autodiff, &x0);
+  EXPECT_NEAR(0.0, solver.summary.final_cost, 1e-10);
+}
+
 }  // namespace ceres

internal/ceres/tiny_solver_cost_function_adapter_test.cc

@@ -65,11 +65,14 @@ class CostFunction2x3 : public SizedCostFunction<2, 3> {
   }
 };
 
-template <int kNumResiduals, int kNumParameters>
+template <int kNumResiduals, int kNumParameters,
+          int kMaxResiduals = kNumResiduals,
+          int kMaxParameters = kNumParameters>
 void TestHelper() {
   std::unique_ptr<CostFunction> cost_function(new CostFunction2x3);
   using CostFunctionAdapter =
-      TinySolverCostFunctionAdapter<kNumResiduals, kNumParameters>;
+      TinySolverCostFunctionAdapter<kNumResiduals, kNumParameters,
+                                    kMaxResiduals, kMaxParameters>;
   CostFunctionAdapter cfa(*cost_function);
   EXPECT_EQ(CostFunctionAdapter::NUM_RESIDUALS, kNumResiduals);
   EXPECT_EQ(CostFunctionAdapter::NUM_PARAMETERS, kNumParameters);
@@ -130,4 +133,9 @@ TEST(TinySolverCostFunctionAdapter, DynamicResidualsDynamicParameterBlock) {
   TestHelper<Eigen::Dynamic, Eigen::Dynamic>();
 }
 
+TEST(TinySolverCostFunctionAdapter, AllDynamicWithMaxSizes) {
+  // Both sizes are Dynamic, but capacity is fixed to 10x20.
+  TestHelper<Eigen::Dynamic, Eigen::Dynamic, 10, 20>();
+}
+
 }  // namespace ceres

internal/ceres/tiny_solver_test.cc

@@ -29,11 +29,14 @@
 //
 // Author: mierle@gmail.com (Keir Mierle)
 
+#define EIGEN_RUNTIME_NO_MALLOC  // Needed for enabling memory allocation
+                                 // assertions in Eigen.
 #include "ceres/tiny_solver.h"
 
 #include <algorithm>
 #include <cmath>
 
+#include "Eigen/Core"
 #include "ceres/tiny_solver_test_util.h"
 #include "gtest/gtest.h"
 
@@ -111,14 +114,16 @@ class ExampleAllDynamic {
   }
 };
 
-template <typename Function, typename Vector>
+template <typename Function, typename Vector,
+          int kMaxResiduals = Function::NUM_RESIDUALS,
+          int kMaxParameters = Function::NUM_PARAMETERS>
 void TestHelper(const Function& f, const Vector& x0) {
   Vector x = x0;
   Vec2 residuals;
   f(x.data(), residuals.data(), nullptr);
   EXPECT_GT(residuals.squaredNorm() / 2.0, 1e-10);
 
-  TinySolver<Function> solver;
+  TinySolver<Function, kMaxResiduals, kMaxParameters> solver;
   solver.Solve(f, &x);
   EXPECT_NEAR(0.0, solver.summary.final_cost, 1e-10);
 
@@ -169,4 +174,56 @@ TEST(TinySolver, ParametersAndResidualsDynamic) {
   TestHelper(f, x0);
 }
 
+// A test case for when the number of parameters and residuals is dynamically
+// sized, but the maximum number of parameters and residuals is statically
+// sized.
+TEST(TinySolver, AllDynamicWithMaxNumResidualsAndParameters) {
+  // Enable assertions for memory allocation in Eigen.
+  Eigen::internal::set_is_malloc_allowed(false);
+
+  Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 5, 1> x0(3);
+  x0 << 0.76026643, -30.01799744, 0.55192142;
+
+  ExampleAllDynamic f;
+
+  TestHelper<ExampleAllDynamic, decltype(x0), 5, 5>(f, x0);
+
+  // Re-enable dynamic memory allocation in Eigen.
+  Eigen::internal::set_is_malloc_allowed(true);
+}
+
+// A test case to make sure dynamic memory allocation assertions can be enabled
+// in Eigen. Eigen assertions are only enabled when NDEBUG is not defined.
+// Otherwise, the assertions are compiled out as no-op.
+#if !defined(EIGEN_NO_DEBUG)
+TEST(TinySolver, EigenMallocAssertions) {
+  // Enable assertions for memory allocation in Eigen.
+  Eigen::internal::set_is_malloc_allowed(false);
+
+  // Make sure dynamic memory allocation is not allowed.
+  ASSERT_DEATH(VecX x0(10), "EIGEN_RUNTIME_NO_MALLOC is defined");
+
+  // Re-enable dynamic memory allocation in Eigen.
+  Eigen::internal::set_is_malloc_allowed(true);
+}
+
+// A test case for when the number of parameters and residuals is
+// dynamically sized requires dynamic allocation.
+TEST(TinySolver, ParametersAndResidualsDynamicNeedsDynamicAllocation) {
+  VecX x0(3);
+  x0 << 0.76026643, -30.01799744, 0.55192142;
+
+  ExampleAllDynamic f;
+
+  // Enable assertions for memory allocation in Eigen.
+  Eigen::internal::set_is_malloc_allowed(false);
+
+  ASSERT_DEATH(TestHelper(f, x0), "EIGEN_RUNTIME_NO_MALLOC is defined");
+
+  // Re-enable dynamic memory allocation in Eigen.
+  Eigen::internal::set_is_malloc_allowed(true);
+}
+
+#endif  // !defined(EIGEN_NO_DEBUG)
+
 }  // namespace ceres