// 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 "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/internal/scoped_ptr.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 N0 = 0, int N1 = 0, int N2 = 0, bool kSucceeds = true>

class ParameterIgnoringCostFunction

    : public SizedCostFunction<kNumResiduals, N0, N1, N2> {

  typedef SizedCostFunction<kNumResiduals, N0, N1, N2> Base;

 public:

  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] != NULL) {

          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 kSucceeds;

  }

};

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 evalutor 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;

    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) {

    scoped_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);

    scoped_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 != NULL ? &residuals[0]  : NULL,

          expected_gradient  != NULL ? &gradient[0]   : NULL,

          expected_jacobian  != NULL ? jacobian.get() : NULL));

    Matrix actual_jacobian;

    if (expected_jacobian != NULL) {

      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 : NULL,

                         (i & 2) ? expected.gradient  : NULL,

                         (i & 4) ? expected.jacobian  : NULL);

    }

  }

  // 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>,

                           NULL,

                           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>,

                           NULL,

                           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>,

                           NULL,

                           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>,

                           NULL,

                           x, y);

  // g(x, z) in R^3

  problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,

                           NULL,

                           x, z);

  // h(y, z) in R^4

  problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,

                           NULL,

                           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>,

                           NULL,

                           x, y);

  // g(x, z) in R^3

  problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,

                           NULL,

                           x, z);

  // h(y, z) in R^4

  problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,

                           NULL,

                           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>,

                           NULL,

                           x, y);

  // g(x, z) in R^3

  problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,

                           NULL,

                           x, z);

  // h(y, z) in R^4

  problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,

                           NULL,

                           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>, NULL, x, y, z);

  // The values are ignored.

  double state[9];

  scoped_ptr<Evaluator> evaluator(CreateEvaluator(problem.mutable_program()));

  scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());

  double cost;

  EXPECT_FALSE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL));

}

// 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 != NULL) {

      double* jacobian = jacobians[0];

      if (jacobian != NULL) {

        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(), NULL, x);

  Program* program = problem.mutable_program();

  program->SetParameterOffsetsAndIndex();

  Evaluator::Options options;

  options.linear_solver_type = DENSE_QR;

  options.num_eliminate_blocks = 0;

  string error;

  scoped_ptr<Evaluator> evaluator(Evaluator::Create(options, program, &error));

  scoped_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, NULL, NULL, NULL));

    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, NULL, NULL));

    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,

                                    NULL,

                                    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