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_
