Sized cost function using variadic templates
This PR changes the interface of sized_cost_fucntion,
autodiff_cost_function and numeric_diff_costfunction from using ten
hardcoded parameter blocks to a variable number of parameter blocks
using variadic templates.
Trailing parameter blocks of size zero are now considered as error.
Change-Id: I37b9a0a420ef0eda6476a46672bbf6bd57e19760
diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h
index 23ed456..4a52cbc 100644
--- a/include/ceres/autodiff_cost_function.h
+++ b/include/ceres/autodiff_cost_function.h
@@ -110,10 +110,6 @@
// Dimension of x ------------------------------------+ |
// Dimension of y ---------------------------------------+
//
-// The framework can currently accommodate cost functions of up to 10
-// independent variables, and there is no limit on the dimensionality
-// of each of them.
-//
// WARNING #1: Since the functor will get instantiated with different types for
// T, you must convert from other numeric types to T before mixing
// computations with other variables of type T. In the example above, this is
@@ -153,19 +149,8 @@
// of residuals for a single autodiff cost function at runtime.
template <typename CostFunctor,
int kNumResiduals, // Number of residuals, or ceres::DYNAMIC.
- int N0, // Number of parameters in block 0.
- int N1 = 0, // Number of parameters in block 1.
- int N2 = 0, // Number of parameters in block 2.
- int N3 = 0, // Number of parameters in block 3.
- int N4 = 0, // Number of parameters in block 4.
- 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<kNumResiduals,
- N0, N1, N2, N3, N4,
- N5, N6, N7, N8, N9> {
+ int... Ns> // Number of parameters in each parameter block.
+class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
public:
// Takes ownership of functor. Uses the template-provided value for the
// number of residuals ("kNumResiduals").
@@ -186,10 +171,7 @@
CHECK_EQ(kNumResiduals, DYNAMIC)
<< "Can't run the dynamic-size constructor if the "
<< "number of residuals is not ceres::DYNAMIC.";
- SizedCostFunction<kNumResiduals,
- N0, N1, N2, N3, N4,
- N5, N6, N7, N8, N9>
- ::set_num_residuals(num_residuals);
+ SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
}
virtual ~AutoDiffCostFunction() {}
@@ -202,20 +184,20 @@
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
+ using ParameterDims =
+ typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
+
if (!jacobians) {
- return internal::VariadicEvaluate<
- CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
- ::Call(*functor_, parameters, residuals);
+ return internal::VariadicEvaluate<ParameterDims>(*functor_,
+ parameters,
+ residuals);
}
- return internal::AutoDiff<CostFunctor, double,
- N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate(
- *functor_,
- parameters,
- SizedCostFunction<kNumResiduals,
- N0, N1, N2, N3, N4,
- N5, N6, N7, N8, N9>::num_residuals(),
- residuals,
- jacobians);
+ return internal::AutoDifferentiate<ParameterDims>(
+ *functor_,
+ parameters,
+ SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
+ residuals,
+ jacobians);
}
private:
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h
index 257e937..649e05d 100644
--- a/include/ceres/autodiff_local_parameterization.h
+++ b/include/ceres/autodiff_local_parameterization.h
@@ -134,12 +134,9 @@
const double* parameter_ptrs[2] = {x, zero_delta};
double* jacobian_ptrs[2] = { NULL, jacobian };
- return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize>
- ::Differentiate(*functor_,
- parameter_ptrs,
- kGlobalSize,
- x_plus_delta,
- jacobian_ptrs);
+ return internal::AutoDifferentiate<
+ internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
+ *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
}
virtual int GlobalSize() const { return kGlobalSize; }
diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h
index d5806ee..33ac5e1 100644
--- a/include/ceres/dynamic_numeric_diff_cost_function.h
+++ b/include/ceres/dynamic_numeric_diff_cost_function.h
@@ -42,6 +42,7 @@
#include "ceres/dynamic_cost_function.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/numeric_diff.h"
+#include "ceres/internal/parameter_dims.h"
#include "ceres/numeric_diff_options.h"
#include "glog/logging.h"
@@ -103,7 +104,11 @@
<< "You must call DynamicNumericDiffCostFunction::AddParameterBlock() "
<< "before DynamicNumericDiffCostFunction::Evaluate().";
- const bool status = EvaluateCostFunctor(parameters, residuals);
+ const bool status =
+ internal::VariadicEvaluate<internal::DynamicParameterDims>(
+ *functor_.get(),
+ parameters,
+ residuals);
if (jacobians == NULL || !status) {
return status;
}
@@ -127,18 +132,18 @@
for (size_t block = 0; block < block_sizes.size(); ++block) {
if (jacobians[block] != NULL &&
- !NumericDiff<CostFunctor, method, DYNAMIC,
- DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
- DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
- DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock(
- functor_.get(),
- residuals,
- options_,
- this->num_residuals(),
- block,
- block_sizes[block],
- ¶meters_references_copy[0],
- jacobians[block])) {
+ !NumericDiff<CostFunctor, method, ceres::DYNAMIC,
+ internal::DynamicParameterDims, ceres::DYNAMIC,
+ ceres::DYNAMIC>::
+ EvaluateJacobianForParameterBlock(
+ functor_.get(),
+ residuals,
+ options_,
+ this->num_residuals(),
+ block,
+ block_sizes[block],
+ ¶meters_references_copy[0],
+ jacobians[block])) {
return false;
}
}
@@ -146,30 +151,6 @@
}
private:
- bool EvaluateCostFunctor(double const* const* parameters,
- double* residuals) const {
- return EvaluateCostFunctorImpl(functor_.get(),
- parameters,
- residuals,
- functor_.get());
- }
-
- // Helper templates to allow evaluation of a functor or a
- // CostFunction.
- bool EvaluateCostFunctorImpl(const CostFunctor* functor,
- double const* const* parameters,
- double* residuals,
- const void* /* NOT USED */) const {
- return (*functor)(parameters, residuals);
- }
-
- bool EvaluateCostFunctorImpl(const CostFunctor* functor,
- double const* const* parameters,
- double* residuals,
- const CostFunction* /* NOT USED */) const {
- return functor->Evaluate(parameters, residuals, NULL);
- }
-
std::unique_ptr<const CostFunctor> functor_;
Ownership ownership_;
NumericDiffOptions options_;
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h
index 903c89b..ff47fbf 100644
--- a/include/ceres/internal/autodiff.h
+++ b/include/ceres/internal/autodiff.h
@@ -33,7 +33,7 @@
// dual numbers in jet.h. Before reading the rest of this file, it is advisable
// to read jet.h's header comment in detail.
//
-// The helper wrapper AutoDiff::Differentiate() computes the jacobian of
+// The helper wrapper AutoDifferentiate() computes the jacobian of
// functors with templated operator() taking this form:
//
// struct F {
@@ -142,10 +142,14 @@
#include <stddef.h>
-#include "ceres/jet.h"
+#include <array>
+
#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/parameter_dims.h"
#include "ceres/internal/variadic_evaluate.h"
+#include "ceres/jet.h"
+#include "ceres/types.h"
#include "glog/logging.h"
namespace ceres {
@@ -165,21 +169,51 @@
//
// is what would get put in dst if N was 3, offset was 3, and the jet type JetT
// was 8-dimensional.
-template <typename JetT, typename T, int N>
-inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) {
+template <int Offset, int N, typename T, typename JetT>
+inline void Make1stOrderPerturbation(const T* src, JetT* dst) {
DCHECK(src);
DCHECK(dst);
for (int j = 0; j < N; ++j) {
dst[j].a = src[j];
dst[j].v.setZero();
- dst[j].v[offset + j] = T(1.0);
+ dst[j].v[Offset + j] = T(1.0);
}
}
+// Calls Make1stOrderPerturbation for every parameter block.
+//
+// Example:
+// If one having three parameter blocks with dimensions (3, 2, 4), the call
+// Make1stOrderPerturbations<integer_sequence<3, 2, 4>::Apply(params, x);
+// will result in the following calls to Make1stOrderPerturbation:
+// Make1stOrderPerturbation<0, 3>(params[0], x + 0);
+// Make1stOrderPerturbation<3, 2>(params[1], x + 3);
+// Make1stOrderPerturbation<5, 4>(params[2], x + 5);
+template <typename Seq, int ParameterIdx = 0, int Offset = 0>
+struct Make1stOrderPerturbations;
+
+template <int N, int... Ns, int ParameterIdx, int Offset>
+struct Make1stOrderPerturbations<integer_sequence<int, N, Ns...>, ParameterIdx,
+ Offset> {
+ template <typename T, typename JetT>
+ static void Apply(T const* const* parameters, JetT* x) {
+ Make1stOrderPerturbation<Offset, N>(parameters[ParameterIdx], x + Offset);
+ Make1stOrderPerturbations<integer_sequence<int, Ns...>, ParameterIdx + 1,
+ Offset + N>::Apply(parameters, x);
+ }
+};
+
+// End of 'recursion'. Nothing more to do.
+template <int ParameterIdx, int Total>
+struct Make1stOrderPerturbations<integer_sequence<int>, ParameterIdx, Total> {
+ template <typename T, typename JetT>
+ static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {}
+};
+
// Takes the 0th order part of src, assumed to be a Jet type, and puts it in
// dst. This is used to pick out the "vector" part of the extended y.
template <typename JetT, typename T>
-inline void Take0thOrderPart(int M, const JetT *src, T dst) {
+inline void Take0thOrderPart(int M, const JetT* src, T dst) {
DCHECK(src);
for (int i = 0; i < M; ++i) {
dst[i] = src[i].a;
@@ -188,8 +222,8 @@
// Takes N 1st order parts, starting at index N0, and puts them in the M x N
// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
-template <typename JetT, typename T, int N0, int N>
-inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
+template <int N0, int N, typename JetT, typename T>
+inline void Take1stOrderPart(const int M, const JetT* src, T* dst) {
DCHECK(src);
DCHECK(dst);
for (int i = 0; i < M; ++i) {
@@ -198,126 +232,86 @@
}
}
-// This is in a struct because default template parameters on a
-// function are not supported in C++03 (though it is available in
-// C++0x). N0 through N9 are the dimension of the input arguments to
-// the user supplied functor.
-template <typename Functor, typename T,
- 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) {
- // This block breaks the 80 column rule to keep it somewhat readable.
- DCHECK_GT(num_outputs, 0);
- DCHECK((!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) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT
- << "Zero block cannot precede a non-zero block. Block sizes are "
- << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
- << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
- << N8 << ", " << N9;
+// Calls Take1stOrderPart for every parameter block.
+//
+// Example:
+// If one having three parameter blocks with dimensions (3, 2, 4), the call
+// Take1stOrderParts<integer_sequence<3, 2, 4>::Apply(num_outputs,
+// output,
+// jacobians);
+// will result in the following calls to Take1stOrderPart:
+// if (jacobians[0]) {
+// Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]);
+// }
+// if (jacobians[1]) {
+// Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]);
+// }
+// if (jacobians[2]) {
+// Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]);
+// }
+template <typename Seq, int ParameterIdx = 0, int Offset = 0>
+struct Take1stOrderParts;
- typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT;
- FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
- N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
-
- // 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[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() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
-
- // Invalidate the output Jets, so that we can detect if the user
- // did not assign values to all of them.
- for (int i = 0; i < num_outputs; ++i) {
- output[i].a = kImpossibleValue;
- output[i].v.setConstant(kImpossibleValue);
+template <int N, int... Ns, int ParameterIdx, int Offset>
+struct Take1stOrderParts<integer_sequence<int, N, Ns...>, ParameterIdx,
+ Offset> {
+ template <typename JetT, typename T>
+ static void Apply(int num_outputs, JetT* output, T** jacobians) {
+ if (jacobians[ParameterIdx]) {
+ Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]);
}
-
-#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \
- if (N ## i) { \
- internal::Make1stOrderPerturbation<JetT, T, N ## i>( \
- jet ## i, \
- parameters[i], \
- x.get() + jet ## i); \
- }
- CERES_MAKE_1ST_ORDER_PERTURBATION(0);
- CERES_MAKE_1ST_ORDER_PERTURBATION(1);
- CERES_MAKE_1ST_ORDER_PERTURBATION(2);
- 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<Functor, JetT,
- N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
- functor, unpacked_parameters, output)) {
- return false;
- }
-
- internal::Take0thOrderPart(num_outputs, output, function_value);
-
-#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
- if (N ## i) { \
- if (jacobians[i]) { \
- internal::Take1stOrderPart<JetT, T, \
- jet ## i, \
- N ## i>(num_outputs, \
- output, \
- jacobians[i]); \
- } \
- }
- CERES_TAKE_1ST_ORDER_PERTURBATION(0);
- CERES_TAKE_1ST_ORDER_PERTURBATION(1);
- CERES_TAKE_1ST_ORDER_PERTURBATION(2);
- 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;
+ Take1stOrderParts<integer_sequence<int, Ns...>, ParameterIdx + 1,
+ Offset + N>::Apply(num_outputs, output, jacobians);
}
};
+// End of 'recursion'. Nothing more to do.
+template <int ParameterIdx, int Offset>
+struct Take1stOrderParts<integer_sequence<int>, ParameterIdx, Offset> {
+ template <typename T, typename JetT>
+ static void Apply(int /* NOT USED*/, JetT* /* NOT USED*/,
+ T** /* NOT USED */) {}
+};
+
+template <typename ParameterDims, typename Functor, typename T>
+inline bool AutoDifferentiate(const Functor& functor,
+ T const *const *parameters,
+ int num_outputs,
+ T* function_value,
+ T** jacobians) {
+ DCHECK_GT(num_outputs, 0);
+
+ typedef Jet<T, ParameterDims::kNumParameters> JetT;
+ FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(ParameterDims::kNumParameters +
+ num_outputs);
+
+ using Parameters = typename ParameterDims::Parameters;
+
+ // These are the positions of the respective jets in the fixed array x.
+ std::array<JetT*, ParameterDims::kNumParameterBlocks> unpacked_parameters =
+ ParameterDims::GetUnpackedParameters(x.get());
+ JetT* output = x.get() + ParameterDims::kNumParameters;
+
+ // Invalidate the output Jets, so that we can detect if the user
+ // did not assign values to all of them.
+ for (int i = 0; i < num_outputs; ++i) {
+ output[i].a = kImpossibleValue;
+ output[i].v.setConstant(kImpossibleValue);
+ }
+
+ Make1stOrderPerturbations<Parameters>::Apply(parameters, x.get());
+
+ if (!VariadicEvaluate<ParameterDims>(functor, unpacked_parameters.data(),
+ output)) {
+ return false;
+ }
+
+ Take0thOrderPart(num_outputs, output, function_value);
+ Take1stOrderParts<Parameters>::Apply(num_outputs, output, jacobians);
+
+ return true;
+}
+
} // namespace internal
} // namespace ceres
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h
index ab1abc0..f24fdb3 100644
--- a/include/ceres/internal/numeric_diff.h
+++ b/include/ceres/internal/numeric_diff.h
@@ -50,42 +50,11 @@
namespace ceres {
namespace internal {
-// Helper templates that allow evaluation of a variadic functor or a
-// CostFunction object.
-template <typename CostFunctor,
- int N0, int N1, int N2, int N3, int N4,
- int N5, int N6, int N7, int N8, int N9 >
-bool EvaluateImpl(const CostFunctor* functor,
- double const* const* parameters,
- double* residuals,
- const void* /* NOT USED */) {
- return VariadicEvaluate<CostFunctor,
- double,
- N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
- *functor,
- parameters,
- residuals);
-}
-
-template <typename CostFunctor,
- int N0, int N1, int N2, int N3, int N4,
- int N5, int N6, int N7, int N8, int N9 >
-bool EvaluateImpl(const CostFunctor* functor,
- double const* const* parameters,
- double* residuals,
- const CostFunction* /* NOT USED */) {
- return functor->Evaluate(parameters, residuals, NULL);
-}
-
// This is split from the main class because C++ doesn't allow partial template
// specializations for member functions. The alternative is to repeat the main
// class for differing numbers of parameters, which is also unfortunate.
-template <typename CostFunctor,
- NumericDiffMethodType kMethod,
- int kNumResiduals,
- int N0, int N1, int N2, int N3, int N4,
- int N5, int N6, int N7, int N8, int N9,
- int kParameterBlock,
+template <typename CostFunctor, NumericDiffMethodType kMethod,
+ int kNumResiduals, typename ParameterDims, int kParameterBlock,
int kParameterBlockSize>
struct NumericDiff {
// Mutates parameters but must restore them before return.
@@ -219,8 +188,9 @@
// Mutate 1 element at a time and then restore.
x_plus_delta(parameter_index) = x(parameter_index) + delta;
- if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
- functor, parameters, residuals.data(), functor)) {
+ if (!VariadicEvaluate<ParameterDims>(*functor,
+ parameters,
+ residuals.data())) {
return false;
}
@@ -233,8 +203,9 @@
// Compute the function on the other side of x(parameter_index).
x_plus_delta(parameter_index) = x(parameter_index) - delta;
- if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
- functor, parameters, temp_residuals.data(), functor)) {
+ if (!VariadicEvaluate<ParameterDims>(*functor,
+ parameters,
+ temp_residuals.data())) {
return false;
}
@@ -406,35 +377,91 @@
}
};
-template <typename CostFunctor,
- NumericDiffMethodType kMethod,
- int kNumResiduals,
- int N0, int N1, int N2, int N3, int N4,
- int N5, int N6, int N7, int N8, int N9,
- int kParameterBlock>
-struct NumericDiff<CostFunctor, kMethod, kNumResiduals,
- N0, N1, N2, N3, N4, N5, N6, N7, N8, N9,
- kParameterBlock, 0> {
- // Mutates parameters but must restore them before return.
- static bool EvaluateJacobianForParameterBlock(
- const CostFunctor* functor,
- const double* residuals_at_eval_point,
- const NumericDiffOptions& options,
- const int num_residuals,
- const int parameter_block_index,
- const int parameter_block_size,
- double **parameters,
- double *jacobian) {
- // Silence unused parameter compiler warnings.
- (void)functor;
- (void)residuals_at_eval_point;
- (void)options;
- (void)num_residuals;
- (void)parameter_block_index;
- (void)parameter_block_size;
- (void)parameters;
- (void)jacobian;
- LOG(FATAL) << "Control should never reach here.";
+// This function calls NumericDiff<...>::EvaluateJacobianForParameterBlock for
+// each parameter block.
+//
+// Example:
+// A call to
+// EvaluateJacobianForParameterBlocks<StaticParameterDims<2, 3>>(
+// functor,
+// residuals_at_eval_point,
+// options,
+// num_residuals,
+// parameters,
+// jacobians);
+// will result in the following calls to
+// NumericDiff<...>::EvaluateJacobianForParameterBlock:
+//
+// if (!NumericDiff<
+// CostFunctor, method, kNumResiduals, ParameterDims, 0,
+// 2>::EvaluateJacobianForParameterBlock(functor,
+// residuals_at_eval_point,
+// options,
+// num_residuals,
+// 0,
+// 2,
+// parameters,
+// jacobians[0])) {
+// return false;
+// }
+// if (!NumericDiff<
+// CostFunctor, method, kNumResiduals, ParameterDims, 1,
+// 3>::EvaluateJacobianForParameterBlock(functor,
+// residuals_at_eval_point,
+// options,
+// num_residuals,
+// 1,
+// 3,
+// parameters,
+// jacobians[1])) {
+// return false;
+// }
+template <typename ParameterDims,
+ typename Parameters = typename ParameterDims::Parameters,
+ int ParameterIdx = 0>
+struct EvaluateJacobianForParameterBlocks;
+
+template <typename ParameterDims, int N, int... Ns, int ParameterIdx>
+struct EvaluateJacobianForParameterBlocks<
+ ParameterDims, integer_sequence<int, N, Ns...>, ParameterIdx> {
+ template <NumericDiffMethodType method, int kNumResiduals,
+ typename CostFunctor>
+ static bool Apply(const CostFunctor* functor,
+ const double* residuals_at_eval_point,
+ const NumericDiffOptions& options, int num_residuals,
+ double** parameters, double** jacobians) {
+ if (!NumericDiff<
+ CostFunctor, method, kNumResiduals, ParameterDims, ParameterIdx,
+ N>::EvaluateJacobianForParameterBlock(functor,
+ residuals_at_eval_point,
+ options,
+ num_residuals,
+ ParameterIdx,
+ N,
+ parameters,
+ jacobians[ParameterIdx])) {
+ return false;
+ }
+ return EvaluateJacobianForParameterBlocks<
+ ParameterDims, integer_sequence<int, Ns...>, ParameterIdx + 1>::
+ template Apply<method, kNumResiduals>(functor,
+ residuals_at_eval_point,
+ options,
+ num_residuals,
+ parameters, jacobians);
+ }
+};
+
+// End of 'recursion'. Nothing more to do.
+template <typename ParameterDims, int ParameterIdx>
+struct EvaluateJacobianForParameterBlocks<ParameterDims, integer_sequence<int>,
+ ParameterIdx> {
+ template <NumericDiffMethodType method, int kNumResiduals,
+ typename CostFunctor>
+ static bool Apply(const CostFunctor* /* NOT USED*/,
+ const double* /* NOT USED*/,
+ const NumericDiffOptions& /* NOT USED*/, int /* NOT USED*/,
+ double** /* NOT USED*/, double** /* NOT USED*/) {
return true;
}
};
diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h
index b3515b9..26428d0 100644
--- a/include/ceres/internal/variadic_evaluate.h
+++ b/include/ceres/internal/variadic_evaluate.h
@@ -28,165 +28,76 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
// mierle@gmail.com (Keir Mierle)
+// jodebo_beck@gmx.de (Johannes Beck)
#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#include <stddef.h>
-#include "ceres/jet.h"
-#include "ceres/types.h"
-#include "ceres/internal/eigen.h"
-#include "ceres/internal/fixed_array.h"
-#include "glog/logging.h"
+#include <type_traits>
+
+#include "ceres/cost_function.h"
+#include "ceres/internal/parameter_dims.h"
namespace ceres {
namespace internal {
-// This block of quasi-repeated code calls the user-supplied functor, which may
-// take a variable number of arguments. This is accomplished by specializing the
-// struct based on the size of the trailing parameters; parameters with 0 size
-// are assumed missing.
-template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
- int N5, int N6, int N7, int N8, int N9>
-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);
- }
-};
+// For fixed size cost functors
+template <typename Functor, typename T, int... Indices>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output, std::false_type /*is_dynamic*/,
+ integer_sequence<int, Indices...>) {
+ static_assert(sizeof...(Indices),
+ "Invalid number of parameter blocks. At least one parameter "
+ "block must be specified.");
+ return functor(input[Indices]..., output);
+}
-template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
- int N5, int N6, int N7, int N8>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> {
- 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);
- }
-};
+// For dynamic sized cost functors
+template <typename Functor, typename T>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output, std::true_type /*is_dynamic*/,
+ integer_sequence<int>) {
+ return functor(input, output);
+}
-template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
- int N5, int N6, int N7>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> {
- 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);
- }
-};
+// For ceres cost functors (not ceres::CostFunction)
+template <typename ParameterDims, typename Functor, typename T>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output, const void* /* NOT USED */) {
+ using ParameterBlockIndices =
+ make_integer_sequence<int, ParameterDims::kNumParameterBlocks>;
+ using IsDynamic = std::integral_constant<bool, ParameterDims::kIsDynamic>;
+ return VariadicEvaluateImpl(functor, input, output, IsDynamic(),
+ ParameterBlockIndices());
+}
-template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
- int N5, int N6>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> {
- 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);
- }
-};
+// For ceres::CostFunction
+template <typename ParameterDims, typename Functor, typename T>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output,
+ const CostFunction* /* NOT USED */) {
+ return functor.Evaluate(input, output, nullptr);
+}
-template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
- int N5>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> {
- 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],
- output);
- }
-};
-
-template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> {
- static bool Call(const Functor& functor, T const *const *input, T* output) {
- return functor(input[0],
- input[1],
- input[2],
- input[3],
- input[4],
- output);
- }
-};
-
-template<typename Functor, typename T, int N0, int N1, int N2, int N3>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> {
- static bool Call(const Functor& functor, T const *const *input, T* output) {
- return functor(input[0],
- input[1],
- input[2],
- input[3],
- output);
- }
-};
-
-template<typename Functor, typename T, int N0, int N1, int N2>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> {
- static bool Call(const Functor& functor, T const *const *input, T* output) {
- return functor(input[0],
- input[1],
- input[2],
- output);
- }
-};
-
-template<typename Functor, typename T, int N0, int N1>
-struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> {
- static bool Call(const Functor& functor, T const *const *input, T* output) {
- return functor(input[0],
- input[1],
- output);
- }
-};
-
-template<typename Functor, typename T, int N0>
-struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> {
- static bool Call(const Functor& functor, T const *const *input, T* output) {
- return functor(input[0],
- output);
- }
-};
-
-// Template instantiation for dynamically-sized functors.
-template<typename Functor, typename T>
-struct VariadicEvaluate<Functor, T, ceres::DYNAMIC, ceres::DYNAMIC,
- ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
- ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
- ceres::DYNAMIC, ceres::DYNAMIC> {
- static bool Call(const Functor& functor, T const *const *input, T* output) {
- return functor(input, output);
- }
-};
+// Variadic evaluate is a helper function to evaluate ceres cost function or
+// functors using an input, output and the parameter dimensions. There are
+// several ways different possibilities:
+// 1) If the passed functor is a 'ceres::CostFunction' its evaluate method is
+// called.
+// 2) If the functor is not a 'ceres::CostFunction' and the specified parameter
+// dims is dynamic, the functor must have the following signature
+// 'bool(T const* const* input, T* output)'.
+// 3) If the functor is not a 'ceres::CostFunction' and the specified parameter
+// dims is not dynamic, the input is expanded by using the number of parameter
+// blocks. The signature of the functor must have the following signature
+// 'bool()(const T* i_1, const T* i_2, ... const T* i_n, T* output)'.
+template <typename ParameterDims, typename Functor, typename T>
+inline bool VariadicEvaluate(const Functor& functor, T const* const* input,
+ T* output) {
+ return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor);
+}
} // namespace internal
} // namespace ceres
diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h
index 6ec86fa..f6cd32a 100644
--- a/include/ceres/numeric_diff_cost_function.h
+++ b/include/ceres/numeric_diff_cost_function.h
@@ -52,8 +52,8 @@
// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
// the squaring is implicitly done by the optimization framework.
//
-// To write an numerically-differentiable cost function for the above model, first
-// define the object
+// To write an numerically-differentiable cost function for the above model,
+// first define the object
//
// class MyScalarCostFunctor {
// explicit MyScalarCostFunctor(double k): k_(k) {}
@@ -110,10 +110,6 @@
// Dimension of x ------------------------------------------------+ |
// Dimension of y ---------------------------------------------------+
//
-// The framework can currently accommodate cost functions of up to 10
-// independent variables, and there is no limit on the dimensionality
-// of each of them.
-//
// The central difference method is considerably more accurate at the cost of
// twice as many function evaluations than forward difference. Consider using
// central differences begin with, and only after that works, trying forward
@@ -161,10 +157,13 @@
#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
+#include <array>
#include <memory>
+
#include "Eigen/Dense"
#include "ceres/cost_function.h"
#include "ceres/internal/numeric_diff.h"
+#include "ceres/internal/parameter_dims.h"
#include "ceres/numeric_diff_options.h"
#include "ceres/sized_cost_function.h"
#include "ceres/types.h"
@@ -175,20 +174,8 @@
template <typename CostFunctor,
NumericDiffMethodType method = CENTRAL,
int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC
- int N0 = 0, // Number of parameters in block 0.
- int N1 = 0, // Number of parameters in block 1.
- int N2 = 0, // Number of parameters in block 2.
- int N3 = 0, // Number of parameters in block 3.
- int N4 = 0, // Number of parameters in block 4.
- 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 NumericDiffCostFunction
- : public SizedCostFunction<kNumResiduals,
- N0, N1, N2, N3, N4,
- N5, N6, N7, N8, N9> {
+ int... Ns> // Parameters dimensions for each block.
+class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
public:
NumericDiffCostFunction(
CostFunctor* functor,
@@ -199,10 +186,7 @@
ownership_(ownership),
options_(options) {
if (kNumResiduals == DYNAMIC) {
- SizedCostFunction<kNumResiduals,
- N0, N1, N2, N3, N4,
- N5, N6, N7, N8, N9>
- ::set_num_residuals(num_residuals);
+ SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
}
}
@@ -218,18 +202,17 @@
using internal::FixedArray;
using internal::NumericDiff;
- const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
- const int kNumParameterBlocks =
- (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) +
- (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
+ using ParameterDims =
+ typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
+ using Parameters = typename ParameterDims::Parameters;
- // Get the function value (residuals) at the point to evaluate.
- if (!internal::EvaluateImpl<CostFunctor,
- N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
- functor_.get(),
- parameters,
- residuals,
- functor_.get())) {
+ constexpr int kNumParameters = ParameterDims::kNumParameters;
+ constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks;
+
+ // Get the function value (residuals) at the the point to evaluate.
+ if (!internal::VariadicEvaluate<ParameterDims>(*functor_,
+ parameters,
+ residuals)) {
return false;
}
@@ -239,71 +222,22 @@
// Create a copy of the parameters which will get mutated.
FixedArray<double> parameters_copy(kNumParameters);
- FixedArray<double*> parameters_reference_copy(kNumParameterBlocks);
+ std::array<double*, kNumParameterBlocks> parameters_reference_copy =
+ ParameterDims::GetUnpackedParameters(parameters_copy.get());
- parameters_reference_copy[0] = parameters_copy.get();
- if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0;
- if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1;
- if (N3) parameters_reference_copy[3] = parameters_reference_copy[2] + N2;
- if (N4) parameters_reference_copy[4] = parameters_reference_copy[3] + N3;
- if (N5) parameters_reference_copy[5] = parameters_reference_copy[4] + N4;
- if (N6) parameters_reference_copy[6] = parameters_reference_copy[5] + N5;
- if (N7) parameters_reference_copy[7] = parameters_reference_copy[6] + N6;
- if (N8) parameters_reference_copy[8] = parameters_reference_copy[7] + N7;
- if (N9) parameters_reference_copy[9] = parameters_reference_copy[8] + N8;
-
-#define CERES_COPY_PARAMETER_BLOCK(block) \
- if (N ## block) memcpy(parameters_reference_copy[block], \
- parameters[block], \
- sizeof(double) * N ## block); // NOLINT
-
- CERES_COPY_PARAMETER_BLOCK(0);
- CERES_COPY_PARAMETER_BLOCK(1);
- CERES_COPY_PARAMETER_BLOCK(2);
- CERES_COPY_PARAMETER_BLOCK(3);
- CERES_COPY_PARAMETER_BLOCK(4);
- CERES_COPY_PARAMETER_BLOCK(5);
- CERES_COPY_PARAMETER_BLOCK(6);
- CERES_COPY_PARAMETER_BLOCK(7);
- CERES_COPY_PARAMETER_BLOCK(8);
- CERES_COPY_PARAMETER_BLOCK(9);
-
-#undef CERES_COPY_PARAMETER_BLOCK
-
-#define CERES_EVALUATE_JACOBIAN_FOR_BLOCK(block) \
- if (N ## block && jacobians[block] != NULL) { \
- if (!NumericDiff<CostFunctor, \
- method, \
- kNumResiduals, \
- N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, \
- block, \
- N ## block >::EvaluateJacobianForParameterBlock( \
- functor_.get(), \
- residuals, \
- options_, \
- SizedCostFunction<kNumResiduals, \
- N0, N1, N2, N3, N4, \
- N5, N6, N7, N8, N9>::num_residuals(), \
- block, \
- N ## block, \
- parameters_reference_copy.get(), \
- jacobians[block])) { \
- return false; \
- } \
+ for (int block = 0; block < kNumParameterBlocks; ++block) {
+ memcpy(parameters_reference_copy[block], parameters[block],
+ sizeof(double) * ParameterDims::GetDim(block));
}
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8);
- CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9);
-
-#undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK
+ internal::EvaluateJacobianForParameterBlocks<ParameterDims>::template Apply<
+ method, kNumResiduals>(
+ functor_.get(),
+ residuals,
+ options_,
+ SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
+ parameters_reference_copy.data(),
+ jacobians);
return true;
}
diff --git a/include/ceres/sized_cost_function.h b/include/ceres/sized_cost_function.h
index f9a984f..50d0363 100644
--- a/include/ceres/sized_cost_function.h
+++ b/include/ceres/sized_cost_function.h
@@ -41,49 +41,24 @@
#include "ceres/cost_function.h"
#include "ceres/types.h"
#include "glog/logging.h"
+#include "internal/parameter_dims.h"
namespace ceres {
-template<int kNumResiduals,
- 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>
+template <int kNumResiduals, int... Ns>
class SizedCostFunction : public CostFunction {
public:
+ static_assert(kNumResiduals > 0 || kNumResiduals == DYNAMIC,
+ "Cost functions must have at least one residual block.");
+ static_assert(internal::StaticParameterDims<Ns...>::kIsValid,
+ "Invalid parameter block dimension detected. Each parameter "
+ "block dimension must be bigger than zero.");
+
+ using ParameterDims = internal::StaticParameterDims<Ns...>;
+
SizedCostFunction() {
- CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC)
- << "Cost functions must have at least one residual block.";
-
- // 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) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT
- ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT
- << "Zero block cannot precede a non-zero block. Block sizes are "
- << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
- << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
- << N8 << ", " << N9;
-
set_num_residuals(kNumResiduals);
-
-#define CERES_ADD_PARAMETER_BLOCK(N) \
- if (N) mutable_parameter_block_sizes()->push_back(N);
- CERES_ADD_PARAMETER_BLOCK(N0);
- CERES_ADD_PARAMETER_BLOCK(N1);
- CERES_ADD_PARAMETER_BLOCK(N2);
- CERES_ADD_PARAMETER_BLOCK(N3);
- CERES_ADD_PARAMETER_BLOCK(N4);
- CERES_ADD_PARAMETER_BLOCK(N5);
- CERES_ADD_PARAMETER_BLOCK(N6);
- CERES_ADD_PARAMETER_BLOCK(N7);
- CERES_ADD_PARAMETER_BLOCK(N8);
- CERES_ADD_PARAMETER_BLOCK(N9);
-#undef CERES_ADD_PARAMETER_BLOCK
+ *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};
}
virtual ~SizedCostFunction() { }
diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h
index 76e4a24..cc93d73 100644
--- a/include/ceres/tiny_solver_autodiff_function.h
+++ b/include/ceres/tiny_solver_autodiff_function.h
@@ -120,7 +120,7 @@
NUM_RESIDUALS = kNumResiduals,
};
- // This is similar to AutoDiff::Differentiate(), but since there is only one
+ // This is similar to AutoDifferentiate(), but since there is only one
// parameter block it is easier to inline to avoid overhead.
bool operator()(const T* parameters,
T* residuals,
diff --git a/internal/ceres/autodiff_test.cc b/internal/ceres/autodiff_test.cc
index 20f4437..04a77ea 100644
--- a/internal/ceres/autodiff_test.cc
+++ b/internal/ceres/autodiff_test.cc
@@ -194,7 +194,7 @@
{
double *parameters[] = { PX };
double *jacobians[] = { J_PX };
- ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate(
+ ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12 + 4>>(
b, parameters, 2, ad_x1, jacobians)));
for (int i = 0; i < 2; ++i) {
@@ -209,7 +209,7 @@
double J_X[2 * 4];
double *parameters[] = { P, X };
double *jacobians[] = { J_P, J_X };
- ASSERT_TRUE((AutoDiff<Projective, double, 12, 4>::Differentiate(
+ ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12, 4>>(
b, parameters, 2, ad_x2, jacobians)));
for (int i = 0; i < 2; ++i) {
@@ -316,7 +316,7 @@
double J_X[2 * 3];
double *parameters[] = { q, c, X };
double *jacobians[] = { J_q, J_c, J_X };
- ASSERT_TRUE((AutoDiff<Metric, double, 4, 3, 3>::Differentiate(
+ ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<4, 3, 3>>(
b, parameters, 2, ad_x, jacobians)));
for (int i = 0; i < 2; ++i) {
@@ -366,7 +366,7 @@
functor.num_residuals = num_residuals;
// Run autodiff with the new number of residuals.
- ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate(
+ ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<2>>(
functor, parameters, num_residuals, residuals, jacobians)));
const double kTolerance = 1e-14;
@@ -528,8 +528,8 @@
{
Residual1Param functor;
int num_variables = 1;
- EXPECT_TRUE((AutoDiff<Residual1Param, double, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -539,8 +539,8 @@
{
Residual2Param functor;
int num_variables = 2;
- EXPECT_TRUE((AutoDiff<Residual2Param, double, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -550,8 +550,8 @@
{
Residual3Param functor;
int num_variables = 3;
- EXPECT_TRUE((AutoDiff<Residual3Param, double, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -561,8 +561,8 @@
{
Residual4Param functor;
int num_variables = 4;
- EXPECT_TRUE((AutoDiff<Residual4Param, double, 1, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -572,8 +572,8 @@
{
Residual5Param functor;
int num_variables = 5;
- EXPECT_TRUE((AutoDiff<Residual5Param, double, 1, 1, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -583,10 +583,8 @@
{
Residual6Param functor;
int num_variables = 6;
- EXPECT_TRUE((AutoDiff<Residual6Param,
- double,
- 1, 1, 1, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -596,10 +594,8 @@
{
Residual7Param functor;
int num_variables = 7;
- EXPECT_TRUE((AutoDiff<Residual7Param,
- double,
- 1, 1, 1, 1, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -609,10 +605,8 @@
{
Residual8Param functor;
int num_variables = 8;
- EXPECT_TRUE((AutoDiff<
- Residual8Param,
- double, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -622,11 +616,9 @@
{
Residual9Param functor;
int num_variables = 9;
- EXPECT_TRUE((AutoDiff<
- Residual9Param,
- double,
- 1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE(
+ (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -636,11 +628,9 @@
{
Residual10Param functor;
int num_variables = 10;
- EXPECT_TRUE((AutoDiff<
- Residual10Param,
- double,
- 1, 1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
- functor, parameters, 1, &residual, jacobians)));
+ EXPECT_TRUE(
+ (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1, 1>>(
+ functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc
index a7833d8..18b7e8c 100644
--- a/internal/ceres/local_parameterization_test.cc
+++ b/internal/ceres/local_parameterization_test.cc
@@ -265,14 +265,12 @@
double* jacobian_array[2] = { NULL, jacobian_ref };
// Autodiff jacobian at delta_x = 0.
- internal::AutoDiff<Plus,
- double,
- kGlobalSize,
- kLocalSize>::Differentiate(Plus(),
- parameters,
- kGlobalSize,
- x_plus_delta,
- jacobian_array);
+ internal::AutoDifferentiate<StaticParameterDims<kGlobalSize, kLocalSize>>(
+ Plus(),
+ parameters,
+ kGlobalSize,
+ x_plus_delta,
+ jacobian_array);
double jacobian[12];
parameterization.ComputeJacobian(x, jacobian);
diff --git a/internal/ceres/parameter_dims_test.cc b/internal/ceres/parameter_dims_test.cc
index 9a95cf5..f33536f 100644
--- a/internal/ceres/parameter_dims_test.cc
+++ b/internal/ceres/parameter_dims_test.cc
@@ -89,11 +89,11 @@
constexpr int N1 = 4;
constexpr int N2 = 2;
- using Params = StaticParameterDims<N0, N1, N2>;
+ using ParameterDims = StaticParameterDims<N0, N1, N2>;
- std::array<double, Params::kNumParameters> packed_parameters{};
+ std::array<double, ParameterDims::kNumParameters> packed_parameters{};
std::array<double*, 3> unpacked_parameters =
- Params::GetUnpackedParameters(packed_parameters.data());
+ ParameterDims::GetUnpackedParameters(packed_parameters.data());
EXPECT_EQ(packed_parameters.data(), unpacked_parameters[0]);
EXPECT_EQ(packed_parameters.data() + N0, unpacked_parameters[1]);
