// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 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: 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/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/types.h"
#include "gtest/gtest.h"

namespace ceres {
namespace internal {

// TODO(keir): Consider pushing this into a common test utils file.
template<int kFactor, int kNumResiduals,
         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 ExpectedEvaluation {
  int num_rows;
  int num_cols;
  double cost;
  const double residuals[50];
  const double gradient[50];
  const double jacobian[200];
};

void CompareEvaluations(int expected_num_rows,
                        int expected_num_cols,
                        double expected_cost,
                        const double* expected_residuals,
                        const double* expected_gradient,
                        const double* expected_jacobian,
                        const double actual_cost,
                        const double* actual_residuals,
                        const double* actual_gradient,
                        const double* actual_jacobian) {
  EXPECT_EQ(expected_cost, actual_cost);

  if (expected_residuals != NULL) {
    ConstVectorRef expected_residuals_vector(expected_residuals,
                                             expected_num_rows);
    ConstVectorRef actual_residuals_vector(actual_residuals,
                                           expected_num_rows);
    EXPECT_TRUE((actual_residuals_vector.array() ==
                 expected_residuals_vector.array()).all())
        << "Actual:\n" << actual_residuals_vector
        << "\nExpected:\n" << expected_residuals_vector;
  }

  if (expected_gradient != NULL) {
    ConstVectorRef expected_gradient_vector(expected_gradient,
                                            expected_num_cols);
    ConstVectorRef actual_gradient_vector(actual_gradient,
                                            expected_num_cols);

    EXPECT_TRUE((actual_gradient_vector.array() ==
                 expected_gradient_vector.array()).all())
        << "Actual:\n" << actual_gradient_vector.transpose()
        << "\nExpected:\n" << expected_gradient_vector.transpose();
  }

  if (expected_jacobian != NULL) {
    ConstMatrixRef expected_jacobian_matrix(expected_jacobian,
                                            expected_num_rows,
                                            expected_num_cols);
    ConstMatrixRef actual_jacobian_matrix(actual_jacobian,
                                          expected_num_rows,
                                          expected_num_cols);
    EXPECT_TRUE((actual_jacobian_matrix.array() ==
                 expected_jacobian_matrix.array()).all())
        << "Actual:\n" << actual_jacobian_matrix
        << "\nExpected:\n" << expected_jacobian_matrix;
  }
}


struct EvaluatorTest
    : public ::testing::TestWithParam<pair<LinearSolverType, int> > {
  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();

    VLOG(1) << "Creating evaluator with type: " << GetParam().first
            << " and num_eliminate_blocks: " << GetParam().second;
    Evaluator::Options options;
    options.linear_solver_type = GetParam().first;
    options.num_eliminate_blocks = GetParam().second;
    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 occured 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(make_pair(DENSE_QR, 0),
                      make_pair(DENSE_SCHUR, 0),
                      make_pair(DENSE_SCHUR, 1),
                      make_pair(DENSE_SCHUR, 2),
                      make_pair(DENSE_SCHUR, 3),
                      make_pair(DENSE_SCHUR, 4),
                      make_pair(SPARSE_SCHUR, 0),
                      make_pair(SPARSE_SCHUR, 1),
                      make_pair(SPARSE_SCHUR, 2),
                      make_pair(SPARSE_SCHUR, 3),
                      make_pair(SPARSE_SCHUR, 4),
                      make_pair(ITERATIVE_SCHUR, 0),
                      make_pair(ITERATIVE_SCHUR, 1),
                      make_pair(ITERATIVE_SCHUR, 2),
                      make_pair(ITERATIVE_SCHUR, 3),
                      make_pair(ITERATIVE_SCHUR, 4),
                      make_pair(SPARSE_NORMAL_CHOLESKY, 0)));

// 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;
  }
}

// Simple cost function used for testing Evaluator::Evaluate.
//
// r_i = i - (j + 1) * x_ij^2
template <int kNumResiduals, int kNumParameterBlocks >
class QuadraticCostFunction : public CostFunction {
 public:
  QuadraticCostFunction() {
    CHECK_GT(kNumResiduals, 0);
    CHECK_GT(kNumParameterBlocks, 0);
    set_num_residuals(kNumResiduals);
    for (int i = 0; i < kNumParameterBlocks; ++i) {
      mutable_parameter_block_sizes()->push_back(kNumResiduals);
    }
  }

  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    for (int i = 0; i < kNumResiduals; ++i) {
      residuals[i] = i;
      for (int j = 0; j < kNumParameterBlocks; ++j) {
        residuals[i] -= (j + 1.0) * parameters[j][i] * parameters[j][i];
      }
    }

    if (jacobians == NULL) {
      return true;
    }

    for (int j = 0; j < kNumParameterBlocks; ++j) {
      if (jacobians[j] != NULL) {
        MatrixRef(jacobians[j], kNumResiduals, kNumResiduals) =
            (-2.0 * (j + 1.0) *
             ConstVectorRef(parameters[j], kNumResiduals)).asDiagonal();
      }
    }

    return true;
  }
};

// Convert a CRSMatrix to a dense Eigen matrix.
void CRSToDenseMatrix(const CRSMatrix& input, Matrix* output) {
  Matrix& m = *CHECK_NOTNULL(output);
  m.resize(input.num_rows, input.num_cols);
  m.setZero();
  for (int row = 0; row < input.num_rows; ++row) {
    for (int j = input.rows[row]; j < input.rows[row + 1]; ++j) {
      const int col = input.cols[j];
      m(row, col) = input.values[j];
    }
  }
}


class StaticEvaluateTest : public ::testing::Test {
 protected:
  void SetUp() {
    for (int i = 0; i < 6; ++i) {
      parameters_[i] = static_cast<double>(i + 1);
    }

    CostFunction* cost_function = new QuadraticCostFunction<2, 2>;

    // f(x, y)
    problem_.AddResidualBlock(cost_function,
                              NULL,
                              parameters_,
                              parameters_ + 2);
    // g(y, z)
    problem_.AddResidualBlock(cost_function,
                              NULL, parameters_ + 2,
                              parameters_ + 4);
    // h(z, x)
    problem_.AddResidualBlock(cost_function,
                              NULL,
                              parameters_ + 4,
                              parameters_);
  }



  void EvaluateAndCompare(const int expected_num_rows,
                          const int expected_num_cols,
                          const double expected_cost,
                          const double* expected_residuals,
                          const double* expected_gradient,
                          const double* expected_jacobian) {
    double cost;
    vector<double> residuals;
    vector<double> gradient;
    CRSMatrix jacobian;

    EXPECT_TRUE(Evaluator::Evaluate(
                    problem_.mutable_program(),
                    1,
                    &cost,
                    expected_residuals != NULL ? &residuals : NULL,
                    expected_gradient != NULL ? &gradient : NULL,
                    expected_jacobian != NULL ? &jacobian : NULL));

    if (expected_residuals != NULL) {
      EXPECT_EQ(residuals.size(), expected_num_rows);
    }

    if (expected_gradient != NULL) {
      EXPECT_EQ(gradient.size(), expected_num_cols);
    }

    if (expected_jacobian != NULL) {
      EXPECT_EQ(jacobian.num_rows, expected_num_rows);
      EXPECT_EQ(jacobian.num_cols, expected_num_cols);
    }

    Matrix dense_jacobian;
    if (expected_jacobian != NULL) {
      CRSToDenseMatrix(jacobian, &dense_jacobian);
    }

    CompareEvaluations(expected_num_rows,
                       expected_num_cols,
                       expected_cost,
                       expected_residuals,
                       expected_gradient,
                       expected_jacobian,
                       cost,
                       residuals.size() > 0 ? &residuals[0] : NULL,
                       gradient.size() > 0 ? &gradient[0] : NULL,
                       dense_jacobian.data());
  }

  void CheckAllEvaluationCombinations(const ExpectedEvaluation& expected ) {
    for (int i = 0; i < 8; ++i) {
      EvaluateAndCompare(expected.num_rows,
                         expected.num_cols,
                         expected.cost,
                         (i & 1) ? expected.residuals : NULL,
                         (i & 2) ? expected.gradient  : NULL,
                         (i & 4) ? expected.jacobian  : NULL);
    }


    double new_parameters[6];
    for (int i = 0; i < 6; ++i) {
      new_parameters[i] = 0.0;
    }

    problem_.mutable_program()->StateVectorToParameterBlocks(new_parameters);

    for (int i = 0; i < 8; ++i) {
      EvaluateAndCompare(expected.num_rows,
                         expected.num_cols,
                         expected.cost,
                         (i & 1) ? expected.residuals : NULL,
                         (i & 2) ? expected.gradient  : NULL,
                         (i & 4) ? expected.jacobian  : NULL);
    }
  }

  ProblemImpl problem_;
  double parameters_[6];
};


TEST_F(StaticEvaluateTest, MultipleParameterAndResidualBlocks) {
  ExpectedEvaluation expected = {
    // Rows/columns
    6, 6,
    // Cost
    7607.0,
    // Residuals
    { -19.0, -35.0,  // f
      -59.0, -87.0,  // g
      -27.0, -43.0   // h
    },
    // Gradient
    {  146.0,  484.0,   // x
       582.0, 1256.0,   // y
      1450.0, 2604.0,   // z
    },
    // Jacobian
    //                       x             y             z
    { /* f(x, y) */ -2.0,  0.0, -12.0,   0.0,   0.0,   0.0,
                     0.0, -4.0,   0.0, -16.0,   0.0,   0.0,
      /* g(y, z) */  0.0,  0.0,  -6.0,   0.0, -20.0,   0.0,
                     0.0,  0.0,   0.0,  -8.0,   0.0, -24.0,
      /* h(z, x) */ -4.0,  0.0,   0.0,   0.0, -10.0,   0.0,
                     0.0, -8.0,   0.0,   0.0,   0.0, -12.0
    }
  };

  CheckAllEvaluationCombinations(expected);
}

TEST_F(StaticEvaluateTest, ConstantParameterBlock) {
  ExpectedEvaluation expected = {
    // Rows/columns
    6, 6,
    // Cost
    7607.0,
    // Residuals
    { -19.0, -35.0,  // f
      -59.0, -87.0,  // g
      -27.0, -43.0   // h
    },

    // Gradient
    {  146.0,  484.0,  // x
         0.0,    0.0,  // y
      1450.0, 2604.0,  // z
    },

    // Jacobian
    //                       x             y             z
    { /* f(x, y) */ -2.0,  0.0,   0.0,   0.0,   0.0,   0.0,
                     0.0, -4.0,   0.0,   0.0,   0.0,   0.0,
      /* g(y, z) */  0.0,  0.0,   0.0,   0.0, -20.0,   0.0,
                     0.0,  0.0,   0.0,   0.0,   0.0, -24.0,
      /* h(z, x) */ -4.0,  0.0,   0.0,   0.0, -10.0,   0.0,
                     0.0, -8.0,   0.0,   0.0,   0.0, -12.0
    }
  };

  problem_.SetParameterBlockConstant(parameters_ + 2);
  CheckAllEvaluationCombinations(expected);
}

TEST_F(StaticEvaluateTest, LocalParameterization) {
  ExpectedEvaluation expected = {
    // Rows/columns
    6, 5,
    // Cost
    7607.0,
    // Residuals
    { -19.0, -35.0,  // f
      -59.0, -87.0,  // g
      -27.0, -43.0   // h
    },
    // Gradient
    {  146.0,  484.0,  // x
      1256.0,          // y with SubsetParameterization
      1450.0, 2604.0,  // z
    },
    // Jacobian
    //                       x      y             z
    { /* f(x, y) */ -2.0,  0.0,   0.0,   0.0,   0.0,
                     0.0, -4.0, -16.0,   0.0,   0.0,
      /* g(y, z) */  0.0,  0.0,   0.0, -20.0,   0.0,
                     0.0,  0.0,  -8.0,   0.0, -24.0,
      /* h(z, x) */ -4.0,  0.0,   0.0, -10.0,   0.0,
                     0.0, -8.0,   0.0,   0.0, -12.0
    }
  };

  vector<int> constant_parameters;
  constant_parameters.push_back(0);
  problem_.SetParameterization(parameters_ + 2,
                               new SubsetParameterization(2,
                                                          constant_parameters));

  CheckAllEvaluationCombinations(expected);
}

}  // namespace internal
}  // namespace ceres