Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/evaluator_test.cc b/internal/ceres/evaluator_test.cc
new file mode 100644
index 0000000..a156b89
--- /dev/null
+++ b/internal/ceres/evaluator_test.cc
@@ -0,0 +1,677 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 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: keir@google.com (Keir Mierle)
+//
+// Tests shared across evaluators. The tests try all combinations of linear
+// solver and num_eliminate_blocks (for schur-based solvers).
+
+#include "ceres/evaluator.h"
+
+#include <memory>
+#include "ceres/casts.h"
+#include "ceres/cost_function.h"
+#include "ceres/crs_matrix.h"
+#include "ceres/evaluator_test_utils.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/problem_impl.h"
+#include "ceres/program.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/sparse_matrix.h"
+#include "ceres/stringprintf.h"
+#include "ceres/types.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+using std::string;
+using std::vector;
+
+// TODO(keir): Consider pushing this into a common test utils file.
+template <int kFactor, int kNumResiduals, int... Ns>
+class ParameterIgnoringCostFunction
+ : public SizedCostFunction<kNumResiduals, Ns...> {
+ typedef SizedCostFunction<kNumResiduals, Ns...> Base;
+
+ public:
+ explicit ParameterIgnoringCostFunction(bool succeeds = true)
+ : succeeds_(succeeds) {}
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ for (int i = 0; i < Base::num_residuals(); ++i) {
+ residuals[i] = i + 1;
+ }
+ if (jacobians) {
+ for (int k = 0; k < Base::parameter_block_sizes().size(); ++k) {
+ // The jacobians here are full sized, but they are transformed in the
+ // evaluator into the "local" jacobian. In the tests, the "subset
+ // constant" parameterization is used, which should pick out columns
+ // from these jacobians. Put values in the jacobian that make this
+ // obvious; in particular, make the jacobians like this:
+ //
+ // 1 2 3 4 ...
+ // 1 2 3 4 ... .* kFactor
+ // 1 2 3 4 ...
+ //
+ // where the multiplication by kFactor makes it easier to distinguish
+ // between Jacobians of different residuals for the same parameter.
+ if (jacobians[k] != nullptr) {
+ MatrixRef jacobian(jacobians[k],
+ Base::num_residuals(),
+ Base::parameter_block_sizes()[k]);
+ for (int j = 0; j < Base::parameter_block_sizes()[k]; ++j) {
+ jacobian.col(j).setConstant(kFactor * (j + 1));
+ }
+ }
+ }
+ }
+ return succeeds_;
+ }
+
+ private:
+ bool succeeds_;
+};
+
+struct EvaluatorTestOptions {
+ EvaluatorTestOptions(LinearSolverType linear_solver_type,
+ int num_eliminate_blocks,
+ bool dynamic_sparsity = false)
+ : linear_solver_type(linear_solver_type),
+ num_eliminate_blocks(num_eliminate_blocks),
+ dynamic_sparsity(dynamic_sparsity) {}
+
+ LinearSolverType linear_solver_type;
+ int num_eliminate_blocks;
+ bool dynamic_sparsity;
+};
+
+struct EvaluatorTest
+ : public ::testing::TestWithParam<EvaluatorTestOptions> {
+ Evaluator* CreateEvaluator(Program* program) {
+ // This program is straight from the ProblemImpl, and so has no index/offset
+ // yet; compute it here as required by the evaluator implementations.
+ program->SetParameterOffsetsAndIndex();
+
+ if (VLOG_IS_ON(1)) {
+ string report;
+ StringAppendF(&report, "Creating evaluator with type: %d",
+ GetParam().linear_solver_type);
+ if (GetParam().linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
+ StringAppendF(&report, ", dynamic_sparsity: %d",
+ GetParam().dynamic_sparsity);
+ }
+ StringAppendF(&report, " and num_eliminate_blocks: %d",
+ GetParam().num_eliminate_blocks);
+ VLOG(1) << report;
+ }
+ Evaluator::Options options;
+ options.linear_solver_type = GetParam().linear_solver_type;
+ options.num_eliminate_blocks = GetParam().num_eliminate_blocks;
+ options.dynamic_sparsity = GetParam().dynamic_sparsity;
+ options.context = problem.context();
+ string error;
+ return Evaluator::Create(options, program, &error);
+ }
+
+ void EvaluateAndCompare(ProblemImpl *problem,
+ int expected_num_rows,
+ int expected_num_cols,
+ double expected_cost,
+ const double* expected_residuals,
+ const double* expected_gradient,
+ const double* expected_jacobian) {
+ std::unique_ptr<Evaluator> evaluator(
+ CreateEvaluator(problem->mutable_program()));
+ int num_residuals = expected_num_rows;
+ int num_parameters = expected_num_cols;
+
+ double cost = -1;
+
+ Vector residuals(num_residuals);
+ residuals.setConstant(-2000);
+
+ Vector gradient(num_parameters);
+ gradient.setConstant(-3000);
+
+ std::unique_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
+
+ ASSERT_EQ(expected_num_rows, evaluator->NumResiduals());
+ ASSERT_EQ(expected_num_cols, evaluator->NumEffectiveParameters());
+ ASSERT_EQ(expected_num_rows, jacobian->num_rows());
+ ASSERT_EQ(expected_num_cols, jacobian->num_cols());
+
+ vector<double> state(evaluator->NumParameters());
+
+ ASSERT_TRUE(evaluator->Evaluate(
+ &state[0],
+ &cost,
+ expected_residuals != nullptr ? &residuals[0] : nullptr,
+ expected_gradient != nullptr ? &gradient[0] : nullptr,
+ expected_jacobian != nullptr ? jacobian.get() : nullptr));
+
+ Matrix actual_jacobian;
+ if (expected_jacobian != nullptr) {
+ jacobian->ToDenseMatrix(&actual_jacobian);
+ }
+
+ CompareEvaluations(expected_num_rows,
+ expected_num_cols,
+ expected_cost,
+ expected_residuals,
+ expected_gradient,
+ expected_jacobian,
+ cost,
+ &residuals[0],
+ &gradient[0],
+ actual_jacobian.data());
+ }
+
+ // Try all combinations of parameters for the evaluator.
+ void CheckAllEvaluationCombinations(const ExpectedEvaluation &expected) {
+ for (int i = 0; i < 8; ++i) {
+ EvaluateAndCompare(&problem,
+ expected.num_rows,
+ expected.num_cols,
+ expected.cost,
+ (i & 1) ? expected.residuals : nullptr,
+ (i & 2) ? expected.gradient : nullptr,
+ (i & 4) ? expected.jacobian : nullptr);
+ }
+ }
+
+ // The values are ignored completely by the cost function.
+ double x[2];
+ double y[3];
+ double z[4];
+
+ ProblemImpl problem;
+};
+
+void SetSparseMatrixConstant(SparseMatrix* sparse_matrix, double value) {
+ VectorRef(sparse_matrix->mutable_values(),
+ sparse_matrix->num_nonzeros()).setConstant(value);
+}
+
+TEST_P(EvaluatorTest, SingleResidualProblem) {
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
+ nullptr,
+ x, y, z);
+
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 3, 9,
+ // Cost
+ 7.0,
+ // Residuals
+ { 1.0, 2.0, 3.0 },
+ // Gradient
+ { 6.0, 12.0, // x
+ 6.0, 12.0, 18.0, // y
+ 6.0, 12.0, 18.0, 24.0, // z
+ },
+ // Jacobian
+ // x y z
+ { 1, 2, 1, 2, 3, 1, 2, 3, 4,
+ 1, 2, 1, 2, 3, 1, 2, 3, 4,
+ 1, 2, 1, 2, 3, 1, 2, 3, 4
+ }
+ };
+ CheckAllEvaluationCombinations(expected);
+}
+
+TEST_P(EvaluatorTest, SingleResidualProblemWithPermutedParameters) {
+ // Add the parameters in explicit order to force the ordering in the program.
+ problem.AddParameterBlock(x, 2);
+ problem.AddParameterBlock(y, 3);
+ problem.AddParameterBlock(z, 4);
+
+ // Then use a cost function which is similar to the others, but swap around
+ // the ordering of the parameters to the cost function. This shouldn't affect
+ // the jacobian evaluation, but requires explicit handling in the evaluators.
+ // At one point the compressed row evaluator had a bug that went undetected
+ // for a long time, since by chance most users added parameters to the problem
+ // in the same order that they occurred as parameters to a cost function.
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 4, 3, 2>,
+ nullptr,
+ z, y, x);
+
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 3, 9,
+ // Cost
+ 7.0,
+ // Residuals
+ { 1.0, 2.0, 3.0 },
+ // Gradient
+ { 6.0, 12.0, // x
+ 6.0, 12.0, 18.0, // y
+ 6.0, 12.0, 18.0, 24.0, // z
+ },
+ // Jacobian
+ // x y z
+ { 1, 2, 1, 2, 3, 1, 2, 3, 4,
+ 1, 2, 1, 2, 3, 1, 2, 3, 4,
+ 1, 2, 1, 2, 3, 1, 2, 3, 4
+ }
+ };
+ CheckAllEvaluationCombinations(expected);
+}
+
+TEST_P(EvaluatorTest, SingleResidualProblemWithNuisanceParameters) {
+ // These parameters are not used.
+ double a[2];
+ double b[1];
+ double c[1];
+ double d[3];
+
+ // Add the parameters in a mixed order so the Jacobian is "checkered" with the
+ // values from the other parameters.
+ problem.AddParameterBlock(a, 2);
+ problem.AddParameterBlock(x, 2);
+ problem.AddParameterBlock(b, 1);
+ problem.AddParameterBlock(y, 3);
+ problem.AddParameterBlock(c, 1);
+ problem.AddParameterBlock(z, 4);
+ problem.AddParameterBlock(d, 3);
+
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
+ nullptr,
+ x, y, z);
+
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 3, 16,
+ // Cost
+ 7.0,
+ // Residuals
+ { 1.0, 2.0, 3.0 },
+ // Gradient
+ { 0.0, 0.0, // a
+ 6.0, 12.0, // x
+ 0.0, // b
+ 6.0, 12.0, 18.0, // y
+ 0.0, // c
+ 6.0, 12.0, 18.0, 24.0, // z
+ 0.0, 0.0, 0.0, // d
+ },
+ // Jacobian
+ // a x b y c z d
+ { 0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0,
+ 0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0,
+ 0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0
+ }
+ };
+ CheckAllEvaluationCombinations(expected);
+}
+
+TEST_P(EvaluatorTest, MultipleResidualProblem) {
+ // Add the parameters in explicit order to force the ordering in the program.
+ problem.AddParameterBlock(x, 2);
+ problem.AddParameterBlock(y, 3);
+ problem.AddParameterBlock(z, 4);
+
+ // f(x, y) in R^2
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
+ nullptr,
+ x, y);
+
+ // g(x, z) in R^3
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
+ nullptr,
+ x, z);
+
+ // h(y, z) in R^4
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
+ nullptr,
+ y, z);
+
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 9, 9,
+ // Cost
+ // f g h
+ ( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
+ // Residuals
+ { 1.0, 2.0, // f
+ 1.0, 2.0, 3.0, // g
+ 1.0, 2.0, 3.0, 4.0 // h
+ },
+ // Gradient
+ { 15.0, 30.0, // x
+ 33.0, 66.0, 99.0, // y
+ 42.0, 84.0, 126.0, 168.0 // z
+ },
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ 1, 2, 1, 2, 3, 0, 0, 0, 0,
+ 1, 2, 1, 2, 3, 0, 0, 0, 0,
+
+ /* g(x, z) */ 2, 4, 0, 0, 0, 2, 4, 6, 8,
+ 2, 4, 0, 0, 0, 2, 4, 6, 8,
+ 2, 4, 0, 0, 0, 2, 4, 6, 8,
+
+ /* h(y, z) */ 0, 0, 3, 6, 9, 3, 6, 9, 12,
+ 0, 0, 3, 6, 9, 3, 6, 9, 12,
+ 0, 0, 3, 6, 9, 3, 6, 9, 12,
+ 0, 0, 3, 6, 9, 3, 6, 9, 12
+ }
+ };
+ CheckAllEvaluationCombinations(expected);
+}
+
+TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) {
+ // Add the parameters in explicit order to force the ordering in the program.
+ problem.AddParameterBlock(x, 2);
+
+ // Fix y's first dimension.
+ vector<int> y_fixed;
+ y_fixed.push_back(0);
+ problem.AddParameterBlock(y, 3, new SubsetParameterization(3, y_fixed));
+
+ // Fix z's second dimension.
+ vector<int> z_fixed;
+ z_fixed.push_back(1);
+ problem.AddParameterBlock(z, 4, new SubsetParameterization(4, z_fixed));
+
+ // f(x, y) in R^2
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
+ nullptr,
+ x, y);
+
+ // g(x, z) in R^3
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
+ nullptr,
+ x, z);
+
+ // h(y, z) in R^4
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
+ nullptr,
+ y, z);
+
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 9, 7,
+ // Cost
+ // f g h
+ ( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
+ // Residuals
+ { 1.0, 2.0, // f
+ 1.0, 2.0, 3.0, // g
+ 1.0, 2.0, 3.0, 4.0 // h
+ },
+ // Gradient
+ { 15.0, 30.0, // x
+ 66.0, 99.0, // y
+ 42.0, 126.0, 168.0 // z
+ },
+ // Jacobian
+ // x y z
+ { /* f(x, y) */ 1, 2, 2, 3, 0, 0, 0,
+ 1, 2, 2, 3, 0, 0, 0,
+
+ /* g(x, z) */ 2, 4, 0, 0, 2, 6, 8,
+ 2, 4, 0, 0, 2, 6, 8,
+ 2, 4, 0, 0, 2, 6, 8,
+
+ /* h(y, z) */ 0, 0, 6, 9, 3, 9, 12,
+ 0, 0, 6, 9, 3, 9, 12,
+ 0, 0, 6, 9, 3, 9, 12,
+ 0, 0, 6, 9, 3, 9, 12
+ }
+ };
+ CheckAllEvaluationCombinations(expected);
+}
+
+TEST_P(EvaluatorTest, MultipleResidualProblemWithSomeConstantParameters) {
+ // The values are ignored completely by the cost function.
+ double x[2];
+ double y[3];
+ double z[4];
+
+ // Add the parameters in explicit order to force the ordering in the program.
+ problem.AddParameterBlock(x, 2);
+ problem.AddParameterBlock(y, 3);
+ problem.AddParameterBlock(z, 4);
+
+ // f(x, y) in R^2
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
+ nullptr,
+ x, y);
+
+ // g(x, z) in R^3
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
+ nullptr,
+ x, z);
+
+ // h(y, z) in R^4
+ problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
+ nullptr,
+ y, z);
+
+ // For this test, "z" is constant.
+ problem.SetParameterBlockConstant(z);
+
+ // Create the reduced program which is missing the fixed "z" variable.
+ // Normally, the preprocessing of the program that happens in solver_impl
+ // takes care of this, but we don't want to invoke the solver here.
+ Program reduced_program;
+ vector<ParameterBlock*>* parameter_blocks =
+ problem.mutable_program()->mutable_parameter_blocks();
+
+ // "z" is the last parameter; save it for later and pop it off temporarily.
+ // Note that "z" will still get read during evaluation, so it cannot be
+ // deleted at this point.
+ ParameterBlock* parameter_block_z = parameter_blocks->back();
+ parameter_blocks->pop_back();
+
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 9, 5,
+ // Cost
+ // f g h
+ ( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
+ // Residuals
+ { 1.0, 2.0, // f
+ 1.0, 2.0, 3.0, // g
+ 1.0, 2.0, 3.0, 4.0 // h
+ },
+ // Gradient
+ { 15.0, 30.0, // x
+ 33.0, 66.0, 99.0, // y
+ },
+ // Jacobian
+ // x y
+ { /* f(x, y) */ 1, 2, 1, 2, 3,
+ 1, 2, 1, 2, 3,
+
+ /* g(x, z) */ 2, 4, 0, 0, 0,
+ 2, 4, 0, 0, 0,
+ 2, 4, 0, 0, 0,
+
+ /* h(y, z) */ 0, 0, 3, 6, 9,
+ 0, 0, 3, 6, 9,
+ 0, 0, 3, 6, 9,
+ 0, 0, 3, 6, 9
+ }
+ };
+ CheckAllEvaluationCombinations(expected);
+
+ // Restore parameter block z, so it will get freed in a consistent way.
+ parameter_blocks->push_back(parameter_block_z);
+}
+
+TEST_P(EvaluatorTest, EvaluatorAbortsForResidualsThatFailToEvaluate) {
+ // Switch the return value to failure.
+ problem.AddResidualBlock(
+ new ParameterIgnoringCostFunction<20, 3, 2, 3, 4>(false),
+ nullptr,
+ x,
+ y,
+ z);
+
+ // The values are ignored.
+ double state[9];
+
+ std::unique_ptr<Evaluator> evaluator(
+ CreateEvaluator(problem.mutable_program()));
+ std::unique_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
+ double cost;
+ EXPECT_FALSE(evaluator->Evaluate(state, &cost, nullptr, nullptr, nullptr));
+}
+
+// In the pairs, the first argument is the linear solver type, and the second
+// argument is num_eliminate_blocks. Changing the num_eliminate_blocks only
+// makes sense for the schur-based solvers.
+//
+// Try all values of num_eliminate_blocks that make sense given that in the
+// tests a maximum of 4 parameter blocks are present.
+INSTANTIATE_TEST_CASE_P(
+ LinearSolvers,
+ EvaluatorTest,
+ ::testing::Values(EvaluatorTestOptions(DENSE_QR, 0),
+ EvaluatorTestOptions(DENSE_SCHUR, 0),
+ EvaluatorTestOptions(DENSE_SCHUR, 1),
+ EvaluatorTestOptions(DENSE_SCHUR, 2),
+ EvaluatorTestOptions(DENSE_SCHUR, 3),
+ EvaluatorTestOptions(DENSE_SCHUR, 4),
+ EvaluatorTestOptions(SPARSE_SCHUR, 0),
+ EvaluatorTestOptions(SPARSE_SCHUR, 1),
+ EvaluatorTestOptions(SPARSE_SCHUR, 2),
+ EvaluatorTestOptions(SPARSE_SCHUR, 3),
+ EvaluatorTestOptions(SPARSE_SCHUR, 4),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 0),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 1),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 2),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 3),
+ EvaluatorTestOptions(ITERATIVE_SCHUR, 4),
+ EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, false),
+ EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, true)));
+
+// Simple cost function used to check if the evaluator is sensitive to
+// state changes.
+class ParameterSensitiveCostFunction : public SizedCostFunction<2, 2> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ double x1 = parameters[0][0];
+ double x2 = parameters[0][1];
+ residuals[0] = x1 * x1;
+ residuals[1] = x2 * x2;
+
+ if (jacobians != nullptr) {
+ double* jacobian = jacobians[0];
+ if (jacobian != nullptr) {
+ jacobian[0] = 2.0 * x1;
+ jacobian[1] = 0.0;
+ jacobian[2] = 0.0;
+ jacobian[3] = 2.0 * x2;
+ }
+ }
+ return true;
+ }
+};
+
+TEST(Evaluator, EvaluatorRespectsParameterChanges) {
+ ProblemImpl problem;
+
+ double x[2];
+ x[0] = 1.0;
+ x[1] = 1.0;
+
+ problem.AddResidualBlock(new ParameterSensitiveCostFunction(), nullptr, x);
+ Program* program = problem.mutable_program();
+ program->SetParameterOffsetsAndIndex();
+
+ Evaluator::Options options;
+ options.linear_solver_type = DENSE_QR;
+ options.num_eliminate_blocks = 0;
+ options.context = problem.context();
+ string error;
+ std::unique_ptr<Evaluator> evaluator(
+ Evaluator::Create(options, program, &error));
+ std::unique_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
+
+ ASSERT_EQ(2, jacobian->num_rows());
+ ASSERT_EQ(2, jacobian->num_cols());
+
+ double state[2];
+ state[0] = 2.0;
+ state[1] = 3.0;
+
+ // The original state of a residual block comes from the user's
+ // state. So the original state is 1.0, 1.0, and the only way we get
+ // the 2.0, 3.0 results in the following tests is if it respects the
+ // values in the state vector.
+
+ // Cost only; no residuals and no jacobian.
+ {
+ double cost = -1;
+ ASSERT_TRUE(evaluator->Evaluate(state, &cost, nullptr, nullptr, nullptr));
+ EXPECT_EQ(48.5, cost);
+ }
+
+ // Cost and residuals, no jacobian.
+ {
+ double cost = -1;
+ double residuals[2] = {-2, -2};
+ ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, nullptr, nullptr));
+ EXPECT_EQ(48.5, cost);
+ EXPECT_EQ(4, residuals[0]);
+ EXPECT_EQ(9, residuals[1]);
+ }
+
+ // Cost, residuals, and jacobian.
+ {
+ double cost = -1;
+ double residuals[2] = {-2, -2};
+ SetSparseMatrixConstant(jacobian.get(), -1);
+ ASSERT_TRUE(
+ evaluator->Evaluate(state, &cost, residuals, nullptr, jacobian.get()));
+ EXPECT_EQ(48.5, cost);
+ EXPECT_EQ(4, residuals[0]);
+ EXPECT_EQ(9, residuals[1]);
+ Matrix actual_jacobian;
+ jacobian->ToDenseMatrix(&actual_jacobian);
+
+ Matrix expected_jacobian(2, 2);
+ expected_jacobian << 2 * state[0], 0, 0, 2 * state[1];
+
+ EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all())
+ << "Actual:\n"
+ << actual_jacobian << "\nExpected:\n"
+ << expected_jacobian;
+ }
+}
+
+} // namespace internal
+} // namespace ceres