Solver:Support autodiff for dyn. NUM_RESIDUALS Enable use of dynamic number of residuals for autodiff. Implemented with "Substitution failure is not an error" similar to tiny_solver.h . Move test from tiny_solver_test.cc to tiny_solver_autodiff_function_test.cc . Use cpplint.py from C++ Google Style Guide for formatting. Change-Id: I2e1a159d17118552943c6ac7a833c5bbd0c927ec
diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h index c54a6e5..76e4a24 100644 --- a/include/ceres/tiny_solver_autodiff_function.h +++ b/include/ceres/tiny_solver_autodiff_function.h
@@ -35,6 +35,8 @@ #ifndef CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ #define CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ +#include <memory> +#include <type_traits> #include "Eigen/Core" #include "ceres/jet.h" @@ -45,6 +47,7 @@ // An adapter around autodiff-style CostFunctors to enable easier use of // TinySolver. See the example below showing how to use it: // +// // Example for cost functor with static residual size. // // Same as an autodiff cost functor, but taking only 1 parameter. // struct MyFunctor { // template<typename T> @@ -68,6 +71,37 @@ // TinySolver<AutoDiffFunction> solver; // solver.Solve(f, &x); // +// // Example for cost functor with dynamic residual size. +// // NumResiduals() supplies dynamic size of residuals. +// // Same functionality as in tiny_solver.h but with autodiff. +// struct MyFunctorWithDynamicResiduals { +// int NumResiduals() const { +// return 2; +// } +// +// template<typename T> +// bool operator()(const T* const parameters, T* residuals) const { +// const T& x = parameters[0]; +// const T& y = parameters[1]; +// const T& z = parameters[2]; +// residuals[0] = x + static_cast<T>(2.)*y + static_cast<T>(4.)*z; +// residuals[1] = y * z; +// return true; +// } +// }; +// +// typedef TinySolverAutoDiffFunction<MyFunctorWithDynamicResiduals, +// Eigen::Dynamic, +// 3> +// AutoDiffFunctionWithDynamicResiduals; +// +// MyFunctorWithDynamicResiduals my_functor_dyn; +// AutoDiffFunctionWithDynamicResiduals f(my_functor_dyn); +// +// Vec3 x = ...; +// TinySolver<AutoDiffFunctionWithDynamicResiduals> solver; +// solver.Solve(f, &x); +// // WARNING: The cost function adapter is not thread safe. template<typename CostFunctor, int kNumResiduals, @@ -75,8 +109,10 @@ typename T = double> class TinySolverAutoDiffFunction { public: - TinySolverAutoDiffFunction(const CostFunctor& cost_functor) - : cost_functor_(cost_functor) {} + TinySolverAutoDiffFunction(const CostFunctor& cost_functor) + : cost_functor_(cost_functor) { + Initialize<kNumResiduals>(cost_functor); + } typedef T Scalar; enum { @@ -102,21 +138,22 @@ } // Initialize the output jets such that we can detect user errors. - for (int i = 0; i < kNumResiduals; ++i) { + for (int i = 0; i < num_residuals_; ++i) { jet_residuals_[i].a = kImpossibleValue; jet_residuals_[i].v.setConstant(kImpossibleValue); } // Execute the cost function, but with jets to find the derivative. - if (!cost_functor_(jet_parameters_, jet_residuals_)) { + if (!cost_functor_(jet_parameters_, jet_residuals_.data())) { return false; } // Copy the jacobian out of the derivative part of the residual jets. - Eigen::Map<Eigen::Matrix<T, - kNumResiduals, - kNumParameters>> jacobian_matrix(jacobian); - for (int r = 0; r < kNumResiduals; ++r) { + Eigen::Map<Eigen::Matrix<T, kNumResiduals, kNumParameters>> jacobian_matrix( + jacobian, + num_residuals_, + kNumParameters); + for (int r = 0; r < num_residuals_; ++r) { residuals[r] = jet_residuals_[r].a; // Note that while this looks like a fast vectorized write, in practice it // unfortunately thrashes the cache since the writes to the column-major @@ -126,16 +163,42 @@ return true; } + int NumResiduals() const { + return num_residuals_; // Set by Initialize. + } + private: const CostFunctor& cost_functor_; + // The number of residuals at runtime. + // This will be overriden if NUM_RESIDUALS == Eigen::Dynamic. + int num_residuals_ = kNumResiduals; + // To evaluate the cost function with jets, temporary storage is needed. These // are the buffers that are used during evaluation; parameters for the input, // and jet_residuals_ are where the final cost and derivatives end up. // // Since this buffer is used for evaluation, the adapter is not thread safe. - mutable Jet<T, kNumParameters> jet_parameters_[kNumParameters]; - mutable Jet<T, kNumParameters> jet_residuals_[kNumResiduals]; + using JetType = Jet<T, kNumParameters>; + mutable JetType jet_parameters_[kNumParameters]; + // Eigen::Matrix serves as static or dynamic container. + mutable Eigen::Matrix<JetType, kNumResiduals, 1> jet_residuals_; + + // The number of residuals is dynamically sized and the number of + // parameters is statically sized. + template<int R> + typename std::enable_if<(R == Eigen::Dynamic), void>::type Initialize( + const CostFunctor& function) { + jet_residuals_.resize(function.NumResiduals()); + num_residuals_ = function.NumResiduals(); + } + + // The number of parameters and residuals are statically sized. + template<int R> + typename std::enable_if<(R != Eigen::Dynamic), void>::type Initialize( + const CostFunctor& /* function */) { + num_residuals_ = kNumResiduals; + } }; } // namespace ceres
diff --git a/internal/ceres/tiny_solver_autodiff_function_test.cc b/internal/ceres/tiny_solver_autodiff_function_test.cc index 7b54c0f..0b542a2 100644 --- a/internal/ceres/tiny_solver_autodiff_function_test.cc +++ b/internal/ceres/tiny_solver_autodiff_function_test.cc
@@ -30,6 +30,8 @@ // Author: mierle@gmail.com (Keir Mierle) #include "ceres/tiny_solver_autodiff_function.h" +#include "ceres/tiny_solver.h" +#include "ceres/tiny_solver_test_util.h" #include <algorithm> #include <cmath> @@ -39,9 +41,6 @@ namespace ceres { -typedef Eigen::Matrix<double, 2, 1> Vec2; -typedef Eigen::Matrix<double, 3, 1> Vec3; - struct AutoDiffTestFunctor { template<typename T> bool operator()(const T* const parameters, T* residuals) const { @@ -66,8 +65,8 @@ AutoDiffTestFunctor autodiff_test_functor; AutoDiffTestFunction f(autodiff_test_functor); - Vec3 x(2.0, 1.0, 4.0); - Vec2 residuals; + Eigen::Vector3d x(2.0, 1.0, 4.0); + Eigen::Vector2d residuals; // Check the case with cost-only evaluation. residuals.setConstant(555); // Arbitrary. @@ -96,4 +95,57 @@ EXPECT_NEAR(6.0, jacobian(1, 2), kTolerance); } +class DynamicResidualsFunctor { + public: + typedef double Scalar; + enum { + NUM_RESIDUALS = Eigen::Dynamic, + NUM_PARAMETERS = 3, + }; + + int NumResiduals() const { + return 2; + } + + template<typename T> + bool operator()(const T* parameters, T* residuals) const { + // Jacobian is not evaluated by cost function, but by autodiff. + T* jacobian = NULL; + return EvaluateResidualsAndJacobians(parameters, residuals, jacobian); + } +}; + +template<typename Function, typename Vector> +void TestHelper(const Function& f, const Vector& x0) { + Vector x = x0; + Eigen::Vector2d residuals; + f(x.data(), residuals.data(), NULL); + EXPECT_GT(residuals.squaredNorm() / 2.0, 1e-10); + + TinySolver<Function> solver; + solver.Solve(f, &x); + EXPECT_NEAR(0.0, solver.summary.final_cost, 1e-10); +} + +// A test case for when the number of residuals is +// dynamically sized and we use autodiff +TEST(TinySolverAutoDiffFunction, ResidualsDynamicAutoDiff) { + Eigen::Vector3d x0(0.76026643, -30.01799744, 0.55192142); + + DynamicResidualsFunctor f; + using AutoDiffCostFunctor = + ceres::TinySolverAutoDiffFunction<DynamicResidualsFunctor, + Eigen::Dynamic, + 3>; + AutoDiffCostFunctor f_autodiff(f); + + Eigen::Vector2d residuals; + f_autodiff(x0.data(), residuals.data(), NULL); + EXPECT_GT(residuals.squaredNorm() / 2.0, 1e-10); + + TinySolver<AutoDiffCostFunctor> solver; + solver.Solve(f, &x0); + EXPECT_NEAR(0.0, solver.summary.final_cost, 1e-10); +} + } // namespace ceres
diff --git a/internal/ceres/tiny_solver_test.cc b/internal/ceres/tiny_solver_test.cc index df0101f..2a8cd39 100644 --- a/internal/ceres/tiny_solver_test.cc +++ b/internal/ceres/tiny_solver_test.cc
@@ -30,6 +30,7 @@ // Author: mierle@gmail.com (Keir Mierle) #include "ceres/tiny_solver.h" +#include "ceres/tiny_solver_test_util.h" #include <algorithm> #include <cmath> @@ -42,29 +43,6 @@ typedef Eigen::Matrix<double, 3, 1> Vec3; typedef Eigen::VectorXd VecX; -bool EvaluateResidualsAndJacobians(const double* parameters, - double* residuals, - double* jacobian) { - double x = parameters[0]; - double y = parameters[1]; - double z = parameters[2]; - - residuals[0] = x + 2*y + 4*z; - residuals[1] = y * z; - - if (jacobian) { - jacobian[0 * 2 + 0] = 1; - jacobian[0 * 2 + 1] = 0; - - jacobian[1 * 2 + 0] = 2; - jacobian[1 * 2 + 1] = z; - - jacobian[2 * 2 + 0] = 4; - jacobian[2 * 2 + 1] = y; - } - return true; -} - class ExampleStatic { public: typedef double Scalar; @@ -141,7 +119,7 @@ } }; -template <typename Function, typename Vector> +template<typename Function, typename Vector> void TestHelper(const Function& f, const Vector& x0) { Vector x = x0; Vec2 residuals; @@ -161,7 +139,6 @@ TestHelper(f, x0); } - // A test case for when the number of parameters is dynamically sized. TEST(TinySolver, ParametersDynamic) { VecX x0(3);
diff --git a/internal/ceres/tiny_solver_test_util.h b/internal/ceres/tiny_solver_test_util.h new file mode 100644 index 0000000..48fe955 --- /dev/null +++ b/internal/ceres/tiny_solver_test_util.h
@@ -0,0 +1,63 @@ + +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) + +#ifndef CERES_INTERNAL_TINY_SOLVER_TEST_UTIL_H_ +#define CERES_INTERNAL_TINY_SOLVER_TEST_UTIL_H_ + +namespace ceres { + +template<typename T> +bool EvaluateResidualsAndJacobians(const T* parameters, + T* residuals, + T* jacobian) { + T x = parameters[0]; + T y = parameters[1]; + T z = parameters[2]; + + residuals[0] = x + static_cast<T>(2) * y + static_cast<T>(4) * z; + residuals[1] = y * z; + + if (jacobian) { + jacobian[0 * 2 + 0] = static_cast<T>(1); + jacobian[0 * 2 + 1] = static_cast<T>(0); + + jacobian[1 * 2 + 0] = static_cast<T>(2); + jacobian[1 * 2 + 1] = z; + + jacobian[2 * 2 + 0] = static_cast<T>(4); + jacobian[2 * 2 + 1] = y; + } + return true; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_TINY_SOLVER_TEST_UTIL_H_