diff --git a/include/ceres/tiny_solver.h b/include/ceres/tiny_solver.h index 69f181bf8..093bc36f1 100644 --- a/include/ceres/tiny_solver.h +++ b/include/ceres/tiny_solver.h @@ -55,6 +55,7 @@ #include #include +#include "Eigen/Core" #include "Eigen/Dense" namespace ceres { @@ -126,11 +127,15 @@ namespace ceres { // // int NumParameters() const; // -template >> + 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; + using ParameterVector = typename Eigen:: + Matrix; + using ResidualVector = + typename Eigen::Matrix; + using JacobianMatrix = typename Eigen::Matrix; + using HessianMatrix = Eigen::Matrix; 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(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 Residuals() const { + ResidualVector Residuals() + const { // Residual updates are stored with the opposite sign. return -residuals_; } - Eigen::Matrix 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 residuals_, f_x_new_; - Eigen::Matrix jacobian_; - Eigen::Matrix jtj_, jtj_regularized_; + ParameterVector dx_, x_new_, g_, jacobi_scaling_, lm_step_; + ResidualVector residuals_, f_x_new_; + JacobianMatrix jacobian_; + HessianMatrix jtj_, jtj_regularized_; template void Initialize(const Function& function) { diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h index fa6753874..6fe0db8f8 100644 --- a/include/ceres/tiny_solver_autodiff_function.h +++ b/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 +template class TinySolverAutoDiffFunction { public: // This class needs to have an Eigen aligned operator new as it contains @@ -118,11 +116,17 @@ class TinySolverAutoDiffFunction { Initialize(cost_functor); } - using Scalar = T; enum { NUM_PARAMETERS = kNumParameters, NUM_RESIDUALS = kNumResiduals, + MAX_NUM_RESIDUALS = kMaxResiduals, }; + using Scalar = T; + using JacobianMatrix = typename Eigen::Matrix; // 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> jacobian_matrix( + Eigen::Map 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; + using JetResidualVector = + Eigen::Matrix; mutable JetType jet_parameters_[kNumParameters]; + // Eigen::Matrix serves as static or dynamic container. - mutable Eigen::Matrix jet_residuals_; + mutable JetResidualVector jet_residuals_; template 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 39ba266d4..3f7a378cc 100644 --- a/include/ceres/tiny_solver_cost_function_adapter.h +++ b/include/ceres/tiny_solver_cost_function_adapter.h @@ -71,15 +71,25 @@ namespace ceres { // // TinySolverCostFunctionAdapter cost_function_adapter(*cost_function); // -template +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 // Eigen::RowMajor or Eigen::ColMajor + using JacobianMatrix = typename Eigen::Matrix; // 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> - col_major_jacobian(jacobian, NumResiduals(), NumParameters()); + Eigen::Map> 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 - row_major_jacobian_; + mutable JacobianMatrix 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 ff55e82da..546b49c49 100644 --- a/internal/ceres/tiny_solver_autodiff_function_test.cc +++ b/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; + AutoDiffCostFunctor f_autodiff(f); + + TinySolver 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 e2c275516..1bf1ee7d8 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 @@ class CostFunction2x3 : public SizedCostFunction<2, 3> { } }; -template +template void TestHelper() { std::unique_ptr cost_function(new CostFunction2x3); using CostFunctionAdapter = - TinySolverCostFunctionAdapter; + TinySolverCostFunctionAdapter; CostFunctionAdapter cfa(*cost_function); EXPECT_EQ(CostFunctionAdapter::NUM_RESIDUALS, kNumResiduals); EXPECT_EQ(CostFunctionAdapter::NUM_PARAMETERS, kNumParameters); @@ -130,4 +133,9 @@ TEST(TinySolverCostFunctionAdapter, DynamicResidualsDynamicParameterBlock) { TestHelper(); } +TEST(TinySolverCostFunctionAdapter, AllDynamicWithMaxSizes) { + // Both sizes are Dynamic, but capacity is fixed to 10x20. + TestHelper(); +} + } // namespace ceres diff --git a/internal/ceres/tiny_solver_test.cc b/internal/ceres/tiny_solver_test.cc index f8b92648f..9a13c2625 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 #include +#include "Eigen/Core" #include "ceres/tiny_solver_test_util.h" #include "gtest/gtest.h" @@ -111,14 +114,16 @@ class ExampleAllDynamic { } }; -template +template 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 solver; + TinySolver 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 x0(3); + x0 << 0.76026643, -30.01799744, 0.55192142; + + ExampleAllDynamic f; + + TestHelper(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