Problem::Evaluate implementation.
1. Add Problem::Evaluate and tests.
2. Remove Solver::Summary::initial/final_*
3. Remove Solver::Options::return_* members.
4. Various cpplint cleanups.
Change-Id: I4266de53489896f72d9c6798c5efde6748d68a47
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index 55f355b..130148d 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -32,14 +32,20 @@
#include "ceres/problem.h"
#include "ceres/problem_impl.h"
-#include "gtest/gtest.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/map_util.h"
#include "ceres/parameter_block.h"
#include "ceres/program.h"
#include "ceres/sized_cost_function.h"
-#include "ceres/internal/scoped_ptr.h"
+#include "ceres/sparse_matrix.h"
+#include "ceres/types.h"
+#include "gtest/gtest.h"
+
namespace ceres {
namespace internal {
@@ -695,5 +701,541 @@
DynamicProblem,
::testing::Values(true, false));
+// Test for Problem::Evaluate
+
+// TODO(sameeragarwal): The following struct and function are shared
+// with evaluator_test.cc. Once things settle down, do an
+// evaluate_utils.h or some such thing to reduce code duplication. The
+// best time is perhaps when we remove the support for
+// Solver::Summary::initial_*
+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;
+ }
+}
+
+// Simple cost function used for testing Problem::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 ProblemEvaluateTest : public ::testing::Test {
+ protected:
+ void SetUp() {
+ for (int i = 0; i < 6; ++i) {
+ parameters_[i] = static_cast<double>(i + 1);
+ }
+
+ parameter_blocks_.push_back(parameters_);
+ parameter_blocks_.push_back(parameters_ + 2);
+ parameter_blocks_.push_back(parameters_ + 4);
+
+
+ CostFunction* cost_function = new QuadraticCostFunction<2, 2>;
+
+ // f(x, y)
+ residual_blocks_.push_back(
+ problem_.AddResidualBlock(cost_function,
+ NULL,
+ parameters_,
+ parameters_ + 2));
+ // g(y, z)
+ residual_blocks_.push_back(
+ problem_.AddResidualBlock(cost_function,
+ NULL, parameters_ + 2,
+ parameters_ + 4));
+ // h(z, x)
+ residual_blocks_.push_back(
+ problem_.AddResidualBlock(cost_function,
+ NULL,
+ parameters_ + 4,
+ parameters_));
+ }
+
+
+
+ void EvaluateAndCompare(const Problem::EvaluateOptions& options,
+ 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(
+ problem_.Evaluate(options,
+ &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 Problem::EvaluateOptions& options,
+ const ExpectedEvaluation& expected) {
+ for (int i = 0; i < 8; ++i) {
+ EvaluateAndCompare(options,
+ 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];
+ vector<double*> parameter_blocks_;
+ vector<ResidualBlockId> residual_blocks_;
+};
+
+
+TEST_F(ProblemEvaluateTest, 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(Problem::EvaluateOptions(), expected);
+}
+
+TEST_F(ProblemEvaluateTest, ParameterAndResidualBlocksPassedInOptions) {
+ 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
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ evaluate_options.parameter_blocks = parameter_blocks_;
+ evaluate_options.residual_blocks = residual_blocks_;
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ReorderedResidualBlocks) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 6,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -27.0, -43.0, // h
+ -59.0, -87.0 // g
+ },
+ // 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,
+ /* 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,
+ /* 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
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ evaluate_options.parameter_blocks = parameter_blocks_;
+
+ // f, h, g
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ReorderedResidualBlocksAndReorderedParameterBlocks) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 6,
+ // Cost
+ 7607.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -27.0, -43.0, // h
+ -59.0, -87.0 // g
+ },
+ // Gradient
+ { 1450.0, 2604.0, // z
+ 582.0, 1256.0, // y
+ 146.0, 484.0, // x
+ },
+ // Jacobian
+ // z y x
+ { /* f(x, y) */ 0.0, 0.0, -12.0, 0.0, -2.0, 0.0,
+ 0.0, 0.0, 0.0, -16.0, 0.0, -4.0,
+ /* h(z, x) */ -10.0, 0.0, 0.0, 0.0, -4.0, 0.0,
+ 0.0, -12.0, 0.0, 0.0, 0.0, -8.0,
+ /* g(y, z) */ -20.0, 0.0, -6.0, 0.0, 0.0, 0.0,
+ 0.0, -24.0, 0.0, -8.0, 0.0, 0.0
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ // z, y, x
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[1]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
+
+ // f, h, g
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, 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(Problem::EvaluateOptions(), expected);
+}
+
+TEST_F(ProblemEvaluateTest, ExcludedAResidualBlock) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 4, 6,
+ // Cost
+ 2082.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -27.0, -43.0 // h
+ },
+ // Gradient
+ { 146.0, 484.0, // x
+ 228.0, 560.0, // y
+ 270.0, 516.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,
+ /* 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::EvaluateOptions evaluate_options;
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ExcludedParameterBlock) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 6, 4,
+ // 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
+ 1450.0, 2604.0, // z
+ },
+
+ // Jacobian
+ // x z
+ { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, -24.0,
+ /* h(z, x) */ -4.0, 0.0, -10.0, 0.0,
+ 0.0, -8.0, 0.0, -12.0
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ // x, z
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
+ evaluate_options.residual_blocks = residual_blocks_;
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, ExcludedParameterBlockAndExcludedResidualBlock) {
+ ExpectedEvaluation expected = {
+ // Rows/columns
+ 4, 4,
+ // Cost
+ 6318.0,
+ // Residuals
+ { -19.0, -35.0, // f
+ -59.0, -87.0, // g
+ },
+
+ // Gradient
+ { 38.0, 140.0, // x
+ 1180.0, 2088.0, // z
+ },
+
+ // Jacobian
+ // x z
+ { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0,
+ 0.0, -4.0, 0.0, 0.0,
+ /* g(y, z) */ 0.0, 0.0, -20.0, 0.0,
+ 0.0, 0.0, 0.0, -24.0,
+ }
+ };
+
+ Problem::EvaluateOptions evaluate_options;
+ // x, z
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
+ evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
+ evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
+
+ CheckAllEvaluationCombinations(evaluate_options, expected);
+}
+
+TEST_F(ProblemEvaluateTest, 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(Problem::EvaluateOptions(), expected);
+}
+
} // namespace internal
} // namespace ceres
