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/numeric_diff_cost_function.h
+31 -97
View File
@@ -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 @@ namespace ceres {
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 @@ class NumericDiffCostFunction
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 @@ class NumericDiffCostFunction
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 @@ class NumericDiffCostFunction
// 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;
}