Add support for dynamic autodiff Change-Id: I17d573696172ab691a9653db99a620e4bc1bd0d0
diff --git a/include/ceres/dynamic_autodiff_cost_function.h b/include/ceres/dynamic_autodiff_cost_function.h new file mode 100644 index 0000000..0bc18eb --- /dev/null +++ b/include/ceres/dynamic_autodiff_cost_function.h
@@ -0,0 +1,189 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// thadh@gmail.com (Thad Hughes) +// +// This autodiff implementation differs from the one found in +// autodiff_cost_function.h by supporting autodiff on cost functions with +// variable numbers of parameters with variable sizes. With the other +// implementation, all the sizes (both the number of parameter blocks and the +// size of each block) must be fixed at compile time. +// +// The functor API differs slightly from the API for fixed size autodiff; the +// expected interface for the cost functors is: +// +// struct MyCostFunctor { +// template<typename T> +// bool operator()(const* const* T parameters, T* residuals) const { +// // Use parameters[i] to access the i'th parameter block. +// } +// } +// +// Since the sizing of the parameters is done at runtime, you must also specify +// the sizes after creating the dynamic autodiff cost function. For example: +// +// DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( +// new MyCostFunctor()); +// cost_function.AddParameterBlock(5); +// cost_function.AddParameterBlock(10); +// cost_function.SetNumResiduals(21); +// +// Under the hood, the implementation evaluates the cost function multiple +// times, computing a small set of the derivatives (four by default, controlled +// by the Stride template parameter) with each pass. There is a tradeoff with +// the size of the passes; you may want to experiment with the stride. + +#ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ + +#include <cmath> +#include <numeric> +#include <glog/logging.h> +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/jet.h" + +namespace ceres { + +template <typename CostFunctor, int Stride = 4> +class DynamicAutoDiffCostFunction : public CostFunction { + public: + DynamicAutoDiffCostFunction(CostFunctor* functor) + : functor_(functor) {} + + virtual ~DynamicAutoDiffCostFunction() {} + + void AddParameterBlock(int size) { + mutable_parameter_block_sizes()->push_back(size); + } + + void SetNumResiduals(int num_residuals) { + set_num_residuals(num_residuals); + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + CHECK_GT(num_residuals(), 0) + << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() " + << "before DynamicAutoDiffCostFunction::Evaluate()."; + + if (jacobians == NULL) { + return (*functor_)(parameters, residuals); + } + + // The difficulty with Jets, as implemented in Ceres, is that they were + // originally designed for strictly compile-sized use. At this point, there + // is a large body of code that assumes inside a cost functor it is + // acceptable to do e.g. T(1.5) and get an appropriately sized jet back. + // + // Unfortunately, it is impossible to communicate the expected size of a + // dynamically sized jet to the static instantiations that existing code + // depends on. + // + // To work around this issue, the solution here is to evaluate the + // jacobians in a series of passes, each one computing Stripe * + // num_residuals() derivatives. This is done with small, fixed-size jets. + const int num_parameter_blocks = parameter_block_sizes().size(); + const int num_parameters = std::accumulate(parameter_block_sizes().begin(), + parameter_block_sizes().end(), + 0); + + // Allocate scratch space for the strided evaluation. + vector<Jet<double, Stride> > input_jets(num_parameters); + vector<Jet<double, Stride> > output_jets(num_residuals()); + + // Make the parameter pack that is sent to the functor (reused). + vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks); + for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + jet_parameters[i] = &input_jets[parameter_cursor]; + for (int j = 0; j < parameter_block_sizes()[i]; ++j, parameter_cursor++) { + input_jets[parameter_cursor].a = parameters[i][j]; + } + } + + // Evaluate all of the strides. Each stride is a chunk of the derivative to + // evaluate, typically some size proportional to the size of the SIMD + // registers of the CPU. + int num_strides = int(ceil(num_parameters / float(Stride))); + for (int pass = 0; pass < num_strides; ++pass) { + const int start_derivative_section = pass * Stride; + const int end_derivative_section = std::min((pass + 1) * Stride, + num_parameters); + // Set most of the jet components to zero, except for the active + // parameters, which occur in a contiguos block of size Stride. + for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + for (int j = 0; j < parameter_block_sizes()[i]; + ++j, parameter_cursor++) { + input_jets[parameter_cursor].v.setZero(); + if (parameter_cursor >= start_derivative_section && + parameter_cursor < end_derivative_section) { + input_jets[parameter_cursor] + .v[parameter_cursor - start_derivative_section] = 1.0; + } + } + } + + if (!(*functor_)(&jet_parameters[0], &output_jets[0])) { + return false; + } + + // Copy the pieces of the jacobians into their final place. + for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + for (int j = 0; j < parameter_block_sizes()[i]; + ++j, parameter_cursor++) { + if (parameter_cursor >= start_derivative_section && + parameter_cursor < end_derivative_section) { + for (int k = 0; k < num_residuals(); ++k) { + jacobians[i][k * parameter_block_sizes()[i] + j] = + output_jets[k].v[parameter_cursor - start_derivative_section]; + } + } + } + } + + // Only copy the residuals over once (even though we compute them on + // every loop). + if (pass == num_strides - 1) { + for (int k = 0; k < num_residuals(); ++k) { + residuals[k] = output_jets[k].a; + } + } + } + return true; + } + + private: + internal::scoped_ptr<CostFunctor> functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index f9329e9..b30f0cc 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -225,6 +225,7 @@ CERES_TEST(corrector) CERES_TEST(cost_function_to_functor) CERES_TEST(dense_sparse_matrix) + CERES_TEST(dynamic_autodiff_cost_function) CERES_TEST(evaluator) CERES_TEST(gradient_checker) CERES_TEST(gradient_checking_cost_function)
diff --git a/internal/ceres/dynamic_autodiff_cost_function_test.cc b/internal/ceres/dynamic_autodiff_cost_function_test.cc new file mode 100644 index 0000000..5d743a7 --- /dev/null +++ b/internal/ceres/dynamic_autodiff_cost_function_test.cc
@@ -0,0 +1,166 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: thadh@gmail.com (Thad Hughes) +// mierle@gmail.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dynamic_autodiff_cost_function.h" + +#include <cstddef> + +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +// Takes 2 parameter blocks: +// parameters[0] is size 10. +// parameters[1] is size 5. +// Emits 21 residuals: +// A: i - parameters[0][i], for i in [0,10) -- this is 10 residuals +// B: parameters[0][i] - i, for i in [0,10) -- this is another 10. +// C: sum(parameters[0][i]^2 - 8*parameters[0][i]) + sum(parameters[1][i]) +class MyCostFunctor { + public: + template <typename T> + bool operator()(T const* const* parameters, T* residuals) const { + const T* params0 = parameters[0]; + int r = 0; + for (int i = 0; i < 10; ++i) { + residuals[r++] = T(i) - params0[i]; + residuals[r++] = params0[i] - T(i); + } + + T c_residual(0.0); + for (int i = 0; i < 10; ++i) { + c_residual += pow(params0[i], 2) - T(8) * params0[i]; + } + + const T* params1 = parameters[1]; + for (int i = 0; i < 5; ++i) { + c_residual += params1[i]; + } + residuals[r++] = c_residual; + return true; + } +}; + +TEST(DynamicAutodiffCostFunctionTest, TestResiduals) { + vector<double> param_block_0(10, 0.0); + vector<double> param_block_1(5, 0.0); + DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( + new MyCostFunctor()); + cost_function.AddParameterBlock(param_block_0.size()); + cost_function.AddParameterBlock(param_block_1.size()); + cost_function.SetNumResiduals(21); + + // Test residual computation. + vector<double> residuals(21, -100000); + vector<double*> parameter_blocks(2); + parameter_blocks[0] = ¶m_block_0[0]; + parameter_blocks[1] = ¶m_block_1[0]; + EXPECT_TRUE(cost_function.Evaluate(¶meter_blocks[0], + residuals.data(), + NULL)); + for (int r = 0; r < 10; ++r) { + EXPECT_EQ(1.0 * r, residuals.at(r * 2)); + EXPECT_EQ(-1.0 * r, residuals.at(r * 2 + 1)); + } + EXPECT_EQ(0, residuals.at(20)); +} + +TEST(DynamicAutodiffCostFunctionTest, TestJacobian) { + // Test the residual counting. + vector<double> param_block_0(10, 0.0); + for (int i = 0; i < 10; ++i) { + param_block_0[i] = 2 * i; + } + vector<double> param_block_1(5, 0.0); + DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( + new MyCostFunctor()); + cost_function.AddParameterBlock(param_block_0.size()); + cost_function.AddParameterBlock(param_block_1.size()); + cost_function.SetNumResiduals(21); + + // Prepare the residuals. + vector<double> residuals(21, -100000); + + // Prepare the parameters. + vector<double*> parameter_blocks(2); + parameter_blocks[0] = ¶m_block_0[0]; + parameter_blocks[1] = ¶m_block_1[0]; + + // Prepare the jacobian. + vector<vector<double> > jacobian_vect(2); + jacobian_vect[0].resize(21 * 10, -100000); + jacobian_vect[1].resize(21 * 5, -100000); + vector<double*> jacobian; + jacobian.push_back(jacobian_vect[0].data()); + jacobian.push_back(jacobian_vect[1].data()); + + // Test jacobian computation. + EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), + residuals.data(), + jacobian.data())); + + for (int r = 0; r < 10; ++r) { + EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); + EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); + } + EXPECT_EQ(420, residuals.at(20)); + for (int p = 0; p < 10; ++p) { + // Check "A" Jacobian. + EXPECT_EQ(-1.0, jacobian_vect[0][2*p * 10 + p]); + // Check "B" Jacobian. + EXPECT_EQ(+1.0, jacobian_vect[0][(2*p+1) * 10 + p]); + jacobian_vect[0][2*p * 10 + p] = 0.0; + jacobian_vect[0][(2*p+1) * 10 + p] = 0.0; + } + + // Check "C" Jacobian for first parameter block. + for (int p = 0; p < 10; ++p) { + EXPECT_EQ(4 * p - 8, jacobian_vect[0][20 * 10 + p]); + jacobian_vect[0][20 * 10 + p] = 0.0; + } + for (int i = 0; i < jacobian_vect[0].size(); ++i) { + EXPECT_EQ(0.0, jacobian_vect[0][i]); + } + + // Check "C" Jacobian for second parameter block. + for (int p = 0; p < 5; ++p) { + EXPECT_EQ(1.0, jacobian_vect[1][20 * 5 + p]); + jacobian_vect[1][20 * 5 + p] = 0.0; + } + for (int i = 0; i < jacobian_vect[1].size(); ++i) { + EXPECT_EQ(0.0, jacobian_vect[1][i]); + } +} + +} // namespace internal +} // namespace ceres