From f1e67cc90b0cda3a1480427dec741bed4491b944 Mon Sep 17 00:00:00 2001 From: Keir Mierle Date: Fri, 19 Oct 2012 10:50:02 -0700 Subject: [PATCH] Add support for up to 10 parameters in autodiff. Supporting only 6 parameters in autodiff was enough for most cases, but 6 was not always sufficient. This extends the current implementation to work with up to 10 parameters. This also increases the number of parameters supported in SizedCostFunction to 10. Change-Id: Ic783602f93e6ddf4af24fa34eff37c0a4b775dc1 --- include/ceres/autodiff_cost_function.h | 22 ++- include/ceres/internal/autodiff.h | 160 +++++++++++++----- include/ceres/sized_cost_function.h | 30 ++-- internal/ceres/autodiff_cost_function_test.cc | 76 +++++++-- 4 files changed, 217 insertions(+), 71 deletions(-) diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h index da9ee2c79..bb08d649e 100644 --- a/include/ceres/autodiff_cost_function.h +++ b/include/ceres/autodiff_cost_function.h @@ -154,16 +154,20 @@ template // Number of parameters in block 5. + int N5 = 0, // Number of parameters in block 5. + int N6 = 0, // Number of parameters in block 6. + int N7 = 0, // Number of parameters in block 7. + int N8 = 0, // Number of parameters in block 8. + int N9 = 0> // Number of parameters in block 9. class AutoDiffCostFunction : - public SizedCostFunction { + public SizedCostFunction { public: // Takes ownership of functor. Uses the template-provided value for the // number of residuals ("M"). explicit AutoDiffCostFunction(CostFunctor* functor) : functor_(functor) { CHECK_NE(M, DYNAMIC) << "Can't run the fixed-size constructor if the " - << "number of residuals is set to ceres::DYNAMIC."; + << "number of residuals is set to ceres::DYNAMIC."; } // Takes ownership of functor. Ignores the template-provided number of @@ -174,8 +178,9 @@ class AutoDiffCostFunction : AutoDiffCostFunction(CostFunctor* functor, int num_residuals) : functor_(functor) { CHECK_EQ(M, DYNAMIC) << "Can't run the dynamic-size constructor if the " - << "number of residuals is not ceres::DYNAMIC."; - SizedCostFunction::set_num_residuals(num_residuals); + << "number of residuals is not ceres::DYNAMIC."; + SizedCostFunction + ::set_num_residuals(num_residuals); } virtual ~AutoDiffCostFunction() {} @@ -190,14 +195,15 @@ class AutoDiffCostFunction : double** jacobians) const { if (!jacobians) { return internal::VariadicEvaluate< - CostFunctor, double, N0, N1, N2, N3, N4, N5> + CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> ::Call(*functor_, parameters, residuals); } return internal::AutoDiff::Differentiate( + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate( *functor_, parameters, - SizedCostFunction::num_residuals(), + SizedCostFunction + ::num_residuals(), residuals, jacobians); } diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h index 4f5081f8f..581e88111 100644 --- a/include/ceres/internal/autodiff.h +++ b/include/ceres/internal/autodiff.h @@ -203,9 +203,75 @@ inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { // Supporting variadic functions is the primary source of complexity in the // autodiff implementation. -template +template struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + input[9], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + output); + } +}; + +template +struct VariadicEvaluate { static bool Call(const Functor& functor, T const *const *input, T* output) { return functor(input[0], input[1], @@ -217,9 +283,8 @@ struct VariadicEvaluate { } }; -template -struct VariadicEvaluate { +template +struct VariadicEvaluate { static bool Call(const Functor& functor, T const *const *input, T* output) { return functor(input[0], input[1], @@ -230,9 +295,8 @@ struct VariadicEvaluate { } }; -template -struct VariadicEvaluate { +template +struct VariadicEvaluate { static bool Call(const Functor& functor, T const *const *input, T* output) { return functor(input[0], input[1], @@ -242,9 +306,8 @@ struct VariadicEvaluate { } }; -template -struct VariadicEvaluate { +template +struct VariadicEvaluate { static bool Call(const Functor& functor, T const *const *input, T* output) { return functor(input[0], input[1], @@ -253,9 +316,8 @@ struct VariadicEvaluate { } }; -template -struct VariadicEvaluate { +template +struct VariadicEvaluate { static bool Call(const Functor& functor, T const *const *input, T* output) { return functor(input[0], input[1], @@ -264,7 +326,7 @@ struct VariadicEvaluate { }; template -struct VariadicEvaluate { +struct VariadicEvaluate { static bool Call(const Functor& functor, T const *const *input, T* output) { return functor(input[0], output); @@ -275,48 +337,58 @@ struct VariadicEvaluate { // supported in C++03 (though it is available in C++0x). N0 through N5 are the // dimension of the input arguments to the user supplied functor. template + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, + int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> struct AutoDiff { static bool Differentiate(const Functor& functor, T const *const *parameters, int num_outputs, T *function_value, T **jacobians) { - typedef Jet JetT; - - DCHECK_GT(N0, 0) - << "Cost functions must have at least one parameter block."; - DCHECK((!N1 && !N2 && !N3 && !N4 && !N5) || - ((N1 > 0) && !N2 && !N3 && !N4 && !N5) || - ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) || - ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) || - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) || - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0))) + // This block breaks the 80 column rule to keep it somewhat readable. + DCHECK_GT(num_outputs, 0); + CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) << "Zero block cannot precede a non-zero block. Block sizes are " << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " - << N3 << ", " << N4 << ", " << N5; - - DCHECK_GT(num_outputs, 0); + << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " + << N8 << ", " << N9; + typedef Jet JetT; FixedArray x( - N0 + N1 + N2 + N3 + N4 + N5 + num_outputs); + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); - // It's ugly, but it works. - const int jet0 = 0; - const int jet1 = N0; - const int jet2 = N0 + N1; - const int jet3 = N0 + N1 + N2; - const int jet4 = N0 + N1 + N2 + N3; - const int jet5 = N0 + N1 + N2 + N3 + N4; - const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; + // These are the positions of the respective jets in the fixed array x. + const int jet0 = 0; + const int jet1 = N0; + const int jet2 = N0 + N1; + const int jet3 = N0 + N1 + N2; + const int jet4 = N0 + N1 + N2 + N3; + const int jet5 = N0 + N1 + N2 + N3 + N4; + const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; + const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; + const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; + const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; - const JetT *unpacked_parameters[6] = { + const JetT *unpacked_parameters[10] = { x.get() + jet0, x.get() + jet1, x.get() + jet2, x.get() + jet3, x.get() + jet4, x.get() + jet5, + x.get() + jet6, + x.get() + jet7, + x.get() + jet8, + x.get() + jet9, }; JetT *output = x.get() + jet6; @@ -333,10 +405,14 @@ struct AutoDiff { CERES_MAKE_1ST_ORDER_PERTURBATION(3); CERES_MAKE_1ST_ORDER_PERTURBATION(4); CERES_MAKE_1ST_ORDER_PERTURBATION(5); + CERES_MAKE_1ST_ORDER_PERTURBATION(6); + CERES_MAKE_1ST_ORDER_PERTURBATION(7); + CERES_MAKE_1ST_ORDER_PERTURBATION(8); + CERES_MAKE_1ST_ORDER_PERTURBATION(9); #undef CERES_MAKE_1ST_ORDER_PERTURBATION if (!VariadicEvaluate::Call( + N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( functor, unpacked_parameters, output)) { return false; } @@ -359,6 +435,10 @@ struct AutoDiff { CERES_TAKE_1ST_ORDER_PERTURBATION(3); CERES_TAKE_1ST_ORDER_PERTURBATION(4); CERES_TAKE_1ST_ORDER_PERTURBATION(5); + CERES_TAKE_1ST_ORDER_PERTURBATION(6); + CERES_TAKE_1ST_ORDER_PERTURBATION(7); + CERES_TAKE_1ST_ORDER_PERTURBATION(8); + CERES_TAKE_1ST_ORDER_PERTURBATION(9); #undef CERES_TAKE_1ST_ORDER_PERTURBATION return true; } diff --git a/include/ceres/sized_cost_function.h b/include/ceres/sized_cost_function.h index 2894a9fba..6bfc1af31 100644 --- a/include/ceres/sized_cost_function.h +++ b/include/ceres/sized_cost_function.h @@ -45,25 +45,29 @@ namespace ceres { template + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, + int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> class SizedCostFunction : public CostFunction { public: SizedCostFunction() { - // Sanity checking. CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC) << "Cost functions must have at least one residual block."; - CHECK_GT(N0, 0) - << "Cost functions must have at least one parameter block."; - CHECK((!N1 && !N2 && !N3 && !N4 && !N5) || - ((N1 > 0) && !N2 && !N3 && !N4 && !N5) || - ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) || - ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) || - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) || - ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0))) + // This block breaks the 80 column rule to keep it somewhat readable. + CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) << "Zero block cannot precede a non-zero block. Block sizes are " << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " - << N3 << ", " << N4 << ", " << N5; + << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " + << N8 << ", " << N9; set_num_residuals(kNumResiduals); @@ -75,6 +79,10 @@ class SizedCostFunction : public CostFunction { ADD_PARAMETER_BLOCK(N3); ADD_PARAMETER_BLOCK(N4); ADD_PARAMETER_BLOCK(N5); + ADD_PARAMETER_BLOCK(N6); + ADD_PARAMETER_BLOCK(N7); + ADD_PARAMETER_BLOCK(N8); + ADD_PARAMETER_BLOCK(N9); #undef ADD_PARAMETER_BLOCK } diff --git a/internal/ceres/autodiff_cost_function_test.cc b/internal/ceres/autodiff_cost_function_test.cc index 33e576f11..e98397aa2 100644 --- a/internal/ceres/autodiff_cost_function_test.cc +++ b/internal/ceres/autodiff_cost_function_test.cc @@ -51,7 +51,7 @@ class BinaryScalarCost { double a_; }; -TEST(AutoDiffResidualAndJacobian, BilinearDifferentiationTest) { +TEST(AutodiffCostFunction, BilinearDifferentiationTest) { CostFunction* cost_function = new AutoDiffCostFunction( new BinaryScalarCost(1.0)); @@ -73,20 +73,72 @@ TEST(AutoDiffResidualAndJacobian, BilinearDifferentiationTest) { double residuals = 0.0; cost_function->Evaluate(parameters, &residuals, NULL); - EXPECT_EQ(residuals, 10); + EXPECT_EQ(10.0, residuals); cost_function->Evaluate(parameters, &residuals, jacobians); - EXPECT_EQ(jacobians[0][0], 3); - EXPECT_EQ(jacobians[0][1], 4); - EXPECT_EQ(jacobians[1][0], 1); - EXPECT_EQ(jacobians[1][1], 2); + EXPECT_EQ(3, jacobians[0][0]); + EXPECT_EQ(4, jacobians[0][1]); + EXPECT_EQ(1, jacobians[1][0]); + EXPECT_EQ(2, jacobians[1][1]); - delete []jacobians[0]; - delete []jacobians[1]; - delete []parameters[0]; - delete []parameters[1]; - delete []jacobians; - delete []parameters; + delete[] jacobians[0]; + delete[] jacobians[1]; + delete[] parameters[0]; + delete[] parameters[1]; + delete[] jacobians; + delete[] parameters; + delete cost_function; +} + +struct TenParameterCost { + template + bool operator()(const T* const x0, + const T* const x1, + const T* const x2, + const T* const x3, + const T* const x4, + const T* const x5, + const T* const x6, + const T* const x7, + const T* const x8, + const T* const x9, + T* cost) const { + cost[0] = *x0 + *x1 + *x2 + *x3 + *x4 + *x5 + *x6 + *x7 + *x8 + *x9; + return true; + } +}; + +TEST(AutodiffCostFunction, ManyParameterAutodiffInstantiates) { + CostFunction* cost_function = + new AutoDiffCostFunction< + TenParameterCost, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1>( + new TenParameterCost); + + double** parameters = new double*[10]; + double** jacobians = new double*[10]; + for (int i = 0; i < 10; ++i) { + parameters[i] = new double[1]; + parameters[i][0] = i; + jacobians[i] = new double[1]; + } + + double residuals = 0.0; + + cost_function->Evaluate(parameters, &residuals, NULL); + EXPECT_EQ(45.0, residuals); + + cost_function->Evaluate(parameters, &residuals, jacobians); + EXPECT_EQ(residuals, 45.0); + for (int i = 0; i < 10; ++i) { + EXPECT_EQ(1.0, jacobians[i][0]); + } + + for (int i = 0; i < 10; ++i) { + delete[] jacobians[i]; + delete[] parameters[i]; + } + delete[] jacobians; + delete[] parameters; delete cost_function; }