saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 08:34:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add support for maximum matrix sizes to TinySolver.

Browse Source 
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
This commit is contained in:
Pez Firoozfam
2026-03-23 20:35:24 +00:00
parent 41958f3fcc
commit 806af056fe
6 changed files with 153 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/tiny_solver.h
+36 -13
View File
@@ -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_;
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) {
include/ceres/tiny_solver_autodiff_function.h
+15 -7
View File
@@ -103,10 +103,8 @@ namespace ceres {
// 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 @@ 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
+16 -7
View File
@@ -71,15 +71,25 @@ namespace ceres {
//
// 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 @@ 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>>
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 @@ class TinySolverCostFunctionAdapter {
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
internal/ceres/tiny_solver_autodiff_function_test.cc
+17
View File
@@ -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
+10 -2
View File
@@ -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
+59 -2
View File
@@ -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