diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h index 61b8b94dc..7552a68e2 100644 --- a/include/ceres/ceres.h +++ b/include/ceres/ceres.h @@ -43,6 +43,8 @@ #include "ceres/cost_function_to_functor.h" #include "ceres/covariance.h" #include "ceres/crs_matrix.h" +#include "ceres/dynamic_autodiff_cost_function.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" #include "ceres/iteration_callback.h" #include "ceres/jet.h" #include "ceres/local_parameterization.h" diff --git a/include/ceres/dynamic_autodiff_cost_function.h b/include/ceres/dynamic_autodiff_cost_function.h index 5d8f188e5..f9342cdba 100644 --- a/include/ceres/dynamic_autodiff_cost_function.h +++ b/include/ceres/dynamic_autodiff_cost_function.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2012 Google Inc. All rights reserved. +// Copyright 2013 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -26,18 +26,17 @@ // 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) +// Author: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) // // 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. +// 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: +// The functor API differs slightly from the API for fixed size +// autodiff; the expected interface for the cost functors is: // // struct MyCostFunctor { // template @@ -46,8 +45,9 @@ // } // } // -// 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: +// 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 cost_function( // new MyCostFunctor()); @@ -55,10 +55,11 @@ // 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. +// 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_ diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h new file mode 100644 index 000000000..c30e0f145 --- /dev/null +++ b/include/ceres/dynamic_numeric_diff_cost_function.h @@ -0,0 +1,240 @@ +// 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 numeric diff implementation differs from the one found in +// numeric_diff_cost_function.h by supporting numericdiff 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 +// numeric diff; the expected interface for the cost functors is: +// +// struct MyCostFunctor { +// template +// bool operator()(double const* const* parameters, double* 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 +// DynamicNumericDiffCostFunction. For example: +// +// DynamicAutoDiffCostFunction cost_function( +// new MyCostFunctor()); +// cost_function.AddParameterBlock(5); +// cost_function.AddParameterBlock(10); +// cost_function.SetNumResiduals(21); + +#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include +#include +#include + +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { + +template +class DynamicNumericDiffCostFunction : public CostFunction { + public: + explicit DynamicNumericDiffCostFunction(CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + double relative_step_size = 1e-6) + : functor_(functor), + ownership_(ownership), + relative_step_size_(relative_step_size) { + } + + virtual ~DynamicNumericDiffCostFunction() { + if (ownership_ != TAKE_OWNERSHIP) { + functor_.release(); + } + } + + 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 DynamicNumericDiffCostFunction::SetNumResiduals() " + << "before DynamicNumericDiffCostFunction::Evaluate()."; + + const vector& block_sizes = parameter_block_sizes(); + CHECK(!block_sizes.empty()) + << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " + << "before DynamicNumericDiffCostFunction::Evaluate()."; + + bool status = (*functor_)(parameters, residuals); + if (jacobians == NULL) { + return status; + } + + // Create local space for a copy of the parameters which will get mutated. + int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); + vector parameters_copy(parameters_size); + vector parameters_references_copy(block_sizes.size()); + parameters_references_copy[0] = ¶meters_copy[0]; + for (int block = 1; block < block_sizes.size(); ++block) { + parameters_references_copy[block] = parameters_references_copy[block - 1] + + block_sizes[block - 1]; + } + + // Copy the parameters into the local temp space. + for (int block = 0; block < block_sizes.size(); ++block) { + memcpy(parameters_references_copy[block], + parameters[block], + block_sizes[block] * sizeof(*parameters[block])); + } + + for (int block = 0; block < block_sizes.size(); ++block) { + if (jacobians[block] != NULL && + !EvaluateJacobianForParameterBlock(block_sizes[block], + block, + relative_step_size_, + residuals, + ¶meters_references_copy[0], + jacobians)) { + return false; + } + } + return true; + } + + private: + bool EvaluateJacobianForParameterBlock(const int parameter_block_size, + const int parameter_block, + const double relative_step_size, + double const* residuals_at_eval_point, + double** parameters, + double** jacobians) const { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::Dynamic; + using Eigen::RowMajor; + + typedef Matrix ResidualVector; + typedef Matrix ParameterVector; + typedef Matrix JacobianMatrix; + + int num_residuals = this->num_residuals(); + + Map parameter_jacobian(jacobians[parameter_block], + num_residuals, + parameter_block_size); + + // Mutate one element at a time and then restore. + Map x_plus_delta(parameters[parameter_block], + parameter_block_size); + ParameterVector x(x_plus_delta); + ParameterVector step_size = x.array().abs() * relative_step_size; + + // To handle cases where a paremeter is exactly zero, instead use + // the mean step_size for the other dimensions. + double fallback_step_size = step_size.sum() / step_size.rows(); + if (fallback_step_size == 0.0) { + // If all the parameters are zero, there's no good answer. Use the given + // relative step_size as absolute step_size and hope for the best. + fallback_step_size = relative_step_size; + } + + // For each parameter in the parameter block, use finite + // differences to compute the derivative for that parameter. + for (int j = 0; j < parameter_block_size; ++j) { + if (step_size(j) == 0.0) { + // The parameter is exactly zero, so compromise and use the + // mean step_size from the other parameters. This can break in + // many cases, but it's hard to pick a good number without + // problem specific knowledge. + step_size(j) = fallback_step_size; + } + x_plus_delta(j) = x(j) + step_size(j); + + ResidualVector residuals(num_residuals); + if (!(*functor_)(parameters, &residuals[0])) { + // Something went wrong; bail. + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + parameter_jacobian.col(j) = residuals; + + double one_over_h = 1 / step_size(j); + if (method == CENTRAL) { + // Compute the function on the other side of x(j). + x_plus_delta(j) = x(j) - step_size(j); + + if (!(*functor_)(parameters, &residuals[0])) { + // Something went wrong; bail. + return false; + } + + parameter_jacobian.col(j) -= residuals; + one_over_h /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + parameter_jacobian.col(j) -= + Map(residuals_at_eval_point, num_residuals); + } + x_plus_delta(j) = x(j); // Restore x_plus_delta. + + // Divide out the run to get slope. + parameter_jacobian.col(j) *= one_over_h; + } + return true; + } + + internal::scoped_ptr functor_; + Ownership ownership_; + const double relative_step_size_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 610e8169b..7c23a683a 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -241,6 +241,7 @@ IF (BUILD_TESTING AND GFLAGS) CERES_TEST(covariance) CERES_TEST(dense_sparse_matrix) CERES_TEST(dynamic_autodiff_cost_function) + CERES_TEST(dynamic_numeric_diff_cost_function) CERES_TEST(evaluator) CERES_TEST(gradient_checker) CERES_TEST(gradient_checking_cost_function) diff --git a/internal/ceres/dynamic_numeric_diff_cost_function_test.cc b/internal/ceres/dynamic_numeric_diff_cost_function_test.cc new file mode 100644 index 000000000..19f4d8846 --- /dev/null +++ b/internal/ceres/dynamic_numeric_diff_cost_function_test.cc @@ -0,0 +1,519 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) + +#include + +#include "ceres/dynamic_numeric_diff_cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +const double kTolerance = 1e-6; + +// 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: + bool operator()(double const* const* parameters, double* residuals) const { + const double* params0 = parameters[0]; + int r = 0; + for (int i = 0; i < 10; ++i) { + residuals[r++] = i - params0[i]; + residuals[r++] = params0[i] - i; + } + + double c_residual = 0.0; + for (int i = 0; i < 10; ++i) { + c_residual += pow(params0[i], 2) - 8.0 * params0[i]; + } + + const double* params1 = parameters[1]; + for (int i = 0; i < 5; ++i) { + c_residual += params1[i]; + } + residuals[r++] = c_residual; + return true; + } +}; + +TEST(DynamicNumericdiffCostFunctionTest, TestResiduals) { + vector param_block_0(10, 0.0); + vector param_block_1(5, 0.0); + DynamicNumericDiffCostFunction 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 residuals(21, -100000); + vector 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(DynamicNumericdiffCostFunctionTest, TestJacobian) { + // Test the residual counting. + vector param_block_0(10, 0.0); + for (int i = 0; i < 10; ++i) { + param_block_0[i] = 2 * i; + } + vector param_block_1(5, 0.0); + DynamicNumericDiffCostFunction 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 residuals(21, -100000); + + // Prepare the parameters. + vector parameter_blocks(2); + parameter_blocks[0] = ¶m_block_0[0]; + parameter_blocks[1] = ¶m_block_1[0]; + + // Prepare the jacobian. + vector > jacobian_vect(2); + jacobian_vect[0].resize(21 * 10, -100000); + jacobian_vect[1].resize(21 * 5, -100000); + vector 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_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance); + // Check "B" Jacobian. + EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance); + 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_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance); + jacobian_vect[0][20 * 10 + p] = 0.0; + } + for (int i = 0; i < jacobian_vect[0].size(); ++i) { + EXPECT_NEAR(0.0, jacobian_vect[0][i], kTolerance); + } + + // Check "C" Jacobian for second parameter block. + for (int p = 0; p < 5; ++p) { + EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance); + jacobian_vect[1][20 * 5 + p] = 0.0; + } + for (int i = 0; i < jacobian_vect[1].size(); ++i) { + EXPECT_NEAR(0.0, jacobian_vect[1][i], kTolerance); + } +} + +TEST(DynamicNumericdiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) { // NOLINT + // Test the residual counting. + vector param_block_0(10, 0.0); + for (int i = 0; i < 10; ++i) { + param_block_0[i] = 2 * i; + } + vector param_block_1(5, 0.0); + DynamicNumericDiffCostFunction 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 residuals(21, -100000); + + // Prepare the parameters. + vector parameter_blocks(2); + parameter_blocks[0] = ¶m_block_0[0]; + parameter_blocks[1] = ¶m_block_1[0]; + + // Prepare the jacobian. + vector > jacobian_vect(2); + jacobian_vect[0].resize(21 * 10, -100000); + jacobian_vect[1].resize(21 * 5, -100000); + vector jacobian; + jacobian.push_back(NULL); + 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)); + + // Check "C" Jacobian for second parameter block. + for (int p = 0; p < 5; ++p) { + EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance); + 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]); + } +} + +TEST(DynamicNumericdiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) { // NOLINT + // Test the residual counting. + vector param_block_0(10, 0.0); + for (int i = 0; i < 10; ++i) { + param_block_0[i] = 2 * i; + } + vector param_block_1(5, 0.0); + DynamicNumericDiffCostFunction 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 residuals(21, -100000); + + // Prepare the parameters. + vector parameter_blocks(2); + parameter_blocks[0] = ¶m_block_0[0]; + parameter_blocks[1] = ¶m_block_1[0]; + + // Prepare the jacobian. + vector > jacobian_vect(2); + jacobian_vect[0].resize(21 * 10, -100000); + jacobian_vect[1].resize(21 * 5, -100000); + vector jacobian; + jacobian.push_back(jacobian_vect[0].data()); + jacobian.push_back(NULL); + + // 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_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance); + // Check "B" Jacobian. + EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance); + 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_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance); + 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]); + } +} + +// Takes 3 parameter blocks: +// parameters[0] (x) is size 1. +// parameters[1] (y) is size 2. +// parameters[2] (z) is size 3. +// Emits 7 residuals: +// A: x[0] (= sum_x) +// B: y[0] + 2.0 * y[1] (= sum_y) +// C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z) +// D: sum_x * sum_y +// E: sum_y * sum_z +// F: sum_x * sum_z +// G: sum_x * sum_y * sum_z +class MyThreeParameterCostFunctor { + public: + template + bool operator()(T const* const* parameters, T* residuals) const { + const T* x = parameters[0]; + const T* y = parameters[1]; + const T* z = parameters[2]; + + T sum_x = x[0]; + T sum_y = y[0] + 2.0 * y[1]; + T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2]; + + residuals[0] = sum_x; + residuals[1] = sum_y; + residuals[2] = sum_z; + residuals[3] = sum_x * sum_y; + residuals[4] = sum_y * sum_z; + residuals[5] = sum_x * sum_z; + residuals[6] = sum_x * sum_y * sum_z; + return true; + } +}; + +class ThreeParameterCostFunctorTest : public ::testing::Test { + protected: + virtual void SetUp() { + // Prepare the parameters. + x_.resize(1); + x_[0] = 0.0; + + y_.resize(2); + y_[0] = 1.0; + y_[1] = 3.0; + + z_.resize(3); + z_[0] = 2.0; + z_[1] = 4.0; + z_[2] = 6.0; + + parameter_blocks_.resize(3); + parameter_blocks_[0] = &x_[0]; + parameter_blocks_[1] = &y_[0]; + parameter_blocks_[2] = &z_[0]; + + // Prepare the cost function. + typedef DynamicNumericDiffCostFunction + DynamicMyThreeParameterCostFunction; + DynamicMyThreeParameterCostFunction * cost_function = + new DynamicMyThreeParameterCostFunction( + new MyThreeParameterCostFunctor()); + cost_function->AddParameterBlock(1); + cost_function->AddParameterBlock(2); + cost_function->AddParameterBlock(3); + cost_function->SetNumResiduals(7); + + cost_function_.reset(cost_function); + + // Setup jacobian data. + jacobian_vect_.resize(3); + jacobian_vect_[0].resize(7 * x_.size(), -100000); + jacobian_vect_[1].resize(7 * y_.size(), -100000); + jacobian_vect_[2].resize(7 * z_.size(), -100000); + + // Prepare the expected residuals. + const double sum_x = x_[0]; + const double sum_y = y_[0] + 2.0 * y_[1]; + const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2]; + + expected_residuals_.resize(7); + expected_residuals_[0] = sum_x; + expected_residuals_[1] = sum_y; + expected_residuals_[2] = sum_z; + expected_residuals_[3] = sum_x * sum_y; + expected_residuals_[4] = sum_y * sum_z; + expected_residuals_[5] = sum_x * sum_z; + expected_residuals_[6] = sum_x * sum_y * sum_z; + + // Prepare the expected jacobian entries. + expected_jacobian_x_.resize(7); + expected_jacobian_x_[0] = 1.0; + expected_jacobian_x_[1] = 0.0; + expected_jacobian_x_[2] = 0.0; + expected_jacobian_x_[3] = sum_y; + expected_jacobian_x_[4] = 0.0; + expected_jacobian_x_[5] = sum_z; + expected_jacobian_x_[6] = sum_y * sum_z; + + expected_jacobian_y_.resize(14); + expected_jacobian_y_[0] = 0.0; + expected_jacobian_y_[1] = 0.0; + expected_jacobian_y_[2] = 1.0; + expected_jacobian_y_[3] = 2.0; + expected_jacobian_y_[4] = 0.0; + expected_jacobian_y_[5] = 0.0; + expected_jacobian_y_[6] = sum_x; + expected_jacobian_y_[7] = 2.0 * sum_x; + expected_jacobian_y_[8] = sum_z; + expected_jacobian_y_[9] = 2.0 * sum_z; + expected_jacobian_y_[10] = 0.0; + expected_jacobian_y_[11] = 0.0; + expected_jacobian_y_[12] = sum_x * sum_z; + expected_jacobian_y_[13] = 2.0 * sum_x * sum_z; + + expected_jacobian_z_.resize(21); + expected_jacobian_z_[0] = 0.0; + expected_jacobian_z_[1] = 0.0; + expected_jacobian_z_[2] = 0.0; + expected_jacobian_z_[3] = 0.0; + expected_jacobian_z_[4] = 0.0; + expected_jacobian_z_[5] = 0.0; + expected_jacobian_z_[6] = 1.0; + expected_jacobian_z_[7] = 3.0; + expected_jacobian_z_[8] = 6.0; + expected_jacobian_z_[9] = 0.0; + expected_jacobian_z_[10] = 0.0; + expected_jacobian_z_[11] = 0.0; + expected_jacobian_z_[12] = sum_y; + expected_jacobian_z_[13] = 3.0 * sum_y; + expected_jacobian_z_[14] = 6.0 * sum_y; + expected_jacobian_z_[15] = sum_x; + expected_jacobian_z_[16] = 3.0 * sum_x; + expected_jacobian_z_[17] = 6.0 * sum_x; + expected_jacobian_z_[18] = sum_x * sum_y; + expected_jacobian_z_[19] = 3.0 * sum_x * sum_y; + expected_jacobian_z_[20] = 6.0 * sum_x * sum_y; + } + + protected: + vector x_; + vector y_; + vector z_; + + vector parameter_blocks_; + + scoped_ptr cost_function_; + + vector > jacobian_vect_; + + vector expected_residuals_; + + vector expected_jacobian_x_; + vector expected_jacobian_y_; + vector expected_jacobian_z_; +}; + +TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) { + vector residuals(7, -100000); + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + NULL)); + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } +} + +TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) { + vector residuals(7, -100000); + + vector jacobian; + jacobian.push_back(jacobian_vect_[0].data()); + jacobian.push_back(jacobian_vect_[1].data()); + jacobian.push_back(jacobian_vect_[2].data()); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 7; ++i) { + EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance); + } + + for (int i = 0; i < 14; ++i) { + EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance); + } + + for (int i = 0; i < 21; ++i) { + EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance); + } +} + +TEST_F(ThreeParameterCostFunctorTest, + ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) { + vector residuals(7, -100000); + + vector jacobian; + jacobian.push_back(NULL); + jacobian.push_back(jacobian_vect_[1].data()); + jacobian.push_back(NULL); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 14; ++i) { + EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance); + } +} + +TEST_F(ThreeParameterCostFunctorTest, + ThreeParameterJacobianWithSecondParameterBlockConstant) { + vector residuals(7, -100000); + + vector jacobian; + jacobian.push_back(jacobian_vect_[0].data()); + jacobian.push_back(NULL); + jacobian.push_back(jacobian_vect_[2].data()); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 7; ++i) { + EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance); + } + + for (int i = 0; i < 21; ++i) { + EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance); + } +} + +} // namespace internal +} // namespace ceres