saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-31 01:20: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/autodiff_cost_function.h
+15 -33
View File
@@ -110,10 +110,6 @@
// Dimension of x ------------------------------------+ | // Dimension of x ------------------------------------+ |
// Dimension of y ---------------------------------------+ // 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 // WARNING #1: Since the functor will get instantiated with different types for
// T, you must convert from other numeric types to T before mixing // 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 // computations with other variables of type T. In the example above, this is
@@ -153,19 +149,8 @@ namespace ceres {
// of residuals for a single autodiff cost function at runtime. // of residuals for a single autodiff cost function at runtime.
template <typename CostFunctor, template <typename CostFunctor,
int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. int kNumResiduals, // Number of residuals, or ceres::DYNAMIC.
int N0, // Number of parameters in block 0. int... Ns> // Number of parameters in each parameter block.
int N1 = 0, // Number of parameters in block 1. class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
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> {
public: public:
// Takes ownership of functor. Uses the template-provided value for the // Takes ownership of functor. Uses the template-provided value for the
// number of residuals ("kNumResiduals"). // number of residuals ("kNumResiduals").
@@ -186,10 +171,7 @@ class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
CHECK_EQ(kNumResiduals, DYNAMIC) CHECK_EQ(kNumResiduals, DYNAMIC)
<< "Can't run the dynamic-size constructor if the " << "Can't run the dynamic-size constructor if the "
<< "number of residuals is not ceres::DYNAMIC."; << "number of residuals is not ceres::DYNAMIC.";
SizedCostFunction<kNumResiduals, SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9>
::set_num_residuals(num_residuals);
} }
virtual ~AutoDiffCostFunction() {} virtual ~AutoDiffCostFunction() {}
@@ -202,20 +184,20 @@ class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
virtual bool Evaluate(double const* const* parameters, virtual bool Evaluate(double const* const* parameters,
double* residuals, double* residuals,
double** jacobians) const { double** jacobians) const {
using ParameterDims =
typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
if (!jacobians) { if (!jacobians) {
return internal::VariadicEvaluate< return internal::VariadicEvaluate<ParameterDims>(*functor_,
CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> parameters,
::Call(*functor_, parameters, residuals); residuals);
} }
return internal::AutoDiff<CostFunctor, double, return internal::AutoDifferentiate<ParameterDims>(
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate( *functor_,
*functor_, parameters,
parameters, SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
SizedCostFunction<kNumResiduals, residuals,
N0, N1, N2, N3, N4, jacobians);
N5, N6, N7, N8, N9>::num_residuals(),
residuals,
jacobians);
} }
private: private:
include/ceres/autodiff_local_parameterization.h
+3 -6
View File
@@ -134,12 +134,9 @@ class AutoDiffLocalParameterization : public LocalParameterization {
const double* parameter_ptrs[2] = {x, zero_delta}; const double* parameter_ptrs[2] = {x, zero_delta};
double* jacobian_ptrs[2] = { NULL, jacobian }; double* jacobian_ptrs[2] = { NULL, jacobian };
return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize> return internal::AutoDifferentiate<
::Differentiate(*functor_, internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
parameter_ptrs, *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
kGlobalSize,
x_plus_delta,
jacobian_ptrs);
} }
virtual int GlobalSize() const { return kGlobalSize; } virtual int GlobalSize() const { return kGlobalSize; }
include/ceres/dynamic_numeric_diff_cost_function.h
+18 -37
View File
@@ -42,6 +42,7 @@
#include "ceres/dynamic_cost_function.h" #include "ceres/dynamic_cost_function.h"
#include "ceres/internal/eigen.h" #include "ceres/internal/eigen.h"
#include "ceres/internal/numeric_diff.h" #include "ceres/internal/numeric_diff.h"
#include "ceres/internal/parameter_dims.h"
#include "ceres/numeric_diff_options.h" #include "ceres/numeric_diff_options.h"
#include "glog/logging.h" #include "glog/logging.h"
@@ -103,7 +104,11 @@ class DynamicNumericDiffCostFunction : public DynamicCostFunction {
<< "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() "
<< "before DynamicNumericDiffCostFunction::Evaluate()."; << "before DynamicNumericDiffCostFunction::Evaluate().";
const bool status = EvaluateCostFunctor(parameters, residuals); const bool status =
internal::VariadicEvaluate<internal::DynamicParameterDims>(
*functor_.get(),
parameters,
residuals);
if (jacobians == NULL || !status) { if (jacobians == NULL || !status) {
return status; return status;
} }
@@ -127,18 +132,18 @@ class DynamicNumericDiffCostFunction : public DynamicCostFunction {
for (size_t block = 0; block < block_sizes.size(); ++block) { for (size_t block = 0; block < block_sizes.size(); ++block) {
if (jacobians[block] != NULL && if (jacobians[block] != NULL &&
!NumericDiff<CostFunctor, method, DYNAMIC, !NumericDiff<CostFunctor, method, ceres::DYNAMIC,
DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, internal::DynamicParameterDims, ceres::DYNAMIC,
DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, ceres::DYNAMIC>::
DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock( EvaluateJacobianForParameterBlock(
functor_.get(), functor_.get(),
residuals, residuals,
options_, options_,
this->num_residuals(), this->num_residuals(),
block, block,
block_sizes[block], block_sizes[block],
&parameters_references_copy[0], &parameters_references_copy[0],
jacobians[block])) { jacobians[block])) {
return false; return false;
} }
} }
@@ -146,30 +151,6 @@ class DynamicNumericDiffCostFunction : public DynamicCostFunction {
} }
private: 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_; std::unique_ptr<const CostFunctor> functor_;
Ownership ownership_; Ownership ownership_;
NumericDiffOptions options_; NumericDiffOptions options_;
include/ceres/internal/autodiff.h
+117 -123
View File
@@ -33,7 +33,7 @@
// dual numbers in jet.h. Before reading the rest of this file, it is advisable // dual numbers in jet.h. Before reading the rest of this file, it is advisable
// to read jet.h's header comment in detail. // 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: // functors with templated operator() taking this form:
// //
// struct F { // struct F {
@@ -142,10 +142,14 @@
#include <stddef.h> #include <stddef.h>
#include "ceres/jet.h" #include <array>
#include "ceres/internal/eigen.h" #include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h" #include "ceres/internal/fixed_array.h"
#include "ceres/internal/parameter_dims.h"
#include "ceres/internal/variadic_evaluate.h" #include "ceres/internal/variadic_evaluate.h"
#include "ceres/jet.h"
#include "ceres/types.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace ceres { namespace ceres {
@@ -165,21 +169,51 @@ namespace internal {
// //
// is what would get put in dst if N was 3, offset was 3, and the jet type JetT // is what would get put in dst if N was 3, offset was 3, and the jet type JetT
// was 8-dimensional. // was 8-dimensional.
template <typename JetT, typename T, int N> template <int Offset, int N, typename T, typename JetT>
inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) { inline void Make1stOrderPerturbation(const T* src, JetT* dst) {
DCHECK(src); DCHECK(src);
DCHECK(dst); DCHECK(dst);
for (int j = 0; j < N; ++j) { for (int j = 0; j < N; ++j) {
dst[j].a = src[j]; dst[j].a = src[j];
dst[j].v.setZero(); 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 // 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. // dst. This is used to pick out the "vector" part of the extended y.
template <typename JetT, typename T> 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); DCHECK(src);
for (int i = 0; i < M; ++i) { for (int i = 0; i < M; ++i) {
dst[i] = src[i].a; dst[i] = src[i].a;
@@ -188,8 +222,8 @@ inline void Take0thOrderPart(int M, const JetT *src, T dst) {
// Takes N 1st order parts, starting at index N0, and puts them in the M x N // 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. // matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
template <typename JetT, typename T, int N0, int N> template <int N0, int N, typename JetT, typename T>
inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { inline void Take1stOrderPart(const int M, const JetT* src, T* dst) {
DCHECK(src); DCHECK(src);
DCHECK(dst); DCHECK(dst);
for (int i = 0; i < M; ++i) { for (int i = 0; i < M; ++i) {
@@ -198,126 +232,86 @@ inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
} }
} }
// This is in a struct because default template parameters on a // Calls Take1stOrderPart for every parameter block.
// 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 // Example:
// the user supplied functor. // If one having three parameter blocks with dimensions (3, 2, 4), the call
template <typename Functor, typename T, // Take1stOrderParts<integer_sequence<3, 2, 4>::Apply(num_outputs,
int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, // output,
int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> // jacobians);
struct AutoDiff { // will result in the following calls to Take1stOrderPart:
static bool Differentiate(const Functor& functor, // if (jacobians[0]) {
T const *const *parameters, // Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]);
int num_outputs, // }
T *function_value, // if (jacobians[1]) {
T **jacobians) { // Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]);
// This block breaks the 80 column rule to keep it somewhat readable. // }
DCHECK_GT(num_outputs, 0); // if (jacobians[2]) {
DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]);
((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // }
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT template <typename Seq, int ParameterIdx = 0, int Offset = 0>
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT struct Take1stOrderParts;
((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;
typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT; template <int N, int... Ns, int ParameterIdx, int Offset>
FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( struct Take1stOrderParts<integer_sequence<int, N, Ns...>, ParameterIdx,
N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); Offset> {
template <typename JetT, typename T>
// These are the positions of the respective jets in the fixed array x. static void Apply(int num_outputs, JetT* output, T** jacobians) {
const int jet0 = 0; if (jacobians[ParameterIdx]) {
const int jet1 = N0; Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]);
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);
} }
Take1stOrderParts<integer_sequence<int, Ns...>, ParameterIdx + 1,
#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ Offset + N>::Apply(num_outputs, output, jacobians);
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;
} }
}; };
// 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 internal
} // namespace ceres } // namespace ceres
include/ceres/internal/numeric_diff.h
+93 -66
View File
@@ -50,42 +50,11 @@
namespace ceres { namespace ceres {
namespace internal { 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 // 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 // specializations for member functions. The alternative is to repeat the main
// class for differing numbers of parameters, which is also unfortunate. // class for differing numbers of parameters, which is also unfortunate.
template <typename CostFunctor, template <typename CostFunctor, NumericDiffMethodType kMethod,
NumericDiffMethodType kMethod, int kNumResiduals, typename ParameterDims, int kParameterBlock,
int kNumResiduals,
int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8, int N9,
int kParameterBlock,
int kParameterBlockSize> int kParameterBlockSize>
struct NumericDiff { struct NumericDiff {
// Mutates parameters but must restore them before return. // Mutates parameters but must restore them before return.
@@ -219,8 +188,9 @@ struct NumericDiff {
// Mutate 1 element at a time and then restore. // Mutate 1 element at a time and then restore.
x_plus_delta(parameter_index) = x(parameter_index) + delta; x_plus_delta(parameter_index) = x(parameter_index) + delta;
if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( if (!VariadicEvaluate<ParameterDims>(*functor,
functor, parameters, residuals.data(), functor)) { parameters,
residuals.data())) {
return false; return false;
} }
@@ -233,8 +203,9 @@ struct NumericDiff {
// Compute the function on the other side of x(parameter_index). // Compute the function on the other side of x(parameter_index).
x_plus_delta(parameter_index) = x(parameter_index) - delta; x_plus_delta(parameter_index) = x(parameter_index) - delta;
if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( if (!VariadicEvaluate<ParameterDims>(*functor,
functor, parameters, temp_residuals.data(), functor)) { parameters,
temp_residuals.data())) {
return false; return false;
} }
@@ -406,35 +377,91 @@ struct NumericDiff {
} }
}; };
template <typename CostFunctor, // This function calls NumericDiff<...>::EvaluateJacobianForParameterBlock for
NumericDiffMethodType kMethod, // each parameter block.
int kNumResiduals, //
int N0, int N1, int N2, int N3, int N4, // Example:
int N5, int N6, int N7, int N8, int N9, // A call to
int kParameterBlock> // EvaluateJacobianForParameterBlocks<StaticParameterDims<2, 3>>(
struct NumericDiff<CostFunctor, kMethod, kNumResiduals, // functor,
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, // residuals_at_eval_point,
kParameterBlock, 0> { // options,
// Mutates parameters but must restore them before return. // num_residuals,
static bool EvaluateJacobianForParameterBlock( // parameters,
const CostFunctor* functor, // jacobians);
const double* residuals_at_eval_point, // will result in the following calls to
const NumericDiffOptions& options, // NumericDiff<...>::EvaluateJacobianForParameterBlock:
const int num_residuals, //
const int parameter_block_index, // if (!NumericDiff<
const int parameter_block_size, // CostFunctor, method, kNumResiduals, ParameterDims, 0,
double **parameters, // 2>::EvaluateJacobianForParameterBlock(functor,
double *jacobian) { // residuals_at_eval_point,
// Silence unused parameter compiler warnings. // options,
(void)functor; // num_residuals,
(void)residuals_at_eval_point; // 0,
(void)options; // 2,
(void)num_residuals; // parameters,
(void)parameter_block_index; // jacobians[0])) {
(void)parameter_block_size; // return false;
(void)parameters; // }
(void)jacobian; // if (!NumericDiff<
LOG(FATAL) << "Control should never reach here."; // 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; return true;
} }
}; };
include/ceres/internal/variadic_evaluate.h
+56 -145
View File
@@ -28,165 +28,76 @@
// //
// Author: sameeragarwal@google.com (Sameer Agarwal) // Author: sameeragarwal@google.com (Sameer Agarwal)
// mierle@gmail.com (Keir Mierle) // mierle@gmail.com (Keir Mierle)
// jodebo_beck@gmx.de (Johannes Beck)
#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ #ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ #define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#include <stddef.h> #include <stddef.h>
#include "ceres/jet.h" #include <type_traits>
#include "ceres/types.h"
#include "ceres/internal/eigen.h" #include "ceres/cost_function.h"
#include "ceres/internal/fixed_array.h" #include "ceres/internal/parameter_dims.h"
#include "glog/logging.h"
namespace ceres { namespace ceres {
namespace internal { namespace internal {
// This block of quasi-repeated code calls the user-supplied functor, which may // For fixed size cost functors
// take a variable number of arguments. This is accomplished by specializing the template <typename Functor, typename T, int... Indices>
// struct based on the size of the trailing parameters; parameters with 0 size inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
// are assumed missing. T* output, std::false_type /*is_dynamic*/,
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, integer_sequence<int, Indices...>) {
int N5, int N6, int N7, int N8, int N9> static_assert(sizeof...(Indices),
struct VariadicEvaluate { "Invalid number of parameter blocks. At least one parameter "
static bool Call(const Functor& functor, T const *const *input, T* output) { "block must be specified.");
return functor(input[0], return functor(input[Indices]..., output);
input[1], }
input[2],
input[3],
input[4],
input[5],
input[6],
input[7],
input[8],
input[9],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, // For dynamic sized cost functors
int N5, int N6, int N7, int N8> template <typename Functor, typename T>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> { inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
static bool Call(const Functor& functor, T const *const *input, T* output) { T* output, std::true_type /*is_dynamic*/,
return functor(input[0], integer_sequence<int>) {
input[1], return functor(input, output);
input[2], }
input[3],
input[4],
input[5],
input[6],
input[7],
input[8],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, // For ceres cost functors (not ceres::CostFunction)
int N5, int N6, int N7> template <typename ParameterDims, typename Functor, typename T>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> { inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
static bool Call(const Functor& functor, T const *const *input, T* output) { T* output, const void* /* NOT USED */) {
return functor(input[0], using ParameterBlockIndices =
input[1], make_integer_sequence<int, ParameterDims::kNumParameterBlocks>;
input[2], using IsDynamic = std::integral_constant<bool, ParameterDims::kIsDynamic>;
input[3], return VariadicEvaluateImpl(functor, input, output, IsDynamic(),
input[4], ParameterBlockIndices());
input[5], }
input[6],
input[7],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, // For ceres::CostFunction
int N5, int N6> template <typename ParameterDims, typename Functor, typename T>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> { inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
static bool Call(const Functor& functor, T const *const *input, T* output) { T* output,
return functor(input[0], const CostFunction* /* NOT USED */) {
input[1], return functor.Evaluate(input, output, nullptr);
input[2], }
input[3],
input[4],
input[5],
input[6],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, // Variadic evaluate is a helper function to evaluate ceres cost function or
int N5> // functors using an input, output and the parameter dimensions. There are
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> { // several ways different possibilities:
static bool Call(const Functor& functor, T const *const *input, T* output) { // 1) If the passed functor is a 'ceres::CostFunction' its evaluate method is
return functor(input[0], // called.
input[1], // 2) If the functor is not a 'ceres::CostFunction' and the specified parameter
input[2], // dims is dynamic, the functor must have the following signature
input[3], // 'bool(T const* const* input, T* output)'.
input[4], // 3) If the functor is not a 'ceres::CostFunction' and the specified parameter
input[5], // dims is not dynamic, the input is expanded by using the number of parameter
output); // 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,
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4> T* output) {
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> { return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor);
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);
}
};
} // namespace internal } // namespace internal
} // namespace ceres } // namespace ceres
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 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. // the squaring is implicitly done by the optimization framework.
// //
// To write an numerically-differentiable cost function for the above model, first // To write an numerically-differentiable cost function for the above model,
// define the object // first define the object
// //
// class MyScalarCostFunctor { // class MyScalarCostFunctor {
// explicit MyScalarCostFunctor(double k): k_(k) {} // explicit MyScalarCostFunctor(double k): k_(k) {}
@@ -110,10 +110,6 @@
// Dimension of x ------------------------------------------------+ | // Dimension of x ------------------------------------------------+ |
// Dimension of y ---------------------------------------------------+ // 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 // The central difference method is considerably more accurate at the cost of
// twice as many function evaluations than forward difference. Consider using // twice as many function evaluations than forward difference. Consider using
// central differences begin with, and only after that works, trying forward // central differences begin with, and only after that works, trying forward
@@ -161,10 +157,13 @@
#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ #ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ #define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
#include <array>
#include <memory> #include <memory>
#include "Eigen/Dense" #include "Eigen/Dense"
#include "ceres/cost_function.h" #include "ceres/cost_function.h"
#include "ceres/internal/numeric_diff.h" #include "ceres/internal/numeric_diff.h"
#include "ceres/internal/parameter_dims.h"
#include "ceres/numeric_diff_options.h" #include "ceres/numeric_diff_options.h"
#include "ceres/sized_cost_function.h" #include "ceres/sized_cost_function.h"
#include "ceres/types.h" #include "ceres/types.h"
@@ -175,20 +174,8 @@ namespace ceres {
template <typename CostFunctor, template <typename CostFunctor,
NumericDiffMethodType method = CENTRAL, NumericDiffMethodType method = CENTRAL,
int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC
int N0 = 0, // Number of parameters in block 0. int... Ns> // Parameters dimensions for each block.
int N1 = 0, // Number of parameters in block 1. class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
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> {
public: public:
NumericDiffCostFunction( NumericDiffCostFunction(
CostFunctor* functor, CostFunctor* functor,
@@ -199,10 +186,7 @@ class NumericDiffCostFunction
ownership_(ownership), ownership_(ownership),
options_(options) { options_(options) {
if (kNumResiduals == DYNAMIC) { if (kNumResiduals == DYNAMIC) {
SizedCostFunction<kNumResiduals, SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
N0, N1, N2, N3, N4,
N5, N6, N7, N8, N9>
::set_num_residuals(num_residuals);
} }
} }
@@ -218,18 +202,17 @@ class NumericDiffCostFunction
using internal::FixedArray; using internal::FixedArray;
using internal::NumericDiff; using internal::NumericDiff;
const int kNumParameters = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; using ParameterDims =
const int kNumParameterBlocks = typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
(N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) + using Parameters = typename ParameterDims::Parameters;
(N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
// Get the function value (residuals) at the point to evaluate. constexpr int kNumParameters = ParameterDims::kNumParameters;
if (!internal::EvaluateImpl<CostFunctor, constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks;
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
functor_.get(), // Get the function value (residuals) at the the point to evaluate.
parameters, if (!internal::VariadicEvaluate<ParameterDims>(*functor_,
residuals, parameters,
functor_.get())) { residuals)) {
return false; return false;
} }
@@ -239,71 +222,22 @@ class NumericDiffCostFunction
// Create a copy of the parameters which will get mutated. // Create a copy of the parameters which will get mutated.
FixedArray<double> parameters_copy(kNumParameters); 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(); for (int block = 0; block < kNumParameterBlocks; ++block) {
if (N1) parameters_reference_copy[1] = parameters_reference_copy[0] + N0; memcpy(parameters_reference_copy[block], parameters[block],
if (N2) parameters_reference_copy[2] = parameters_reference_copy[1] + N1; sizeof(double) * ParameterDims::GetDim(block));
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; \
} \
} }
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(0); internal::EvaluateJacobianForParameterBlocks<ParameterDims>::template Apply<
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(1); method, kNumResiduals>(
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(2); functor_.get(),
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(3); residuals,
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(4); options_,
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(5); SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(6); parameters_reference_copy.data(),
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(7); jacobians);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(8);
CERES_EVALUATE_JACOBIAN_FOR_BLOCK(9);
#undef CERES_EVALUATE_JACOBIAN_FOR_BLOCK
return true; return true;
} }
include/ceres/sized_cost_function.h
+11 -36
View File
@@ -41,49 +41,24 @@
#include "ceres/cost_function.h" #include "ceres/cost_function.h"
#include "ceres/types.h" #include "ceres/types.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "internal/parameter_dims.h"
namespace ceres { namespace ceres {
template<int kNumResiduals, template <int kNumResiduals, int... Ns>
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 { class SizedCostFunction : public CostFunction {
public: 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() { 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); set_num_residuals(kNumResiduals);
*mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};
#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
} }
virtual ~SizedCostFunction() { } virtual ~SizedCostFunction() { }
include/ceres/tiny_solver_autodiff_function.h
+1 -1
View File
@@ -120,7 +120,7 @@ class TinySolverAutoDiffFunction {
NUM_RESIDUALS = kNumResiduals, 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. // parameter block it is easier to inline to avoid overhead.
bool operator()(const T* parameters, bool operator()(const T* parameters,
T* residuals, T* residuals,
internal/ceres/autodiff_test.cc
+26 -36
View File
@@ -194,7 +194,7 @@ TEST(AutoDiff, ProjectiveCameraModel) {
{ {
double *parameters[] = { PX }; double *parameters[] = { PX };
double *jacobians[] = { J_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))); b, parameters, 2, ad_x1, jacobians)));
for (int i = 0; i < 2; ++i) { for (int i = 0; i < 2; ++i) {
@@ -209,7 +209,7 @@ TEST(AutoDiff, ProjectiveCameraModel) {
double J_X[2 * 4]; double J_X[2 * 4];
double *parameters[] = { P, X }; double *parameters[] = { P, X };
double *jacobians[] = { J_P, J_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))); b, parameters, 2, ad_x2, jacobians)));
for (int i = 0; i < 2; ++i) { for (int i = 0; i < 2; ++i) {
@@ -316,7 +316,7 @@ TEST(AutoDiff, Metric) {
double J_X[2 * 3]; double J_X[2 * 3];
double *parameters[] = { q, c, X }; double *parameters[] = { q, c, X };
double *jacobians[] = { J_q, J_c, J_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))); b, parameters, 2, ad_x, jacobians)));
for (int i = 0; i < 2; ++i) { for (int i = 0; i < 2; ++i) {
@@ -366,7 +366,7 @@ TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
functor.num_residuals = num_residuals; functor.num_residuals = num_residuals;
// Run autodiff with the new number of 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))); functor, parameters, num_residuals, residuals, jacobians)));
const double kTolerance = 1e-14; const double kTolerance = 1e-14;
@@ -528,8 +528,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual1Param functor; Residual1Param functor;
int num_variables = 1; int num_variables = 1;
EXPECT_TRUE((AutoDiff<Residual1Param, double, 1>::Differentiate( EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1>>(
functor, parameters, 1, &residual, jacobians))); functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -539,8 +539,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual2Param functor; Residual2Param functor;
int num_variables = 2; int num_variables = 2;
EXPECT_TRUE((AutoDiff<Residual2Param, double, 1, 1>::Differentiate( EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1>>(
functor, parameters, 1, &residual, jacobians))); functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -550,8 +550,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual3Param functor; Residual3Param functor;
int num_variables = 3; int num_variables = 3;
EXPECT_TRUE((AutoDiff<Residual3Param, double, 1, 1, 1>::Differentiate( EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1>>(
functor, parameters, 1, &residual, jacobians))); functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -561,8 +561,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual4Param functor; Residual4Param functor;
int num_variables = 4; int num_variables = 4;
EXPECT_TRUE((AutoDiff<Residual4Param, double, 1, 1, 1, 1>::Differentiate( EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians))); functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -572,8 +572,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual5Param functor; Residual5Param functor;
int num_variables = 5; int num_variables = 5;
EXPECT_TRUE((AutoDiff<Residual5Param, double, 1, 1, 1, 1, 1>::Differentiate( EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians))); functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -583,10 +583,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual6Param functor; Residual6Param functor;
int num_variables = 6; int num_variables = 6;
EXPECT_TRUE((AutoDiff<Residual6Param, EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1>>(
double, functor, parameters, 1, &residual, jacobians)));
1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -596,10 +594,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual7Param functor; Residual7Param functor;
int num_variables = 7; int num_variables = 7;
EXPECT_TRUE((AutoDiff<Residual7Param, EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1>>(
double, functor, parameters, 1, &residual, jacobians)));
1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -609,10 +605,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual8Param functor; Residual8Param functor;
int num_variables = 8; int num_variables = 8;
EXPECT_TRUE((AutoDiff< EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1>>(
Residual8Param, functor, parameters, 1, &residual, jacobians)));
double, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -622,11 +616,9 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual9Param functor; Residual9Param functor;
int num_variables = 9; int num_variables = 9;
EXPECT_TRUE((AutoDiff< EXPECT_TRUE(
Residual9Param, (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1>>(
double, functor, parameters, 1, &residual, jacobians)));
1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -636,11 +628,9 @@ TEST(AutoDiff, VariadicAutoDiff) {
{ {
Residual10Param functor; Residual10Param functor;
int num_variables = 10; int num_variables = 10;
EXPECT_TRUE((AutoDiff< EXPECT_TRUE(
Residual10Param, (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1, 1>>(
double, functor, parameters, 1, &residual, jacobians)));
1, 1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2); EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) { for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i)); EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
internal/ceres/local_parameterization_test.cc
+6 -8
View File
@@ -265,14 +265,12 @@ void QuaternionParameterizationTestHelper(
double* jacobian_array[2] = { NULL, jacobian_ref }; double* jacobian_array[2] = { NULL, jacobian_ref };
// Autodiff jacobian at delta_x = 0. // Autodiff jacobian at delta_x = 0.
internal::AutoDiff<Plus, internal::AutoDifferentiate<StaticParameterDims<kGlobalSize, kLocalSize>>(
double, Plus(),
kGlobalSize, parameters,
kLocalSize>::Differentiate(Plus(), kGlobalSize,
parameters, x_plus_delta,
kGlobalSize, jacobian_array);
x_plus_delta,
jacobian_array);
double jacobian[12]; double jacobian[12];
parameterization.ComputeJacobian(x, jacobian); parameterization.ComputeJacobian(x, jacobian);
internal/ceres/parameter_dims_test.cc
+3 -3
View File
@@ -89,11 +89,11 @@ TEST(ParameterDims, GetUnpackedParameters) {
constexpr int N1 = 4; constexpr int N1 = 4;
constexpr int N2 = 2; 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 = 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(), unpacked_parameters[0]);
EXPECT_EQ(packed_parameters.data() + N0, unpacked_parameters[1]); EXPECT_EQ(packed_parameters.data() + N0, unpacked_parameters[1]);