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
diff --git a/include/ceres/tiny_solver.h b/include/ceres/tiny_solver.h index 69f181b..093bc36 100644 --- a/include/ceres/tiny_solver.h +++ b/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 @@ // // 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 @@ 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 @@ 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 @@ 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 @@ 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 @@ // linear system. This allows reusing the intermediate storage across solves. LinearSolver linear_solver_; Scalar cost_; - Parameters dx_, x_new_, g_, jacobi_scaling_, lm_step_; - Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> residuals_, f_x_new_; - Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_; - Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_; + ParameterVector dx_, x_new_, g_, jacobi_scaling_, lm_step_; + ResidualVector residuals_, f_x_new_; + JacobianMatrix jacobian_; + HessianMatrix jtj_, jtj_regularized_; template <int R, int P> void Initialize(const Function& function) {
diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h index fa67538..6fe0db8 100644 --- a/include/ceres/tiny_solver_autodiff_function.h +++ b/include/ceres/tiny_solver_autodiff_function.h
@@ -103,10 +103,8 @@ // solver.Solve(f, &x); // // WARNING: The cost function adapter is not thread safe. -template <typename CostFunctor, - int kNumResiduals, - int kNumParameters, - typename T = double> +template <typename CostFunctor, int kNumResiduals, int kNumParameters, + typename T = double, int kMaxResiduals = kNumResiduals> class TinySolverAutoDiffFunction { public: // This class needs to have an Eigen aligned operator new as it contains @@ -118,11 +116,17 @@ 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 @@ } // 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 @@ // 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) {
diff --git a/include/ceres/tiny_solver_cost_function_adapter.h b/include/ceres/tiny_solver_cost_function_adapter.h index 39ba266..3f7a378 100644 --- a/include/ceres/tiny_solver_cost_function_adapter.h +++ b/include/ceres/tiny_solver_cost_function_adapter.h
@@ -71,15 +71,25 @@ // // TinySolverCostFunctionAdapter cost_function_adapter(*cost_function); // -template <int kNumResiduals = Eigen::Dynamic, - int kNumParameters = Eigen::Dynamic> +template < + 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 @@ // 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>> - col_major_jacobian(jacobian, NumResiduals(), NumParameters()); + Eigen::Map<JacobianMatrix<Eigen::ColMajor>> col_major_jacobian( + jacobian, NumResiduals(), NumParameters()); col_major_jacobian = row_major_jacobian_; return true; } @@ -133,8 +143,7 @@ private: const CostFunction& cost_function_; - mutable Eigen::Matrix<double, NUM_RESIDUALS, NUM_PARAMETERS, Eigen::RowMajor> - row_major_jacobian_; + mutable JacobianMatrix<Eigen::RowMajor> row_major_jacobian_; }; } // namespace ceres
diff --git a/internal/ceres/tiny_solver_autodiff_function_test.cc b/internal/ceres/tiny_solver_autodiff_function_test.cc index ff55e82..546b49c 100644 --- a/internal/ceres/tiny_solver_autodiff_function_test.cc +++ b/internal/ceres/tiny_solver_autodiff_function_test.cc
@@ -143,4 +143,21 @@ 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
diff --git a/internal/ceres/tiny_solver_cost_function_adapter_test.cc b/internal/ceres/tiny_solver_cost_function_adapter_test.cc index e2c2755..1bf1ee7 100644 --- a/internal/ceres/tiny_solver_cost_function_adapter_test.cc +++ b/internal/ceres/tiny_solver_cost_function_adapter_test.cc
@@ -65,11 +65,14 @@ } }; -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 @@ 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
diff --git a/internal/ceres/tiny_solver_test.cc b/internal/ceres/tiny_solver_test.cc index f8b9264..9a13c26 100644 --- a/internal/ceres/tiny_solver_test.cc +++ b/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 @@ } }; -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 @@ 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