Support varying numbers of residuals in autodiff.
This commit modifies the only function in autodiff that takes a
templated number of outputs (i.e. residuals) and makes that
template parameter a normal parameter. With that change, it
is a trivial matter to support a dynamic number of residuals.
The API for dynamic residuals is to pass a fake number of
residuals as the second template argument to
AutoDiffCostFunction, and to pass the real number of
parameters as a second constructor argument.
diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h
index 6e51115..da9ee2c 100644
--- a/include/ceres/autodiff_cost_function.h
+++ b/include/ceres/autodiff_cost_function.h
@@ -93,6 +93,20 @@
// "MyScalarCostFunction", "1, 2, 2", describe the functor as computing a
// 1-dimensional output from two arguments, both 2-dimensional.
//
+// The autodiff cost function also supports cost functions with a
+// runtime-determined number of residuals. For example:
+//
+// CostFunction* cost_function
+// = new AutoDiffCostFunction<MyScalarCostFunction, DYNAMIC, 2, 2>(
+// new CostFunctionWithDynamicNumResiduals(1.0), ^ ^ ^
+// runtime_number_of_residuals); <----+ | | |
+// | | | |
+// | | | |
+// Actual number of residuals ------+ | | |
+// Indicate dynamic number of residuals ---------+ | |
+// Dimension of x -------------------------------------+ |
+// Dimension of y ----------------------------------------+
+//
// The framework can currently accommodate cost functions of up to 6 independent
// variables, and there is no limit on the dimensionality of each of them.
//
@@ -115,18 +129,26 @@
#include "ceres/internal/autodiff.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/sized_cost_function.h"
+#include "ceres/types.h"
namespace ceres {
-// A cost function which computes the derivative of the cost with respect to the
-// parameters (a.k.a. the jacobian) using an autodifferentiation framework. The
-// first template argument is the functor object, described in the header
-// comment. The second argument is the dimension of the residual, and subsequent
+// A cost function which computes the derivative of the cost with respect to
+// the parameters (a.k.a. the jacobian) using an autodifferentiation framework.
+// The first template argument is the functor object, described in the header
+// comment. The second argument is the dimension of the residual (or
+// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent
// arguments describe the size of the Nth parameter, one per parameter.
//
-// The constructor, which takes a cost functor, takes ownership of the functor.
+// The constructors take ownership of the cost functor.
+//
+// If the number of residuals (argument "M" below) is ceres::DYNAMIC, then the
+// two-argument constructor must be used. The second constructor takes a number
+// of residuals (in addition to the templated number of residuals). This allows
+// for varying the number of residuals for a single autodiff cost function at
+// runtime.
template <typename CostFunctor,
- int M, // Number of residuals.
+ int M, // 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.
@@ -136,8 +158,25 @@
class AutoDiffCostFunction :
public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> {
public:
- // Takes ownership of functor.
- explicit AutoDiffCostFunction(CostFunctor* functor) : functor_(functor) {}
+ // Takes ownership of functor. Uses the template-provided value for the
+ // number of residuals ("M").
+ explicit AutoDiffCostFunction(CostFunctor* functor)
+ : functor_(functor) {
+ CHECK_NE(M, DYNAMIC) << "Can't run the fixed-size constructor if the "
+ << "number of residuals is set to ceres::DYNAMIC.";
+ }
+
+ // Takes ownership of functor. Ignores the template-provided number of
+ // residuals ("M") in favor of the "num_residuals" argument provided.
+ //
+ // This allows for having autodiff cost functions which return varying
+ // numbers of residuals at runtime.
+ AutoDiffCostFunction(CostFunctor* functor, int num_residuals)
+ : functor_(functor) {
+ CHECK_EQ(M, DYNAMIC) << "Can't run the dynamic-size constructor if the "
+ << "number of residuals is not ceres::DYNAMIC.";
+ SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::set_num_residuals(num_residuals);
+ }
virtual ~AutoDiffCostFunction() {}
@@ -155,10 +194,12 @@
::Call(*functor_, parameters, residuals);
}
return internal::AutoDiff<CostFunctor, double,
- M, N0, N1, N2, N3, N4, N5>::Differentiate(*functor_,
- parameters,
- residuals,
- jacobians);
+ N0, N1, N2, N3, N4, N5>::Differentiate(
+ *functor_,
+ parameters,
+ SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::num_residuals(),
+ residuals,
+ jacobians);
}
private:
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h
index 0dc0d4f..4f5081f 100644
--- a/include/ceres/internal/autodiff.h
+++ b/include/ceres/internal/autodiff.h
@@ -144,6 +144,7 @@
#include <glog/logging.h>
#include "ceres/jet.h"
+#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
namespace ceres {
@@ -185,18 +186,12 @@
// 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 M, int N0, int N>
-inline void Take1stOrderPart(const JetT *src, T *dst) {
+template <typename JetT, typename T, int N0, int N>
+inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
DCHECK(src);
DCHECK(dst);
- // TODO(keir): Change Jet to use a single array, where v[0] is the
- // non-infinitesimal part rather than "a". That way it's possible to use a
- // single memcpy or eigen operation, rather than the explicit loop. The loop
- // doesn't exploit any SSE or other intrinsics.
for (int i = 0; i < M; ++i) {
- for (int j = 0; j < N; ++j) {
- dst[N * i + j] = src[i].v[N0 + j];
- }
+ Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = src[i].v.template segment<N>(N0);
}
}
@@ -279,11 +274,12 @@
// 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 N5 are the
// dimension of the input arguments to the user supplied functor.
-template <typename Functor, typename T, int kNumOutputs,
+template <typename Functor, typename T,
int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5=0>
struct AutoDiff {
static bool Differentiate(const Functor& functor,
T const *const *parameters,
+ int num_outputs,
T *function_value,
T **jacobians) {
typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5> JetT;
@@ -300,10 +296,10 @@
<< "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
<< N3 << ", " << N4 << ", " << N5;
- DCHECK_GT(kNumOutputs, 0);
+ DCHECK_GT(num_outputs, 0);
FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
- N0 + N1 + N2 + N3 + N4 + N5 + kNumOutputs);
+ N0 + N1 + N2 + N3 + N4 + N5 + num_outputs);
// It's ugly, but it works.
const int jet0 = 0;
@@ -345,16 +341,16 @@
return false;
}
- internal::Take0thOrderPart(kNumOutputs, output, function_value);
+ internal::Take0thOrderPart(num_outputs, output, function_value);
#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
if (N ## i) { \
if (jacobians[i]) { \
internal::Take1stOrderPart<JetT, T, \
- kNumOutputs, \
jet ## i, \
- N ## i>(output, \
- jacobians[i]); \
+ N ## i>(num_outputs, \
+ output, \
+ jacobians[i]); \
} \
}
CERES_TAKE_1ST_ORDER_PERTURBATION(0);
diff --git a/include/ceres/sized_cost_function.h b/include/ceres/sized_cost_function.h
index 968285b..2894a9f 100644
--- a/include/ceres/sized_cost_function.h
+++ b/include/ceres/sized_cost_function.h
@@ -30,11 +30,16 @@
//
// A convenience class for cost functions which are statically sized.
// Compared to the dynamically-sized base class, this reduces boilerplate.
+//
+// The kNumResiduals template parameter can be a constant such as 2 or 5, or it
+// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses
+// are responsible for calling set_num_residuals() at runtime.
#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_
#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_
#include <glog/logging.h>
+#include "ceres/types.h"
#include "ceres/cost_function.h"
namespace ceres {
@@ -45,11 +50,12 @@
public:
SizedCostFunction() {
// Sanity checking.
- DCHECK_GT(kNumResiduals, 0) << "Cost functions must have at least "
- << "one residual block.";
- DCHECK_GT(N0, 0)
+ CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC)
+ << "Cost functions must have at least one residual block.";
+
+ CHECK_GT(N0, 0)
<< "Cost functions must have at least one parameter block.";
- DCHECK((!N1 && !N2 && !N3 && !N4 && !N5) ||
+ CHECK((!N1 && !N2 && !N3 && !N4 && !N5) ||
((N1 > 0) && !N2 && !N3 && !N4 && !N5) ||
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) ||
diff --git a/include/ceres/types.h b/include/ceres/types.h
index 433baa8..a30c790 100644
--- a/include/ceres/types.h
+++ b/include/ceres/types.h
@@ -237,6 +237,13 @@
TEXTFILE
};
+// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be specified for
+// the number of residuals. If specified, then the number of residuas for that
+// cost function can vary at runtime.
+enum DimensionType {
+ DYNAMIC = -1
+};
+
const char* LinearSolverTypeToString(LinearSolverType type);
const char* PreconditionerTypeToString(PreconditionerType type);
const char* LinearSolverTerminationTypeToString(
diff --git a/internal/ceres/autodiff_test.cc b/internal/ceres/autodiff_test.cc
index e9f0c5e..86861ee 100644
--- a/internal/ceres/autodiff_test.cc
+++ b/internal/ceres/autodiff_test.cc
@@ -37,7 +37,7 @@
namespace internal {
template <typename T> inline
-T &RowMajor(T *base, int rows, int cols, int i, int j) {
+T &RowMajorAccess(T *base, int rows, int cols, int i, int j) {
return base[cols * i + j];
}
@@ -86,7 +86,7 @@
// Symmetric differencing:
// f'(a) = (f(a + h) - f(a - h)) / (2 h)
for (int i = 0; i < M; ++i) {
- RowMajor(jac, M, N, i, j) =
+ RowMajorAccess(jac, M, N, i, j) =
(fwd_fun[i] - bwd_fun[i]) / (T(2) * del);
}
@@ -116,7 +116,7 @@
A cc = c*c;
A cd = c*d;
A dd = d*d;
-#define R(i, j) RowMajor(R, 3, 3, (i), (j))
+#define R(i, j) RowMajorAccess(R, 3, 3, (i), (j))
R(0, 0) = aa+bb-cc-dd; R(0, 1) = A(2)*(bc-ad); R(0, 2) = A(2)*(ac+bd); // NOLINT
R(1, 0) = A(2)*(ad+bc); R(1, 1) = aa-bb+cc-dd; R(1, 2) = A(2)*(cd-ab); // NOLINT
R(2, 0) = A(2)*(bd-ac); R(2, 1) = A(2)*(ab+cd); R(2, 2) = aa-bb-cc+dd; // NOLINT
@@ -132,10 +132,10 @@
bool operator()(A const P[12], A const X[4], A x[2]) const {
A PX[3];
for (int i = 0; i < 3; ++i) {
- PX[i] = RowMajor(P, 3, 4, i, 0) * X[0] +
- RowMajor(P, 3, 4, i, 1) * X[1] +
- RowMajor(P, 3, 4, i, 2) * X[2] +
- RowMajor(P, 3, 4, i, 3) * X[3];
+ PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
+ RowMajorAccess(P, 3, 4, i, 1) * X[1] +
+ RowMajorAccess(P, 3, 4, i, 2) * X[2] +
+ RowMajorAccess(P, 3, 4, i, 3) * X[3];
}
if (PX[2] != 0.0) {
x[0] = PX[0] / PX[2];
@@ -194,8 +194,8 @@
{
double *parameters[] = { PX };
double *jacobians[] = { J_PX };
- ASSERT_TRUE((AutoDiff<Projective, double, 2, 12 + 4>::Differentiate(
- b, parameters, ad_x1, jacobians)));
+ ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate(
+ b, parameters, 2, ad_x1, jacobians)));
for (int i = 0; i < 2; ++i) {
ASSERT_NEAR(ad_x1[i], b_x[i], tol);
@@ -209,8 +209,8 @@
double J_X[2 * 4];
double *parameters[] = { P, X };
double *jacobians[] = { J_P, J_X };
- ASSERT_TRUE((AutoDiff<Projective, double, 2, 12, 4>::Differentiate(
- b, parameters, ad_x2, jacobians)));
+ ASSERT_TRUE((AutoDiff<Projective, double, 12, 4>::Differentiate(
+ b, parameters, 2, ad_x2, jacobians)));
for (int i = 0; i < 2; ++i) {
ASSERT_NEAR(ad_x2[i], b_x[i], tol);
@@ -252,16 +252,16 @@
// Set P(:, 1:3) = R
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
- RowMajor(P, 3, 4, i, j) = RowMajor(R, 3, 3, i, j);
+ RowMajorAccess(P, 3, 4, i, j) = RowMajorAccess(R, 3, 3, i, j);
}
}
// Set P(:, 4) = - R c
for (int i = 0; i < 3; ++i) {
- RowMajor(P, 3, 4, i, 3) =
- - (RowMajor(R, 3, 3, i, 0) * c[0] +
- RowMajor(R, 3, 3, i, 1) * c[1] +
- RowMajor(R, 3, 3, i, 2) * c[2]);
+ RowMajorAccess(P, 3, 4, i, 3) =
+ - (RowMajorAccess(R, 3, 3, i, 0) * c[0] +
+ RowMajorAccess(R, 3, 3, i, 1) * c[1] +
+ RowMajorAccess(R, 3, 3, i, 2) * c[2]);
}
A X1[4] = { X[0], X[1], X[2], A(1) };
@@ -316,8 +316,8 @@
double J_X[2 * 3];
double *parameters[] = { q, c, X };
double *jacobians[] = { J_q, J_c, J_X };
- ASSERT_TRUE((AutoDiff<Metric, double, 2, 4, 3, 3>::Differentiate(
- b, parameters, ad_x, jacobians)));
+ ASSERT_TRUE((AutoDiff<Metric, double, 4, 3, 3>::Differentiate(
+ b, parameters, 2, ad_x, jacobians)));
for (int i = 0; i < 2; ++i) {
ASSERT_NEAR(ad_x[i], b_x[i], tol);
@@ -337,5 +337,45 @@
}
}
+struct VaryingResidualFunctor {
+ template <typename T>
+ bool operator()(const T x[2], T* y) const {
+ for (int i = 0; i < num_residuals; ++i) {
+ y[i] = T(i) * x[0] * x[1] * x[1];
+ }
+ return true;
+ }
+
+ int num_residuals;
+};
+
+TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
+ double x[2] = { 1.0, 5.5 };
+ double *parameters[] = { x };
+ const int kMaxResiduals = 10;
+ double J_x[2 * kMaxResiduals];
+ double residuals[kMaxResiduals];
+ double *jacobians[] = { J_x };
+
+ // Use a single functor, but tweak it to produce different numbers of
+ // residuals.
+ VaryingResidualFunctor functor;
+
+ for (int num_residuals = 0; num_residuals < kMaxResiduals; ++num_residuals) {
+ // Tweak the number of residuals to produce.
+ functor.num_residuals = num_residuals;
+
+ // Run autodiff with the new number of residuals.
+ ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate(
+ functor, parameters, num_residuals, residuals, jacobians)));
+
+ const double kTolerance = 1e-14;
+ for (int i = 0; i < num_residuals; ++i) {
+ EXPECT_NEAR(J_x[2 * i + 0], i * x[1] * x[1], kTolerance) << "i: " << i;
+ EXPECT_NEAR(J_x[2 * i + 1], 2 * i * x[0] * x[1], kTolerance) << "i: " << i;
+ }
+ }
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc
index 1533884..26a57c1 100644
--- a/internal/ceres/local_parameterization_test.cc
+++ b/internal/ceres/local_parameterization_test.cc
@@ -176,8 +176,8 @@
double* jacobian_array[2] = { NULL, jacobian_ref };
// Autodiff jacobian at delta_x = 0.
- internal::AutoDiff<QuaternionPlus, double, 4, 4, 3>::Differentiate(
- QuaternionPlus(), parameters, x_plus_delta, jacobian_array);
+ internal::AutoDiff<QuaternionPlus, double, 4, 3>::Differentiate(
+ QuaternionPlus(), parameters, 4, x_plus_delta, jacobian_array);
double jacobian[12];
param.ComputeJacobian(x, jacobian);
