Merge branch 'master' of https://code.google.com/p/ceres-solver
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);