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

Sized cost function using variadic templates

Browse Source 
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
This commit is contained in:
Johannes Beck
2018-10-13 22:34:53 +02:00
parent f8e8951611
commit 8eef94de4e
12 changed files with 380 additions and 591 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/internal/numeric_diff.h
+93 -66
View File
@@ -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 @@ struct NumericDiff {
// 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 @@ struct NumericDiff {
// 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 @@ struct NumericDiff {
}
};
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;
}
};