Return jacobians and gradients to the user.
1. Added CRSMatrix object which will store the initial
and final jacobians if requested by the user.
2. Conversion routine and test for converting a
CompressedRowSparseMatrix to CRSMatrix.
3. New Evaluator::Evaluate function to do the actual evaluation.
4. Changes to Program::StateVectorToParmeterBlocks and
Program::SetParameterBlockStatePtrstoUserStatePtrs so that
they do not try to set the state of constant parameter blocks.
5. Tests for Evaluator::Evaluate.
6. Minor cleanups in SolverImpl.
7. Minor cpplint cleanups triggered by this CL.
Change-Id: I3ac446484692f943c28f2723b719676f8c83ca3d
diff --git a/internal/ceres/compressed_row_sparse_matrix.cc b/internal/ceres/compressed_row_sparse_matrix.cc
index e006e10..00feda8 100644
--- a/internal/ceres/compressed_row_sparse_matrix.cc
+++ b/internal/ceres/compressed_row_sparse_matrix.cc
@@ -32,8 +32,9 @@
#include <algorithm>
#include <vector>
-#include "ceres/matrix_proto.h"
+#include "ceres/crs_matrix.h"
#include "ceres/internal/port.h"
+#include "ceres/matrix_proto.h"
namespace ceres {
namespace internal {
@@ -336,5 +337,18 @@
}
}
+void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const {
+ matrix->num_rows = num_rows();
+ matrix->num_cols = num_cols();
+
+ matrix->rows.resize(matrix->num_rows + 1);
+ matrix->cols.resize(num_nonzeros());
+ matrix->values.resize(num_nonzeros());
+
+ copy(rows_.get(), rows_.get() + matrix->num_rows + 1, matrix->rows.begin());
+ copy(cols_.get(), cols_.get() + num_nonzeros(), matrix->cols.begin());
+ copy(values_.get(), values_.get() + num_nonzeros(), matrix->values.begin());
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/compressed_row_sparse_matrix.h b/internal/ceres/compressed_row_sparse_matrix.h
index 7fb460a..04b5542 100644
--- a/internal/ceres/compressed_row_sparse_matrix.h
+++ b/internal/ceres/compressed_row_sparse_matrix.h
@@ -41,6 +41,9 @@
#include "ceres/types.h"
namespace ceres {
+
+class CRSMatrix;
+
namespace internal {
class SparseMatrixProto;
@@ -105,6 +108,8 @@
// have the same number of columns as this matrix.
void AppendRows(const CompressedRowSparseMatrix& m);
+ void ToCRSMatrix(CRSMatrix* matrix) const;
+
// Low level access methods that expose the structure of the matrix.
const int* cols() const { return cols_.get(); }
int* mutable_cols() { return cols_.get(); }
@@ -112,11 +117,11 @@
const int* rows() const { return rows_.get(); }
int* mutable_rows() { return rows_.get(); }
- const vector<int>& row_blocks() const { return row_blocks_; };
- vector<int>* mutable_row_blocks() { return &row_blocks_; };
+ const vector<int>& row_blocks() const { return row_blocks_; }
+ vector<int>* mutable_row_blocks() { return &row_blocks_; }
- const vector<int>& col_blocks() const { return col_blocks_; };
- vector<int>* mutable_col_blocks() { return &col_blocks_; };
+ const vector<int>& col_blocks() const { return col_blocks_; }
+ vector<int>* mutable_col_blocks() { return &col_blocks_; }
private:
scoped_array<int> cols_;
diff --git a/internal/ceres/compressed_row_sparse_matrix_test.cc b/internal/ceres/compressed_row_sparse_matrix_test.cc
index bfb4457..8a01ea8 100644
--- a/internal/ceres/compressed_row_sparse_matrix_test.cc
+++ b/internal/ceres/compressed_row_sparse_matrix_test.cc
@@ -30,13 +30,14 @@
#include "ceres/compressed_row_sparse_matrix.h"
-#include "gtest/gtest.h"
#include "ceres/casts.h"
+#include "ceres/crs_matrix.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/scoped_ptr.h"
#include "ceres/linear_least_squares_problems.h"
#include "ceres/matrix_proto.h"
#include "ceres/triplet_sparse_matrix.h"
-#include "ceres/internal/eigen.h"
-#include "ceres/internal/scoped_ptr.h"
+#include "gtest/gtest.h"
namespace ceres {
namespace internal {
@@ -179,5 +180,24 @@
EXPECT_EQ((tsm_dense - crsm_dense).norm(), 0.0);
}
+TEST_F(CompressedRowSparseMatrixTest, ToCRSMatrix) {
+ CRSMatrix crs_matrix;
+ crsm->ToCRSMatrix(&crs_matrix);
+ EXPECT_EQ(crsm->num_rows(), crs_matrix.num_rows);
+ EXPECT_EQ(crsm->num_cols(), crs_matrix.num_cols);
+ EXPECT_EQ(crsm->num_rows() + 1, crs_matrix.rows.size());
+ EXPECT_EQ(crsm->num_nonzeros(), crs_matrix.cols.size());
+ EXPECT_EQ(crsm->num_nonzeros(), crs_matrix.values.size());
+
+ for (int i = 0; i < crsm->num_rows() + 1; ++i) {
+ EXPECT_EQ(crsm->rows()[i], crs_matrix.rows[i]);
+ }
+
+ for (int i = 0; i < crsm->num_nonzeros(); ++i) {
+ EXPECT_EQ(crsm->cols()[i], crs_matrix.cols[i]);
+ EXPECT_EQ(crsm->values()[i], crs_matrix.values[i]);
+ }
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/evaluator.cc b/internal/ceres/evaluator.cc
index ea05aef..5e64a92 100644
--- a/internal/ceres/evaluator.cc
+++ b/internal/ceres/evaluator.cc
@@ -28,14 +28,18 @@
//
// Author: keir@google.com (Keir Mierle)
+#include <vector>
#include <glog/logging.h>
-#include "ceres/evaluator.h"
#include "ceres/block_evaluate_preparer.h"
#include "ceres/block_jacobian_writer.h"
#include "ceres/compressed_row_jacobian_writer.h"
-#include "ceres/scratch_evaluate_preparer.h"
+#include "ceres/compressed_row_sparse_matrix.h"
+#include "ceres/crs_matrix.h"
#include "ceres/dense_jacobian_writer.h"
+#include "ceres/evaluator.h"
+#include "ceres/internal/port.h"
#include "ceres/program_evaluator.h"
+#include "ceres/scratch_evaluate_preparer.h"
namespace ceres {
namespace internal {
@@ -67,5 +71,76 @@
}
}
+bool Evaluator::Evaluate(Program* program,
+ int num_threads,
+ double* cost,
+ vector<double>* residuals,
+ vector<double>* gradient,
+ CRSMatrix* output_jacobian) {
+ CHECK_GE(num_threads, 1)
+ << "This is a Ceres bug; please contact the developers!";
+ CHECK_NOTNULL(cost);
+
+ // Setup the Parameter indices and offsets before an evaluator can
+ // be constructed and used.
+ program->SetParameterOffsetsAndIndex();
+
+ Evaluator::Options evaluator_options;
+ evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
+ evaluator_options.num_threads = num_threads;
+
+ string error;
+ scoped_ptr<Evaluator> evaluator(
+ Evaluator::Create(evaluator_options, program, &error));
+ if (evaluator.get() == NULL) {
+ LOG(ERROR) << "Unable to create an Evaluator object. "
+ << "Error: " << error
+ << "This is a Ceres bug; please contact the developers!";
+ return false;
+ }
+
+ if (residuals !=NULL) {
+ residuals->resize(evaluator->NumResiduals());
+ }
+
+ if (gradient != NULL) {
+ gradient->resize(evaluator->NumEffectiveParameters());
+ }
+
+ scoped_ptr<CompressedRowSparseMatrix> jacobian;
+ if (output_jacobian != NULL) {
+ jacobian.reset(
+ down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian()));
+ }
+
+ // Point the state pointers to the user state pointers. This is
+ // needed so that we can extract a parameter vector which is then
+ // passed to Evaluator::Evaluate.
+ program->SetParameterBlockStatePtrsToUserStatePtrs();
+
+ // Copy the value of the parameter blocks into a vector, since the
+ // Evaluate::Evaluate method needs its input as such. The previous
+ // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that
+ // these values are the ones corresponding to the actual state of
+ // the parameter blocks, rather than the temporary state pointer
+ // used for evaluation.
+ Vector parameters(program->NumParameters());
+ program->ParameterBlocksToStateVector(parameters.data());
+
+ if (!evaluator->Evaluate(parameters.data(),
+ cost,
+ residuals != NULL ? &(*residuals)[0] : NULL,
+ gradient != NULL ? &(*gradient)[0] : NULL,
+ jacobian.get())) {
+ return false;
+ }
+
+ if (output_jacobian != NULL) {
+ jacobian->ToCRSMatrix(output_jacobian);
+ }
+
+ return true;
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/evaluator.h b/internal/ceres/evaluator.h
index e80efa1..6aa30d7 100644
--- a/internal/ceres/evaluator.h
+++ b/internal/ceres/evaluator.h
@@ -33,10 +33,14 @@
#define CERES_INTERNAL_EVALUATOR_H_
#include <string>
+#include <vector>
#include "ceres/internal/port.h"
#include "ceres/types.h"
namespace ceres {
+
+class CRSMatrix;
+
namespace internal {
class Program;
@@ -65,6 +69,32 @@
Program* program,
string* error);
+
+ // This is used for computing the cost, residual and Jacobian for
+ // returning to the user. For actually solving the optimization
+ // problem, the optimization algorithm uses the ProgramEvaluator
+ // objects directly.
+ //
+ // The residual, gradients and jacobian pointers can be NULL, in
+ // which case they will not be evaluated. cost cannot be NULL.
+ //
+ // The parallelism of the evaluator is controlled by num_threads; it
+ // should be at least 1.
+ //
+ // Note: That this function does not take a parameter vector as
+ // input. The parameter blocks are evaluated on the values contained
+ // in the arrays pointed to by their user_state pointers.
+ //
+ // Also worth noting is that this function mutates program by
+ // calling Program::SetParameterOffsetsAndIndex() on it so that an
+ // evaluator object can be constructed.
+ static bool Evaluate(Program* program,
+ int num_threads,
+ double* cost,
+ vector<double>* residuals,
+ vector<double>* gradient,
+ CRSMatrix* jacobian);
+
// Build and return a sparse matrix for storing and working with the Jacobian
// of the objective function. The jacobian has dimensions
// NumEffectiveParameters() by NumParameters(), and is typically extremely
diff --git a/internal/ceres/evaluator_test.cc b/internal/ceres/evaluator_test.cc
index 6fbb759..c3b3fa4 100644
--- a/internal/ceres/evaluator_test.cc
+++ b/internal/ceres/evaluator_test.cc
@@ -33,16 +33,18 @@
#include "ceres/evaluator.h"
-#include "gtest/gtest.h"
#include "ceres/casts.h"
-#include "ceres/problem_impl.h"
-#include "ceres/program.h"
-#include "ceres/sparse_matrix.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/types.h"
+#include "ceres/problem_impl.h"
+#include "ceres/program.h"
#include "ceres/sized_cost_function.h"
-#include "ceres/internal/eigen.h"
+#include "ceres/sparse_matrix.h"
+#include "ceres/types.h"
+#include "gtest/gtest.h"
namespace ceres {
namespace internal {
@@ -97,6 +99,56 @@
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) {
@@ -113,14 +165,15 @@
return Evaluator::Create(options, program, &error);
}
- void CheckEvaluation(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()));
+ 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;
@@ -148,48 +201,33 @@
expected_gradient != NULL ? &gradient[0] : NULL,
expected_jacobian != NULL ? jacobian.get() : NULL));
- EXPECT_EQ(expected_cost, cost);
-
- if (expected_residuals != NULL) {
- for (int i = 0; i < num_residuals; ++i) {
- EXPECT_EQ(expected_residuals[i], residuals[i]) << i;
- }
- }
-
- if (expected_gradient != NULL) {
- ConstVectorRef expected_gradient_vector(expected_gradient,
- expected_num_cols);
-
- EXPECT_TRUE((gradient.array() ==
- expected_gradient_vector.array()).all())
- << "Actual:\n" << gradient.transpose()
- << "\nExpected:\n" << expected_gradient_vector.transpose();
- }
-
+ Matrix actual_jacobian;
if (expected_jacobian != NULL) {
- ConstMatrixRef expected_jacobian_matrix(expected_jacobian,
- expected_num_rows,
- expected_num_cols);
- Matrix actual_jacobian;
jacobian->ToDenseMatrix(&actual_jacobian);
-
- EXPECT_TRUE((actual_jacobian.array() ==
- expected_jacobian_matrix.array()).all())
- << "Actual:\n" << actual_jacobian
- << "\nExpected:\n" << expected_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) {
- CheckEvaluation(&problem,
- expected.num_rows,
- expected.num_cols,
- expected.cost,
- (i & 1) ? expected.residuals : NULL,
- (i & 2) ? expected.gradient : NULL,
- (i & 4) ? expected.jacobian : NULL);
+ 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);
}
}
@@ -232,6 +270,7 @@
};
CheckAllEvaluationCombinations(expected);
}
+
TEST_P(EvaluatorTest, SingleResidualProblemWithPermutedParameters) {
// Add the parameters in explicit order to force the ordering in the program.
problem.AddParameterBlock(x, 2);
@@ -345,7 +384,10 @@
// f g h
( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
// Residuals
- { 1.0, 2.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0 },
+ { 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
@@ -363,7 +405,7 @@
/* 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
+ 0, 0, 3, 6, 9, 3, 6, 9, 12
}
};
CheckAllEvaluationCombinations(expected);
@@ -405,7 +447,10 @@
// f g h
( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
// Residuals
- { 1.0, 2.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0 },
+ { 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
@@ -423,7 +468,7 @@
/* 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
+ 0, 0, 6, 9, 3, 9, 12
}
};
CheckAllEvaluationCombinations(expected);
@@ -479,13 +524,16 @@
// f g h
( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
// Residuals
- { 1.0, 2.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0 },
+ { 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
+ // x y
{ /* f(x, y) */ 1, 2, 1, 2, 3,
1, 2, 1, 2, 3,
@@ -623,7 +671,11 @@
double cost = -1;
double residuals[2] = { -2, -2};
SetSparseMatrixConstant(jacobian.get(), -1);
- ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL, jacobian.get()));
+ 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]);
@@ -641,5 +693,268 @@
}
}
+// 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[0],
+ &gradient[0],
+ 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
diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc
index f6704e4..4dfb3a1 100644
--- a/internal/ceres/program.cc
+++ b/internal/ceres/program.cc
@@ -32,14 +32,18 @@
#include <map>
#include <vector>
+#include "ceres/casts.h"
+#include "ceres/compressed_row_sparse_matrix.h"
+#include "ceres/cost_function.h"
+#include "ceres/evaluator.h"
+#include "ceres/internal/port.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/loss_function.h"
+#include "ceres/map_util.h"
#include "ceres/parameter_block.h"
+#include "ceres/problem.h"
#include "ceres/residual_block.h"
#include "ceres/stl_util.h"
-#include "ceres/map_util.h"
-#include "ceres/problem.h"
-#include "ceres/cost_function.h"
-#include "ceres/loss_function.h"
-#include "ceres/local_parameterization.h"
namespace ceres {
namespace internal {
@@ -69,7 +73,8 @@
bool Program::StateVectorToParameterBlocks(const double *state) {
for (int i = 0; i < parameter_blocks_.size(); ++i) {
- if (!parameter_blocks_[i]->SetState(state)) {
+ if (!parameter_blocks_[i]->IsConstant() &&
+ !parameter_blocks_[i]->SetState(state)) {
return false;
}
state += parameter_blocks_[i]->Size();
@@ -92,7 +97,8 @@
bool Program::SetParameterBlockStatePtrsToUserStatePtrs() {
for (int i = 0; i < parameter_blocks_.size(); ++i) {
- if (!parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
+ if (!parameter_blocks_[i]->IsConstant() &&
+ !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
return false;
}
}
@@ -210,41 +216,5 @@
return max_residuals;
}
-bool Program::Evaluate(double* cost, double* residuals) {
- *cost = 0.0;
-
- // Scratch space is only needed if residuals is NULL.
- scoped_array<double> scratch;
- if (residuals == NULL) {
- scratch.reset(new double[MaxScratchDoublesNeededForEvaluate()]);
- } else {
- // TODO(keir): Is this needed? Check by removing the equivalent statement in
- // dense_evaluator.cc and running the tests.
- VectorRef(residuals, NumResiduals()).setZero();
- }
-
- for (int i = 0; i < residual_blocks_.size(); ++i) {
- ResidualBlock* residual_block = residual_blocks_[i];
-
- // Evaluate the cost function for this residual.
- double residual_cost;
- if (!residual_block->Evaluate(&residual_cost,
- residuals,
- NULL, // No jacobian.
- scratch.get())) {
- return false;
- }
-
- // Accumulate residual cost into the total cost.
- *cost += residual_cost;
-
- // Update the residuals cursor.
- if (residuals != NULL) {
- residuals += residual_block->NumResiduals();
- }
- }
- return true;
-}
-
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/program.h b/internal/ceres/program.h
index 61fa06b..7ae7db9 100644
--- a/internal/ceres/program.h
+++ b/internal/ceres/program.h
@@ -110,16 +110,6 @@
int MaxParametersPerResidualBlock() const;
int MaxResidualsPerResidualBlock() const;
- // Evaluate the cost and maybe the residuals for the program. If residuals is
- // NULL, then residuals are not calculated. If the jacobian is needed, instead
- // use the various evaluators (e.g. dense_evaluator.h).
- //
- // This is a trivial implementation of evaluate not intended for use in the
- // core solving loop. The other evaluators, which support constructing the
- // jacobian in addition to the cost and residuals, are considerably
- // complicated by the need to construct the jacobian.
- bool Evaluate(double* cost, double* residuals);
-
private:
// The Program does not own the ParameterBlock or ResidualBlock objects.
vector<ParameterBlock*> parameter_blocks_;
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 46c8b1a..28627eb 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -52,19 +52,6 @@
namespace internal {
namespace {
-void EvaluateCostAndResiduals(ProblemImpl* problem_impl,
- double* cost,
- vector<double>* residuals) {
- CHECK_NOTNULL(cost);
- Program* program = CHECK_NOTNULL(problem_impl)->mutable_program();
- if (residuals != NULL) {
- residuals->resize(program->NumResiduals());
- program->Evaluate(cost, &(*residuals)[0]);
- } else {
- program->Evaluate(cost, NULL);
- }
-}
-
// Callback for updating the user's parameter blocks. Updates are only
// done if the step is successful.
class StateUpdatingCallback : public IterationCallback {
@@ -168,10 +155,15 @@
}
void SolverImpl::Solve(const Solver::Options& original_options,
- ProblemImpl* problem_impl,
+ ProblemImpl* original_problem_impl,
Solver::Summary* summary) {
time_t solver_start_time = time(NULL);
Solver::Options options(original_options);
+ Program* original_program = original_problem_impl->mutable_program();
+ ProblemImpl* problem_impl = original_problem_impl;
+ // Reset the summary object to its default values.
+ *CHECK_NOTNULL(summary) = Solver::Summary();
+
#ifndef CERES_USE_OPENMP
if (options.num_threads > 1) {
@@ -190,8 +182,6 @@
}
#endif
- // Reset the summary object to its default values;
- *CHECK_NOTNULL(summary) = Solver::Summary();
summary->linear_solver_type_given = options.linear_solver_type;
summary->num_eliminate_blocks_given = original_options.num_eliminate_blocks;
summary->num_threads_given = original_options.num_threads;
@@ -209,23 +199,19 @@
options.sparse_linear_algebra_library;
summary->trust_region_strategy_type = options.trust_region_strategy_type;
- // Evaluate the initial cost and residual vector (if needed). The
- // initial cost needs to be computed on the original unpreprocessed
- // problem, as it is used to determine the value of the "fixed" part
- // of the objective function after the problem has undergone
- // reduction. Also the initial residuals are in the order in which
- // the user added the ResidualBlocks to the optimization problem.
- //
- // Note: This assumes the parameter block states are pointing to the
- // user state at start of Solve(), instead of some other pointer.
- // The invariant is ensured by the ParameterBlock constructor and by
- // the call to SetParameterBlockStatePtrsToUserStatePtrs() at the
- // bottom of this function.
- EvaluateCostAndResiduals(problem_impl,
- &summary->initial_cost,
- options.return_initial_residuals
- ? &summary->initial_residuals
- : NULL);
+ // Evaluate the initial cost, residual vector and the jacobian
+ // matrix if requested by the user. The initial cost needs to be
+ // computed on the original unpreprocessed problem, as it is used to
+ // determine the value of the "fixed" part of the objective function
+ // after the problem has undergone reduction.
+ Evaluator::Evaluate(
+ original_program,
+ options.num_threads,
+ &(summary->initial_cost),
+ options.return_initial_residuals ? &summary->initial_residuals : NULL,
+ options.return_initial_gradient ? &summary->initial_gradient : NULL,
+ options.return_initial_jacobian ? &summary->initial_jacobian : NULL);
+ original_program->SetParameterBlockStatePtrsToUserStatePtrs();
// If the user requests gradient checking, construct a new
// ProblemImpl by wrapping the CostFunctions of problem_impl inside
@@ -234,7 +220,6 @@
scoped_ptr<ProblemImpl> gradient_checking_problem_impl;
// Save the original problem impl so we don't use the gradient
// checking one when computing the residuals.
- ProblemImpl* original_problem_impl = problem_impl;
if (options.check_gradients) {
VLOG(1) << "Checking Gradients";
gradient_checking_problem_impl.reset(
@@ -311,24 +296,25 @@
}
time_t post_process_start_time = time(NULL);
+
// Push the contiguous optimized parameters back to the user's parameters.
reduced_program->StateVectorToParameterBlocks(parameters.data());
reduced_program->CopyParameterBlockStateToUserState();
+ // Evaluate the final cost, residual vector and the jacobian
+ // matrix if requested by the user.
+ Evaluator::Evaluate(
+ original_program,
+ options.num_threads,
+ &summary->final_cost,
+ options.return_final_residuals ? &summary->final_residuals : NULL,
+ options.return_final_gradient ? &summary->final_gradient : NULL,
+ options.return_final_jacobian ? &summary->final_jacobian : NULL);
+
// Ensure the program state is set to the user parameters on the way out.
- reduced_program->SetParameterBlockStatePtrsToUserStatePtrs();
-
- // Return the final cost and residuals for the original problem.
- EvaluateCostAndResiduals(original_problem_impl,
- &summary->final_cost,
- options.return_final_residuals
- ? &summary->final_residuals
- : NULL);
-
+ original_program->SetParameterBlockStatePtrsToUserStatePtrs();
// Stick a fork in it, we're done.
- time_t post_process_end_time = time(NULL);
- summary->postprocessor_time_in_seconds =
- post_process_end_time - post_process_start_time;
+ summary->postprocessor_time_in_seconds = time(NULL) - post_process_start_time;
}
// Strips varying parameters and residuals, maintaining order, and updating
diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h
index 6b0340c..3ba4d3e 100644
--- a/internal/ceres/solver_impl.h
+++ b/internal/ceres/solver_impl.h
@@ -31,6 +31,9 @@
#ifndef CERES_INTERNAL_SOLVER_IMPL_H_
#define CERES_INTERNAL_SOLVER_IMPL_H_
+#include <string>
+#include <vector>
+#include "ceres/internal/port.h"
#include "ceres/solver.h"
namespace ceres {
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc
index 4713143..36dd959 100644
--- a/internal/ceres/solver_impl_test.cc
+++ b/internal/ceres/solver_impl_test.cc
@@ -572,7 +572,7 @@
};
struct RememberingCallback : public IterationCallback {
- RememberingCallback(double *x) : calls(0), x(x) {}
+ explicit RememberingCallback(double *x) : calls(0), x(x) {}
virtual ~RememberingCallback() {}
virtual CallbackReturnType operator()(const IterationSummary& summary) {
x_values.push_back(*x);
@@ -686,5 +686,75 @@
EXPECT_EQ(&w, problem.program().parameter_blocks()[3]->state());
}
+#define CHECK_ARRAY(name, value) \
+ if (options.return_ ## name) { \
+ EXPECT_EQ(summary.name.size(), 1); \
+ EXPECT_EQ(summary.name[0], value); \
+ } else { \
+ EXPECT_EQ(summary.name.size(), 0); \
+ }
+
+#define CHECK_JACOBIAN(name) \
+ if (options.return_ ## name) { \
+ EXPECT_EQ(summary.name.num_rows, 1); \
+ EXPECT_EQ(summary.name.num_cols, 1); \
+ EXPECT_EQ(summary.name.cols.size(), 2); \
+ EXPECT_EQ(summary.name.cols[0], 0); \
+ EXPECT_EQ(summary.name.cols[1], 1); \
+ EXPECT_EQ(summary.name.rows.size(), 1); \
+ EXPECT_EQ(summary.name.rows[0], 0); \
+ EXPECT_EQ(summary.name.values.size(), 0); \
+ EXPECT_EQ(summary.name.values[0], name); \
+ } else { \
+ EXPECT_EQ(summary.name.num_rows, 0); \
+ EXPECT_EQ(summary.name.num_cols, 0); \
+ EXPECT_EQ(summary.name.cols.size(), 0); \
+ EXPECT_EQ(summary.name.rows.size(), 0); \
+ EXPECT_EQ(summary.name.values.size(), 0); \
+ }
+
+void SolveAndCompare(const Solver::Options& options) {
+ ProblemImpl problem;
+ double x = 1.0;
+
+ const double initial_residual = 5.0 - x;
+ const double initial_jacobian = -1.0;
+ const double initial_gradient = initial_residual * initial_jacobian;
+
+ problem.AddResidualBlock(
+ new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>(
+ new QuadraticCostFunction),
+ NULL,
+ &x);
+ Solver::Summary summary;
+ SolverImpl::Solve(options, &problem, &summary);
+
+ const double final_residual = 5.0 - x;
+ const double final_jacobian = -1.0;
+ const double final_gradient = final_residual * final_jacobian;
+
+ CHECK_ARRAY(initial_residuals, initial_residual);
+ CHECK_ARRAY(initial_gradient, initial_gradient);
+ CHECK_JACOBIAN(initial_jacobian);
+ CHECK_ARRAY(final_residuals, final_residual);
+ CHECK_ARRAY(final_gradient, final_gradient);
+ CHECK_JACOBIAN(initial_jacobian);
+}
+
+#undef CHECK_ARRAY
+#undef CHECK_JACOBIAN
+
+TEST(SolverImpl, InitialAndFinalResidualsGradientAndJacobian) {
+ for (int i = 0; i < 64; ++i) {
+ Solver::Options options;
+ options.return_initial_residuals = (i & 1);
+ options.return_initial_gradient = (i & 2);
+ options.return_initial_jacobian = (i & 4);
+ options.return_final_residuals = (i & 8);
+ options.return_final_gradient = (i & 16);
+ options.return_final_jacobian = (i & 64);
+ }
+}
+
} // namespace internal
} // namespace ceres
