diff options
Diffstat (limited to 'extern/libmv/third_party/ceres/internal')
73 files changed, 4342 insertions, 2974 deletions
diff --git a/extern/libmv/third_party/ceres/internal/ceres/CMakeLists.txt b/extern/libmv/third_party/ceres/internal/ceres/CMakeLists.txt deleted file mode 100644 index 1dd40900031..00000000000 --- a/extern/libmv/third_party/ceres/internal/ceres/CMakeLists.txt +++ /dev/null @@ -1,287 +0,0 @@ -# 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) - -SET(CERES_INTERNAL_SRC - array_utils.cc - blas.cc - block_evaluate_preparer.cc - block_jacobi_preconditioner.cc - block_jacobian_writer.cc - block_random_access_dense_matrix.cc - block_random_access_diagonal_matrix.cc - block_random_access_matrix.cc - block_random_access_sparse_matrix.cc - block_sparse_matrix.cc - block_structure.cc - c_api.cc - canonical_views_clustering.cc - cgnr_solver.cc - compressed_col_sparse_matrix_utils.cc - compressed_row_jacobian_writer.cc - compressed_row_sparse_matrix.cc - conditioned_cost_function.cc - conjugate_gradients_solver.cc - coordinate_descent_minimizer.cc - corrector.cc - covariance.cc - covariance_impl.cc - cxsparse.cc - dense_normal_cholesky_solver.cc - dense_qr_solver.cc - dense_sparse_matrix.cc - detect_structure.cc - dogleg_strategy.cc - dynamic_compressed_row_jacobian_writer.cc - dynamic_compressed_row_sparse_matrix.cc - evaluator.cc - file.cc - gradient_checking_cost_function.cc - implicit_schur_complement.cc - incomplete_lq_factorization.cc - iterative_schur_complement_solver.cc - levenberg_marquardt_strategy.cc - lapack.cc - line_search.cc - line_search_direction.cc - line_search_minimizer.cc - linear_least_squares_problems.cc - linear_operator.cc - linear_solver.cc - local_parameterization.cc - loss_function.cc - low_rank_inverse_hessian.cc - minimizer.cc - normal_prior.cc - parameter_block_ordering.cc - partitioned_matrix_view.cc - polynomial.cc - preconditioner.cc - problem.cc - problem_impl.cc - program.cc - residual_block.cc - residual_block_utils.cc - schur_complement_solver.cc - schur_eliminator.cc - schur_jacobi_preconditioner.cc - scratch_evaluate_preparer.cc - single_linkage_clustering.cc - solver.cc - solver_impl.cc - sparse_matrix.cc - sparse_normal_cholesky_solver.cc - split.cc - stringprintf.cc - suitesparse.cc - triplet_sparse_matrix.cc - trust_region_minimizer.cc - trust_region_strategy.cc - types.cc - visibility.cc - visibility_based_preconditioner.cc - wall_time.cc -) - -# Heuristic for determining LIB_SUFFIX. FHS recommends that 64-bit systems -# install native libraries to lib64 rather than lib. Most distros seem to -# follow this convention with a couple notable exceptions (Debian-based and -# Arch-based distros) which we try to detect here. -IF (CMAKE_SYSTEM_NAME MATCHES "Linux" AND - NOT DEFINED LIB_SUFFIX AND - NOT CMAKE_CROSSCOMPILING AND - CMAKE_SIZEOF_VOID_P EQUAL "8" AND - NOT EXISTS "/etc/debian_version" AND - NOT EXISTS "/etc/arch-release") - SET(LIB_SUFFIX "64") -ENDIF () - -# Also depend on the header files so that they appear in IDEs. -FILE(GLOB CERES_INTERNAL_HDRS *.h) - -# Include the specialized schur solvers. -IF (SCHUR_SPECIALIZATIONS) - FILE(GLOB CERES_INTERNAL_SCHUR_FILES generated/*.cc) -ELSE (SCHUR_SPECIALIZATIONS) - # Only the fully dynamic solver. The build is much faster this way. - FILE(GLOB CERES_INTERNAL_SCHUR_FILES generated/*_d_d_d.cc) -ENDIF (SCHUR_SPECIALIZATIONS) - -# Primarily for Android, but optionally for others, use the minimal internal -# Glog implementation. -IF (MINIGLOG) - ADD_LIBRARY(miniglog STATIC miniglog/glog/logging.cc) - INSTALL(TARGETS miniglog - EXPORT CeresExport - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib${LIB_SUFFIX} - ARCHIVE DESTINATION lib${LIB_SUFFIX}) -ENDIF (MINIGLOG) - -SET(CERES_LIBRARY_PUBLIC_DEPENDENCIES ${GLOG_LIBRARIES}) - -IF (SUITESPARSE AND SUITESPARSE_FOUND) - LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${SUITESPARSE_LIBRARIES}) -ENDIF (SUITESPARSE AND SUITESPARSE_FOUND) - -IF (CXSPARSE AND CXSPARSE_FOUND) - LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${CXSPARSE_LIBRARIES}) -ENDIF (CXSPARSE AND CXSPARSE_FOUND) - -IF (BLAS_FOUND AND LAPACK_FOUND) - LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${LAPACK_LIBRARIES}) - LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${BLAS_LIBRARIES}) -ENDIF (BLAS_FOUND AND LAPACK_FOUND) - -IF (OPENMP_FOUND) - IF (NOT MSVC) - LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES gomp) - LIST(APPEND CERES_LIBRARY_PRIVATE_DEPENDENCIES ${CMAKE_THREAD_LIBS_INIT}) - ENDIF (NOT MSVC) -ENDIF (OPENMP_FOUND) - -SET(CERES_LIBRARY_SOURCE - ${CERES_INTERNAL_SRC} - ${CERES_INTERNAL_HDRS} - ${CERES_INTERNAL_SCHUR_FILES}) - -ADD_LIBRARY(ceres ${CERES_LIBRARY_SOURCE}) -SET_TARGET_PROPERTIES(ceres PROPERTIES - VERSION ${CERES_VERSION} - SOVERSION ${CERES_VERSION_MAJOR} -) - -IF (BUILD_SHARED_LIBS) - # When building a shared library, mark all external libraries as - # PRIVATE so they don't show up as a dependency. - TARGET_LINK_LIBRARIES(ceres - LINK_PUBLIC ${CERES_LIBRARY_PUBLIC_DEPENDENCIES} - LINK_PRIVATE ${CERES_LIBRARY_PRIVATE_DEPENDENCIES}) -ELSE (BUILD_SHARED_LIBS) - # When building a static library, all external libraries are - # PUBLIC(default) since the user needs to link to them. - # They will be listed in CeresTargets.cmake. - SET(CERES_LIBRARY_DEPENDENCIES - ${CERES_LIBRARY_PUBLIC_DEPENDENCIES} - ${CERES_LIBRARY_PRIVATE_DEPENDENCIES}) - TARGET_LINK_LIBRARIES(ceres ${CERES_LIBRARY_DEPENDENCIES}) -ENDIF (BUILD_SHARED_LIBS) - -INSTALL(TARGETS ceres - EXPORT CeresExport - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib${LIB_SUFFIX} - ARCHIVE DESTINATION lib${LIB_SUFFIX}) - -IF (BUILD_TESTING AND GFLAGS) - ADD_LIBRARY(gtest gmock_gtest_all.cc gmock_main.cc) - ADD_LIBRARY(test_util - evaluator_test_utils.cc - numeric_diff_test_utils.cc - test_util.cc) - - TARGET_LINK_LIBRARIES(gtest ${GFLAGS_LIBRARIES} ${GLOG_LIBRARIES}) - TARGET_LINK_LIBRARIES(test_util ceres gtest ${GLOG_LIBRARIES}) - - MACRO (CERES_TEST NAME) - ADD_EXECUTABLE(${NAME}_test ${NAME}_test.cc) - TARGET_LINK_LIBRARIES(${NAME}_test test_util ceres gtest) - ADD_TEST(NAME ${NAME}_test - COMMAND ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${NAME}_test - --test_srcdir - ${CMAKE_SOURCE_DIR}/data) - ENDMACRO (CERES_TEST) - - CERES_TEST(array_utils) - CERES_TEST(autodiff) - CERES_TEST(autodiff_cost_function) - CERES_TEST(autodiff_local_parameterization) - CERES_TEST(block_random_access_dense_matrix) - CERES_TEST(block_random_access_diagonal_matrix) - CERES_TEST(block_random_access_sparse_matrix) - CERES_TEST(block_sparse_matrix) - CERES_TEST(c_api) - CERES_TEST(canonical_views_clustering) - CERES_TEST(compressed_row_sparse_matrix) - CERES_TEST(conditioned_cost_function) - CERES_TEST(corrector) - CERES_TEST(cost_function_to_functor) - CERES_TEST(covariance) - CERES_TEST(dense_sparse_matrix) - CERES_TEST(dynamic_autodiff_cost_function) - CERES_TEST(dynamic_compressed_row_sparse_matrix) - CERES_TEST(dynamic_numeric_diff_cost_function) - CERES_TEST(evaluator) - CERES_TEST(gradient_checker) - CERES_TEST(gradient_checking_cost_function) - CERES_TEST(graph) - CERES_TEST(graph_algorithms) - CERES_TEST(implicit_schur_complement) - CERES_TEST(incomplete_lq_factorization) - CERES_TEST(iterative_schur_complement_solver) - CERES_TEST(jet) - CERES_TEST(levenberg_marquardt_strategy) - CERES_TEST(dogleg_strategy) - CERES_TEST(local_parameterization) - CERES_TEST(loss_function) - CERES_TEST(minimizer) - CERES_TEST(normal_prior) - CERES_TEST(numeric_diff_cost_function) - CERES_TEST(numeric_diff_functor) - CERES_TEST(ordered_groups) - CERES_TEST(parameter_block) - CERES_TEST(parameter_block_ordering) - CERES_TEST(partitioned_matrix_view) - CERES_TEST(polynomial) - CERES_TEST(problem) - CERES_TEST(residual_block) - CERES_TEST(residual_block_utils) - CERES_TEST(rotation) - CERES_TEST(schur_complement_solver) - CERES_TEST(schur_eliminator) - CERES_TEST(single_linkage_clustering) - CERES_TEST(small_blas) - CERES_TEST(solver_impl) - - # TODO(sameeragarwal): This test should ultimately be made - # independent of SuiteSparse. - IF (SUITESPARSE AND SUITESPARSE_FOUND) - CERES_TEST(compressed_col_sparse_matrix_utils) - ENDIF (SUITESPARSE AND SUITESPARSE_FOUND) - - CERES_TEST(symmetric_linear_solver) - CERES_TEST(triplet_sparse_matrix) - CERES_TEST(trust_region_minimizer) - CERES_TEST(unsymmetric_linear_solver) - CERES_TEST(visibility) - CERES_TEST(visibility_based_preconditioner) - - # Put the large end to end test last. - CERES_TEST(system) -ENDIF (BUILD_TESTING AND GFLAGS) diff --git a/extern/libmv/third_party/ceres/internal/ceres/array_utils.cc b/extern/libmv/third_party/ceres/internal/ceres/array_utils.cc index 3eea042d511..205ddaf27c9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/array_utils.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/array_utils.cc @@ -30,10 +30,11 @@ #include "ceres/array_utils.h" +#include <algorithm> #include <cmath> #include <cstddef> #include <string> - +#include <vector> #include "ceres/fpclassify.h" #include "ceres/stringprintf.h" @@ -94,5 +95,19 @@ void AppendArrayToString(const int size, const double* x, string* result) { } } +void MapValuesToContiguousRange(const int size, int* array) { + std::vector<int> unique_values(array, array + size); + std::sort(unique_values.begin(), unique_values.end()); + unique_values.erase(std::unique(unique_values.begin(), + unique_values.end()), + unique_values.end()); + + for (int i = 0; i < size; ++i) { + array[i] = std::lower_bound(unique_values.begin(), + unique_values.end(), + array[i]) - unique_values.begin(); + } +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/array_utils.h b/extern/libmv/third_party/ceres/internal/ceres/array_utils.h index 34fda6fd475..7f56947066b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/array_utils.h +++ b/extern/libmv/third_party/ceres/internal/ceres/array_utils.h @@ -67,6 +67,21 @@ void AppendArrayToString(const int size, const double* x, string* result); extern const double kImpossibleValue; +// This routine takes an array of integer values, sorts and uniques +// them and then maps each value in the array to its position in the +// sorted+uniqued array. By doing this, if there are are k unique +// values in the array, each value is replaced by an integer in the +// range [0, k-1], while preserving their relative order. +// +// For example +// +// [1 0 3 5 0 1 5] +// +// gets mapped to +// +// [1 0 2 3 0 1 3] +void MapValuesToContiguousRange(int size, int* array); + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc index 19b749bfc39..7f79a4f993d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -30,9 +30,9 @@ #include "ceres/block_jacobi_preconditioner.h" -#include "Eigen/Cholesky" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" +#include "ceres/block_random_access_diagonal_matrix.h" #include "ceres/casts.h" #include "ceres/integral_types.h" #include "ceres/internal/eigen.h" @@ -41,27 +41,14 @@ namespace ceres { namespace internal { BlockJacobiPreconditioner::BlockJacobiPreconditioner( - const BlockSparseMatrix& A) - : num_rows_(A.num_rows()), - block_structure_(*A.block_structure()) { - // Calculate the amount of storage needed. - int storage_needed = 0; - for (int c = 0; c < block_structure_.cols.size(); ++c) { - int size = block_structure_.cols[c].size; - storage_needed += size * size; + const BlockSparseMatrix& A) { + const CompressedRowBlockStructure* bs = A.block_structure(); + vector<int> blocks(bs->cols.size()); + for (int i = 0; i < blocks.size(); ++i) { + blocks[i] = bs->cols[i].size; } - // Size the offsets and storage. - blocks_.resize(block_structure_.cols.size()); - block_storage_.resize(storage_needed); - - // Put pointers to the storage in the offsets. - double* block_cursor = &block_storage_[0]; - for (int c = 0; c < block_structure_.cols.size(); ++c) { - int size = block_structure_.cols[c].size; - blocks_[c] = block_cursor; - block_cursor += size * size; - } + m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); } BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} @@ -69,70 +56,50 @@ BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} bool BlockJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, const double* D) { const CompressedRowBlockStructure* bs = A.block_structure(); - - // Compute the diagonal blocks by block inner products. - std::fill(block_storage_.begin(), block_storage_.end(), 0.0); const double* values = A.values(); - for (int r = 0; r < bs->rows.size(); ++r) { - const int row_block_size = bs->rows[r].block.size; - const vector<Cell>& cells = bs->rows[r].cells; - for (int c = 0; c < cells.size(); ++c) { - const int col_block_size = bs->cols[cells[c].block_id].size; - ConstMatrixRef m(values + cells[c].position, + m_->SetZero(); + for (int i = 0; i < bs->rows.size(); ++i) { + const int row_block_size = bs->rows[i].block.size; + const vector<Cell>& cells = bs->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + const int block_id = cells[j].block_id; + const int col_block_size = bs->cols[block_id].size; + + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(block_id, block_id, + &r, &c, + &row_stride, &col_stride); + MatrixRef m(cell_info->values, row_stride, col_stride); + ConstMatrixRef b(values + cells[j].position, row_block_size, col_block_size); - - MatrixRef(blocks_[cells[c].block_id], - col_block_size, - col_block_size).noalias() += m.transpose() * m; - - // TODO(keir): Figure out when the below expression is actually faster - // than doing the full rank update. The issue is that for smaller sizes, - // the rankUpdate() function is slower than the full product done above. - // - // On the typical bundling problems, the above product is ~5% faster. - // - // MatrixRef(blocks_[cells[c].block_id], - // col_block_size, - // col_block_size) - // .selfadjointView<Eigen::Upper>() - // .rankUpdate(m); - // + m.block(r, c, col_block_size, col_block_size) += b.transpose() * b; } } - // Add the diagonal and invert each block. - for (int c = 0; c < bs->cols.size(); ++c) { - const int size = block_structure_.cols[c].size; - const int position = block_structure_.cols[c].position; - MatrixRef block(blocks_[c], size, size); - - if (D != NULL) { - block.diagonal() += - ConstVectorRef(D + position, size).array().square().matrix(); + if (D != NULL) { + // Add the diagonal. + int position = 0; + for (int i = 0; i < bs->cols.size(); ++i) { + const int block_size = bs->cols[i].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(i, i, + &r, &c, + &row_stride, &col_stride); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() += + ConstVectorRef(D + position, block_size).array().square().matrix(); + position += block_size; } - - block = block.selfadjointView<Eigen::Upper>() - .llt() - .solve(Matrix::Identity(size, size)); } + + m_->Invert(); return true; } void BlockJacobiPreconditioner::RightMultiply(const double* x, double* y) const { - for (int c = 0; c < block_structure_.cols.size(); ++c) { - const int size = block_structure_.cols[c].size; - const int position = block_structure_.cols[c].position; - ConstMatrixRef D(blocks_[c], size, size); - ConstVectorRef x_block(x + position, size); - VectorRef y_block(y + position, size); - y_block += D * x_block; - } -} - -void BlockJacobiPreconditioner::LeftMultiply(const double* x, double* y) const { - RightMultiply(x, y); + m_->RightMultiply(x, y); } } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h index 3505a01248b..e23e0e2d24c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -32,6 +32,8 @@ #define CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ #include <vector> +#include "ceres/block_random_access_diagonal_matrix.h" +#include "ceres/internal/scoped_ptr.h" #include "ceres/preconditioner.h" namespace ceres { @@ -39,7 +41,6 @@ namespace internal { class BlockSparseMatrix; struct CompressedRowBlockStructure; -class LinearOperator; // A block Jacobi preconditioner. This is intended for use with // conjugate gradients, or other iterative symmetric solvers. To use @@ -59,19 +60,14 @@ class BlockJacobiPreconditioner : public BlockSparseMatrixPreconditioner { // Preconditioner interface virtual void RightMultiply(const double* x, double* y) const; - virtual void LeftMultiply(const double* x, double* y) const; - virtual int num_rows() const { return num_rows_; } - virtual int num_cols() const { return num_rows_; } + virtual int num_rows() const { return m_->num_rows(); } + virtual int num_cols() const { return m_->num_rows(); } + const BlockRandomAccessDiagonalMatrix& matrix() const { return *m_; } private: virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); - std::vector<double*> blocks_; - std::vector<double> block_storage_; - int num_rows_; - - // The block structure of the matrix this preconditioner is for (e.g. J). - const CompressedRowBlockStructure& block_structure_; + scoped_ptr<BlockRandomAccessDiagonalMatrix> m_; }; } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.cc index d8bf4ef0cb5..b7ff33184cb 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.cc @@ -34,16 +34,19 @@ #include <set> #include <utility> #include <vector> +#include "Eigen/Dense" #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/stl_util.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" -#include "ceres/stl_util.h" #include "glog/logging.h" namespace ceres { namespace internal { +// TODO(sameeragarwal): Drop the dependence on TripletSparseMatrix. + BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( const vector<int>& blocks) : blocks_(blocks) { @@ -51,9 +54,9 @@ BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( // rows/columns. int num_cols = 0; int num_nonzeros = 0; - vector<int> col_layout; + vector<int> block_positions; for (int i = 0; i < blocks_.size(); ++i) { - col_layout.push_back(num_cols); + block_positions.push_back(num_cols); num_cols += blocks_[i]; num_nonzeros += blocks_[i] * blocks_[i]; } @@ -72,7 +75,7 @@ BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( for (int i = 0; i < blocks_.size(); ++i) { const int block_size = blocks_[i]; layout_.push_back(new CellInfo(values + pos)); - const int block_begin = col_layout[i]; + const int block_begin = block_positions[i]; for (int r = 0; r < block_size; ++r) { for (int c = 0; c < block_size; ++c, ++pos) { rows[pos] = block_begin + r; @@ -116,5 +119,34 @@ void BlockRandomAccessDiagonalMatrix::SetZero() { } } +void BlockRandomAccessDiagonalMatrix::Invert() { + double* values = tsm_->mutable_values(); + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + MatrixRef block(values, block_size, block_size); + block = + block + .selfadjointView<Eigen::Upper>() + .llt() + .solve(Matrix::Identity(block_size, block_size)); + values += block_size * block_size; + } +} + +void BlockRandomAccessDiagonalMatrix::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + const double* values = tsm_->values(); + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + ConstMatrixRef block(values, block_size, block_size); + VectorRef(y, block_size).noalias() += block * ConstVectorRef(x, block_size); + x += block_size; + y += block_size; + values += block_size * block_size; + } +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.h index 6b3cff2338f..ea9967817db 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_diagonal_matrix.h @@ -52,7 +52,7 @@ namespace internal { class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { public: // blocks is an array of block sizes. - BlockRandomAccessDiagonalMatrix(const vector<int>& blocks); + explicit BlockRandomAccessDiagonalMatrix(const vector<int>& blocks); // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. @@ -70,11 +70,16 @@ class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { // locked. virtual void SetZero(); + // Invert the matrix assuming that each block is positive definite. + void Invert(); + + // y += S * x + void RightMultiply(const double* x, double* y) const; + // Since the matrix is square, num_rows() == num_cols(). virtual int num_rows() const { return tsm_->num_rows(); } virtual int num_cols() const { return tsm_->num_cols(); } - // Access to the underlying matrix object. const TripletSparseMatrix* matrix() const { return tsm_.get(); } TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc index f789436364a..c43a9b78feb 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc @@ -54,9 +54,9 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( // Build the row/column layout vector and count the number of scalar // rows/columns. int num_cols = 0; - vector<int> col_layout; + block_positions_.reserve(blocks_.size()); for (int i = 0; i < blocks_.size(); ++i) { - col_layout.push_back(num_cols); + block_positions_.push_back(num_cols); num_cols += blocks_[i]; } @@ -88,6 +88,8 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( ++it) { const int row_block_size = blocks_[it->first]; const int col_block_size = blocks_[it->second]; + cell_values_.push_back(make_pair(make_pair(it->first, it->second), + values + pos)); layout_[IntPairToLong(it->first, it->second)] = new CellInfo(values + pos); pos += row_block_size * col_block_size; @@ -105,8 +107,8 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( layout_[IntPairToLong(row_block_id, col_block_id)]->values - values; for (int r = 0; r < row_block_size; ++r) { for (int c = 0; c < col_block_size; ++c, ++pos) { - rows[pos] = col_layout[row_block_id] + r; - cols[pos] = col_layout[col_block_id] + c; + rows[pos] = block_positions_[row_block_id] + r; + cols[pos] = block_positions_[col_block_id] + c; values[pos] = 1.0; DCHECK_LT(rows[pos], tsm_->num_rows()); DCHECK_LT(cols[pos], tsm_->num_rows()); @@ -154,5 +156,36 @@ void BlockRandomAccessSparseMatrix::SetZero() { } } +void BlockRandomAccessSparseMatrix::SymmetricRightMultiply(const double* x, + double* y) const { + vector< pair<pair<int, int>, double*> >::const_iterator it = + cell_values_.begin(); + for (; it != cell_values_.end(); ++it) { + const int row = it->first.first; + const int row_block_size = blocks_[row]; + const int row_block_pos = block_positions_[row]; + + const int col = it->first.second; + const int col_block_size = blocks_[col]; + const int col_block_pos = block_positions_[col]; + + MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + it->second, row_block_size, col_block_size, + x + col_block_pos, + y + row_block_pos); + + // Since the matrix is symmetric, but only the upper triangular + // part is stored, if the block being accessed is not a diagonal + // block, then use the same block to do the corresponding lower + // triangular multiply also. + if (row != col) { + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + it->second, row_block_size, col_block_size, + x + row_block_pos, + y + col_block_pos); + } + } +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h index 27b10296d6c..51b5d20cfe0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h @@ -43,6 +43,7 @@ #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" +#include "ceres/small_blas.h" namespace ceres { namespace internal { @@ -75,6 +76,12 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { // locked. virtual void SetZero(); + // Assume that the matrix is symmetric and only one half of the + // matrix is stored. + // + // y += S * x + void SymmetricRightMultiply(const double* x, double* y) const; + // Since the matrix is square, num_rows() == num_cols(). virtual int num_rows() const { return tsm_->num_rows(); } virtual int num_cols() const { return tsm_->num_cols(); } @@ -84,19 +91,30 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } private: - int64 IntPairToLong(int a, int b) { - return a * kMaxRowBlocks + b; + int64 IntPairToLong(int row, int col) const { + return row * kMaxRowBlocks + col; + } + + void LongToIntPair(int64 index, int* row, int* col) const { + *row = index / kMaxRowBlocks; + *col = index % kMaxRowBlocks; } const int64 kMaxRowBlocks; + // row/column block sizes. const vector<int> blocks_; + vector<int> block_positions_; // A mapping from <row_block_id, col_block_id> to the position in // the values array of tsm_ where the block is stored. typedef HashMap<long int, CellInfo* > LayoutType; LayoutType layout_; + // In order traversal of contents of the matrix. This allows us to + // implement a matrix-vector which is 20% faster than using the + // iterator in the Layout object instead. + vector<pair<pair<int, int>, double*> > cell_values_; // The underlying matrix object which actually stores the cells. scoped_ptr<TripletSparseMatrix> tsm_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/callbacks.cc b/extern/libmv/third_party/ceres/internal/ceres/callbacks.cc new file mode 100644 index 00000000000..7d5ce2548e4 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/callbacks.cc @@ -0,0 +1,109 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include <iostream> // NO LINT +#include "ceres/callbacks.h" +#include "ceres/program.h" +#include "ceres/stringprintf.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +StateUpdatingCallback::StateUpdatingCallback(Program* program, + double* parameters) + : program_(program), parameters_(parameters) {} + +StateUpdatingCallback::~StateUpdatingCallback() {} + +CallbackReturnType StateUpdatingCallback::operator()( + const IterationSummary& summary) { + if (summary.step_is_successful) { + program_->StateVectorToParameterBlocks(parameters_); + program_->CopyParameterBlockStateToUserState(); + } + return SOLVER_CONTINUE; +} + +LoggingCallback::LoggingCallback(const MinimizerType minimizer_type, + const bool log_to_stdout) + : minimizer_type(minimizer_type), + log_to_stdout_(log_to_stdout) {} + +LoggingCallback::~LoggingCallback() {} + +CallbackReturnType LoggingCallback::operator()( + const IterationSummary& summary) { + string output; + if (minimizer_type == LINE_SEARCH) { + const char* kReportRowFormat = + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " + "s:% 3.2e e:% 3d it:% 3.2e tt:% 3.2e"; + output = StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.step_size, + summary.line_search_function_evaluations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); + } else if (minimizer_type == TRUST_REGION) { + if (summary.iteration == 0) { + output = "iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time\n"; + } + const char* kReportRowFormat = + "% 4d % 8e % 3.2e % 3.2e % 3.2e % 3.2e % 3.2e % 4d % 3.2e % 3.2e"; + output += StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.relative_decrease, + summary.trust_region_radius, + summary.linear_solver_iterations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); + } else { + LOG(FATAL) << "Unknown minimizer type."; + } + + if (log_to_stdout_) { + cout << output << endl; + } else { + VLOG(1) << output; + } + return SOLVER_CONTINUE; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/callbacks.h b/extern/libmv/third_party/ceres/internal/ceres/callbacks.h new file mode 100644 index 00000000000..93704dfd6d1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/callbacks.h @@ -0,0 +1,71 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_CALLBACKS_H_ +#define CERES_INTERNAL_CALLBACKS_H_ + +#include <string> +#include "ceres/iteration_callback.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class Program; + +// Callback for updating the externally visible state of parameter +// blocks. +class StateUpdatingCallback : public IterationCallback { + public: + StateUpdatingCallback(Program* program, double* parameters); + virtual ~StateUpdatingCallback(); + virtual CallbackReturnType operator()(const IterationSummary& summary); + private: + Program* program_; + double* parameters_; +}; + +// Callback for logging the state of the minimizer to STDERR or +// STDOUT depending on the user's preferences and logging level. +class LoggingCallback : public IterationCallback { + public: + LoggingCallback(MinimizerType minimizer_type, bool log_to_stdout); + virtual ~LoggingCallback(); + virtual CallbackReturnType operator()(const IterationSummary& summary); + + private: + const MinimizerType minimizer_type; + const bool log_to_stdout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CALLBACKS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc index 2f032e6580a..9bbab4b2377 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc @@ -61,7 +61,7 @@ class CanonicalViewsClustering { // vertices may not be assigned to any cluster. In this case they // are assigned to a cluster with id = kInvalidClusterId. void ComputeClustering(const CanonicalViewsClusteringOptions& options, - const Graph<int>& graph, + const WeightedGraph<int>& graph, vector<int>* centers, IntMap* membership); @@ -74,7 +74,7 @@ class CanonicalViewsClustering { IntMap* membership) const; CanonicalViewsClusteringOptions options_; - const Graph<int>* graph_; + const WeightedGraph<int>* graph_; // Maps a view to its representative canonical view (its cluster // center). IntMap view_to_canonical_view_; @@ -85,7 +85,7 @@ class CanonicalViewsClustering { void ComputeCanonicalViewsClustering( const CanonicalViewsClusteringOptions& options, - const Graph<int>& graph, + const WeightedGraph<int>& graph, vector<int>* centers, IntMap* membership) { time_t start_time = time(NULL); @@ -98,7 +98,7 @@ void ComputeCanonicalViewsClustering( // Implementation of CanonicalViewsClustering void CanonicalViewsClustering::ComputeClustering( const CanonicalViewsClusteringOptions& options, - const Graph<int>& graph, + const WeightedGraph<int>& graph, vector<int>* centers, IntMap* membership) { options_ = options; @@ -150,7 +150,7 @@ void CanonicalViewsClustering::FindValidViews( for (IntSet::const_iterator view = views.begin(); view != views.end(); ++view) { - if (graph_->VertexWeight(*view) != Graph<int>::InvalidWeight()) { + if (graph_->VertexWeight(*view) != WeightedGraph<int>::InvalidWeight()) { valid_views->insert(*view); } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h index 1b4c4ee059f..d3fa5725831 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h @@ -101,7 +101,7 @@ struct CanonicalViewsClusteringOptions; // cluster. In this case they are assigned to a cluster with id = -1; void ComputeCanonicalViewsClustering( const CanonicalViewsClusteringOptions& options, - const Graph<int>& graph, + const WeightedGraph<int>& graph, vector<int>* centers, HashMap<int, int>* membership); diff --git a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc index 524cb8ad988..3071a0918e4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -101,7 +101,7 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( A->RightMultiply(x, tmp.data()); r = bref - tmp; double norm_r = r.norm(); - if (norm_r <= tol_r) { + if (options_.min_num_iterations == 0 && norm_r <= tol_r) { summary.termination_type = LINEAR_SOLVER_SUCCESS; summary.message = StringPrintf("Convergence. |r| = %e <= %e.", norm_r, tol_r); @@ -113,9 +113,8 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( // Initial value of the quadratic model Q = x'Ax - 2 * b'x. double Q0 = -1.0 * xref.dot(bref + r); - for (summary.num_iterations = 1; - summary.num_iterations < options_.max_num_iterations; - ++summary.num_iterations) { + for (summary.num_iterations = 1;; ++summary.num_iterations) { + // Apply preconditioner if (per_solve_options.preconditioner != NULL) { z.setZero(); @@ -207,7 +206,8 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( // 124(1-2), 45-59, 2000. // const double zeta = summary.num_iterations * (Q1 - Q0) / Q1; - if (zeta < per_solve_options.q_tolerance) { + if (zeta < per_solve_options.q_tolerance && + summary.num_iterations >= options_.min_num_iterations) { summary.termination_type = LINEAR_SOLVER_SUCCESS; summary.message = StringPrintf("Convergence: zeta = %e < %e", @@ -219,12 +219,17 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( // Residual based termination. norm_r = r. norm(); - if (norm_r <= tol_r) { + if (norm_r <= tol_r && + summary.num_iterations >= options_.min_num_iterations) { summary.termination_type = LINEAR_SOLVER_SUCCESS; summary.message = StringPrintf("Convergence. |r| = %e <= %e.", norm_r, tol_r); break; } + + if (summary.num_iterations >= options_.max_num_iterations) { + break; + } } return summary; diff --git a/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.cc b/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.cc index bfe93c49826..1d55458bb69 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.cc @@ -40,15 +40,15 @@ #include "ceres/evaluator.h" #include "ceres/linear_solver.h" #include "ceres/minimizer.h" -#include "ceres/ordered_groups.h" #include "ceres/parameter_block.h" +#include "ceres/parameter_block_ordering.h" #include "ceres/problem_impl.h" #include "ceres/program.h" #include "ceres/residual_block.h" #include "ceres/solver.h" -#include "ceres/solver_impl.h" #include "ceres/trust_region_minimizer.h" #include "ceres/trust_region_strategy.h" +#include "ceres/parameter_block_ordering.h" namespace ceres { namespace internal { @@ -210,28 +210,54 @@ void CoordinateDescentMinimizer::Solve(Program* program, summary->final_cost = 0.0; string error; - scoped_ptr<Evaluator> evaluator( - Evaluator::Create(evaluator_options_, program, &error)); - CHECK_NOTNULL(evaluator.get()); - - scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); - CHECK_NOTNULL(jacobian.get()); + Minimizer::Options minimizer_options; + minimizer_options.evaluator.reset( + CHECK_NOTNULL(Evaluator::Create(evaluator_options_, program, &error))); + minimizer_options.jacobian.reset( + CHECK_NOTNULL(minimizer_options.evaluator->CreateJacobian())); TrustRegionStrategy::Options trs_options; trs_options.linear_solver = linear_solver; - - scoped_ptr<TrustRegionStrategy>trust_region_strategy( + minimizer_options.trust_region_strategy.reset( CHECK_NOTNULL(TrustRegionStrategy::Create(trs_options))); - - Minimizer::Options minimizer_options; - minimizer_options.evaluator = evaluator.get(); - minimizer_options.jacobian = jacobian.get(); - minimizer_options.trust_region_strategy = trust_region_strategy.get(); minimizer_options.is_silent = true; TrustRegionMinimizer minimizer; minimizer.Minimize(minimizer_options, parameter, summary); } +bool CoordinateDescentMinimizer::IsOrderingValid( + const Program& program, + const ParameterBlockOrdering& ordering, + string* message) { + const map<int, set<double*> >& group_to_elements = + ordering.group_to_elements(); + + // Verify that each group is an independent set + map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + for ( ; it != group_to_elements.end(); ++it) { + if (!program.IsParameterBlockSetIndependent(it->second)) { + *message = + StringPrintf("The user-provided " + "parameter_blocks_for_inner_iterations does not " + "form an independent set. Group Id: %d", it->first); + return false; + } + } + return true; +} + +// Find a recursive decomposition of the Hessian matrix as a set +// of independent sets of decreasing size and invert it. This +// seems to work better in practice, i.e., Cameras before +// points. +ParameterBlockOrdering* CoordinateDescentMinimizer::CreateOrdering( + const Program& program) { + scoped_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering); + ComputeRecursiveIndependentSetOrdering(program, ordering.get()); + ordering->Reverse(); + return ordering.release(); +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h index 424acda94ae..c1f8ffcd02a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h +++ b/extern/libmv/third_party/ceres/internal/ceres/coordinate_descent_minimizer.h @@ -37,12 +37,14 @@ #include "ceres/evaluator.h" #include "ceres/minimizer.h" #include "ceres/problem_impl.h" -#include "ceres/program.h" #include "ceres/solver.h" namespace ceres { namespace internal { +class Program; +class LinearSolver; + // Given a Program, and a ParameterBlockOrdering which partitions // (non-exhaustively) the Hessian matrix into independent sets, // perform coordinate descent on the parameter blocks in the @@ -66,6 +68,17 @@ class CoordinateDescentMinimizer : public Minimizer { double* parameters, Solver::Summary* summary); + // Verify that each group in the ordering forms an independent set. + static bool IsOrderingValid(const Program& program, + const ParameterBlockOrdering& ordering, + string* message); + + // Find a recursive decomposition of the Hessian matrix as a set + // of independent sets of decreasing size and invert it. This + // seems to work better in practice, i.e., Cameras before + // points. + static ParameterBlockOrdering* CreateOrdering(const Program& program); + private: void Solve(Program* program, LinearSolver* linear_solver, diff --git a/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc index 75c80bf5687..cfbfb445343 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc @@ -38,6 +38,25 @@ #include <cstdlib> #include <utility> #include <vector> +#include "Eigen/SparseCore" + +// Suppress unused local variable warning from Eigen Ordering.h #included by +// SparseQR in Eigen 3.2.0. This was fixed in Eigen 3.2.1, but 3.2.0 is still +// widely used (Ubuntu 14.04), and Ceres won't compile otherwise due to -Werror. +#if defined(_MSC_VER) +#pragma warning( push ) +#pragma warning( disable : 4189 ) +#else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-but-set-variable" +#endif +#include "Eigen/SparseQR" +#if defined(_MSC_VER) +#pragma warning( pop ) +#else +#pragma GCC diagnostic pop +#endif + #include "Eigen/SVD" #include "ceres/compressed_col_sparse_matrix_utils.h" #include "ceres/compressed_row_sparse_matrix.h" @@ -53,40 +72,6 @@ namespace ceres { namespace internal { -namespace { - -// Per thread storage for SuiteSparse. -#ifndef CERES_NO_SUITESPARSE - -struct PerThreadContext { - explicit PerThreadContext(int num_rows) - : solution(NULL), - solution_set(NULL), - y_workspace(NULL), - e_workspace(NULL), - rhs(NULL) { - rhs = ss.CreateDenseVector(NULL, num_rows, num_rows); - } - - ~PerThreadContext() { - ss.Free(solution); - ss.Free(solution_set); - ss.Free(y_workspace); - ss.Free(e_workspace); - ss.Free(rhs); - } - - cholmod_dense* solution; - cholmod_sparse* solution_set; - cholmod_dense* y_workspace; - cholmod_dense* e_workspace; - cholmod_dense* rhs; - SuiteSparse ss; -}; - -#endif - -} // namespace typedef vector<pair<const double*, const double*> > CovarianceBlocks; @@ -94,8 +79,17 @@ CovarianceImpl::CovarianceImpl(const Covariance::Options& options) : options_(options), is_computed_(false), is_valid_(false) { - evaluate_options_.num_threads = options.num_threads; - evaluate_options_.apply_loss_function = options.apply_loss_function; +#ifndef CERES_USE_OPENMP + if (options_.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; + options_.num_threads = 1; + } +#endif + evaluate_options_.num_threads = options_.num_threads; + evaluate_options_.apply_loss_function = options_.apply_loss_function; } CovarianceImpl::~CovarianceImpl() { @@ -396,11 +390,15 @@ bool CovarianceImpl::ComputeCovarianceValues() { case DENSE_SVD: return ComputeCovarianceValuesUsingDenseSVD(); #ifndef CERES_NO_SUITESPARSE - case SPARSE_CHOLESKY: - return ComputeCovarianceValuesUsingSparseCholesky(); - case SPARSE_QR: - return ComputeCovarianceValuesUsingSparseQR(); + case SUITE_SPARSE_QR: + return ComputeCovarianceValuesUsingSuiteSparseQR(); +#else + LOG(ERROR) << "SuiteSparse is required to use the " + << "SUITE_SPARSE_QR algorithm."; + return false; #endif + case EIGEN_SPARSE_QR: + return ComputeCovarianceValuesUsingEigenSparseQR(); default: LOG(ERROR) << "Unsupported covariance estimation algorithm type: " << CovarianceAlgorithmTypeToString(options_.algorithm_type); @@ -409,197 +407,7 @@ bool CovarianceImpl::ComputeCovarianceValues() { return false; } -bool CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky() { - EventLogger event_logger( - "CovarianceImpl::ComputeCovarianceValuesUsingSparseCholesky"); -#ifndef CERES_NO_SUITESPARSE - if (covariance_matrix_.get() == NULL) { - // Nothing to do, all zeros covariance matrix. - return true; - } - - SuiteSparse ss; - - CRSMatrix jacobian; - problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); - - event_logger.AddEvent("Evaluate"); - // m is a transposed view of the Jacobian. - cholmod_sparse cholmod_jacobian_view; - cholmod_jacobian_view.nrow = jacobian.num_cols; - cholmod_jacobian_view.ncol = jacobian.num_rows; - cholmod_jacobian_view.nzmax = jacobian.values.size(); - cholmod_jacobian_view.nz = NULL; - cholmod_jacobian_view.p = reinterpret_cast<void*>(&jacobian.rows[0]); - cholmod_jacobian_view.i = reinterpret_cast<void*>(&jacobian.cols[0]); - cholmod_jacobian_view.x = reinterpret_cast<void*>(&jacobian.values[0]); - cholmod_jacobian_view.z = NULL; - cholmod_jacobian_view.stype = 0; // Matrix is not symmetric. - cholmod_jacobian_view.itype = CHOLMOD_INT; - cholmod_jacobian_view.xtype = CHOLMOD_REAL; - cholmod_jacobian_view.dtype = CHOLMOD_DOUBLE; - cholmod_jacobian_view.sorted = 1; - cholmod_jacobian_view.packed = 1; - - string message; - cholmod_factor* factor = ss.AnalyzeCholesky(&cholmod_jacobian_view, &message); - event_logger.AddEvent("Symbolic Factorization"); - if (factor == NULL) { - LOG(ERROR) << "Covariance estimation failed. " - << "CHOLMOD symbolic cholesky factorization returned with: " - << message; - return false; - } - - LinearSolverTerminationType termination_type = - ss.Cholesky(&cholmod_jacobian_view, factor, &message); - event_logger.AddEvent("Numeric Factorization"); - if (termination_type != LINEAR_SOLVER_SUCCESS) { - LOG(ERROR) << "Covariance estimation failed. " - << "CHOLMOD numeric cholesky factorization returned with: " - << message; - ss.Free(factor); - return false; - } - - const double reciprocal_condition_number = - cholmod_rcond(factor, ss.mutable_cc()); - - if (reciprocal_condition_number < - options_.min_reciprocal_condition_number) { - LOG(ERROR) << "Cholesky factorization of J'J is not reliable. " - << "Reciprocal condition number: " - << reciprocal_condition_number << " " - << "min_reciprocal_condition_number: " - << options_.min_reciprocal_condition_number; - ss.Free(factor); - return false; - } - - const int num_rows = covariance_matrix_->num_rows(); - const int* rows = covariance_matrix_->rows(); - const int* cols = covariance_matrix_->cols(); - double* values = covariance_matrix_->mutable_values(); - - // The following loop exploits the fact that the i^th column of A^{-1} - // is given by the solution to the linear system - // - // A x = e_i - // - // where e_i is a vector with e(i) = 1 and all other entries zero. - // - // Since the covariance matrix is symmetric, the i^th row and column - // are equal. - // - // The ifdef separates two different version of SuiteSparse. Newer - // versions of SuiteSparse have the cholmod_solve2 function which - // re-uses memory across calls. -#if (SUITESPARSE_VERSION < 4002) - cholmod_dense* rhs = ss.CreateDenseVector(NULL, num_rows, num_rows); - double* rhs_x = reinterpret_cast<double*>(rhs->x); - - for (int r = 0; r < num_rows; ++r) { - int row_begin = rows[r]; - int row_end = rows[r + 1]; - if (row_end == row_begin) { - continue; - } - - rhs_x[r] = 1.0; - cholmod_dense* solution = ss.Solve(factor, rhs, &message); - double* solution_x = reinterpret_cast<double*>(solution->x); - for (int idx = row_begin; idx < row_end; ++idx) { - const int c = cols[idx]; - values[idx] = solution_x[c]; - } - ss.Free(solution); - rhs_x[r] = 0.0; - } - - ss.Free(rhs); -#else // SUITESPARSE_VERSION < 4002 - - const int num_threads = options_.num_threads; - vector<PerThreadContext*> contexts(num_threads); - for (int i = 0; i < num_threads; ++i) { - contexts[i] = new PerThreadContext(num_rows); - } - - // The first call to cholmod_solve2 is not thread safe, since it - // changes the factorization from supernodal to simplicial etc. - { - PerThreadContext* context = contexts[0]; - double* context_rhs_x = reinterpret_cast<double*>(context->rhs->x); - context_rhs_x[0] = 1.0; - cholmod_solve2(CHOLMOD_A, - factor, - context->rhs, - NULL, - &context->solution, - &context->solution_set, - &context->y_workspace, - &context->e_workspace, - context->ss.mutable_cc()); - context_rhs_x[0] = 0.0; - } - -#pragma omp parallel for num_threads(num_threads) schedule(dynamic) - for (int r = 0; r < num_rows; ++r) { - int row_begin = rows[r]; - int row_end = rows[r + 1]; - if (row_end == row_begin) { - continue; - } - -# ifdef CERES_USE_OPENMP - int thread_id = omp_get_thread_num(); -# else - int thread_id = 0; -# endif - - PerThreadContext* context = contexts[thread_id]; - double* context_rhs_x = reinterpret_cast<double*>(context->rhs->x); - context_rhs_x[r] = 1.0; - - // TODO(sameeragarwal) There should be a more efficient way - // involving the use of Bset but I am unable to make it work right - // now. - cholmod_solve2(CHOLMOD_A, - factor, - context->rhs, - NULL, - &context->solution, - &context->solution_set, - &context->y_workspace, - &context->e_workspace, - context->ss.mutable_cc()); - - double* solution_x = reinterpret_cast<double*>(context->solution->x); - for (int idx = row_begin; idx < row_end; ++idx) { - const int c = cols[idx]; - values[idx] = solution_x[c]; - } - context_rhs_x[r] = 0.0; - } - - for (int i = 0; i < num_threads; ++i) { - delete contexts[i]; - } - -#endif // SUITESPARSE_VERSION < 4002 - - ss.Free(factor); - event_logger.AddEvent("Inversion"); - return true; - -#else // CERES_NO_SUITESPARSE - - return false; - -#endif // CERES_NO_SUITESPARSE -}; - -bool CovarianceImpl::ComputeCovarianceValuesUsingSparseQR() { +bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { EventLogger event_logger( "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR"); @@ -851,7 +659,102 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { } event_logger.AddEvent("CopyToCovarianceMatrix"); return true; -}; +} + +bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() { + EventLogger event_logger( + "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR"); + if (covariance_matrix_.get() == NULL) { + // Nothing to do, all zeros covariance matrix. + return true; + } + + CRSMatrix jacobian; + problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian); + event_logger.AddEvent("Evaluate"); + + typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix; + + // Convert the matrix to column major order as required by SparseQR. + EigenSparseMatrix sparse_jacobian = + Eigen::MappedSparseMatrix<double, Eigen::RowMajor>( + jacobian.num_rows, jacobian.num_cols, + static_cast<int>(jacobian.values.size()), + jacobian.rows.data(), jacobian.cols.data(), jacobian.values.data()); + event_logger.AddEvent("ConvertToSparseMatrix"); + + Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int> > + qr_solver(sparse_jacobian); + event_logger.AddEvent("QRDecomposition"); + + if(qr_solver.info() != Eigen::Success) { + LOG(ERROR) << "Eigen::SparseQR decomposition failed."; + return false; + } + + if (qr_solver.rank() < jacobian.num_cols) { + LOG(ERROR) << "Jacobian matrix is rank deficient. " + << "Number of columns: " << jacobian.num_cols + << " rank: " << qr_solver.rank(); + return false; + } + + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + double* values = covariance_matrix_->mutable_values(); + + // Compute the inverse column permutation used by QR factorization. + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation = + qr_solver.colsPermutation().inverse(); + + // The following loop exploits the fact that the i^th column of A^{-1} + // is given by the solution to the linear system + // + // A x = e_i + // + // where e_i is a vector with e(i) = 1 and all other entries zero. + // + // Since the covariance matrix is symmetric, the i^th row and column + // are equal. + const int num_cols = jacobian.num_cols; + const int num_threads = options_.num_threads; + scoped_array<double> workspace(new double[num_threads * num_cols]); + +#pragma omp parallel for num_threads(num_threads) schedule(dynamic) + for (int r = 0; r < num_cols; ++r) { + const int row_begin = rows[r]; + const int row_end = rows[r + 1]; + if (row_end == row_begin) { + continue; + } + +# ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +# else + int thread_id = 0; +# endif + + double* solution = workspace.get() + thread_id * num_cols; + SolveRTRWithSparseRHS<int>( + num_cols, + qr_solver.matrixR().innerIndexPtr(), + qr_solver.matrixR().outerIndexPtr(), + &qr_solver.matrixR().data().value(0), + inverse_permutation.indices().coeff(r), + solution); + + // Assign the values of the computed covariance using the + // inverse permutation used in the QR factorization. + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution[inverse_permutation.indices().coeff(c)]; + } + } + + event_logger.AddEvent("Inverse"); + + return true; +} } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h index 0e7e2173079..135f4a1d624 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h @@ -64,9 +64,9 @@ class CovarianceImpl { ProblemImpl* problem); bool ComputeCovarianceValues(); - bool ComputeCovarianceValuesUsingSparseCholesky(); - bool ComputeCovarianceValuesUsingSparseQR(); bool ComputeCovarianceValuesUsingDenseSVD(); + bool ComputeCovarianceValuesUsingSuiteSparseQR(); + bool ComputeCovarianceValuesUsingEigenSparseQR(); const CompressedRowSparseMatrix* covariance_matrix() const { return covariance_matrix_.get(); diff --git a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h index 1fed82f7866..5868401b961 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h +++ b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h @@ -38,7 +38,6 @@ #include <vector> #include "cs.h" -#include "ceres/internal/port.h" namespace ceres { namespace internal { @@ -130,9 +129,13 @@ class CXSparse { #else // CERES_NO_CXSPARSE -class CXSparse {}; typedef void cs_dis; +class CXSparse { + public: + void Free(void*) {}; + +}; #endif // CERES_NO_CXSPARSE #endif // CERES_INTERNAL_CXSPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc b/extern/libmv/third_party/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc index 2f01617749d..b46cb797ff2 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc @@ -54,8 +54,15 @@ SparseMatrix* DynamicCompressedRowJacobianWriter::CreateJacobian() const { num_effective_parameters, 0); - CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( - program_, jacobian); + vector<int>* row_blocks = jacobian->mutable_row_blocks(); + for (int i = 0; i < jacobian->num_rows(); ++i) { + row_blocks->push_back(1); + } + + vector<int>* col_blocks = jacobian->mutable_col_blocks(); + for (int i = 0; i < jacobian->num_cols(); ++i) { + col_blocks->push_back(1); + } return jacobian; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc index bca22e6de03..3272848a499 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -310,6 +310,17 @@ ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, parameter_blocks); } + // Normally, when a problem is given to the solver, we guarantee + // that the state pointers for each parameter block point to the + // user provided data. Since we are creating this new problem from a + // problem given to us at an arbitrary stage of the solve, we cannot + // depend on this being the case, so we explicitly call + // SetParameterBlockStatePtrsToUserStatePtrs to ensure that this is + // the case. + gradient_checking_problem_impl + ->mutable_program() + ->SetParameterBlockStatePtrsToUserStatePtrs(); + return gradient_checking_problem_impl; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_problem.cc b/extern/libmv/third_party/ceres/internal/ceres/gradient_problem.cc new file mode 100644 index 00000000000..8f9a842bd89 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_problem.cc @@ -0,0 +1,81 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/gradient_problem.h" +#include "ceres/local_parameterization.h" +#include "glog/logging.h" + +namespace ceres { + +GradientProblem::GradientProblem(FirstOrderFunction* function) + : function_(function), + parameterization_( + new IdentityParameterization(function_->NumParameters())), + scratch_(new double[function_->NumParameters()]) { +} + +GradientProblem::GradientProblem(FirstOrderFunction* function, + LocalParameterization* parameterization) + : function_(function), + parameterization_(parameterization), + scratch_(new double[function_->NumParameters()]) { + CHECK_EQ(function_->NumParameters(), parameterization_->GlobalSize()); +} + +int GradientProblem::NumParameters() const { + return function_->NumParameters(); +} + +int GradientProblem::NumLocalParameters() const { + return parameterization_->LocalSize(); +} + + +bool GradientProblem::Evaluate(const double* parameters, + double* cost, + double* gradient) const { + if (gradient == NULL) { + return function_->Evaluate(parameters, cost, NULL); + } + + return (function_->Evaluate(parameters, cost, scratch_.get()) && + parameterization_->MultiplyByJacobian(parameters, + 1, + scratch_.get(), + gradient)); +} + +bool GradientProblem::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + return parameterization_->Plus(x, delta, x_plus_delta); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_problem_evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/gradient_problem_evaluator.h new file mode 100644 index 00000000000..20053de2af6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_problem_evaluator.h @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_GRADIENT_PROBLEM_EVALUATOR_H_ +#define CERES_INTERNAL_GRADIENT_PROBLEM_EVALUATOR_H_ + +#include <map> +#include <string> + +#include "ceres/evaluator.h" +#include "ceres/execution_summary.h" +#include "ceres/gradient_problem.h" +#include "ceres/internal/port.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { + +class GradientProblemEvaluator : public Evaluator { + public: + explicit GradientProblemEvaluator(const GradientProblem& problem) + : problem_(problem) {} + virtual ~GradientProblemEvaluator() {} + virtual SparseMatrix* CreateJacobian() const { return NULL; } + virtual bool Evaluate(const EvaluateOptions& evaluate_options, + const double* state, + double* cost, + double* residuals, + double* gradient, + SparseMatrix* jacobian) { + CHECK(jacobian == NULL); + ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); + ScopedExecutionTimer call_type_timer( + gradient == NULL ? "Evaluator::Cost" : "Evaluator::Gradient", + &execution_summary_); + return problem_.Evaluate(state, cost, gradient); + } + + virtual bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + return problem_.Plus(state, delta, state_plus_delta); + } + + virtual int NumParameters() const { + return problem_.NumParameters(); + } + + virtual int NumEffectiveParameters() const { + return problem_.NumLocalParameters(); + } + + virtual int NumResiduals() const { return 1; } + + virtual map<string, int> CallStatistics() const { + return execution_summary_.calls(); + } + + virtual map<string, double> TimeStatistics() const { + return execution_summary_.times(); + } + + private: + const GradientProblem& problem_; + ::ceres::internal::ExecutionSummary execution_summary_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRADIENT_PROBLEM_EVALUATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_problem_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/gradient_problem_solver.cc new file mode 100644 index 00000000000..4024f4cc4e6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_problem_solver.cc @@ -0,0 +1,270 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/gradient_problem_solver.h" + +#include "ceres/callbacks.h" +#include "ceres/gradient_problem.h" +#include "ceres/gradient_problem_evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/map_util.h" +#include "ceres/minimizer.h" +#include "ceres/solver.h" +#include "ceres/solver_utils.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" + +namespace ceres { +using internal::StringPrintf; +using internal::StringAppendF; + +namespace { + +Solver::Options GradientProblemSolverOptionsToSolverOptions( + const GradientProblemSolver::Options& options) { +#define COPY_OPTION(x) solver_options.x = options.x + + Solver::Options solver_options; + solver_options.minimizer_type = LINE_SEARCH; + COPY_OPTION(line_search_direction_type); + COPY_OPTION(line_search_type); + COPY_OPTION(nonlinear_conjugate_gradient_type); + COPY_OPTION(max_lbfgs_rank); + COPY_OPTION(use_approximate_eigenvalue_bfgs_scaling); + COPY_OPTION(line_search_interpolation_type); + COPY_OPTION(min_line_search_step_size); + COPY_OPTION(line_search_sufficient_function_decrease); + COPY_OPTION(max_line_search_step_contraction); + COPY_OPTION(min_line_search_step_contraction); + COPY_OPTION(max_num_line_search_step_size_iterations); + COPY_OPTION(max_num_line_search_direction_restarts); + COPY_OPTION(line_search_sufficient_curvature_decrease); + COPY_OPTION(max_line_search_step_expansion); + COPY_OPTION(max_num_iterations); + COPY_OPTION(max_solver_time_in_seconds); + COPY_OPTION(function_tolerance); + COPY_OPTION(gradient_tolerance); + COPY_OPTION(logging_type); + COPY_OPTION(minimizer_progress_to_stdout); + COPY_OPTION(callbacks); + return solver_options; +#undef COPY_OPTION +} + + +} // namespace + +GradientProblemSolver::~GradientProblemSolver() { +} + +void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters_ptr, + GradientProblemSolver::Summary* summary) { + using internal::scoped_ptr; + using internal::WallTimeInSeconds; + using internal::Minimizer; + using internal::GradientProblemEvaluator; + using internal::LoggingCallback; + using internal::SetSummaryFinalCost; + + double start_time = WallTimeInSeconds(); + Solver::Options solver_options = + GradientProblemSolverOptionsToSolverOptions(options); + + *CHECK_NOTNULL(summary) = Summary(); + summary->num_parameters = problem.NumParameters(); + summary->num_local_parameters = problem.NumLocalParameters(); + summary->line_search_direction_type = options.line_search_direction_type; // NOLINT + summary->line_search_interpolation_type = options.line_search_interpolation_type; // NOLINT + summary->line_search_type = options.line_search_type; + summary->max_lbfgs_rank = options.max_lbfgs_rank; + summary->nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; // NOLINT + + // Check validity + if (!solver_options.IsValid(&summary->message)) { + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + // Assuming that the parameter blocks in the program have been + Minimizer::Options minimizer_options; + minimizer_options = Minimizer::Options(solver_options); + minimizer_options.evaluator.reset(new GradientProblemEvaluator(problem)); + + scoped_ptr<IterationCallback> logging_callback; + if (options.logging_type != SILENT) { + logging_callback.reset( + new LoggingCallback(LINE_SEARCH, options.minimizer_progress_to_stdout)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + logging_callback.get()); + } + + scoped_ptr<Minimizer> minimizer(Minimizer::Create(LINE_SEARCH)); + Vector solution(problem.NumParameters()); + VectorRef parameters(parameters_ptr, problem.NumParameters()); + solution = parameters; + + Solver::Summary solver_summary; + solver_summary.fixed_cost = 0.0; + solver_summary.preprocessor_time_in_seconds = 0.0; + solver_summary.postprocessor_time_in_seconds = 0.0; + + minimizer->Minimize(minimizer_options, solution.data(), &solver_summary); + + summary->termination_type = solver_summary.termination_type; + summary->message = solver_summary.message; + summary->initial_cost = solver_summary.initial_cost; + summary->final_cost = solver_summary.final_cost; + summary->iterations = solver_summary.iterations; + + if (summary->IsSolutionUsable()) { + parameters = solution; + SetSummaryFinalCost(summary); + } + + const map<string, double>& evaluator_time_statistics = + minimizer_options.evaluator->TimeStatistics(); + summary->cost_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); + summary->gradient_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); + + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; +} + +// Invalid values for most fields, to ensure that we are not +// accidentally reporting default values. +GradientProblemSolver::Summary::Summary() + : termination_type(FAILURE), + message("ceres::GradientProblemSolve was not called."), + initial_cost(-1.0), + final_cost(-1.0), + total_time_in_seconds(-1.0), + cost_evaluation_time_in_seconds(-1.0), + gradient_evaluation_time_in_seconds(-1.0), + num_parameters(-1), + num_local_parameters(-1), + line_search_direction_type(LBFGS), + line_search_type(ARMIJO), + line_search_interpolation_type(BISECTION), + nonlinear_conjugate_gradient_type(FLETCHER_REEVES), + max_lbfgs_rank(-1) { +} + +bool GradientProblemSolver::Summary::IsSolutionUsable() const { + return internal::IsSolutionUsable(*this); +} + +string GradientProblemSolver::Summary::BriefReport() const { + return StringPrintf("Ceres GradientProblemSolver Report: " + "Iterations: %d, " + "Initial cost: %e, " + "Final cost: %e, " + "Termination: %s", + static_cast<int>(iterations.size()), + initial_cost, + final_cost, + TerminationTypeToString(termination_type)); +}; + +string GradientProblemSolver::Summary::FullReport() const { + using internal::VersionString; + + string report = string("\nSolver Summary (v " + VersionString() + ")\n\n"); + + StringAppendF(&report, "Parameters % 25d\n", num_parameters); + if (num_local_parameters != num_parameters) { + StringAppendF(&report, "Local parameters % 25d\n", + num_local_parameters); + } + + string line_search_direction_string; + if (line_search_direction_type == LBFGS) { + line_search_direction_string = StringPrintf("LBFGS (%d)", max_lbfgs_rank); + } else if (line_search_direction_type == NONLINEAR_CONJUGATE_GRADIENT) { + line_search_direction_string = + NonlinearConjugateGradientTypeToString( + nonlinear_conjugate_gradient_type); + } else { + line_search_direction_string = + LineSearchDirectionTypeToString(line_search_direction_type); + } + + StringAppendF(&report, "Line search direction %19s\n", + line_search_direction_string.c_str()); + + const string line_search_type_string = + StringPrintf("%s %s", + LineSearchInterpolationTypeToString( + line_search_interpolation_type), + LineSearchTypeToString(line_search_type)); + StringAppendF(&report, "Line search type %19s\n", + line_search_type_string.c_str()); + StringAppendF(&report, "\n"); + + StringAppendF(&report, "\nCost:\n"); + StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != FAILURE && + termination_type != USER_FAILURE) { + StringAppendF(&report, "Final % 30e\n", final_cost); + StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } + + StringAppendF(&report, "\nMinimizer iterations % 16d\n", + static_cast<int>(iterations.size())); + + StringAppendF(&report, "\nTime (in seconds):\n"); + + StringAppendF(&report, "\n Cost evaluation %23.3f\n", + cost_evaluation_time_in_seconds); + StringAppendF(&report, " Gradient evaluation %23.3f\n", + gradient_evaluation_time_in_seconds); + + StringAppendF(&report, "Total %25.3f\n\n", + total_time_in_seconds); + + StringAppendF(&report, "Termination: %25s (%s)\n", + TerminationTypeToString(termination_type), message.c_str()); + return report; +}; + +void Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters, + GradientProblemSolver::Summary* summary) { + GradientProblemSolver solver; + solver.Solve(options, problem, parameters, summary); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph.h b/extern/libmv/third_party/ceres/internal/ceres/graph.h index 5f92d4d4df2..f639d15323d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/graph.h +++ b/extern/libmv/third_party/ceres/internal/ceres/graph.h @@ -43,13 +43,75 @@ namespace ceres { namespace internal { -// A weighted undirected graph templated over the vertex ids. Vertex -// should be hashable and comparable. +// A unweighted undirected graph templated over the vertex ids. Vertex +// should be hashable. template <typename Vertex> class Graph { public: Graph() {} + // Add a vertex. + void AddVertex(const Vertex& vertex) { + if (vertices_.insert(vertex).second) { + edges_[vertex] = HashSet<Vertex>(); + } + } + + bool RemoveVertex(const Vertex& vertex) { + if (vertices_.find(vertex) == vertices_.end()) { + return false; + } + + vertices_.erase(vertex); + const HashSet<Vertex>& sinks = edges_[vertex]; + for (typename HashSet<Vertex>::const_iterator it = sinks.begin(); + it != sinks.end(); ++it) { + edges_[*it].erase(vertex); + } + + edges_.erase(vertex); + return true; + } + + // Add an edge between the vertex1 and vertex2. Calling AddEdge on a + // pair of vertices which do not exist in the graph yet will result + // in undefined behavior. + // + // It is legal to call this method repeatedly for the same set of + // vertices. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2) { + DCHECK(vertices_.find(vertex1) != vertices_.end()); + DCHECK(vertices_.find(vertex2) != vertices_.end()); + + if (edges_[vertex1].insert(vertex2).second) { + edges_[vertex2].insert(vertex1); + } + } + + // Calling Neighbors on a vertex not in the graph will result in + // undefined behaviour. + const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + return FindOrDie(edges_, vertex); + } + + const HashSet<Vertex>& vertices() const { + return vertices_; + } + + private: + HashSet<Vertex> vertices_; + HashMap<Vertex, HashSet<Vertex> > edges_; + + CERES_DISALLOW_COPY_AND_ASSIGN(Graph); +}; + +// A weighted undirected graph templated over the vertex ids. Vertex +// should be hashable and comparable. +template <typename Vertex> +class WeightedGraph { + public: + WeightedGraph() {} + // Add a weighted vertex. If the vertex already exists in the graph, // its weight is set to the new weight. void AddVertex(const Vertex& vertex, double weight) { @@ -152,7 +214,7 @@ class Graph { HashMap<Vertex, HashSet<Vertex> > edges_; HashMap<pair<Vertex, Vertex>, double> edge_weights_; - CERES_DISALLOW_COPY_AND_ASSIGN(Graph); + CERES_DISALLOW_COPY_AND_ASSIGN(WeightedGraph); }; } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h index ca3a2fe1a88..fb75e2f45f2 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h +++ b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h @@ -38,6 +38,7 @@ #include <utility> #include "ceres/collections_port.h" #include "ceres/graph.h" +#include "ceres/wall_time.h" #include "glog/logging.h" namespace ceres { @@ -270,11 +271,11 @@ Vertex FindConnectedComponent(const Vertex& vertex, // spanning forest, or a collection of linear paths that span the // graph G. template <typename Vertex> -Graph<Vertex>* -Degree2MaximumSpanningForest(const Graph<Vertex>& graph) { +WeightedGraph<Vertex>* +Degree2MaximumSpanningForest(const WeightedGraph<Vertex>& graph) { // Array of edges sorted in decreasing order of their weights. vector<pair<double, pair<Vertex, Vertex> > > weighted_edges; - Graph<Vertex>* forest = new Graph<Vertex>(); + WeightedGraph<Vertex>* forest = new WeightedGraph<Vertex>(); // Disjoint-set to keep track of the connected components in the // maximum spanning tree. diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc index 6de410bf80f..0cf08fef5f7 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc @@ -101,6 +101,7 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( // complement matrix with the block diagonal of the matrix F'F as // the preconditioner. LinearSolver::Options cg_options; + cg_options.min_num_iterations = options_.min_num_iterations; cg_options.max_num_iterations = options_.max_num_iterations; ConjugateGradientsSolver cg_solver(cg_options); LinearSolver::PerSolveOptions cg_per_solve_options; diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc index e04c65b63be..dddcecdb196 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.cc @@ -65,7 +65,7 @@ class NonlinearConjugateGradient : public LineSearchDirection { case FLETCHER_REEVES: beta = current.gradient_squared_norm / previous.gradient_squared_norm; break; - case POLAK_RIBIRERE: + case POLAK_RIBIERE: gradient_change = current.gradient - previous.gradient; beta = (current.gradient.dot(gradient_change) / previous.gradient_squared_norm); diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc b/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc index ae77a73805c..ad28ffb137d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_minimizer.cc @@ -103,7 +103,7 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, double start_time = WallTimeInSeconds(); double iteration_start_time = start_time; - Evaluator* evaluator = CHECK_NOTNULL(options.evaluator); + Evaluator* evaluator = CHECK_NOTNULL(options.evaluator.get()); const int num_parameters = evaluator->NumParameters(); const int num_effective_parameters = evaluator->NumEffectiveParameters(); @@ -375,7 +375,6 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, WallTimeInSeconds() - start_time + summary->preprocessor_time_in_seconds; - summary->iterations.push_back(iteration_summary); ++summary->num_successful_steps; if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) { @@ -401,6 +400,8 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; break; } + + summary->iterations.push_back(iteration_summary); } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_preprocessor.cc b/extern/libmv/third_party/ceres/internal/ceres/line_search_preprocessor.cc new file mode 100644 index 00000000000..bf17dee351d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_preprocessor.cc @@ -0,0 +1,106 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/line_search_preprocessor.h" + +#include <numeric> +#include <string> +#include "ceres/evaluator.h" +#include "ceres/minimizer.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { +namespace { + +bool IsProgramValid(const Program& program, string* error) { + if (program.IsBoundsConstrained()) { + *error = "LINE_SEARCH Minimizer does not support bounds."; + return false; + } + return program.ParameterBlocksAreFinite(error); +} + +bool SetupEvaluator(PreprocessedProblem* pp) { + pp->evaluator_options = Evaluator::Options(); + // This ensures that we get a Block Jacobian Evaluator without any + // requirement on orderings. + pp->evaluator_options.linear_solver_type = CGNR; + pp->evaluator_options.num_eliminate_blocks = 0; + pp->evaluator_options.num_threads = pp->options.num_threads; + pp->evaluator.reset(Evaluator::Create(pp->evaluator_options, + pp->reduced_program.get(), + &pp->error)); + return (pp->evaluator.get() != NULL); +} + +} // namespace + +LineSearchPreprocessor::~LineSearchPreprocessor() { +} + +bool LineSearchPreprocessor::Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* pp) { + CHECK_NOTNULL(pp); + pp->options = options; + ChangeNumThreadsIfNeeded(&pp->options); + + pp->problem = problem; + Program* program = problem->mutable_program(); + if (!IsProgramValid(*program, &pp->error)) { + return false; + } + + pp->reduced_program.reset( + program->CreateReducedProgram(&pp->removed_parameter_blocks, + &pp->fixed_cost, + &pp->error)); + + if (pp->reduced_program.get() == NULL) { + return false; + } + + if (pp->reduced_program->NumParameterBlocks() == 0) { + return true; + } + + if (!SetupEvaluator(pp)) { + return false; + } + + SetupCommonMinimizerOptions(pp); + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_preprocessor.h b/extern/libmv/third_party/ceres/internal/ceres/line_search_preprocessor.h new file mode 100644 index 00000000000..54b968bc29e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_preprocessor.h @@ -0,0 +1,50 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ +#define CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ + +#include "ceres/preprocessor.h" + +namespace ceres { +namespace internal { + +class LineSearchPreprocessor : public Preprocessor { + public: + virtual ~LineSearchPreprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* preprocessed_problem); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINE_SEARCH_PREPROCESSOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc index 08c3ba110d0..e479b751363 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc @@ -45,26 +45,48 @@ namespace internal { LinearSolver::~LinearSolver() { } +LinearSolverType LinearSolver::LinearSolverForZeroEBlocks( + LinearSolverType linear_solver_type) { + if (!IsSchurType(linear_solver_type)) { + return linear_solver_type; + } + + if (linear_solver_type == SPARSE_SCHUR) { + return SPARSE_NORMAL_CHOLESKY; + } + + if (linear_solver_type == DENSE_SCHUR) { + // TODO(sameeragarwal): This is probably not a great choice. + // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can take + // a BlockSparseMatrix as input. + return DENSE_QR; + } + + if (linear_solver_type == ITERATIVE_SCHUR) { + return CGNR; + } + + return linear_solver_type; +} + LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { switch (options.type) { case CGNR: return new CgnrSolver(options); case SPARSE_NORMAL_CHOLESKY: -#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) - LOG(WARNING) << "SPARSE_NORMAL_CHOLESKY is not available. Please " - << "build Ceres with SuiteSparse or CXSparse. " - << "Returning NULL."; +#if defined(CERES_NO_SUITESPARSE) && \ + defined(CERES_NO_CXSPARSE) && \ + !defined(CERES_USE_EIGEN_SPARSE) return NULL; #else return new SparseNormalCholeskySolver(options); #endif case SPARSE_SCHUR: -#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) - LOG(WARNING) << "SPARSE_SCHUR is not available. Please " - << "build Ceres with SuiteSparse or CXSparse. " - << "Returning NULL."; +#if defined(CERES_NO_SUITESPARSE) && \ + defined(CERES_NO_CXSPARSE) && \ + !defined(CERES_USE_EIGEN_SPARSE) return NULL; #else return new SparseSchurComplementSolver(options); @@ -74,7 +96,11 @@ LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { return new DenseSchurComplementSolver(options); case ITERATIVE_SCHUR: - return new IterativeSchurComplementSolver(options); + if (options.use_explicit_schur_complement) { + return new SparseSchurComplementSolver(options); + } else { + return new IterativeSchurComplementSolver(options); + } case DENSE_QR: return new DenseQRSolver(options); diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h index f091bc5b187..5f59765f074 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h @@ -99,6 +99,7 @@ class LinearSolver { sparse_linear_algebra_library_type(SUITE_SPARSE), use_postordering(false), dynamic_sparsity(false), + use_explicit_schur_complement(false), min_num_iterations(1), max_num_iterations(1), num_threads(1), @@ -114,9 +115,10 @@ class LinearSolver { DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; - // See solver.h for information about this flag. + // See solver.h for information about these flags. bool use_postordering; bool dynamic_sparsity; + bool use_explicit_schur_complement; // Number of internal iterations that the solver uses. This // parameter only makes sense for iterative solvers like CG. @@ -274,6 +276,14 @@ class LinearSolver { string message; }; + // If the optimization problem is such that there are no remaining + // e-blocks, a Schur type linear solver cannot be used. If the + // linear solver is of Schur type, this function implements a policy + // to select an alternate nearest linear solver to the one selected + // by the user. The input linear_solver_type is returned otherwise. + static LinearSolverType LinearSolverForZeroEBlocks( + LinearSolverType linear_solver_type); + virtual ~LinearSolver(); // Solve Ax = b. diff --git a/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc index 26e7f4908a4..a4832c57443 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -36,6 +36,23 @@ namespace ceres { +LocalParameterization::~LocalParameterization() { +} + +bool LocalParameterization::MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const { + Matrix jacobian(GlobalSize(), LocalSize()); + if (!ComputeJacobian(x, jacobian.data())) { + return false; + } + + MatrixRef(local_matrix, num_rows, LocalSize()) = + ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; + return true; +} + IdentityParameterization::IdentityParameterization(const int size) : size_(size) { CHECK_GT(size, 0); @@ -55,6 +72,16 @@ bool IdentityParameterization::ComputeJacobian(const double* x, return true; } +bool IdentityParameterization::MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const { + std::copy(global_matrix, + global_matrix + num_cols * GlobalSize(), + local_matrix); + return true; +} + SubsetParameterization::SubsetParameterization( int size, const vector<int>& constant_parameters) @@ -108,6 +135,21 @@ bool SubsetParameterization::ComputeJacobian(const double* x, return true; } +bool SubsetParameterization::MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const { + for (int row = 0; row < num_rows; ++row) { + for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) { + if (!constancy_mask_[col]) { + local_matrix[row * LocalSize() + j++] = + global_matrix[row * GlobalSize() + col]; + } + } + } + return true; +} + bool QuaternionParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const { diff --git a/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc index b948f289f21..62b545be12f 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc @@ -34,13 +34,14 @@ #include <cmath> #include <cstddef> +#include <limits> namespace ceres { void TrivialLoss::Evaluate(double s, double rho[3]) const { rho[0] = s; - rho[1] = 1; - rho[2] = 0; + rho[1] = 1.0; + rho[2] = 0.0; } void HuberLoss::Evaluate(double s, double rho[3]) const { @@ -48,32 +49,32 @@ void HuberLoss::Evaluate(double s, double rho[3]) const { // Outlier region. // 'r' is always positive. const double r = sqrt(s); - rho[0] = 2 * a_ * r - b_; - rho[1] = a_ / r; - rho[2] = - rho[1] / (2 * s); + rho[0] = 2.0 * a_ * r - b_; + rho[1] = std::max(std::numeric_limits<double>::min(), a_ / r); + rho[2] = - rho[1] / (2.0 * s); } else { // Inlier region. rho[0] = s; - rho[1] = 1; - rho[2] = 0; + rho[1] = 1.0; + rho[2] = 0.0; } } void SoftLOneLoss::Evaluate(double s, double rho[3]) const { - const double sum = 1 + s * c_; + const double sum = 1.0 + s * c_; const double tmp = sqrt(sum); // 'sum' and 'tmp' are always positive, assuming that 's' is. - rho[0] = 2 * b_ * (tmp - 1); - rho[1] = 1 / tmp; - rho[2] = - (c_ * rho[1]) / (2 * sum); + rho[0] = 2.0 * b_ * (tmp - 1.0); + rho[1] = std::max(std::numeric_limits<double>::min(), 1.0 / tmp); + rho[2] = - (c_ * rho[1]) / (2.0 * sum); } void CauchyLoss::Evaluate(double s, double rho[3]) const { - const double sum = 1 + s * c_; - const double inv = 1 / sum; + const double sum = 1.0 + s * c_; + const double inv = 1.0 / sum; // 'sum' and 'inv' are always positive, assuming that 's' is. rho[0] = b_ * log(sum); - rho[1] = inv; + rho[1] = std::max(std::numeric_limits<double>::min(), inv); rho[2] = - c_ * (inv * inv); } @@ -82,8 +83,8 @@ void ArctanLoss::Evaluate(double s, double rho[3]) const { const double inv = 1 / sum; // 'sum' and 'inv' are always positive. rho[0] = a_ * atan2(s, a_); - rho[1] = inv; - rho[2] = -2 * s * b_ * (inv * inv); + rho[1] = std::max(std::numeric_limits<double>::min(), inv); + rho[2] = -2.0 * s * b_ * (inv * inv); } TolerantLoss::TolerantLoss(double a, double b) @@ -108,7 +109,7 @@ void TolerantLoss::Evaluate(double s, double rho[3]) const { } else { const double e_x = exp(x); rho[0] = b_ * log(1.0 + e_x) - c_; - rho[1] = e_x / (1.0 + e_x); + rho[1] = std::max(std::numeric_limits<double>::min(), e_x / (1.0 + e_x)); rho[2] = 0.5 / (b_ * (1.0 + cosh(x))); } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.cc b/extern/libmv/third_party/ceres/internal/ceres/minimizer.cc index 6c3b68dbbc2..558921b8441 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/minimizer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.cc @@ -28,13 +28,29 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) +#include "ceres/line_search_minimizer.h" #include "ceres/minimizer.h" +#include "ceres/trust_region_minimizer.h" #include "ceres/types.h" #include "glog/logging.h" namespace ceres { namespace internal { +Minimizer* Minimizer::Create(MinimizerType minimizer_type) { + if (minimizer_type == TRUST_REGION) { + return new TrustRegionMinimizer; + } + + if (minimizer_type == LINE_SEARCH) { + return new LineSearchMinimizer; + } + + LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type; + return NULL; +} + + Minimizer::~Minimizer() {} bool Minimizer::RunCallbacks(const Minimizer::Options& options, diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h index f1da3f704fa..dabf07e583a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h @@ -41,9 +41,10 @@ namespace ceres { namespace internal { class Evaluator; -class LinearSolver; class SparseMatrix; class TrustRegionStrategy; +class CoordinateDescentMinimizer; +class LinearSolver; // Interface for non-linear least squares solvers. class Minimizer { @@ -107,14 +108,10 @@ class Minimizer { options.line_search_sufficient_curvature_decrease; max_line_search_step_expansion = options.max_line_search_step_expansion; - is_silent = (options.logging_type == SILENT); - evaluator = NULL; - trust_region_strategy = NULL; - jacobian = NULL; - callbacks = options.callbacks; - inner_iteration_minimizer = NULL; inner_iteration_tolerance = options.inner_iteration_tolerance; + is_silent = (options.logging_type == SILENT); is_constrained = false; + callbacks = options.callbacks; } int max_num_iterations; @@ -154,10 +151,14 @@ class Minimizer { int max_num_line_search_direction_restarts; double line_search_sufficient_curvature_decrease; double max_line_search_step_expansion; + double inner_iteration_tolerance; // If true, then all logging is disabled. bool is_silent; + // Use a bounds constrained optimization algorithm. + bool is_constrained; + // List of callbacks that are executed by the Minimizer at the end // of each iteration. // @@ -165,27 +166,23 @@ class Minimizer { vector<IterationCallback*> callbacks; // Object responsible for evaluating the cost, residuals and - // Jacobian matrix. The Options struct does not own this pointer. - Evaluator* evaluator; + // Jacobian matrix. + shared_ptr<Evaluator> evaluator; // Object responsible for actually computing the trust region - // step, and sizing the trust region radius. The Options struct - // does not own this pointer. - TrustRegionStrategy* trust_region_strategy; + // step, and sizing the trust region radius. + shared_ptr<TrustRegionStrategy> trust_region_strategy; // Object holding the Jacobian matrix. It is assumed that the // sparsity structure of the matrix has already been initialized // and will remain constant for the life time of the - // optimization. The Options struct does not own this pointer. - SparseMatrix* jacobian; + // optimization. + shared_ptr<SparseMatrix> jacobian; - Minimizer* inner_iteration_minimizer; - double inner_iteration_tolerance; - - // Use a bounds constrained optimization algorithm. - bool is_constrained; + shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; }; + static Minimizer* Create(MinimizerType minimizer_type); static bool RunCallbacks(const Options& options, const IterationSummary& iteration_summary, Solver::Summary* summary); diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc index 190715bee43..1525de90d60 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.cc @@ -37,6 +37,7 @@ #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" +#include "ceres/wall_time.h" #include "glog/logging.h" namespace ceres { @@ -45,8 +46,10 @@ namespace internal { int ComputeStableSchurOrdering(const Program& program, vector<ParameterBlock*>* ordering) { CHECK_NOTNULL(ordering)->clear(); - + EventLogger event_logger("ComputeStableSchurOrdering"); scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + event_logger.AddEvent("CreateHessianGraph"); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); const HashSet<ParameterBlock*>& vertices = graph->vertices(); for (int i = 0; i < parameter_blocks.size(); ++i) { @@ -54,8 +57,10 @@ int ComputeStableSchurOrdering(const Program& program, ordering->push_back(parameter_blocks[i]); } } + event_logger.AddEvent("Preordering"); int independent_set_size = StableIndependentSetOrdering(*graph, ordering); + event_logger.AddEvent("StableIndependentSet"); // Add the excluded blocks to back of the ordering vector. for (int i = 0; i < parameter_blocks.size(); ++i) { @@ -64,6 +69,7 @@ int ComputeStableSchurOrdering(const Program& program, ordering->push_back(parameter_block); } } + event_logger.AddEvent("ConstantParameterBlocks"); return independent_set_size; } @@ -109,8 +115,7 @@ void ComputeRecursiveIndependentSetOrdering(const Program& program, } } -Graph<ParameterBlock*>* -CreateHessianGraph(const Program& program) { +Graph<ParameterBlock*>* CreateHessianGraph(const Program& program) { Graph<ParameterBlock*>* graph = CHECK_NOTNULL(new Graph<ParameterBlock*>); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); for (int i = 0; i < parameter_blocks.size(); ++i) { @@ -144,5 +149,21 @@ CreateHessianGraph(const Program& program) { return graph; } +void OrderingToGroupSizes(const ParameterBlockOrdering* ordering, + vector<int>* group_sizes) { + CHECK_NOTNULL(group_sizes)->clear(); + if (ordering == NULL) { + return; + } + + const map<int, set<double*> >& group_to_elements = + ordering->group_to_elements(); + for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + it != group_to_elements.end(); + ++it) { + group_sizes->push_back(it->second.size()); + } +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h index 4675cb8dc7c..5de99511138 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block_ordering.h @@ -78,6 +78,11 @@ void ComputeRecursiveIndependentSetOrdering(const Program& program, // parameter blocks, if they co-occur in a residual block. Graph<ParameterBlock*>* CreateHessianGraph(const Program& program); +// Iterate over each of the groups in order of their priority and fill +// summary with their sizes. +void OrderingToGroupSizes(const ParameterBlockOrdering* ordering, + vector<int>* group_sizes); + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc index 505a47d3d61..062347fccc1 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc @@ -37,6 +37,16 @@ namespace internal { Preconditioner::~Preconditioner() { } +PreconditionerType Preconditioner::PreconditionerForZeroEBlocks( + PreconditionerType preconditioner_type) { + if (preconditioner_type == SCHUR_JACOBI || + preconditioner_type == CLUSTER_JACOBI || + preconditioner_type == CLUSTER_TRIDIAGONAL) { + return JACOBI; + } + return preconditioner_type; +} + SparseMatrixPreconditionerWrapper::SparseMatrixPreconditionerWrapper( const SparseMatrix* matrix) : matrix_(CHECK_NOTNULL(matrix)) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h index 21cbc00b542..e8d5994269a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h @@ -36,6 +36,7 @@ #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/linear_operator.h" #include "ceres/sparse_matrix.h" +#include "ceres/types.h" namespace ceres { namespace internal { @@ -95,6 +96,14 @@ class Preconditioner : public LinearOperator { int f_block_size; }; + // If the optimization problem is such that there are no remaining + // e-blocks, ITERATIVE_SCHUR with a Schur type preconditioner cannot + // be used. This function returns JACOBI if a preconditioner for + // ITERATIVE_SCHUR is used. The input preconditioner_type is + // returned otherwise. + static PreconditionerType PreconditionerForZeroEBlocks( + PreconditionerType preconditioner_type); + virtual ~Preconditioner(); // Update the numerical value of the preconditioner for the linear diff --git a/extern/libmv/third_party/ceres/internal/ceres/preprocessor.cc b/extern/libmv/third_party/ceres/internal/ceres/preprocessor.cc new file mode 100644 index 00000000000..318c5e2ebef --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/preprocessor.cc @@ -0,0 +1,113 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameragarwal@google.com (Sameer Agarwal) + +#include "ceres/callbacks.h" +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/line_search_preprocessor.h" +#include "ceres/preprocessor.h" +#include "ceres/problem_impl.h" +#include "ceres/solver.h" +#include "ceres/trust_region_preprocessor.h" + +namespace ceres { +namespace internal { + +Preprocessor* Preprocessor::Create(MinimizerType minimizer_type) { + if (minimizer_type == TRUST_REGION) { + return new TrustRegionPreprocessor; + } + + if (minimizer_type == LINE_SEARCH) { + return new LineSearchPreprocessor; + } + + LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type; + return NULL; +} + +Preprocessor::~Preprocessor() { +} + +void ChangeNumThreadsIfNeeded(Solver::Options* options) { +#ifndef CERES_USE_OPENMP + if (options->num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; + options->num_threads = 1; + } + + // Only the Trust Region solver currently uses a linear solver. + if (options->minimizer_type == TRUST_REGION && + options->num_linear_solver_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_linear_solver_threads=1 is supported. Switching " + << "to single threaded mode."; + options->num_linear_solver_threads = 1; + } +#endif // CERES_USE_OPENMP +} + +void SetupCommonMinimizerOptions(PreprocessedProblem* pp) { + const Solver::Options& options = pp->options; + Program* program = pp->reduced_program.get(); + + // Assuming that the parameter blocks in the program have been + // reordered as needed, extract them into a contiguous vector. + pp->reduced_parameters.resize(program->NumParameters()); + double* reduced_parameters = pp->reduced_parameters.data(); + program->ParameterBlocksToStateVector(reduced_parameters); + + Minimizer::Options& minimizer_options = pp->minimizer_options; + minimizer_options = Minimizer::Options(options); + minimizer_options.evaluator = pp->evaluator; + + if (options.logging_type != SILENT) { + pp->logging_callback.reset( + new LoggingCallback(options.minimizer_type, + options.minimizer_progress_to_stdout)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + pp->logging_callback.get()); + } + + if (options.update_state_every_iteration) { + pp->state_updating_callback.reset( + new StateUpdatingCallback(program, reduced_parameters)); + // This must get pushed to the front of the callbacks so that it + // is run before any of the user callbacks. + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + pp->state_updating_callback.get()); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/preprocessor.h b/extern/libmv/third_party/ceres/internal/ceres/preprocessor.h new file mode 100644 index 00000000000..b4ca5b11747 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/preprocessor.h @@ -0,0 +1,119 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_PREPROCESSOR_H_ +#define CERES_INTERNAL_PREPROCESSOR_H_ + +#include "ceres/coordinate_descent_minimizer.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/iteration_callback.h" +#include "ceres/linear_solver.h" +#include "ceres/minimizer.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +struct PreprocessedProblem; + +// Given a Problem object and a Solver::Options object indicating the +// configuration of the solver, the job of the Preprocessor is to +// analyze the Problem and perform the setup needed to solve it using +// the desired Minimization algorithm. The setup involves removing +// redundancies in the input problem (inactive parameter and residual +// blocks), finding fill reducing orderings as needed, configuring and +// creating various objects needed by the Minimizer to solve the +// problem such as an evaluator, a linear solver etc. +// +// Each Minimizer (LineSearchMinimizer and TrustRegionMinimizer) comes +// with a corresponding Preprocessor (LineSearchPreprocessor and +// TrustRegionPreprocessor) that knows about its needs and performs +// the preprocessing needed. +// +// The output of the Preprocessor is stored in a PreprocessedProblem +// object. +class Preprocessor { +public: + // Factory. + static Preprocessor* Create(MinimizerType minimizer_type); + virtual ~Preprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* pp) = 0; +}; + +// A PreprocessedProblem is the result of running the Preprocessor on +// a Problem and Solver::Options object. +struct PreprocessedProblem { + PreprocessedProblem() + : fixed_cost(0.0) { + } + + string error; + Solver::Options options; + LinearSolver::Options linear_solver_options; + Evaluator::Options evaluator_options; + Minimizer::Options minimizer_options; + + ProblemImpl* problem; + scoped_ptr<ProblemImpl> gradient_checking_problem; + scoped_ptr<Program> reduced_program; + scoped_ptr<LinearSolver> linear_solver; + scoped_ptr<IterationCallback> logging_callback; + scoped_ptr<IterationCallback> state_updating_callback; + + shared_ptr<Evaluator> evaluator; + shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; + + vector<double*> removed_parameter_blocks; + Vector reduced_parameters; + double fixed_cost; +}; + +// Common functions used by various preprocessors. + +// If OpenMP support is not available and user has requested more than +// one thread, then set the *_num_threads options as needed to 1. +void ChangeNumThreadsIfNeeded(Solver::Options* options); + +// Extract the effective parameter vector from the preprocessed +// problem and setup bits of the Minimizer::Options object that are +// common to all Preprocessors. +void SetupCommonMinimizerOptions(PreprocessedProblem* pp); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PREPROCESSOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem.cc b/extern/libmv/third_party/ceres/internal/ceres/problem.cc index 674694dd506..bbfaa98769f 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/problem.cc @@ -251,6 +251,16 @@ void Problem::GetParameterBlocksForResidualBlock( parameter_blocks); } +const CostFunction* Problem::GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return problem_impl_->GetCostFunctionForResidualBlock(residual_block); +} + +const LossFunction* Problem::GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return problem_impl_->GetLossFunctionForResidualBlock(residual_block); +} + void Problem::GetResidualBlocksForParameterBlock( const double* values, vector<ResidualBlockId>* residual_blocks) const { diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc index 7c86efb4921..67cac94d6ac 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc @@ -823,6 +823,16 @@ void ProblemImpl::GetParameterBlocksForResidualBlock( } } +const CostFunction* ProblemImpl::GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return residual_block->cost_function(); +} + +const LossFunction* ProblemImpl::GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const { + return residual_block->loss_function(); +} + void ProblemImpl::GetResidualBlocksForParameterBlock( const double* values, vector<ResidualBlockId>* residual_blocks) const { diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h index 7b5547bd95a..3d84de83c5a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h @@ -157,6 +157,11 @@ class ProblemImpl { const ResidualBlockId residual_block, vector<double*>* parameter_blocks) const; + const CostFunction* GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + const LossFunction* GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + void GetResidualBlocksForParameterBlock( const double* values, vector<ResidualBlockId>* residual_blocks) const; diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.cc b/extern/libmv/third_party/ceres/internal/ceres/program.cc index 9e5c51bd696..1d0a1573e3b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/program.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/program.cc @@ -32,6 +32,7 @@ #include <map> #include <vector> +#include "ceres/array_utils.h" #include "ceres/casts.h" #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/cost_function.h" @@ -44,6 +45,7 @@ #include "ceres/problem.h" #include "ceres/residual_block.h" #include "ceres/stl_util.h" +#include "ceres/triplet_sparse_matrix.h" namespace ceres { namespace internal { @@ -170,6 +172,259 @@ bool Program::IsValid() const { return true; } +bool Program::ParameterBlocksAreFinite(string* message) const { + CHECK_NOTNULL(message); + for (int i = 0; i < parameter_blocks_.size(); ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + const double* array = parameter_block->user_state(); + const int size = parameter_block->Size(); + const int invalid_index = FindInvalidValue(size, array); + if (invalid_index != size) { + *message = StringPrintf( + "ParameterBlock: %p with size %d has at least one invalid value.\n" + "First invalid value is at index: %d.\n" + "Parameter block values: ", + array, size, invalid_index); + AppendArrayToString(size, array, message); + return false; + } + } + return true; +} + +bool Program::IsBoundsConstrained() const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (parameter_block->IsConstant()) { + continue; + } + const int size = parameter_block->Size(); + for (int j = 0; j < size; ++j) { + const double lower_bound = parameter_block->LowerBoundForParameter(j); + const double upper_bound = parameter_block->UpperBoundForParameter(j); + if (lower_bound > -std::numeric_limits<double>::max() || + upper_bound < std::numeric_limits<double>::max()) { + return true; + } + } + } + return false; +} + +bool Program::IsFeasible(string* message) const { + CHECK_NOTNULL(message); + for (int i = 0; i < parameter_blocks_.size(); ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + const double* parameters = parameter_block->user_state(); + const int size = parameter_block->Size(); + if (parameter_block->IsConstant()) { + // Constant parameter blocks must start in the feasible region + // to ultimately produce a feasible solution, since Ceres cannot + // change them. + for (int j = 0; j < size; ++j) { + const double lower_bound = parameter_block->LowerBoundForParameter(j); + const double upper_bound = parameter_block->UpperBoundForParameter(j); + if (parameters[j] < lower_bound || parameters[j] > upper_bound) { + *message = StringPrintf( + "ParameterBlock: %p with size %d has at least one infeasible " + "value." + "\nFirst infeasible value is at index: %d." + "\nLower bound: %e, value: %e, upper bound: %e" + "\nParameter block values: ", + parameters, size, j, lower_bound, parameters[j], upper_bound); + AppendArrayToString(size, parameters, message); + return false; + } + } + } else { + // Variable parameter blocks must have non-empty feasible + // regions, otherwise there is no way to produce a feasible + // solution. + for (int j = 0; j < size; ++j) { + const double lower_bound = parameter_block->LowerBoundForParameter(j); + const double upper_bound = parameter_block->UpperBoundForParameter(j); + if (lower_bound >= upper_bound) { + *message = StringPrintf( + "ParameterBlock: %p with size %d has at least one infeasible " + "bound." + "\nFirst infeasible bound is at index: %d." + "\nLower bound: %e, upper bound: %e" + "\nParameter block values: ", + parameters, size, j, lower_bound, upper_bound); + AppendArrayToString(size, parameters, message); + return false; + } + } + } + } + + return true; +} + +Program* Program::CreateReducedProgram(vector<double*>* removed_parameter_blocks, + double* fixed_cost, + string* error) const { + CHECK_NOTNULL(removed_parameter_blocks); + CHECK_NOTNULL(fixed_cost); + CHECK_NOTNULL(error); + + scoped_ptr<Program> reduced_program(new Program(*this)); + if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks, + fixed_cost, + error)) { + return NULL; + } + + reduced_program->SetParameterOffsetsAndIndex(); + return reduced_program.release(); +} + +bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, + double* fixed_cost, + string* error) { + CHECK_NOTNULL(removed_parameter_blocks); + CHECK_NOTNULL(fixed_cost); + CHECK_NOTNULL(error); + + scoped_array<double> residual_block_evaluate_scratch; + residual_block_evaluate_scratch.reset( + new double[MaxScratchDoublesNeededForEvaluate()]); + *fixed_cost = 0.0; + + // Mark all the parameters as unused. Abuse the index member of the + // parameter blocks for the marking. + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->set_index(-1); + } + + // Filter out residual that have all-constant parameters, and mark + // all the parameter blocks that appear in residuals. + int num_active_residual_blocks = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Determine if the residual block is fixed, and also mark varying + // parameters that appear in the residual block. + bool all_constant = true; + for (int k = 0; k < num_parameter_blocks; k++) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; + if (!parameter_block->IsConstant()) { + all_constant = false; + parameter_block->set_index(1); + } + } + + if (!all_constant) { + residual_blocks_[num_active_residual_blocks++] = residual_block; + continue; + } + + // The residual is constant and will be removed, so its cost is + // added to the variable fixed_cost. + double cost = 0.0; + if (!residual_block->Evaluate(true, + &cost, + NULL, + NULL, + residual_block_evaluate_scratch.get())) { + *error = StringPrintf("Evaluation of the residual %d failed during " + "removal of fixed residual blocks.", i); + return false; + } + *fixed_cost += cost; + } + residual_blocks_.resize(num_active_residual_blocks); + + // Filter out unused or fixed parameter blocks. + int num_active_parameter_blocks = 0; + removed_parameter_blocks->clear(); + for (int i = 0; i < parameter_blocks_.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks_[i]; + if (parameter_block->index() == -1) { + removed_parameter_blocks->push_back(parameter_block->mutable_user_state()); + } else { + parameter_blocks_[num_active_parameter_blocks++] = parameter_block; + } + } + parameter_blocks_.resize(num_active_parameter_blocks); + + if (!(((NumResidualBlocks() == 0) && + (NumParameterBlocks() == 0)) || + ((NumResidualBlocks() != 0) && + (NumParameterBlocks() != 0)))) { + *error = "Congratulations, you found a bug in Ceres. Please report it."; + return false; + } + + return true; +} + +bool Program::IsParameterBlockSetIndependent(const set<double*>& independent_set) const { + // Loop over each residual block and ensure that no two parameter + // blocks in the same residual block are part of + // parameter_block_ptrs as that would violate the assumption that it + // is an independent set in the Hessian matrix. + for (vector<ResidualBlock*>::const_iterator it = residual_blocks_.begin(); + it != residual_blocks_.end(); + ++it) { + ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks(); + const int num_parameter_blocks = (*it)->NumParameterBlocks(); + int count = 0; + for (int i = 0; i < num_parameter_blocks; ++i) { + count += independent_set.count( + parameter_blocks[i]->mutable_user_state()); + } + if (count > 1) { + return false; + } + } + return true; +} + +TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const { + // Matrix to store the block sparsity structure of the Jacobian. + TripletSparseMatrix* tsm = + new TripletSparseMatrix(NumParameterBlocks(), + NumResidualBlocks(), + 10 * NumResidualBlocks()); + int num_nonzeros = 0; + int* rows = tsm->mutable_rows(); + int* cols = tsm->mutable_cols(); + double* values = tsm->mutable_values(); + + for (int c = 0; c < residual_blocks_.size(); ++c) { + const ResidualBlock* residual_block = residual_blocks_[c]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + ParameterBlock* const* parameter_blocks = + residual_block->parameter_blocks(); + + for (int j = 0; j < num_parameter_blocks; ++j) { + if (parameter_blocks[j]->IsConstant()) { + continue; + } + + // Re-size the matrix if needed. + if (num_nonzeros >= tsm->max_num_nonzeros()) { + tsm->set_num_nonzeros(num_nonzeros); + tsm->Reserve(2 * num_nonzeros); + rows = tsm->mutable_rows(); + cols = tsm->mutable_cols(); + values = tsm->mutable_values(); + } + + const int r = parameter_blocks[j]->index(); + rows[num_nonzeros] = r; + cols[num_nonzeros] = c; + values[num_nonzeros] = 1.0; + ++num_nonzeros; + } + } + + tsm->set_num_nonzeros(num_nonzeros); + return tsm; +} + int Program::NumResidualBlocks() const { return residual_blocks_.size(); } diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.h b/extern/libmv/third_party/ceres/internal/ceres/program.h index 4288f609cf8..c7b22c4d244 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/program.h +++ b/extern/libmv/third_party/ceres/internal/ceres/program.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_PROGRAM_H_ #define CERES_INTERNAL_PROGRAM_H_ +#include <set> #include <string> #include <vector> #include "ceres/internal/port.h" @@ -41,6 +42,7 @@ namespace internal { class ParameterBlock; class ProblemImpl; class ResidualBlock; +class TripletSparseMatrix; // A nonlinear least squares optimization problem. This is different from the // similarly-named "Problem" object, which offers a mutation interface for @@ -103,6 +105,47 @@ class Program { // offsets) are correct. bool IsValid() const; + bool ParameterBlocksAreFinite(string* message) const; + + // Returns true if the program has any non-constant parameter blocks + // which have non-trivial bounds constraints. + bool IsBoundsConstrained() const; + + // Returns false, if the program has any constant parameter blocks + // which are not feasible, or any variable parameter blocks which + // have a lower bound greater than or equal to the upper bound. + bool IsFeasible(string* message) const; + + // Loop over each residual block and ensure that no two parameter + // blocks in the same residual block are part of + // parameter_blocks as that would violate the assumption that it + // is an independent set in the Hessian matrix. + bool IsParameterBlockSetIndependent(const set<double*>& independent_set) const; + + // Create a TripletSparseMatrix which contains the zero-one + // structure corresponding to the block sparsity of the transpose of + // the Jacobian matrix. + // + // Caller owns the result. + TripletSparseMatrix* CreateJacobianBlockSparsityTranspose() const; + + // Create a copy of this program and removes constant parameter + // blocks and residual blocks with no varying parameter blocks while + // preserving their relative order. + // + // removed_parameter_blocks on exit will contain the list of + // parameter blocks that were removed. + // + // fixed_cost will be equal to the sum of the costs of the residual + // blocks that were removed. + // + // If there was a problem, then the function will return a NULL + // pointer and error will contain a human readable description of + // the problem. + Program* CreateReducedProgram(vector<double*>* removed_parameter_blocks, + double* fixed_cost, + string* error) const; + // See problem.h for what these do. int NumParameterBlocks() const; int NumParameters() const; @@ -120,6 +163,21 @@ class Program { string ToString() const; private: + // Remove constant parameter blocks and residual blocks with no + // varying parameter blocks while preserving their relative order. + // + // removed_parameter_blocks on exit will contain the list of + // parameter blocks that were removed. + // + // fixed_cost will be equal to the sum of the costs of the residual + // blocks that were removed. + // + // If there was a problem, then the function will return false and + // error will contain a human readable description of the problem. + bool RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, + double* fixed_cost, + string* message); + // The Program does not own the ParameterBlock or ResidualBlock objects. vector<ParameterBlock*> parameter_blocks_; vector<ResidualBlock*> residual_blocks_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/reorder_program.cc b/extern/libmv/third_party/ceres/internal/ceres/reorder_program.cc new file mode 100644 index 00000000000..aa3d4cec396 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/reorder_program.cc @@ -0,0 +1,578 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/reorder_program.h" + +#include <algorithm> +#include <numeric> +#include <vector> + +#include "ceres/cxsparse.h" +#include "ceres/internal/port.h" +#include "ceres/ordered_groups.h" +#include "ceres/parameter_block.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "Eigen/SparseCore" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/OrderingMethods" +#endif + +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { + +// Find the minimum index of any parameter block to the given +// residual. Parameter blocks that have indices greater than +// size_of_first_elimination_group are considered to have an index +// equal to size_of_first_elimination_group. +static int MinParameterBlock(const ResidualBlock* residual_block, + int size_of_first_elimination_group) { + int min_parameter_block_position = size_of_first_elimination_group; + for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; + if (!parameter_block->IsConstant()) { + CHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " + << "This is a Ceres bug; please contact the developers!"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } + } + return min_parameter_block_position; +} + +#if EIGEN_VERSION_AT_LEAST(3, 2, 2) && defined(CERES_USE_EIGEN_SPARSE) +Eigen::SparseMatrix<int> CreateBlockJacobian( + const TripletSparseMatrix& block_jacobian_transpose) { + typedef Eigen::SparseMatrix<int> SparseMatrix; + typedef Eigen::Triplet<int> Triplet; + + const int* rows = block_jacobian_transpose.rows(); + const int* cols = block_jacobian_transpose.cols(); + int num_nonzeros = block_jacobian_transpose.num_nonzeros(); + std::vector<Triplet> triplets; + triplets.reserve(num_nonzeros); + for (int i = 0; i < num_nonzeros; ++i) { + triplets.push_back(Triplet(cols[i], rows[i], 1)); + } + + SparseMatrix block_jacobian(block_jacobian_transpose.num_cols(), + block_jacobian_transpose.num_rows()); + block_jacobian.setFromTriplets(triplets.begin(), triplets.end()); + return block_jacobian; +} +#endif + +void OrderingForSparseNormalCholeskyUsingSuiteSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + const vector<ParameterBlock*>& parameter_blocks, + const ParameterBlockOrdering& parameter_block_ordering, + int* ordering) { +#ifdef CERES_NO_SUITESPARSE + LOG(FATAL) << "Congratulations, you found a Ceres bug! " + << "Please report this error to the developers."; +#else + SuiteSparse ss; + cholmod_sparse* block_jacobian_transpose = + ss.CreateSparseMatrix( + const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose)); + + // No CAMD or the user did not supply a useful ordering, then just + // use regular AMD. + if (parameter_block_ordering.NumGroups() <= 1 || + !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); + } else { + vector<int> constraints; + for (int i = 0; i < parameter_blocks.size(); ++i) { + constraints.push_back( + parameter_block_ordering.GroupId( + parameter_blocks[i]->mutable_user_state())); + } + ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, + &constraints[0], + ordering); + } + + ss.Free(block_jacobian_transpose); +#endif // CERES_NO_SUITESPARSE +} + +void OrderingForSparseNormalCholeskyUsingCXSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + int* ordering) { +#ifdef CERES_NO_CXSPARSE + LOG(FATAL) << "Congratulations, you found a Ceres bug! " + << "Please report this error to the developers."; +#else // CERES_NO_CXSPARSE + // CXSparse works with J'J instead of J'. So compute the block + // sparsity for J'J and compute an approximate minimum degree + // ordering. + CXSparse cxsparse; + cs_di* block_jacobian_transpose; + block_jacobian_transpose = + cxsparse.CreateSparseMatrix( + const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose)); + cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose); + cs_di* block_hessian = + cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian); + cxsparse.Free(block_jacobian); + cxsparse.Free(block_jacobian_transpose); + + cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, ordering); + cxsparse.Free(block_hessian); +#endif // CERES_NO_CXSPARSE +} + + +#if EIGEN_VERSION_AT_LEAST(3, 2, 2) +void OrderingForSparseNormalCholeskyUsingEigenSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + int* ordering) { +#ifndef CERES_USE_EIGEN_SPARSE + LOG(FATAL) << + "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE " + "because Ceres was not built with support for " + "Eigen's SimplicialLDLT decomposition. " + "This requires enabling building with -DEIGENSPARSE=ON."; +#else + + // This conversion from a TripletSparseMatrix to a Eigen::Triplet + // matrix is unfortunate, but unavoidable for now. It is not a + // significant performance penalty in the grand scheme of + // things. The right thing to do here would be to get a compressed + // row sparse matrix representation of the jacobian and go from + // there. But that is a project for another day. + typedef Eigen::SparseMatrix<int> SparseMatrix; + + const SparseMatrix block_jacobian = + CreateBlockJacobian(tsm_block_jacobian_transpose); + const SparseMatrix block_hessian = + block_jacobian.transpose() * block_jacobian; + + Eigen::AMDOrdering<int> amd_ordering; + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm; + amd_ordering(block_hessian, perm); + for (int i = 0; i < block_hessian.rows(); ++i) { + ordering[i] = perm.indices()[i]; + } +#endif // CERES_USE_EIGEN_SPARSE +} +#endif + +} // namespace + +bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + Program* program, + string* error) { + const int num_parameter_blocks = program->NumParameterBlocks(); + if (ordering.NumElements() != num_parameter_blocks) { + *error = StringPrintf("User specified ordering does not have the same " + "number of parameters as the problem. The problem" + "has %d blocks while the ordering has %d blocks.", + num_parameter_blocks, + ordering.NumElements()); + return false; + } + + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + parameter_blocks->clear(); + + const map<int, set<double*> >& groups = + ordering.group_to_elements(); + + for (map<int, set<double*> >::const_iterator group_it = groups.begin(); + group_it != groups.end(); + ++group_it) { + const set<double*>& group = group_it->second; + for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); + parameter_block_ptr_it != group.end(); + ++parameter_block_ptr_it) { + ProblemImpl::ParameterMap::const_iterator parameter_block_it = + parameter_map.find(*parameter_block_ptr_it); + if (parameter_block_it == parameter_map.end()) { + *error = StringPrintf("User specified ordering contains a pointer " + "to a double that is not a parameter block in " + "the problem. The invalid double is in group: %d", + group_it->first); + return false; + } + parameter_blocks->push_back(parameter_block_it->second); + } + } + return true; +} + +bool LexicographicallyOrderResidualBlocks( + const int size_of_first_elimination_group, + Program* program, + string* error) { + CHECK_GE(size_of_first_elimination_group, 1) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Create a histogram of the number of residuals for each E block. There is an + // extra bucket at the end to catch all non-eliminated F blocks. + vector<int> residual_blocks_per_e_block(size_of_first_elimination_group + 1); + vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); + vector<int> min_position_per_residual(residual_blocks->size()); + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int position = MinParameterBlock(residual_block, + size_of_first_elimination_group); + min_position_per_residual[i] = position; + DCHECK_LE(position, size_of_first_elimination_group); + residual_blocks_per_e_block[position]++; + } + + // Run a cumulative sum on the histogram, to obtain offsets to the start of + // each histogram bucket (where each bucket is for the residuals for that + // E-block). + vector<int> offsets(size_of_first_elimination_group + 1); + std::partial_sum(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end(), + offsets.begin()); + CHECK_EQ(offsets.back(), residual_blocks->size()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + CHECK(find(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end() - 1, 0) != + residual_blocks_per_e_block.end()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Fill in each bucket with the residual blocks for its corresponding E block. + // Each bucket is individually filled from the back of the bucket to the front + // of the bucket. The filling order among the buckets is dictated by the + // residual blocks. This loop uses the offsets as counters; subtracting one + // from each offset as a residual block is placed in the bucket. When the + // filling is finished, the offset pointerts should have shifted down one + // entry (this is verified below). + vector<ResidualBlock*> reordered_residual_blocks( + (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + for (int i = 0; i < residual_blocks->size(); ++i) { + int bucket = min_position_per_residual[i]; + + // Decrement the cursor, which should now point at the next empty position. + offsets[bucket]--; + + // Sanity. + CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; + } + + // Sanity check #1: The difference in bucket offsets should match the + // histogram sizes. + for (int i = 0; i < size_of_first_elimination_group; ++i) { + CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + // Sanity check #2: No NULL's left behind. + for (int i = 0; i < reordered_residual_blocks.size(); ++i) { + CHECK(reordered_residual_blocks[i] != NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + + // Now that the residuals are collected by E block, swap them in place. + swap(*program->mutable_residual_blocks(), reordered_residual_blocks); + return true; +} + +// Pre-order the columns corresponding to the schur complement if +// possible. +void MaybeReorderSchurComplementColumnsUsingSuiteSparse( + const ParameterBlockOrdering& parameter_block_ordering, + Program* program) { +#ifndef CERES_NO_SUITESPARSE + SuiteSparse ss; + if (!SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + return; + } + + vector<int> constraints; + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); + + for (int i = 0; i < parameter_blocks.size(); ++i) { + constraints.push_back( + parameter_block_ordering.GroupId( + parameter_blocks[i]->mutable_user_state())); + } + + // Renumber the entries of constraints to be contiguous integers + // as camd requires that the group ids be in the range [0, + // parameter_blocks.size() - 1]. + MapValuesToContiguousRange(constraints.size(), &constraints[0]); + + // Compute a block sparse presentation of J'. + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + cholmod_sparse* block_jacobian_transpose = + ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); + + vector<int> ordering(parameter_blocks.size(), 0); + ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, + &constraints[0], + &ordering[0]); + ss.Free(block_jacobian_transpose); + + const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); + for (int i = 0; i < program->NumParameterBlocks(); ++i) { + parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; + } + + program->SetParameterOffsetsAndIndex(); +#endif +} + +void MaybeReorderSchurComplementColumnsUsingEigen( + const int size_of_first_elimination_group, + const ProblemImpl::ParameterMap& parameter_map, + Program* program) { +#if !EIGEN_VERSION_AT_LEAST(3, 2, 2) || !defined(CERES_USE_EIGEN_SPARSE) + return; +#else + + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + typedef Eigen::SparseMatrix<int> SparseMatrix; + const SparseMatrix block_jacobian = + CreateBlockJacobian(*tsm_block_jacobian_transpose); + const int num_rows = block_jacobian.rows(); + const int num_cols = block_jacobian.cols(); + + // Vertically partition the jacobian in parameter blocks of type E + // and F. + const SparseMatrix E = + block_jacobian.block(0, + 0, + num_rows, + size_of_first_elimination_group); + const SparseMatrix F = + block_jacobian.block(0, + size_of_first_elimination_group, + num_rows, + num_cols - size_of_first_elimination_group); + + // Block sparsity pattern of the schur complement. + const SparseMatrix block_schur_complement = + F.transpose() * F - F.transpose() * E * E.transpose() * F; + + Eigen::AMDOrdering<int> amd_ordering; + Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm; + amd_ordering(block_schur_complement, perm); + + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + vector<ParameterBlock*> ordering(num_cols); + + // The ordering of the first size_of_first_elimination_group does + // not matter, so we preserve the existing ordering. + for (int i = 0; i < size_of_first_elimination_group; ++i) { + ordering[i] = parameter_blocks[i]; + } + + // For the rest of the blocks, use the ordering computed using AMD. + for (int i = 0; i < block_schur_complement.cols(); ++i) { + ordering[size_of_first_elimination_group + i] = + parameter_blocks[size_of_first_elimination_group + perm.indices()[i]]; + } + + swap(*program->mutable_parameter_blocks(), ordering); + program->SetParameterOffsetsAndIndex(); +#endif +} + +bool ReorderProgramForSchurTypeLinearSolver( + const LinearSolverType linear_solver_type, + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ProblemImpl::ParameterMap& parameter_map, + ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error) { + if (parameter_block_ordering->NumElements() != + program->NumParameterBlocks()) { + *error = StringPrintf( + "The program has %d parameter blocks, but the parameter block " + "ordering has %d parameter blocks.", + program->NumParameterBlocks(), + parameter_block_ordering->NumElements()); + return false; + } + + if (parameter_block_ordering->NumGroups() == 1) { + // If the user supplied an parameter_block_ordering with just one + // group, it is equivalent to the user supplying NULL as an + // parameter_block_ordering. Ceres is completely free to choose the + // parameter block ordering as it sees fit. For Schur type solvers, + // this means that the user wishes for Ceres to identify the + // e_blocks, which we do by computing a maximal independent set. + vector<ParameterBlock*> schur_ordering; + const int size_of_first_elimination_group = + ComputeStableSchurOrdering(*program, &schur_ordering); + + CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Update the parameter_block_ordering object. + for (int i = 0; i < schur_ordering.size(); ++i) { + double* parameter_block = schur_ordering[i]->mutable_user_state(); + const int group_id = (i < size_of_first_elimination_group) ? 0 : 1; + parameter_block_ordering->AddElementToGroup(parameter_block, group_id); + } + + // We could call ApplyOrdering but this is cheaper and + // simpler. + swap(*program->mutable_parameter_blocks(), schur_ordering); + } else { + // The user provided an ordering with more than one elimination + // group. + + // Verify that the first elimination group is an independent set. + const set<double*>& first_elimination_group = + parameter_block_ordering + ->group_to_elements() + .begin() + ->second; + if (!program->IsParameterBlockSetIndependent(first_elimination_group)) { + *error = + StringPrintf("The first elimination group in the parameter block " + "ordering of size %zd is not an independent set", + first_elimination_group.size()); + return false; + } + + if (!ApplyOrdering(parameter_map, + *parameter_block_ordering, + program, + error)) { + return false; + } + } + + program->SetParameterOffsetsAndIndex(); + + const int size_of_first_elimination_group = + parameter_block_ordering->group_to_elements().begin()->second.size(); + + if (linear_solver_type == SPARSE_SCHUR) { + if (sparse_linear_algebra_library_type == SUITE_SPARSE) { + MaybeReorderSchurComplementColumnsUsingSuiteSparse( + *parameter_block_ordering, + program); + } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) { + MaybeReorderSchurComplementColumnsUsingEigen( + size_of_first_elimination_group, + parameter_map, + program); + } + } + + // Schur type solvers also require that their residual blocks be + // lexicographically ordered. + if (!LexicographicallyOrderResidualBlocks(size_of_first_elimination_group, + program, + error)) { + return false; + } + + return true; +} + +bool ReorderProgramForSparseNormalCholesky( + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering& parameter_block_ordering, + Program* program, + string* error) { + // Compute a block sparse presentation of J'. + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + vector<int> ordering(program->NumParameterBlocks(), 0); + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); + + if (sparse_linear_algebra_library_type == SUITE_SPARSE) { + OrderingForSparseNormalCholeskyUsingSuiteSparse( + *tsm_block_jacobian_transpose, + parameter_blocks, + parameter_block_ordering, + &ordering[0]); + } else if (sparse_linear_algebra_library_type == CX_SPARSE) { + OrderingForSparseNormalCholeskyUsingCXSparse( + *tsm_block_jacobian_transpose, + &ordering[0]); + } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) { +#if EIGEN_VERSION_AT_LEAST(3, 2, 2) + OrderingForSparseNormalCholeskyUsingEigenSparse( + *tsm_block_jacobian_transpose, + &ordering[0]); +#else + // For Eigen versions less than 3.2.2, there is nothing to do as + // older versions of Eigen do not expose a method for doing + // symbolic analysis on pre-ordered matrices, so a block + // pre-ordering is a bit pointless. + + return true; +#endif + } + + // Apply ordering. + const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); + for (int i = 0; i < program->NumParameterBlocks(); ++i) { + parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; + } + + program->SetParameterOffsetsAndIndex(); + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/reorder_program.h b/extern/libmv/third_party/ceres/internal/ceres/reorder_program.h new file mode 100644 index 00000000000..0474c1fb912 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/reorder_program.h @@ -0,0 +1,101 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_REORDER_PROGRAM_H_ +#define CERES_INTERNAL_REORDER_PROGRAM_H_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem_impl.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; + +// Reorder the parameter blocks in program using the ordering +bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + Program* program, + string* error); + +// Reorder the residuals for program, if necessary, so that the residuals +// involving each E block occur together. This is a necessary condition for the +// Schur eliminator, which works on these "row blocks" in the jacobian. +bool LexicographicallyOrderResidualBlocks(int size_of_first_elimination_group, + Program* program, + string* error); + +// Schur type solvers require that all parameter blocks eliminated +// by the Schur eliminator occur before others and the residuals be +// sorted in lexicographic order of their parameter blocks. +// +// If the parameter_block_ordering only contains one elimination +// group then a maximal independent set is computed and used as the +// first elimination group, otherwise the user's ordering is used. +// +// If the linear solver type is SPARSE_SCHUR and support for +// constrained fill-reducing ordering is available in the sparse +// linear algebra library (SuiteSparse version >= 4.2.0) then +// columns of the schur complement matrix are ordered to reduce the +// fill-in the Cholesky factorization. +// +// Upon return, ordering contains the parameter block ordering that +// was used to order the program. +bool ReorderProgramForSchurTypeLinearSolver( + LinearSolverType linear_solver_type, + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ProblemImpl::ParameterMap& parameter_map, + ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error); + +// Sparse cholesky factorization routines when doing the sparse +// cholesky factorization of the Jacobian matrix, reorders its +// columns to reduce the fill-in. Compute this permutation and +// re-order the parameter blocks. +// +// When using SuiteSparse, if the parameter_block_ordering contains +// more than one elimination group and support for constrained +// fill-reducing ordering is available in the sparse linear algebra +// library (SuiteSparse version >= 4.2.0) then the fill reducing +// ordering will take it into account, otherwise it will be ignored. +bool ReorderProgramForSparseNormalCholesky( + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering& parameter_block_ordering, + Program* program, + string* error); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_REORDER_PROGRAM_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc index d3eef5d4328..33f812b8c96 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -28,21 +28,22 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) +#include "ceres/internal/port.h" + #include <algorithm> #include <ctime> #include <set> #include <vector> -#include "Eigen/Dense" #include "ceres/block_random_access_dense_matrix.h" #include "ceres/block_random_access_matrix.h" #include "ceres/block_random_access_sparse_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" +#include "ceres/conjugate_gradients_solver.h" #include "ceres/cxsparse.h" #include "ceres/detect_structure.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/lapack.h" #include "ceres/linear_solver.h" @@ -51,9 +52,66 @@ #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "ceres/wall_time.h" +#include "Eigen/Dense" +#include "Eigen/SparseCore" namespace ceres { namespace internal { +namespace { + +class BlockRandomAccessSparseMatrixAdapter : public LinearOperator { + public: + explicit BlockRandomAccessSparseMatrixAdapter( + const BlockRandomAccessSparseMatrix& m) + : m_(m) { + } + + virtual ~BlockRandomAccessSparseMatrixAdapter() {} + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const { + m_.SymmetricRightMultiply(x, y); + } + + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const { + m_.SymmetricRightMultiply(x, y); + } + + virtual int num_rows() const { return m_.num_rows(); } + virtual int num_cols() const { return m_.num_rows(); } + + private: + const BlockRandomAccessSparseMatrix& m_; +}; + +class BlockRandomAccessDiagonalMatrixAdapter : public LinearOperator { + public: + explicit BlockRandomAccessDiagonalMatrixAdapter( + const BlockRandomAccessDiagonalMatrix& m) + : m_(m) { + } + + virtual ~BlockRandomAccessDiagonalMatrixAdapter() {} + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const { + m_.RightMultiply(x, y); + } + + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const { + m_.RightMultiply(x, y); + } + + virtual int num_rows() const { return m_.num_rows(); } + virtual int num_cols() const { return m_.num_rows(); } + + private: + const BlockRandomAccessDiagonalMatrix& m_; +}; + +} // namespace LinearSolver::Summary SchurComplementSolver::SolveImpl( BlockSparseMatrix* A, @@ -80,7 +138,7 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( double* reduced_solution = x + A->num_cols() - lhs_->num_cols(); const LinearSolver::Summary summary = - SolveReducedLinearSystem(reduced_solution); + SolveReducedLinearSystem(per_solve_options, reduced_solution); event_logger.AddEvent("ReducedSolve"); if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { @@ -113,7 +171,9 @@ void DenseSchurComplementSolver::InitStorage( // BlockRandomAccessDenseMatrix. The linear system is solved using // Eigen's Cholesky factorization. LinearSolver::Summary -DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { +DenseSchurComplementSolver::SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { LinearSolver::Summary summary; summary.num_iterations = 0; summary.termination_type = LINEAR_SOLVER_SUCCESS; @@ -138,7 +198,8 @@ DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { .llt(); if (llt.info() != Eigen::Success) { summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "Eigen LLT decomposition failed."; + summary.message = + "Eigen failure. Unable to perform dense Cholesky factorization."; return summary; } @@ -155,8 +216,6 @@ DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { return summary; } -#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE) - SparseSchurComplementSolver::SparseSchurComplementSolver( const LinearSolver::Options& options) : SchurComplementSolver(options), @@ -165,19 +224,15 @@ SparseSchurComplementSolver::SparseSchurComplementSolver( } SparseSchurComplementSolver::~SparseSchurComplementSolver() { -#ifndef CERES_NO_SUITESPARSE if (factor_ != NULL) { ss_.Free(factor_); factor_ = NULL; } -#endif // CERES_NO_SUITESPARSE -#ifndef CERES_NO_CXSPARSE if (cxsparse_factor_ != NULL) { cxsparse_.Free(cxsparse_factor_); cxsparse_factor_ = NULL; } -#endif // CERES_NO_CXSPARSE } // Determine the non-zero blocks in the Schur Complement matrix, and @@ -252,12 +307,25 @@ void SparseSchurComplementSolver::InitStorage( } LinearSolver::Summary -SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { +SparseSchurComplementSolver::SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { + if (options().type == ITERATIVE_SCHUR) { + CHECK(options().use_explicit_schur_complement); + return SolveReducedLinearSystemUsingConjugateGradients(per_solve_options, + solution); + } + switch (options().sparse_linear_algebra_library_type) { case SUITE_SPARSE: - return SolveReducedLinearSystemUsingSuiteSparse(solution); + return SolveReducedLinearSystemUsingSuiteSparse(per_solve_options, + solution); case CX_SPARSE: - return SolveReducedLinearSystemUsingCXSparse(solution); + return SolveReducedLinearSystemUsingCXSparse(per_solve_options, + solution); + case EIGEN_SPARSE: + return SolveReducedLinearSystemUsingEigen(per_solve_options, + solution); default: LOG(FATAL) << "Unknown sparse linear algebra library : " << options().sparse_linear_algebra_library_type; @@ -266,13 +334,24 @@ SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { return LinearSolver::Summary(); } -#ifndef CERES_NO_SUITESPARSE // Solve the system Sx = r, assuming that the matrix S is stored in a // BlockRandomAccessSparseMatrix. The linear system is solved using // CHOLMOD's sparse cholesky factorization routines. LinearSolver::Summary SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution) { +#ifdef CERES_NO_SUITESPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = "Ceres was not built with SuiteSparse support. " + "Therefore, SPARSE_SCHUR cannot be used with SUITE_SPARSE"; + return summary; + +#else + LinearSolver::Summary summary; summary.num_iterations = 0; summary.termination_type = LINEAR_SOLVER_SUCCESS; @@ -326,6 +405,8 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( if (factor_ == NULL) { ss_.Free(cholmod_lhs); summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + // No need to set message as it has already been set by the + // symbolic analysis routines above. return summary; } @@ -335,6 +416,8 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( ss_.Free(cholmod_lhs); if (summary.termination_type != LINEAR_SOLVER_SUCCESS) { + // No need to set message as it has already been set by the + // numeric factorization routine above. return summary; } @@ -346,6 +429,8 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( ss_.Free(cholmod_rhs); if (cholmod_solution == NULL) { + summary.message = + "SuiteSparse failure. Unable to perform triangular solve."; summary.termination_type = LINEAR_SOLVER_FAILURE; return summary; } @@ -354,23 +439,27 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); ss_.Free(cholmod_solution); return summary; -} -#else -LinearSolver::Summary -SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( - double* solution) { - LOG(FATAL) << "No SuiteSparse support in Ceres."; - return LinearSolver::Summary(); -} #endif // CERES_NO_SUITESPARSE +} -#ifndef CERES_NO_CXSPARSE // Solve the system Sx = r, assuming that the matrix S is stored in a // BlockRandomAccessSparseMatrix. The linear system is solved using // CXSparse's sparse cholesky factorization routines. LinearSolver::Summary SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution) { +#ifdef CERES_NO_CXSPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = "Ceres was not built with CXSparse support. " + "Therefore, SPARSE_SCHUR cannot be used with CX_SPARSE"; + return summary; + +#else + LinearSolver::Summary summary; summary.num_iterations = 0; summary.termination_type = LINEAR_SOLVER_SUCCESS; @@ -407,16 +496,169 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( cxsparse_.Free(lhs); return summary; +#endif // CERES_NO_CXPARSE } + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// Eigen's sparse cholesky factorization routines. +LinearSolver::Summary +SparseSchurComplementSolver::SolveReducedLinearSystemUsingEigen( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution) { +#ifndef CERES_USE_EIGEN_SPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_SCHUR cannot be used with EIGEN_SPARSE. " + "Ceres was not built with support for " + "Eigen's SimplicialLDLT decomposition. " + "This requires enabling building with -DEIGENSPARSE=ON."; + return summary; + #else + EventLogger event_logger("SchurComplementSolver::EigenSolve"); + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + // Extract the TripletSparseMatrix that is used for actually storing S. + TripletSparseMatrix* tsm = + const_cast<TripletSparseMatrix*>( + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); + const int num_rows = tsm->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return summary; + } + + // This is an upper triangular matrix. + CompressedRowSparseMatrix crsm(*tsm); + // Map this to a column major, lower triangular matrix. + Eigen::MappedSparseMatrix<double, Eigen::ColMajor> eigen_lhs( + crsm.num_rows(), + crsm.num_rows(), + crsm.num_nonzeros(), + crsm.mutable_rows(), + crsm.mutable_cols(), + crsm.mutable_values()); + event_logger.AddEvent("ToCompressedRowSparseMatrix"); + + // Compute symbolic factorization if one does not exist. + if (simplicial_ldlt_.get() == NULL) { + simplicial_ldlt_.reset(new SimplicialLDLT); + // This ordering is quite bad. The scalar ordering produced by the + // AMD algorithm is quite bad and can be an order of magnitude + // worse than the one computed using the block version of the + // algorithm. + simplicial_ldlt_->analyzePattern(eigen_lhs); + event_logger.AddEvent("Analysis"); + if (simplicial_ldlt_->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "Eigen failure. Unable to find symbolic factorization."; + return summary; + } + } + + simplicial_ldlt_->factorize(eigen_lhs); + event_logger.AddEvent("Factorize"); + if (simplicial_ldlt_->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to find numeric factoriztion."; + return summary; + } + + VectorRef(solution, num_rows) = + simplicial_ldlt_->solve(ConstVectorRef(rhs(), num_rows)); + event_logger.AddEvent("Solve"); + if (simplicial_ldlt_->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to do triangular solve."; + } + + return summary; +#endif // CERES_USE_EIGEN_SPARSE +} + LinearSolver::Summary -SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( +SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution) { - LOG(FATAL) << "No CXSparse support in Ceres."; - return LinearSolver::Summary(); + const int num_rows = lhs()->num_rows(); + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + return summary; + } + + // Only SCHUR_JACOBI is supported over here right now. + CHECK_EQ(options().preconditioner_type, SCHUR_JACOBI); + + if (preconditioner_.get() == NULL) { + preconditioner_.reset(new BlockRandomAccessDiagonalMatrix(blocks_)); + } + + BlockRandomAccessSparseMatrix* sc = + down_cast<BlockRandomAccessSparseMatrix*>( + const_cast<BlockRandomAccessMatrix*>(lhs())); + + // Extract block diagonal from the Schur complement to construct the + // schur_jacobi preconditioner. + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + + int sc_r, sc_c, sc_row_stride, sc_col_stride; + CellInfo* sc_cell_info = + CHECK_NOTNULL(sc->GetCell(i, i, + &sc_r, &sc_c, + &sc_row_stride, &sc_col_stride)); + MatrixRef sc_m(sc_cell_info->values, sc_row_stride, sc_col_stride); + + int pre_r, pre_c, pre_row_stride, pre_col_stride; + CellInfo* pre_cell_info = CHECK_NOTNULL( + preconditioner_->GetCell(i, i, + &pre_r, &pre_c, + &pre_row_stride, &pre_col_stride)); + MatrixRef pre_m(pre_cell_info->values, pre_row_stride, pre_col_stride); + + pre_m.block(pre_r, pre_c, block_size, block_size) = + sc_m.block(sc_r, sc_c, block_size, block_size); + } + preconditioner_->Invert(); + + VectorRef(solution, num_rows).setZero(); + + scoped_ptr<LinearOperator> lhs_adapter( + new BlockRandomAccessSparseMatrixAdapter(*sc)); + scoped_ptr<LinearOperator> preconditioner_adapter( + new BlockRandomAccessDiagonalMatrixAdapter(*preconditioner_)); + + + LinearSolver::Options cg_options; + cg_options.min_num_iterations = options().min_num_iterations; + cg_options.max_num_iterations = options().max_num_iterations; + ConjugateGradientsSolver cg_solver(cg_options); + + LinearSolver::PerSolveOptions cg_per_solve_options; + cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance; + cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; + cg_per_solve_options.preconditioner = preconditioner_adapter.get(); + + return cg_solver.Solve(lhs_adapter.get(), + rhs(), + cg_per_solve_options, + solution); } -#endif // CERES_NO_CXPARSE -#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE) } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h index a9978518b17..93d23e3d012 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h @@ -35,6 +35,8 @@ #include <utility> #include <vector> +#include "ceres/internal/port.h" + #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" @@ -44,6 +46,12 @@ #include "ceres/suitesparse.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" +#include "ceres/block_random_access_diagonal_matrix.h" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/SparseCholesky" +#include "Eigen/OrderingMethods" +#endif namespace ceres { namespace internal { @@ -127,6 +135,7 @@ class SchurComplementSolver : public BlockSparseMatrixSolver { private: virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0; virtual LinearSolver::Summary SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution) = 0; LinearSolver::Options options_; @@ -148,12 +157,12 @@ class DenseSchurComplementSolver : public SchurComplementSolver { private: virtual void InitStorage(const CompressedRowBlockStructure* bs); virtual LinearSolver::Summary SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution); CERES_DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); }; -#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE) // Sparse Cholesky factorization based solver. class SparseSchurComplementSolver : public SchurComplementSolver { public: @@ -163,10 +172,19 @@ class SparseSchurComplementSolver : public SchurComplementSolver { private: virtual void InitStorage(const CompressedRowBlockStructure* bs); virtual LinearSolver::Summary SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution); LinearSolver::Summary SolveReducedLinearSystemUsingSuiteSparse( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution); LinearSolver::Summary SolveReducedLinearSystemUsingCXSparse( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + LinearSolver::Summary SolveReducedLinearSystemUsingEigen( + const LinearSolver::PerSolveOptions& per_solve_options, + double* solution); + LinearSolver::Summary SolveReducedLinearSystemUsingConjugateGradients( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution); // Size of the blocks in the Schur complement. @@ -180,10 +198,28 @@ class SparseSchurComplementSolver : public SchurComplementSolver { CXSparse cxsparse_; // Cached factorization cs_dis* cxsparse_factor_; + +#ifdef CERES_USE_EIGEN_SPARSE + + // The preprocessor gymnastics here are dealing with the fact that + // before version 3.2.2, Eigen did not support a third template + // parameter to specify the ordering. +#if EIGEN_VERSION_AT_LEAST(3,2,2) + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Lower, + Eigen::NaturalOrdering<int> > + SimplicialLDLT; +#else + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Lower> + SimplicialLDLT; +#endif + + scoped_ptr<SimplicialLDLT> simplicial_ldlt_; +#endif + + scoped_ptr<BlockRandomAccessDiagonalMatrix> preconditioner_; CERES_DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); }; -#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE) } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc index 6dc9e89d3cc..cbdb7086102 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.cc @@ -32,7 +32,6 @@ #include <utility> #include <vector> -#include "Eigen/Dense" #include "ceres/block_random_access_diagonal_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/collections_port.h" @@ -55,12 +54,12 @@ SchurJacobiPreconditioner::SchurJacobiPreconditioner( << "Jacobian should have atleast 1 f_block for " << "SCHUR_JACOBI preconditioner."; - block_size_.resize(num_blocks); + vector<int> blocks(num_blocks); for (int i = 0; i < num_blocks; ++i) { - block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size; + blocks[i] = bs.cols[i + options_.elimination_groups[0]].size; } - m_.reset(new BlockRandomAccessDiagonalMatrix(block_size_)); + m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); InitEliminator(bs); } @@ -99,32 +98,13 @@ bool SchurJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, // Compute a subset of the entries of the Schur complement. eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); + m_->Invert(); return true; } void SchurJacobiPreconditioner::RightMultiply(const double* x, double* y) const { - CHECK_NOTNULL(x); - CHECK_NOTNULL(y); - - const double* lhs_values = - down_cast<BlockRandomAccessDiagonalMatrix*>(m_.get())->matrix()->values(); - - // This loop can be easily multi-threaded with OpenMP if need be. - for (int i = 0; i < block_size_.size(); ++i) { - const int block_size = block_size_[i]; - ConstMatrixRef block(lhs_values, block_size, block_size); - - VectorRef(y, block_size) = - block - .selfadjointView<Eigen::Upper>() - .llt() - .solve(ConstVectorRef(x, block_size)); - - x += block_size; - y += block_size; - lhs_values += block_size * block_size; - } + m_->RightMultiply(x, y); } int SchurJacobiPreconditioner::num_rows() const { diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h index aecb0151083..8b528e25075 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_jacobi_preconditioner.h @@ -94,11 +94,7 @@ class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); Preconditioner::Options options_; - - // Sizes of the blocks in the schur complement. - vector<int> block_size_; scoped_ptr<SchurEliminatorBase> eliminator_; - // Preconditioner matrix. scoped_ptr<BlockRandomAccessDiagonalMatrix> m_; CERES_DISALLOW_COPY_AND_ASSIGN(SchurJacobiPreconditioner); diff --git a/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.cc b/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.cc index 0a8b20cfe29..f0f7e0e1e06 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.cc @@ -44,7 +44,7 @@ namespace internal { int ComputeSingleLinkageClustering( const SingleLinkageClusteringOptions& options, - const Graph<int>& graph, + const WeightedGraph<int>& graph, HashMap<int, int>* membership) { CHECK_NOTNULL(membership)->clear(); diff --git a/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.h b/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.h index e6fdeabea61..79c4da114c2 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.h +++ b/extern/libmv/third_party/ceres/internal/ceres/single_linkage_clustering.h @@ -64,7 +64,7 @@ struct SingleLinkageClusteringOptions { // identified by the algorithm. int ComputeSingleLinkageClustering( const SingleLinkageClusteringOptions& options, - const Graph<int>& graph, + const WeightedGraph<int>& graph, HashMap<int, int>* membership); } // namespace internal diff --git a/extern/libmv/third_party/ceres/internal/ceres/small_blas.h b/extern/libmv/third_party/ceres/internal/ceres/small_blas.h index 26228e49306..5639664b925 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/small_blas.h +++ b/extern/libmv/third_party/ceres/internal/ceres/small_blas.h @@ -42,30 +42,6 @@ namespace ceres { namespace internal { -// Remove the ".noalias()" annotation from the matrix matrix -// mutliplies to produce a correct build with the Android NDK, -// including versions 6, 7, 8, and 8b, when built with STLPort and the -// non-standalone toolchain (i.e. ndk-build). This appears to be a -// compiler bug; if the workaround is not in place, the line -// -// block.noalias() -= A * B; -// -// gets compiled to -// -// block.noalias() += A * B; -// -// which breaks schur elimination. Introducing a temporary by removing the -// .noalias() annotation causes the issue to disappear. Tracking this -// issue down was tricky, since the test suite doesn't run when built with -// the non-standalone toolchain. -// -// TODO(keir): Make a reproduction case for this and send it upstream. -#ifdef CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG -#define CERES_MAYBE_NOALIAS -#else -#define CERES_MAYBE_NOALIAS .noalias() -#endif - // The following three macros are used to share code and reduce // template junk across the various GEMM variants. #define CERES_GEMM_BEGIN(name) \ @@ -168,11 +144,11 @@ CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen) { block(Cref, start_row_c, start_col_c, num_row_a, num_col_b); if (kOperation > 0) { - block CERES_MAYBE_NOALIAS += Aref * Bref; + block.noalias() += Aref * Bref; } else if (kOperation < 0) { - block CERES_MAYBE_NOALIAS -= Aref * Bref; + block.noalias() -= Aref * Bref; } else { - block CERES_MAYBE_NOALIAS = Aref * Bref; + block.noalias() = Aref * Bref; } } @@ -228,11 +204,11 @@ CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen) { start_row_c, start_col_c, num_col_a, num_col_b); if (kOperation > 0) { - block CERES_MAYBE_NOALIAS += Aref.transpose() * Bref; + block.noalias() += Aref.transpose() * Bref; } else if (kOperation < 0) { - block CERES_MAYBE_NOALIAS -= Aref.transpose() * Bref; + block.noalias() -= Aref.transpose() * Bref; } else { - block CERES_MAYBE_NOALIAS = Aref.transpose() * Bref; + block.noalias() = Aref.transpose() * Bref; } } @@ -394,8 +370,6 @@ inline void MatrixTransposeVectorMultiply(const double* A, #endif // CERES_NO_CUSTOM_BLAS } - -#undef CERES_MAYBE_NOALIAS #undef CERES_GEMM_BEGIN #undef CERES_GEMM_EIGEN_HEADER #undef CERES_GEMM_NAIVE_HEADER diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver.cc b/extern/libmv/third_party/ceres/internal/ceres/solver.cc index 53a9b4b7220..3512e156f9e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/solver.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -31,17 +31,282 @@ #include "ceres/solver.h" +#include <algorithm> +#include <sstream> // NOLINT #include <vector> +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/internal/port.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/preprocessor.h" #include "ceres/problem.h" #include "ceres/problem_impl.h" #include "ceres/program.h" -#include "ceres/solver_impl.h" +#include "ceres/solver_utils.h" #include "ceres/stringprintf.h" +#include "ceres/types.h" #include "ceres/wall_time.h" namespace ceres { namespace { +#define OPTION_OP(x, y, OP) \ + if (!(options.x OP y)) { \ + std::stringstream ss; \ + ss << "Invalid configuration. "; \ + ss << string("Solver::Options::" #x " = ") << options.x << ". "; \ + ss << "Violated constraint: "; \ + ss << string("Solver::Options::" #x " " #OP " "#y); \ + *error = ss.str(); \ + return false; \ + } + +#define OPTION_OP_OPTION(x, y, OP) \ + if (!(options.x OP options.y)) { \ + std::stringstream ss; \ + ss << "Invalid configuration. "; \ + ss << string("Solver::Options::" #x " = ") << options.x << ". "; \ + ss << string("Solver::Options::" #y " = ") << options.y << ". "; \ + ss << "Violated constraint: "; \ + ss << string("Solver::Options::" #x); \ + ss << string(#OP " Solver::Options::" #y "."); \ + *error = ss.str(); \ + return false; \ + } + +#define OPTION_GE(x, y) OPTION_OP(x, y, >=); +#define OPTION_GT(x, y) OPTION_OP(x, y, >); +#define OPTION_LE(x, y) OPTION_OP(x, y, <=); +#define OPTION_LT(x, y) OPTION_OP(x, y, <); +#define OPTION_LE_OPTION(x, y) OPTION_OP_OPTION(x, y, <=) +#define OPTION_LT_OPTION(x, y) OPTION_OP_OPTION(x, y, <) + +bool CommonOptionsAreValid(const Solver::Options& options, string* error) { + OPTION_GE(max_num_iterations, 0); + OPTION_GE(max_solver_time_in_seconds, 0.0); + OPTION_GE(function_tolerance, 0.0); + OPTION_GE(gradient_tolerance, 0.0); + OPTION_GE(parameter_tolerance, 0.0); + OPTION_GT(num_threads, 0); + OPTION_GT(num_linear_solver_threads, 0); + if (options.check_gradients) { + OPTION_GT(gradient_check_relative_precision, 0.0); + OPTION_GT(numeric_derivative_relative_step_size, 0.0); + } + return true; +} + +bool TrustRegionOptionsAreValid(const Solver::Options& options, string* error) { + OPTION_GT(initial_trust_region_radius, 0.0); + OPTION_GT(min_trust_region_radius, 0.0); + OPTION_GT(max_trust_region_radius, 0.0); + OPTION_LE_OPTION(min_trust_region_radius, max_trust_region_radius); + OPTION_LE_OPTION(min_trust_region_radius, initial_trust_region_radius); + OPTION_LE_OPTION(initial_trust_region_radius, max_trust_region_radius); + OPTION_GE(min_relative_decrease, 0.0); + OPTION_GE(min_lm_diagonal, 0.0); + OPTION_GE(max_lm_diagonal, 0.0); + OPTION_LE_OPTION(min_lm_diagonal, max_lm_diagonal); + OPTION_GE(max_num_consecutive_invalid_steps, 0); + OPTION_GT(eta, 0.0); + OPTION_GE(min_linear_solver_iterations, 0); + OPTION_GE(max_linear_solver_iterations, 1); + OPTION_LE_OPTION(min_linear_solver_iterations, max_linear_solver_iterations); + + if (options.use_inner_iterations) { + OPTION_GE(inner_iteration_tolerance, 0.0); + } + + if (options.use_nonmonotonic_steps) { + OPTION_GT(max_consecutive_nonmonotonic_steps, 0); + } + + if (options.linear_solver_type == ITERATIVE_SCHUR && + options.use_explicit_schur_complement && + options.preconditioner_type != SCHUR_JACOBI) { + *error = "use_explicit_schur_complement only supports " + "SCHUR_JACOBI as the preconditioner."; + return false; + } + + if (options.preconditioner_type == CLUSTER_JACOBI && + options.sparse_linear_algebra_library_type != SUITE_SPARSE) { + *error = "CLUSTER_JACOBI requires " + "Solver::Options::sparse_linear_algebra_library_type to be " + "SUITE_SPARSE"; + return false; + } + + if (options.preconditioner_type == CLUSTER_TRIDIAGONAL && + options.sparse_linear_algebra_library_type != SUITE_SPARSE) { + *error = "CLUSTER_TRIDIAGONAL requires " + "Solver::Options::sparse_linear_algebra_library_type to be " + "SUITE_SPARSE"; + return false; + } + +#ifdef CERES_NO_LAPACK + if (options.dense_linear_algebra_library_type == LAPACK) { + if (options.linear_solver_type == DENSE_NORMAL_CHOLESKY) { + *error = "Can't use DENSE_NORMAL_CHOLESKY with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return false; + } + + if (options.linear_solver_type == DENSE_QR) { + *error = "Can't use DENSE_QR with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return false; + } + + if (options.linear_solver_type == DENSE_SCHUR) { + *error = "Can't use DENSE_SCHUR with LAPACK because " + "LAPACK was not enabled when Ceres was built."; + return false; + } + } +#endif + +#ifdef CERES_NO_SUITESPARSE + if (options.sparse_linear_algebra_library_type == SUITE_SPARSE) { + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } + + if (options.linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR with SUITESPARSE because " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } + + if (options.preconditioner_type == CLUSTER_JACOBI) { + *error = "CLUSTER_JACOBI preconditioner not supported. " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } + + if (options.preconditioner_type == CLUSTER_TRIDIAGONAL) { + *error = "CLUSTER_TRIDIAGONAL preconditioner not supported. " + "SuiteSparse was not enabled when Ceres was built."; + return false; + } + } +#endif + +#ifdef CERES_NO_CXSPARSE + if (options.sparse_linear_algebra_library_type == CX_SPARSE) { + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because " + "CXSparse was not enabled when Ceres was built."; + return false; + } + + if (options.linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR with CX_SPARSE because " + "CXSparse was not enabled when Ceres was built."; + return false; + } + } +#endif + +#ifndef CERES_USE_EIGEN_SPARSE + if (options.sparse_linear_algebra_library_type == EIGEN_SPARSE) { + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with EIGEN_SPARSE because " + "Eigen's sparse linear algebra was not enabled when Ceres was " + "built."; + return false; + } + + if (options.linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR with EIGEN_SPARSE because " + "Eigen's sparse linear algebra was not enabled when Ceres was " + "built."; + return false; + } + } +#endif + + if (options.trust_region_strategy_type == DOGLEG) { + if (options.linear_solver_type == ITERATIVE_SCHUR || + options.linear_solver_type == CGNR) { + *error = "DOGLEG only supports exact factorization based linear " + "solvers. If you want to use an iterative solver please " + "use LEVENBERG_MARQUARDT as the trust_region_strategy_type"; + return false; + } + } + + if (options.trust_region_minimizer_iterations_to_dump.size() > 0 && + options.trust_region_problem_dump_format_type != CONSOLE && + options.trust_region_problem_dump_directory.empty()) { + *error = "Solver::Options::trust_region_problem_dump_directory is empty."; + return false; + } + + if (options.dynamic_sparsity && + options.linear_solver_type != SPARSE_NORMAL_CHOLESKY) { + *error = "Dynamic sparsity is only supported with SPARSE_NORMAL_CHOLESKY."; + return false; + } + + return true; +} + +bool LineSearchOptionsAreValid(const Solver::Options& options, string* error) { + OPTION_GT(max_lbfgs_rank, 0); + OPTION_GT(min_line_search_step_size, 0.0); + OPTION_GT(max_line_search_step_contraction, 0.0); + OPTION_LT(max_line_search_step_contraction, 1.0); + OPTION_LT_OPTION(max_line_search_step_contraction, + min_line_search_step_contraction); + OPTION_LE(min_line_search_step_contraction, 1.0); + OPTION_GT(max_num_line_search_step_size_iterations, 0); + OPTION_GT(line_search_sufficient_function_decrease, 0.0); + OPTION_LT_OPTION(line_search_sufficient_function_decrease, + line_search_sufficient_curvature_decrease); + OPTION_LT(line_search_sufficient_curvature_decrease, 1.0); + OPTION_GT(max_line_search_step_expansion, 1.0); + + if ((options.line_search_direction_type == ceres::BFGS || + options.line_search_direction_type == ceres::LBFGS) && + options.line_search_type != ceres::WOLFE) { + *error = + string("Invalid configuration: Solver::Options::line_search_type = ") + + string(LineSearchTypeToString(options.line_search_type)) + + string(". When using (L)BFGS, " + "Solver::Options::line_search_type must be set to WOLFE."); + return false; + } + + // Warn user if they have requested BISECTION interpolation, but constraints + // on max/min step size change during line search prevent bisection scaling + // from occurring. Warn only, as this is likely a user mistake, but one which + // does not prevent us from continuing. + LOG_IF(WARNING, + (options.line_search_interpolation_type == ceres::BISECTION && + (options.max_line_search_step_contraction > 0.5 || + options.min_line_search_step_contraction < 0.5))) + << "Line search interpolation type is BISECTION, but specified " + << "max_line_search_step_contraction: " + << options.max_line_search_step_contraction << ", and " + << "min_line_search_step_contraction: " + << options.min_line_search_step_contraction + << ", prevent bisection (0.5) scaling, continuing with solve regardless."; + + return true; +} + +#undef OPTION_OP +#undef OPTION_OP_OPTION +#undef OPTION_GT +#undef OPTION_GE +#undef OPTION_LE +#undef OPTION_LT +#undef OPTION_LE_OPTION +#undef OPTION_LT_OPTION + void StringifyOrdering(const vector<int>& ordering, string* report) { if (ordering.size() == 0) { internal::StringAppendF(report, "AUTOMATIC"); @@ -54,19 +319,211 @@ void StringifyOrdering(const vector<int>& ordering, string* report) { internal::StringAppendF(report, "%d", ordering.back()); } +void SummarizeGivenProgram(const internal::Program& program, + Solver::Summary* summary) { + summary->num_parameter_blocks = program.NumParameterBlocks(); + summary->num_parameters = program.NumParameters(); + summary->num_effective_parameters = program.NumEffectiveParameters(); + summary->num_residual_blocks = program.NumResidualBlocks(); + summary->num_residuals = program.NumResiduals(); +} + +void SummarizeReducedProgram(const internal::Program& program, + Solver::Summary* summary) { + summary->num_parameter_blocks_reduced = program.NumParameterBlocks(); + summary->num_parameters_reduced = program.NumParameters(); + summary->num_effective_parameters_reduced = program.NumEffectiveParameters(); + summary->num_residual_blocks_reduced = program.NumResidualBlocks(); + summary->num_residuals_reduced = program.NumResiduals(); +} + +void PreSolveSummarize(const Solver::Options& options, + const internal::ProblemImpl* problem, + Solver::Summary* summary) { + SummarizeGivenProgram(problem->program(), summary); + internal::OrderingToGroupSizes(options.linear_solver_ordering.get(), + &(summary->linear_solver_ordering_given)); + internal::OrderingToGroupSizes(options.inner_iteration_ordering.get(), + &(summary->inner_iteration_ordering_given)); + + summary->dense_linear_algebra_library_type = options.dense_linear_algebra_library_type; // NOLINT + summary->dogleg_type = options.dogleg_type; + summary->inner_iteration_time_in_seconds = 0.0; + summary->inner_iterations_given = options.use_inner_iterations; + summary->line_search_direction_type = options.line_search_direction_type; // NOLINT + summary->line_search_interpolation_type = options.line_search_interpolation_type; // NOLINT + summary->line_search_type = options.line_search_type; + summary->linear_solver_type_given = options.linear_solver_type; + summary->max_lbfgs_rank = options.max_lbfgs_rank; + summary->minimizer_type = options.minimizer_type; + summary->nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; // NOLINT + summary->num_linear_solver_threads_given = options.num_linear_solver_threads; // NOLINT + summary->num_threads_given = options.num_threads; + summary->preconditioner_type_given = options.preconditioner_type; + summary->sparse_linear_algebra_library_type = options.sparse_linear_algebra_library_type; // NOLINT + summary->trust_region_strategy_type = options.trust_region_strategy_type; // NOLINT + summary->visibility_clustering_type = options.visibility_clustering_type; // NOLINT +} + +void PostSolveSummarize(const internal::PreprocessedProblem& pp, + Solver::Summary* summary) { + internal::OrderingToGroupSizes(pp.options.linear_solver_ordering.get(), + &(summary->linear_solver_ordering_used)); + internal::OrderingToGroupSizes(pp.options.inner_iteration_ordering.get(), + &(summary->inner_iteration_ordering_used)); + + summary->inner_iterations_used = pp.inner_iteration_minimizer.get() != NULL; // NOLINT + summary->linear_solver_type_used = pp.options.linear_solver_type; + summary->num_linear_solver_threads_used = pp.options.num_linear_solver_threads; // NOLINT + summary->num_threads_used = pp.options.num_threads; + summary->preconditioner_type_used = pp.options.preconditioner_type; // NOLINT + + internal::SetSummaryFinalCost(summary); + + if (pp.reduced_program.get() != NULL) { + SummarizeReducedProgram(*pp.reduced_program, summary); + } + + // It is possible that no evaluator was created. This would be the + // case if the preprocessor failed, or if the reduced problem did + // not contain any parameter blocks. Thus, only extract the + // evaluator statistics if one exists. + if (pp.evaluator.get() != NULL) { + const map<string, double>& evaluator_time_statistics = + pp.evaluator->TimeStatistics(); + summary->residual_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); + summary->jacobian_evaluation_time_in_seconds = + FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); + } + + // Again, like the evaluator, there may or may not be a linear + // solver from which we can extract run time statistics. In + // particular the line search solver does not use a linear solver. + if (pp.linear_solver.get() != NULL) { + const map<string, double>& linear_solver_time_statistics = + pp.linear_solver->TimeStatistics(); + summary->linear_solver_time_in_seconds = + FindWithDefault(linear_solver_time_statistics, + "LinearSolver::Solve", + 0.0); + } +} + +void Minimize(internal::PreprocessedProblem* pp, + Solver::Summary* summary) { + using internal::Program; + using internal::scoped_ptr; + using internal::Minimizer; + + Program* program = pp->reduced_program.get(); + if (pp->reduced_program->NumParameterBlocks() == 0) { + summary->message = "Function tolerance reached. " + "No non-constant parameter blocks found."; + summary->termination_type = CONVERGENCE; + VLOG_IF(1, pp->options.logging_type != SILENT) << summary->message; + summary->initial_cost = summary->fixed_cost; + summary->final_cost = summary->fixed_cost; + return; + } + + scoped_ptr<Minimizer> minimizer( + Minimizer::Create(pp->options.minimizer_type)); + minimizer->Minimize(pp->minimizer_options, + pp->reduced_parameters.data(), + summary); + + if (summary->IsSolutionUsable()) { + program->StateVectorToParameterBlocks(pp->reduced_parameters.data()); + program->CopyParameterBlockStateToUserState(); + } +} + } // namespace +bool Solver::Options::IsValid(string* error) const { + if (!CommonOptionsAreValid(*this, error)) { + return false; + } + + if (minimizer_type == TRUST_REGION) { + return TrustRegionOptionsAreValid(*this, error); + } + + CHECK_EQ(minimizer_type, LINE_SEARCH); + return LineSearchOptionsAreValid(*this, error); +} + Solver::~Solver() {} void Solver::Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary) { - double start_time_seconds = internal::WallTimeInSeconds(); - internal::ProblemImpl* problem_impl = - CHECK_NOTNULL(problem)->problem_impl_.get(); - internal::SolverImpl::Solve(options, problem_impl, summary); - summary->total_time_in_seconds = - internal::WallTimeInSeconds() - start_time_seconds; + using internal::PreprocessedProblem; + using internal::Preprocessor; + using internal::ProblemImpl; + using internal::Program; + using internal::scoped_ptr; + using internal::WallTimeInSeconds; + + CHECK_NOTNULL(problem); + CHECK_NOTNULL(summary); + + double start_time = WallTimeInSeconds(); + *summary = Summary(); + if (!options.IsValid(&summary->message)) { + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + ProblemImpl* problem_impl = problem->problem_impl_.get(); + Program* program = problem_impl->mutable_program(); + PreSolveSummarize(options, problem_impl, summary); + + // Make sure that all the parameter blocks states are set to the + // values provided by the user. + program->SetParameterBlockStatePtrsToUserStatePtrs(); + + scoped_ptr<internal::ProblemImpl> gradient_checking_problem; + if (options.check_gradients) { + gradient_checking_problem.reset( + CreateGradientCheckingProblemImpl( + problem_impl, + options.numeric_derivative_relative_step_size, + options.gradient_check_relative_precision)); + problem_impl = gradient_checking_problem.get(); + program = problem_impl->mutable_program(); + } + + scoped_ptr<Preprocessor> preprocessor( + Preprocessor::Create(options.minimizer_type)); + PreprocessedProblem pp; + const bool status = preprocessor->Preprocess(options, problem_impl, &pp); + summary->fixed_cost = pp.fixed_cost; + summary->preprocessor_time_in_seconds = WallTimeInSeconds() - start_time; + + if (status) { + const double minimizer_start_time = WallTimeInSeconds(); + Minimize(&pp, summary); + summary->minimizer_time_in_seconds = + WallTimeInSeconds() - minimizer_start_time; + } else { + summary->message = pp.error; + } + + const double postprocessor_start_time = WallTimeInSeconds(); + problem_impl = problem->problem_impl_.get(); + program = problem_impl->mutable_program(); + // On exit, ensure that the parameter blocks again point at the user + // provided values and the parameter blocks are numbered according + // to their position in the original user provided program. + program->SetParameterBlockStatePtrsToUserStatePtrs(); + program->SetParameterOffsetsAndIndex(); + PostSolveSummarize(pp, summary); + summary->postprocessor_time_in_seconds = + WallTimeInSeconds() - postprocessor_start_time; + + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; } void Solve(const Solver::Options& options, @@ -114,7 +571,8 @@ Solver::Summary::Summary() linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), inner_iterations_given(false), inner_iterations_used(false), - preconditioner_type(IDENTITY), + preconditioner_type_given(IDENTITY), + preconditioner_type_used(IDENTITY), visibility_clustering_type(CANONICAL_VIEWS), trust_region_strategy_type(LEVENBERG_MARQUARDT), dense_linear_algebra_library_type(EIGEN), @@ -142,10 +600,9 @@ string Solver::Summary::BriefReport() const { }; string Solver::Summary::FullReport() const { - string report = - "\n" - "Ceres Solver Report\n" - "-------------------\n"; + using internal::VersionString; + + string report = string("\nSolver Summary (v " + VersionString() + ")\n\n"); StringAppendF(&report, "%45s %21s\n", "Original", "Reduced"); StringAppendF(&report, "Parameter blocks % 25d% 25d\n", @@ -177,8 +634,8 @@ string Solver::Summary::FullReport() const { if (linear_solver_type_used == SPARSE_NORMAL_CHOLESKY || linear_solver_type_used == SPARSE_SCHUR || (linear_solver_type_used == ITERATIVE_SCHUR && - (preconditioner_type == CLUSTER_JACOBI || - preconditioner_type == CLUSTER_TRIDIAGONAL))) { + (preconditioner_type_used == CLUSTER_JACOBI || + preconditioner_type_used == CLUSTER_TRIDIAGONAL))) { StringAppendF(&report, "\nSparse linear algebra library %15s\n", SparseLinearAlgebraLibraryTypeToString( sparse_linear_algebra_library_type)); @@ -205,12 +662,12 @@ string Solver::Summary::FullReport() const { if (linear_solver_type_given == CGNR || linear_solver_type_given == ITERATIVE_SCHUR) { StringAppendF(&report, "Preconditioner %25s%25s\n", - PreconditionerTypeToString(preconditioner_type), - PreconditionerTypeToString(preconditioner_type)); + PreconditionerTypeToString(preconditioner_type_given), + PreconditionerTypeToString(preconditioner_type_used)); } - if (preconditioner_type == CLUSTER_JACOBI || - preconditioner_type == CLUSTER_TRIDIAGONAL) { + if (preconditioner_type_used == CLUSTER_JACOBI || + preconditioner_type_used == CLUSTER_TRIDIAGONAL) { StringAppendF(&report, "Visibility clustering%24s%25s\n", VisibilityClusteringTypeToString( visibility_clustering_type), @@ -345,9 +802,7 @@ string Solver::Summary::FullReport() const { }; bool Solver::Summary::IsSolutionUsable() const { - return (termination_type == CONVERGENCE || - termination_type == NO_CONVERGENCE || - termination_type == USER_SUCCESS); + return internal::IsSolutionUsable(*this); } } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc deleted file mode 100644 index 2bf6cd26212..00000000000 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc +++ /dev/null @@ -1,1851 +0,0 @@ -// 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) - -#include "ceres/solver_impl.h" - -#include <cstdio> -#include <iostream> // NOLINT -#include <numeric> -#include <string> -#include "ceres/coordinate_descent_minimizer.h" -#include "ceres/cxsparse.h" -#include "ceres/evaluator.h" -#include "ceres/gradient_checking_cost_function.h" -#include "ceres/iteration_callback.h" -#include "ceres/levenberg_marquardt_strategy.h" -#include "ceres/line_search_minimizer.h" -#include "ceres/linear_solver.h" -#include "ceres/map_util.h" -#include "ceres/minimizer.h" -#include "ceres/ordered_groups.h" -#include "ceres/parameter_block.h" -#include "ceres/parameter_block_ordering.h" -#include "ceres/problem.h" -#include "ceres/problem_impl.h" -#include "ceres/program.h" -#include "ceres/residual_block.h" -#include "ceres/stringprintf.h" -#include "ceres/suitesparse.h" -#include "ceres/trust_region_minimizer.h" -#include "ceres/wall_time.h" - -namespace ceres { -namespace internal { -namespace { - -// Callback for updating the user's parameter blocks. Updates are only -// done if the step is successful. -class StateUpdatingCallback : public IterationCallback { - public: - StateUpdatingCallback(Program* program, double* parameters) - : program_(program), parameters_(parameters) {} - - CallbackReturnType operator()(const IterationSummary& summary) { - if (summary.step_is_successful) { - program_->StateVectorToParameterBlocks(parameters_); - program_->CopyParameterBlockStateToUserState(); - } - return SOLVER_CONTINUE; - } - - private: - Program* program_; - double* parameters_; -}; - -void SetSummaryFinalCost(Solver::Summary* summary) { - summary->final_cost = summary->initial_cost; - // We need the loop here, instead of just looking at the last - // iteration because the minimizer maybe making non-monotonic steps. - for (int i = 0; i < summary->iterations.size(); ++i) { - const IterationSummary& iteration_summary = summary->iterations[i]; - summary->final_cost = min(iteration_summary.cost, summary->final_cost); - } -} - -// Callback for logging the state of the minimizer to STDERR or STDOUT -// depending on the user's preferences and logging level. -class TrustRegionLoggingCallback : public IterationCallback { - public: - explicit TrustRegionLoggingCallback(bool log_to_stdout) - : log_to_stdout_(log_to_stdout) {} - - ~TrustRegionLoggingCallback() {} - - CallbackReturnType operator()(const IterationSummary& summary) { - const char* kReportRowFormat = - "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " - "rho:% 3.2e mu:% 3.2e li:% 3d it:% 3.2e tt:% 3.2e"; - string output = StringPrintf(kReportRowFormat, - summary.iteration, - summary.cost, - summary.cost_change, - summary.gradient_max_norm, - summary.step_norm, - summary.relative_decrease, - summary.trust_region_radius, - summary.linear_solver_iterations, - summary.iteration_time_in_seconds, - summary.cumulative_time_in_seconds); - if (log_to_stdout_) { - cout << output << endl; - } else { - VLOG(1) << output; - } - return SOLVER_CONTINUE; - } - - private: - const bool log_to_stdout_; -}; - -// Callback for logging the state of the minimizer to STDERR or STDOUT -// depending on the user's preferences and logging level. -class LineSearchLoggingCallback : public IterationCallback { - public: - explicit LineSearchLoggingCallback(bool log_to_stdout) - : log_to_stdout_(log_to_stdout) {} - - ~LineSearchLoggingCallback() {} - - CallbackReturnType operator()(const IterationSummary& summary) { - const char* kReportRowFormat = - "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " - "s:% 3.2e e:% 3d it:% 3.2e tt:% 3.2e"; - string output = StringPrintf(kReportRowFormat, - summary.iteration, - summary.cost, - summary.cost_change, - summary.gradient_max_norm, - summary.step_norm, - summary.step_size, - summary.line_search_function_evaluations, - summary.iteration_time_in_seconds, - summary.cumulative_time_in_seconds); - if (log_to_stdout_) { - cout << output << endl; - } else { - VLOG(1) << output; - } - return SOLVER_CONTINUE; - } - - private: - const bool log_to_stdout_; -}; - - -// Basic callback to record the execution of the solver to a file for -// offline analysis. -class FileLoggingCallback : public IterationCallback { - public: - explicit FileLoggingCallback(const string& filename) - : fptr_(NULL) { - fptr_ = fopen(filename.c_str(), "w"); - CHECK_NOTNULL(fptr_); - } - - virtual ~FileLoggingCallback() { - if (fptr_ != NULL) { - fclose(fptr_); - } - } - - virtual CallbackReturnType operator()(const IterationSummary& summary) { - fprintf(fptr_, - "%4d %e %e\n", - summary.iteration, - summary.cost, - summary.cumulative_time_in_seconds); - return SOLVER_CONTINUE; - } - private: - FILE* fptr_; -}; - -// Iterate over each of the groups in order of their priority and fill -// summary with their sizes. -void SummarizeOrdering(ParameterBlockOrdering* ordering, - vector<int>* summary) { - CHECK_NOTNULL(summary)->clear(); - if (ordering == NULL) { - return; - } - - const map<int, set<double*> >& group_to_elements = - ordering->group_to_elements(); - for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); - it != group_to_elements.end(); - ++it) { - summary->push_back(it->second.size()); - } -} - -void SummarizeGivenProgram(const Program& program, Solver::Summary* summary) { - summary->num_parameter_blocks = program.NumParameterBlocks(); - summary->num_parameters = program.NumParameters(); - summary->num_effective_parameters = program.NumEffectiveParameters(); - summary->num_residual_blocks = program.NumResidualBlocks(); - summary->num_residuals = program.NumResiduals(); -} - -void SummarizeReducedProgram(const Program& program, Solver::Summary* summary) { - summary->num_parameter_blocks_reduced = program.NumParameterBlocks(); - summary->num_parameters_reduced = program.NumParameters(); - summary->num_effective_parameters_reduced = program.NumEffectiveParameters(); - summary->num_residual_blocks_reduced = program.NumResidualBlocks(); - summary->num_residuals_reduced = program.NumResiduals(); -} - -bool ParameterBlocksAreFinite(const ProblemImpl* problem, - string* message) { - CHECK_NOTNULL(message); - const Program& program = problem->program(); - const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - for (int i = 0; i < parameter_blocks.size(); ++i) { - const double* array = parameter_blocks[i]->user_state(); - const int size = parameter_blocks[i]->Size(); - const int invalid_index = FindInvalidValue(size, array); - if (invalid_index != size) { - *message = StringPrintf( - "ParameterBlock: %p with size %d has at least one invalid value.\n" - "First invalid value is at index: %d.\n" - "Parameter block values: ", - array, size, invalid_index); - AppendArrayToString(size, array, message); - return false; - } - } - return true; -} - -bool LineSearchOptionsAreValid(const Solver::Options& options, - string* message) { - // Validate values for configuration parameters supplied by user. - if ((options.line_search_direction_type == ceres::BFGS || - options.line_search_direction_type == ceres::LBFGS) && - options.line_search_type != ceres::WOLFE) { - *message = - string("Invalid configuration: require line_search_type == " - "ceres::WOLFE when using (L)BFGS to ensure that underlying " - "assumptions are guaranteed to be satisfied."); - return false; - } - if (options.max_lbfgs_rank <= 0) { - *message = - string("Invalid configuration: require max_lbfgs_rank > 0"); - return false; - } - if (options.min_line_search_step_size <= 0.0) { - *message = - "Invalid configuration: require min_line_search_step_size > 0.0."; - return false; - } - if (options.line_search_sufficient_function_decrease <= 0.0) { - *message = - string("Invalid configuration: require ") + - string("line_search_sufficient_function_decrease > 0.0."); - return false; - } - if (options.max_line_search_step_contraction <= 0.0 || - options.max_line_search_step_contraction >= 1.0) { - *message = string("Invalid configuration: require ") + - string("0.0 < max_line_search_step_contraction < 1.0."); - return false; - } - if (options.min_line_search_step_contraction <= - options.max_line_search_step_contraction || - options.min_line_search_step_contraction > 1.0) { - *message = string("Invalid configuration: require ") + - string("max_line_search_step_contraction < ") + - string("min_line_search_step_contraction <= 1.0."); - return false; - } - // Warn user if they have requested BISECTION interpolation, but constraints - // on max/min step size change during line search prevent bisection scaling - // from occurring. Warn only, as this is likely a user mistake, but one which - // does not prevent us from continuing. - LOG_IF(WARNING, - (options.line_search_interpolation_type == ceres::BISECTION && - (options.max_line_search_step_contraction > 0.5 || - options.min_line_search_step_contraction < 0.5))) - << "Line search interpolation type is BISECTION, but specified " - << "max_line_search_step_contraction: " - << options.max_line_search_step_contraction << ", and " - << "min_line_search_step_contraction: " - << options.min_line_search_step_contraction - << ", prevent bisection (0.5) scaling, continuing with solve regardless."; - if (options.max_num_line_search_step_size_iterations <= 0) { - *message = string("Invalid configuration: require ") + - string("max_num_line_search_step_size_iterations > 0."); - return false; - } - if (options.line_search_sufficient_curvature_decrease <= - options.line_search_sufficient_function_decrease || - options.line_search_sufficient_curvature_decrease > 1.0) { - *message = string("Invalid configuration: require ") + - string("line_search_sufficient_function_decrease < ") + - string("line_search_sufficient_curvature_decrease < 1.0."); - return false; - } - if (options.max_line_search_step_expansion <= 1.0) { - *message = string("Invalid configuration: require ") + - string("max_line_search_step_expansion > 1.0."); - return false; - } - return true; -} - -// Returns true if the program has any non-constant parameter blocks -// which have non-trivial bounds constraints. -bool IsBoundsConstrained(const Program& program) { - const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - for (int i = 0; i < parameter_blocks.size(); ++i) { - const ParameterBlock* parameter_block = parameter_blocks[i]; - if (parameter_block->IsConstant()) { - continue; - } - const int size = parameter_block->Size(); - for (int j = 0; j < size; ++j) { - const double lower_bound = parameter_block->LowerBoundForParameter(j); - const double upper_bound = parameter_block->UpperBoundForParameter(j); - if (lower_bound > -std::numeric_limits<double>::max() || - upper_bound < std::numeric_limits<double>::max()) { - return true; - } - } - } - return false; -} - -// Returns false, if the problem has any constant parameter blocks -// which are not feasible, or any variable parameter blocks which have -// a lower bound greater than or equal to the upper bound. -bool ParameterBlocksAreFeasible(const ProblemImpl* problem, string* message) { - CHECK_NOTNULL(message); - const Program& program = problem->program(); - const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - for (int i = 0; i < parameter_blocks.size(); ++i) { - const ParameterBlock* parameter_block = parameter_blocks[i]; - const double* parameters = parameter_block->user_state(); - const int size = parameter_block->Size(); - if (parameter_block->IsConstant()) { - // Constant parameter blocks must start in the feasible region - // to ultimately produce a feasible solution, since Ceres cannot - // change them. - for (int j = 0; j < size; ++j) { - const double lower_bound = parameter_block->LowerBoundForParameter(j); - const double upper_bound = parameter_block->UpperBoundForParameter(j); - if (parameters[j] < lower_bound || parameters[j] > upper_bound) { - *message = StringPrintf( - "ParameterBlock: %p with size %d has at least one infeasible " - "value." - "\nFirst infeasible value is at index: %d." - "\nLower bound: %e, value: %e, upper bound: %e" - "\nParameter block values: ", - parameters, size, j, lower_bound, parameters[j], upper_bound); - AppendArrayToString(size, parameters, message); - return false; - } - } - } else { - // Variable parameter blocks must have non-empty feasible - // regions, otherwise there is no way to produce a feasible - // solution. - for (int j = 0; j < size; ++j) { - const double lower_bound = parameter_block->LowerBoundForParameter(j); - const double upper_bound = parameter_block->UpperBoundForParameter(j); - if (lower_bound >= upper_bound) { - *message = StringPrintf( - "ParameterBlock: %p with size %d has at least one infeasible " - "bound." - "\nFirst infeasible bound is at index: %d." - "\nLower bound: %e, upper bound: %e" - "\nParameter block values: ", - parameters, size, j, lower_bound, upper_bound); - AppendArrayToString(size, parameters, message); - return false; - } - } - } - } - - return true; -} - - -} // namespace - -void SolverImpl::TrustRegionMinimize( - const Solver::Options& options, - Program* program, - CoordinateDescentMinimizer* inner_iteration_minimizer, - Evaluator* evaluator, - LinearSolver* linear_solver, - Solver::Summary* summary) { - Minimizer::Options minimizer_options(options); - minimizer_options.is_constrained = IsBoundsConstrained(*program); - - // The optimizer works on contiguous parameter vectors; allocate - // some. - Vector parameters(program->NumParameters()); - - // Collect the discontiguous parameters into a contiguous state - // vector. - program->ParameterBlocksToStateVector(parameters.data()); - - scoped_ptr<IterationCallback> file_logging_callback; - if (!options.solver_log.empty()) { - file_logging_callback.reset(new FileLoggingCallback(options.solver_log)); - minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), - file_logging_callback.get()); - } - - TrustRegionLoggingCallback logging_callback( - options.minimizer_progress_to_stdout); - if (options.logging_type != SILENT) { - minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), - &logging_callback); - } - - StateUpdatingCallback updating_callback(program, parameters.data()); - if (options.update_state_every_iteration) { - // This must get pushed to the front of the callbacks so that it is run - // before any of the user callbacks. - minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), - &updating_callback); - } - - minimizer_options.evaluator = evaluator; - scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); - - minimizer_options.jacobian = jacobian.get(); - minimizer_options.inner_iteration_minimizer = inner_iteration_minimizer; - - TrustRegionStrategy::Options trust_region_strategy_options; - trust_region_strategy_options.linear_solver = linear_solver; - trust_region_strategy_options.initial_radius = - options.initial_trust_region_radius; - trust_region_strategy_options.max_radius = options.max_trust_region_radius; - trust_region_strategy_options.min_lm_diagonal = options.min_lm_diagonal; - trust_region_strategy_options.max_lm_diagonal = options.max_lm_diagonal; - trust_region_strategy_options.trust_region_strategy_type = - options.trust_region_strategy_type; - trust_region_strategy_options.dogleg_type = options.dogleg_type; - scoped_ptr<TrustRegionStrategy> strategy( - TrustRegionStrategy::Create(trust_region_strategy_options)); - minimizer_options.trust_region_strategy = strategy.get(); - - TrustRegionMinimizer minimizer; - double minimizer_start_time = WallTimeInSeconds(); - minimizer.Minimize(minimizer_options, parameters.data(), summary); - - // If the user aborted mid-optimization or the optimization - // terminated because of a numerical failure, then do not update - // user state. - if (summary->termination_type != USER_FAILURE && - summary->termination_type != FAILURE) { - program->StateVectorToParameterBlocks(parameters.data()); - program->CopyParameterBlockStateToUserState(); - } - - summary->minimizer_time_in_seconds = - WallTimeInSeconds() - minimizer_start_time; -} - -void SolverImpl::LineSearchMinimize( - const Solver::Options& options, - Program* program, - Evaluator* evaluator, - Solver::Summary* summary) { - Minimizer::Options minimizer_options(options); - - // The optimizer works on contiguous parameter vectors; allocate some. - Vector parameters(program->NumParameters()); - - // Collect the discontiguous parameters into a contiguous state vector. - program->ParameterBlocksToStateVector(parameters.data()); - - // TODO(sameeragarwal): Add support for logging the configuration - // and more detailed stats. - scoped_ptr<IterationCallback> file_logging_callback; - if (!options.solver_log.empty()) { - file_logging_callback.reset(new FileLoggingCallback(options.solver_log)); - minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), - file_logging_callback.get()); - } - - LineSearchLoggingCallback logging_callback( - options.minimizer_progress_to_stdout); - if (options.logging_type != SILENT) { - minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), - &logging_callback); - } - - StateUpdatingCallback updating_callback(program, parameters.data()); - if (options.update_state_every_iteration) { - // This must get pushed to the front of the callbacks so that it is run - // before any of the user callbacks. - minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), - &updating_callback); - } - - minimizer_options.evaluator = evaluator; - - LineSearchMinimizer minimizer; - double minimizer_start_time = WallTimeInSeconds(); - minimizer.Minimize(minimizer_options, parameters.data(), summary); - - // If the user aborted mid-optimization or the optimization - // terminated because of a numerical failure, then do not update - // user state. - if (summary->termination_type != USER_FAILURE && - summary->termination_type != FAILURE) { - program->StateVectorToParameterBlocks(parameters.data()); - program->CopyParameterBlockStateToUserState(); - } - - summary->minimizer_time_in_seconds = - WallTimeInSeconds() - minimizer_start_time; -} - -void SolverImpl::Solve(const Solver::Options& options, - ProblemImpl* problem_impl, - Solver::Summary* summary) { - VLOG(2) << "Initial problem: " - << problem_impl->NumParameterBlocks() - << " parameter blocks, " - << problem_impl->NumParameters() - << " parameters, " - << problem_impl->NumResidualBlocks() - << " residual blocks, " - << problem_impl->NumResiduals() - << " residuals."; - *CHECK_NOTNULL(summary) = Solver::Summary(); - if (options.minimizer_type == TRUST_REGION) { - TrustRegionSolve(options, problem_impl, summary); - } else { - LineSearchSolve(options, problem_impl, summary); - } -} - -void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, - ProblemImpl* original_problem_impl, - Solver::Summary* summary) { - EventLogger event_logger("TrustRegionSolve"); - double solver_start_time = WallTimeInSeconds(); - - Program* original_program = original_problem_impl->mutable_program(); - ProblemImpl* problem_impl = original_problem_impl; - - summary->minimizer_type = TRUST_REGION; - - SummarizeGivenProgram(*original_program, summary); - SummarizeOrdering(original_options.linear_solver_ordering.get(), - &(summary->linear_solver_ordering_given)); - SummarizeOrdering(original_options.inner_iteration_ordering.get(), - &(summary->inner_iteration_ordering_given)); - - Solver::Options options(original_options); - -#ifndef CERES_USE_OPENMP - if (options.num_threads > 1) { - LOG(WARNING) - << "OpenMP support is not compiled into this binary; " - << "only options.num_threads=1 is supported. Switching " - << "to single threaded mode."; - options.num_threads = 1; - } - if (options.num_linear_solver_threads > 1) { - LOG(WARNING) - << "OpenMP support is not compiled into this binary; " - << "only options.num_linear_solver_threads=1 is supported. Switching " - << "to single threaded mode."; - options.num_linear_solver_threads = 1; - } -#endif - - summary->num_threads_given = original_options.num_threads; - summary->num_threads_used = options.num_threads; - - if (options.trust_region_minimizer_iterations_to_dump.size() > 0 && - options.trust_region_problem_dump_format_type != CONSOLE && - options.trust_region_problem_dump_directory.empty()) { - summary->message = - "Solver::Options::trust_region_problem_dump_directory is empty."; - LOG(ERROR) << summary->message; - return; - } - - if (!ParameterBlocksAreFinite(problem_impl, &summary->message)) { - LOG(ERROR) << "Terminating: " << summary->message; - return; - } - - if (!ParameterBlocksAreFeasible(problem_impl, &summary->message)) { - LOG(ERROR) << "Terminating: " << summary->message; - return; - } - - event_logger.AddEvent("Init"); - - original_program->SetParameterBlockStatePtrsToUserStatePtrs(); - event_logger.AddEvent("SetParameterBlockPtrs"); - - // If the user requests gradient checking, construct a new - // ProblemImpl by wrapping the CostFunctions of problem_impl inside - // GradientCheckingCostFunction and replacing problem_impl with - // gradient_checking_problem_impl. - scoped_ptr<ProblemImpl> gradient_checking_problem_impl; - if (options.check_gradients) { - VLOG(1) << "Checking Gradients"; - gradient_checking_problem_impl.reset( - CreateGradientCheckingProblemImpl( - problem_impl, - options.numeric_derivative_relative_step_size, - options.gradient_check_relative_precision)); - - // From here on, problem_impl will point to the gradient checking - // version. - problem_impl = gradient_checking_problem_impl.get(); - } - - if (options.linear_solver_ordering.get() != NULL) { - if (!IsOrderingValid(options, problem_impl, &summary->message)) { - LOG(ERROR) << summary->message; - return; - } - event_logger.AddEvent("CheckOrdering"); - } else { - options.linear_solver_ordering.reset(new ParameterBlockOrdering); - const ProblemImpl::ParameterMap& parameter_map = - problem_impl->parameter_map(); - for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin(); - it != parameter_map.end(); - ++it) { - options.linear_solver_ordering->AddElementToGroup(it->first, 0); - } - event_logger.AddEvent("ConstructOrdering"); - } - - // Create the three objects needed to minimize: the transformed program, the - // evaluator, and the linear solver. - scoped_ptr<Program> reduced_program(CreateReducedProgram(&options, - problem_impl, - &summary->fixed_cost, - &summary->message)); - - event_logger.AddEvent("CreateReducedProgram"); - if (reduced_program == NULL) { - return; - } - - SummarizeOrdering(options.linear_solver_ordering.get(), - &(summary->linear_solver_ordering_used)); - SummarizeReducedProgram(*reduced_program, summary); - - if (summary->num_parameter_blocks_reduced == 0) { - summary->preprocessor_time_in_seconds = - WallTimeInSeconds() - solver_start_time; - - double post_process_start_time = WallTimeInSeconds(); - - summary->message = - "Terminating: Function tolerance reached. " - "No non-constant parameter blocks found."; - summary->termination_type = CONVERGENCE; - VLOG_IF(1, options.logging_type != SILENT) << summary->message; - - summary->initial_cost = summary->fixed_cost; - summary->final_cost = summary->fixed_cost; - - // Ensure the program state is set to the user parameters on the way out. - original_program->SetParameterBlockStatePtrsToUserStatePtrs(); - original_program->SetParameterOffsetsAndIndex(); - - summary->postprocessor_time_in_seconds = - WallTimeInSeconds() - post_process_start_time; - return; - } - - scoped_ptr<LinearSolver> - linear_solver(CreateLinearSolver(&options, &summary->message)); - event_logger.AddEvent("CreateLinearSolver"); - if (linear_solver == NULL) { - return; - } - - summary->linear_solver_type_given = original_options.linear_solver_type; - summary->linear_solver_type_used = options.linear_solver_type; - - summary->preconditioner_type = options.preconditioner_type; - summary->visibility_clustering_type = options.visibility_clustering_type; - - summary->num_linear_solver_threads_given = - original_options.num_linear_solver_threads; - summary->num_linear_solver_threads_used = options.num_linear_solver_threads; - - summary->dense_linear_algebra_library_type = - options.dense_linear_algebra_library_type; - summary->sparse_linear_algebra_library_type = - options.sparse_linear_algebra_library_type; - - summary->trust_region_strategy_type = options.trust_region_strategy_type; - summary->dogleg_type = options.dogleg_type; - - scoped_ptr<Evaluator> evaluator(CreateEvaluator(options, - problem_impl->parameter_map(), - reduced_program.get(), - &summary->message)); - - event_logger.AddEvent("CreateEvaluator"); - - if (evaluator == NULL) { - return; - } - - scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; - if (options.use_inner_iterations) { - if (reduced_program->parameter_blocks().size() < 2) { - LOG(WARNING) << "Reduced problem only contains one parameter block." - << "Disabling inner iterations."; - } else { - inner_iteration_minimizer.reset( - CreateInnerIterationMinimizer(options, - *reduced_program, - problem_impl->parameter_map(), - summary)); - if (inner_iteration_minimizer == NULL) { - LOG(ERROR) << summary->message; - return; - } - } - } - event_logger.AddEvent("CreateInnerIterationMinimizer"); - - double minimizer_start_time = WallTimeInSeconds(); - summary->preprocessor_time_in_seconds = - minimizer_start_time - solver_start_time; - - // Run the optimization. - TrustRegionMinimize(options, - reduced_program.get(), - inner_iteration_minimizer.get(), - evaluator.get(), - linear_solver.get(), - summary); - event_logger.AddEvent("Minimize"); - - double post_process_start_time = WallTimeInSeconds(); - - SetSummaryFinalCost(summary); - - // Ensure the program state is set to the user parameters on the way - // out. - original_program->SetParameterBlockStatePtrsToUserStatePtrs(); - original_program->SetParameterOffsetsAndIndex(); - - const map<string, double>& linear_solver_time_statistics = - linear_solver->TimeStatistics(); - summary->linear_solver_time_in_seconds = - FindWithDefault(linear_solver_time_statistics, - "LinearSolver::Solve", - 0.0); - - const map<string, double>& evaluator_time_statistics = - evaluator->TimeStatistics(); - - summary->residual_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); - summary->jacobian_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); - - // Stick a fork in it, we're done. - summary->postprocessor_time_in_seconds = - WallTimeInSeconds() - post_process_start_time; - event_logger.AddEvent("PostProcess"); -} - -void SolverImpl::LineSearchSolve(const Solver::Options& original_options, - ProblemImpl* original_problem_impl, - Solver::Summary* summary) { - double solver_start_time = WallTimeInSeconds(); - - Program* original_program = original_problem_impl->mutable_program(); - ProblemImpl* problem_impl = original_problem_impl; - - SummarizeGivenProgram(*original_program, summary); - summary->minimizer_type = LINE_SEARCH; - summary->line_search_direction_type = - original_options.line_search_direction_type; - summary->max_lbfgs_rank = original_options.max_lbfgs_rank; - summary->line_search_type = original_options.line_search_type; - summary->line_search_interpolation_type = - original_options.line_search_interpolation_type; - summary->nonlinear_conjugate_gradient_type = - original_options.nonlinear_conjugate_gradient_type; - - if (!LineSearchOptionsAreValid(original_options, &summary->message)) { - LOG(ERROR) << summary->message; - return; - } - - if (IsBoundsConstrained(problem_impl->program())) { - summary->message = "LINE_SEARCH Minimizer does not support bounds."; - LOG(ERROR) << "Terminating: " << summary->message; - return; - } - - Solver::Options options(original_options); - - // This ensures that we get a Block Jacobian Evaluator along with - // none of the Schur nonsense. This file will have to be extensively - // refactored to deal with the various bits of cleanups related to - // line search. - options.linear_solver_type = CGNR; - - -#ifndef CERES_USE_OPENMP - if (options.num_threads > 1) { - LOG(WARNING) - << "OpenMP support is not compiled into this binary; " - << "only options.num_threads=1 is supported. Switching " - << "to single threaded mode."; - options.num_threads = 1; - } -#endif // CERES_USE_OPENMP - - summary->num_threads_given = original_options.num_threads; - summary->num_threads_used = options.num_threads; - - if (!ParameterBlocksAreFinite(problem_impl, &summary->message)) { - LOG(ERROR) << "Terminating: " << summary->message; - return; - } - - if (options.linear_solver_ordering.get() != NULL) { - if (!IsOrderingValid(options, problem_impl, &summary->message)) { - LOG(ERROR) << summary->message; - return; - } - } else { - options.linear_solver_ordering.reset(new ParameterBlockOrdering); - const ProblemImpl::ParameterMap& parameter_map = - problem_impl->parameter_map(); - for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin(); - it != parameter_map.end(); - ++it) { - options.linear_solver_ordering->AddElementToGroup(it->first, 0); - } - } - - - original_program->SetParameterBlockStatePtrsToUserStatePtrs(); - - // If the user requests gradient checking, construct a new - // ProblemImpl by wrapping the CostFunctions of problem_impl inside - // GradientCheckingCostFunction and replacing problem_impl with - // gradient_checking_problem_impl. - scoped_ptr<ProblemImpl> gradient_checking_problem_impl; - if (options.check_gradients) { - VLOG(1) << "Checking Gradients"; - gradient_checking_problem_impl.reset( - CreateGradientCheckingProblemImpl( - problem_impl, - options.numeric_derivative_relative_step_size, - options.gradient_check_relative_precision)); - - // From here on, problem_impl will point to the gradient checking - // version. - problem_impl = gradient_checking_problem_impl.get(); - } - - // Create the three objects needed to minimize: the transformed program, the - // evaluator, and the linear solver. - scoped_ptr<Program> reduced_program(CreateReducedProgram(&options, - problem_impl, - &summary->fixed_cost, - &summary->message)); - if (reduced_program == NULL) { - return; - } - - SummarizeReducedProgram(*reduced_program, summary); - if (summary->num_parameter_blocks_reduced == 0) { - summary->preprocessor_time_in_seconds = - WallTimeInSeconds() - solver_start_time; - - summary->message = - "Terminating: Function tolerance reached. " - "No non-constant parameter blocks found."; - summary->termination_type = CONVERGENCE; - VLOG_IF(1, options.logging_type != SILENT) << summary->message; - - const double post_process_start_time = WallTimeInSeconds(); - SetSummaryFinalCost(summary); - - // Ensure the program state is set to the user parameters on the way out. - original_program->SetParameterBlockStatePtrsToUserStatePtrs(); - original_program->SetParameterOffsetsAndIndex(); - - summary->postprocessor_time_in_seconds = - WallTimeInSeconds() - post_process_start_time; - return; - } - - scoped_ptr<Evaluator> evaluator(CreateEvaluator(options, - problem_impl->parameter_map(), - reduced_program.get(), - &summary->message)); - if (evaluator == NULL) { - return; - } - - const double minimizer_start_time = WallTimeInSeconds(); - summary->preprocessor_time_in_seconds = - minimizer_start_time - solver_start_time; - - // Run the optimization. - LineSearchMinimize(options, reduced_program.get(), evaluator.get(), summary); - - const double post_process_start_time = WallTimeInSeconds(); - - SetSummaryFinalCost(summary); - - // Ensure the program state is set to the user parameters on the way out. - original_program->SetParameterBlockStatePtrsToUserStatePtrs(); - original_program->SetParameterOffsetsAndIndex(); - - const map<string, double>& evaluator_time_statistics = - evaluator->TimeStatistics(); - - summary->residual_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); - summary->jacobian_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); - - // Stick a fork in it, we're done. - summary->postprocessor_time_in_seconds = - WallTimeInSeconds() - post_process_start_time; -} - -bool SolverImpl::IsOrderingValid(const Solver::Options& options, - const ProblemImpl* problem_impl, - string* error) { - if (options.linear_solver_ordering->NumElements() != - problem_impl->NumParameterBlocks()) { - *error = "Number of parameter blocks in user supplied ordering " - "does not match the number of parameter blocks in the problem"; - return false; - } - - const Program& program = problem_impl->program(); - const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - for (vector<ParameterBlock*>::const_iterator it = parameter_blocks.begin(); - it != parameter_blocks.end(); - ++it) { - if (!options.linear_solver_ordering - ->IsMember(const_cast<double*>((*it)->user_state()))) { - *error = "Problem contains a parameter block that is not in " - "the user specified ordering."; - return false; - } - } - - if (IsSchurType(options.linear_solver_type) && - options.linear_solver_ordering->NumGroups() > 1) { - const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); - const set<double*>& e_blocks = - options.linear_solver_ordering->group_to_elements().begin()->second; - if (!IsParameterBlockSetIndependent(e_blocks, residual_blocks)) { - *error = "The user requested the use of a Schur type solver. " - "But the first elimination group in the ordering is not an " - "independent set."; - return false; - } - } - return true; -} - -bool SolverImpl::IsParameterBlockSetIndependent( - const set<double*>& parameter_block_ptrs, - const vector<ResidualBlock*>& residual_blocks) { - // Loop over each residual block and ensure that no two parameter - // blocks in the same residual block are part of - // parameter_block_ptrs as that would violate the assumption that it - // is an independent set in the Hessian matrix. - for (vector<ResidualBlock*>::const_iterator it = residual_blocks.begin(); - it != residual_blocks.end(); - ++it) { - ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks(); - const int num_parameter_blocks = (*it)->NumParameterBlocks(); - int count = 0; - for (int i = 0; i < num_parameter_blocks; ++i) { - count += parameter_block_ptrs.count( - parameter_blocks[i]->mutable_user_state()); - } - if (count > 1) { - return false; - } - } - return true; -} - - -// Strips varying parameters and residuals, maintaining order, and updating -// orderings. -bool SolverImpl::RemoveFixedBlocksFromProgram( - Program* program, - ParameterBlockOrdering* linear_solver_ordering, - ParameterBlockOrdering* inner_iteration_ordering, - double* fixed_cost, - string* error) { - scoped_array<double> residual_block_evaluate_scratch; - if (fixed_cost != NULL) { - residual_block_evaluate_scratch.reset( - new double[program->MaxScratchDoublesNeededForEvaluate()]); - *fixed_cost = 0.0; - } - - vector<ParameterBlock*>* parameter_blocks = - program->mutable_parameter_blocks(); - vector<ResidualBlock*>* residual_blocks = - program->mutable_residual_blocks(); - - // Mark all the parameters as unused. Abuse the index member of the - // parameter blocks for the marking. - for (int i = 0; i < parameter_blocks->size(); ++i) { - (*parameter_blocks)[i]->set_index(-1); - } - - // Filter out residual that have all-constant parameters, and mark all the - // parameter blocks that appear in residuals. - int num_active_residual_blocks = 0; - for (int i = 0; i < residual_blocks->size(); ++i) { - ResidualBlock* residual_block = (*residual_blocks)[i]; - int num_parameter_blocks = residual_block->NumParameterBlocks(); - - // Determine if the residual block is fixed, and also mark varying - // parameters that appear in the residual block. - bool all_constant = true; - for (int k = 0; k < num_parameter_blocks; k++) { - ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; - if (!parameter_block->IsConstant()) { - all_constant = false; - parameter_block->set_index(1); - } - } - - if (!all_constant) { - (*residual_blocks)[num_active_residual_blocks++] = residual_block; - } else if (fixed_cost != NULL) { - // The residual is constant and will be removed, so its cost is - // added to the variable fixed_cost. - double cost = 0.0; - if (!residual_block->Evaluate(true, - &cost, - NULL, - NULL, - residual_block_evaluate_scratch.get())) { - *error = StringPrintf("Evaluation of the residual %d failed during " - "removal of fixed residual blocks.", i); - return false; - } - *fixed_cost += cost; - } - } - residual_blocks->resize(num_active_residual_blocks); - - // Filter out unused or fixed parameter blocks, and update the - // linear_solver_ordering and the inner_iteration_ordering (if - // present). - int num_active_parameter_blocks = 0; - for (int i = 0; i < parameter_blocks->size(); ++i) { - ParameterBlock* parameter_block = (*parameter_blocks)[i]; - if (parameter_block->index() == -1) { - // Parameter block is constant. - if (linear_solver_ordering != NULL) { - linear_solver_ordering->Remove(parameter_block->mutable_user_state()); - } - - // It is not necessary that the inner iteration ordering contain - // this parameter block. But calling Remove is safe, as it will - // just return false. - if (inner_iteration_ordering != NULL) { - inner_iteration_ordering->Remove(parameter_block->mutable_user_state()); - } - continue; - } - - (*parameter_blocks)[num_active_parameter_blocks++] = parameter_block; - } - parameter_blocks->resize(num_active_parameter_blocks); - - if (!(((program->NumResidualBlocks() == 0) && - (program->NumParameterBlocks() == 0)) || - ((program->NumResidualBlocks() != 0) && - (program->NumParameterBlocks() != 0)))) { - *error = "Congratulations, you found a bug in Ceres. Please report it."; - return false; - } - - return true; -} - -Program* SolverImpl::CreateReducedProgram(Solver::Options* options, - ProblemImpl* problem_impl, - double* fixed_cost, - string* error) { - CHECK_NOTNULL(options->linear_solver_ordering.get()); - Program* original_program = problem_impl->mutable_program(); - scoped_ptr<Program> transformed_program(new Program(*original_program)); - - ParameterBlockOrdering* linear_solver_ordering = - options->linear_solver_ordering.get(); - const int min_group_id = - linear_solver_ordering->group_to_elements().begin()->first; - ParameterBlockOrdering* inner_iteration_ordering = - options->inner_iteration_ordering.get(); - if (!RemoveFixedBlocksFromProgram(transformed_program.get(), - linear_solver_ordering, - inner_iteration_ordering, - fixed_cost, - error)) { - return NULL; - } - - VLOG(2) << "Reduced problem: " - << transformed_program->NumParameterBlocks() - << " parameter blocks, " - << transformed_program->NumParameters() - << " parameters, " - << transformed_program->NumResidualBlocks() - << " residual blocks, " - << transformed_program->NumResiduals() - << " residuals."; - - if (transformed_program->NumParameterBlocks() == 0) { - LOG(WARNING) << "No varying parameter blocks to optimize; " - << "bailing early."; - return transformed_program.release(); - } - - if (IsSchurType(options->linear_solver_type) && - linear_solver_ordering->GroupSize(min_group_id) == 0) { - // If the user requested the use of a Schur type solver, and - // supplied a non-NULL linear_solver_ordering object with more than - // one elimination group, then it can happen that after all the - // parameter blocks which are fixed or unused have been removed from - // the program and the ordering, there are no more parameter blocks - // in the first elimination group. - // - // In such a case, the use of a Schur type solver is not possible, - // as they assume there is at least one e_block. Thus, we - // automatically switch to the closest solver to the one indicated - // by the user. - AlternateLinearSolverForSchurTypeLinearSolver(options); - } - - if (IsSchurType(options->linear_solver_type)) { - if (!ReorderProgramForSchurTypeLinearSolver( - options->linear_solver_type, - options->sparse_linear_algebra_library_type, - problem_impl->parameter_map(), - linear_solver_ordering, - transformed_program.get(), - error)) { - return NULL; - } - return transformed_program.release(); - } - - if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && - !options->dynamic_sparsity) { - if (!ReorderProgramForSparseNormalCholesky( - options->sparse_linear_algebra_library_type, - linear_solver_ordering, - transformed_program.get(), - error)) { - return NULL; - } - - return transformed_program.release(); - } - - transformed_program->SetParameterOffsetsAndIndex(); - return transformed_program.release(); -} - -LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, - string* error) { - CHECK_NOTNULL(options); - CHECK_NOTNULL(options->linear_solver_ordering.get()); - CHECK_NOTNULL(error); - - if (options->trust_region_strategy_type == DOGLEG) { - if (options->linear_solver_type == ITERATIVE_SCHUR || - options->linear_solver_type == CGNR) { - *error = "DOGLEG only supports exact factorization based linear " - "solvers. If you want to use an iterative solver please " - "use LEVENBERG_MARQUARDT as the trust_region_strategy_type"; - return NULL; - } - } - -#ifdef CERES_NO_LAPACK - if (options->linear_solver_type == DENSE_NORMAL_CHOLESKY && - options->dense_linear_algebra_library_type == LAPACK) { - *error = "Can't use DENSE_NORMAL_CHOLESKY with LAPACK because " - "LAPACK was not enabled when Ceres was built."; - return NULL; - } - - if (options->linear_solver_type == DENSE_QR && - options->dense_linear_algebra_library_type == LAPACK) { - *error = "Can't use DENSE_QR with LAPACK because " - "LAPACK was not enabled when Ceres was built."; - return NULL; - } - - if (options->linear_solver_type == DENSE_SCHUR && - options->dense_linear_algebra_library_type == LAPACK) { - *error = "Can't use DENSE_SCHUR with LAPACK because " - "LAPACK was not enabled when Ceres was built."; - return NULL; - } -#endif - -#ifdef CERES_NO_SUITESPARSE - if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && - options->sparse_linear_algebra_library_type == SUITE_SPARSE) { - *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " - "SuiteSparse was not enabled when Ceres was built."; - return NULL; - } - - if (options->preconditioner_type == CLUSTER_JACOBI) { - *error = "CLUSTER_JACOBI preconditioner not suppored. Please build Ceres " - "with SuiteSparse support."; - return NULL; - } - - if (options->preconditioner_type == CLUSTER_TRIDIAGONAL) { - *error = "CLUSTER_TRIDIAGONAL preconditioner not suppored. Please build " - "Ceres with SuiteSparse support."; - return NULL; - } -#endif - -#ifdef CERES_NO_CXSPARSE - if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && - options->sparse_linear_algebra_library_type == CX_SPARSE) { - *error = "Can't use SPARSE_NORMAL_CHOLESKY with CXSPARSE because " - "CXSparse was not enabled when Ceres was built."; - return NULL; - } -#endif - -#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) - if (options->linear_solver_type == SPARSE_SCHUR) { - *error = "Can't use SPARSE_SCHUR because neither SuiteSparse nor" - "CXSparse was enabled when Ceres was compiled."; - return NULL; - } -#endif - - if (options->max_linear_solver_iterations <= 0) { - *error = "Solver::Options::max_linear_solver_iterations is not positive."; - return NULL; - } - if (options->min_linear_solver_iterations <= 0) { - *error = "Solver::Options::min_linear_solver_iterations is not positive."; - return NULL; - } - if (options->min_linear_solver_iterations > - options->max_linear_solver_iterations) { - *error = "Solver::Options::min_linear_solver_iterations > " - "Solver::Options::max_linear_solver_iterations."; - return NULL; - } - - LinearSolver::Options linear_solver_options; - linear_solver_options.min_num_iterations = - options->min_linear_solver_iterations; - linear_solver_options.max_num_iterations = - options->max_linear_solver_iterations; - linear_solver_options.type = options->linear_solver_type; - linear_solver_options.preconditioner_type = options->preconditioner_type; - linear_solver_options.visibility_clustering_type = - options->visibility_clustering_type; - linear_solver_options.sparse_linear_algebra_library_type = - options->sparse_linear_algebra_library_type; - linear_solver_options.dense_linear_algebra_library_type = - options->dense_linear_algebra_library_type; - linear_solver_options.use_postordering = options->use_postordering; - linear_solver_options.dynamic_sparsity = options->dynamic_sparsity; - - // Ignore user's postordering preferences and force it to be true if - // cholmod_camd is not available. This ensures that the linear - // solver does not assume that a fill-reducing pre-ordering has been - // done. -#if !defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CAMD) - if (IsSchurType(linear_solver_options.type) && - options->sparse_linear_algebra_library_type == SUITE_SPARSE) { - linear_solver_options.use_postordering = true; - } -#endif - - linear_solver_options.num_threads = options->num_linear_solver_threads; - options->num_linear_solver_threads = linear_solver_options.num_threads; - - const map<int, set<double*> >& groups = - options->linear_solver_ordering->group_to_elements(); - for (map<int, set<double*> >::const_iterator it = groups.begin(); - it != groups.end(); - ++it) { - linear_solver_options.elimination_groups.push_back(it->second.size()); - } - // Schur type solvers, expect at least two elimination groups. If - // there is only one elimination group, then CreateReducedProgram - // guarantees that this group only contains e_blocks. Thus we add a - // dummy elimination group with zero blocks in it. - if (IsSchurType(linear_solver_options.type) && - linear_solver_options.elimination_groups.size() == 1) { - linear_solver_options.elimination_groups.push_back(0); - } - - return LinearSolver::Create(linear_solver_options); -} - - -// Find the minimum index of any parameter block to the given residual. -// Parameter blocks that have indices greater than num_eliminate_blocks are -// considered to have an index equal to num_eliminate_blocks. -static int MinParameterBlock(const ResidualBlock* residual_block, - int num_eliminate_blocks) { - int min_parameter_block_position = num_eliminate_blocks; - for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { - ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; - if (!parameter_block->IsConstant()) { - CHECK_NE(parameter_block->index(), -1) - << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " - << "This is a Ceres bug; please contact the developers!"; - min_parameter_block_position = std::min(parameter_block->index(), - min_parameter_block_position); - } - } - return min_parameter_block_position; -} - -// Reorder the residuals for program, if necessary, so that the residuals -// involving each E block occur together. This is a necessary condition for the -// Schur eliminator, which works on these "row blocks" in the jacobian. -bool SolverImpl::LexicographicallyOrderResidualBlocks( - const int num_eliminate_blocks, - Program* program, - string* error) { - CHECK_GE(num_eliminate_blocks, 1) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - // Create a histogram of the number of residuals for each E block. There is an - // extra bucket at the end to catch all non-eliminated F blocks. - vector<int> residual_blocks_per_e_block(num_eliminate_blocks + 1); - vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); - vector<int> min_position_per_residual(residual_blocks->size()); - for (int i = 0; i < residual_blocks->size(); ++i) { - ResidualBlock* residual_block = (*residual_blocks)[i]; - int position = MinParameterBlock(residual_block, num_eliminate_blocks); - min_position_per_residual[i] = position; - DCHECK_LE(position, num_eliminate_blocks); - residual_blocks_per_e_block[position]++; - } - - // Run a cumulative sum on the histogram, to obtain offsets to the start of - // each histogram bucket (where each bucket is for the residuals for that - // E-block). - vector<int> offsets(num_eliminate_blocks + 1); - std::partial_sum(residual_blocks_per_e_block.begin(), - residual_blocks_per_e_block.end(), - offsets.begin()); - CHECK_EQ(offsets.back(), residual_blocks->size()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - CHECK(find(residual_blocks_per_e_block.begin(), - residual_blocks_per_e_block.end() - 1, 0) != - residual_blocks_per_e_block.end()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - // Fill in each bucket with the residual blocks for its corresponding E block. - // Each bucket is individually filled from the back of the bucket to the front - // of the bucket. The filling order among the buckets is dictated by the - // residual blocks. This loop uses the offsets as counters; subtracting one - // from each offset as a residual block is placed in the bucket. When the - // filling is finished, the offset pointerts should have shifted down one - // entry (this is verified below). - vector<ResidualBlock*> reordered_residual_blocks( - (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); - for (int i = 0; i < residual_blocks->size(); ++i) { - int bucket = min_position_per_residual[i]; - - // Decrement the cursor, which should now point at the next empty position. - offsets[bucket]--; - - // Sanity. - CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; - } - - // Sanity check #1: The difference in bucket offsets should match the - // histogram sizes. - for (int i = 0; i < num_eliminate_blocks; ++i) { - CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - } - // Sanity check #2: No NULL's left behind. - for (int i = 0; i < reordered_residual_blocks.size(); ++i) { - CHECK(reordered_residual_blocks[i] != NULL) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - } - - // Now that the residuals are collected by E block, swap them in place. - swap(*program->mutable_residual_blocks(), reordered_residual_blocks); - return true; -} - -Evaluator* SolverImpl::CreateEvaluator( - const Solver::Options& options, - const ProblemImpl::ParameterMap& parameter_map, - Program* program, - string* error) { - Evaluator::Options evaluator_options; - evaluator_options.linear_solver_type = options.linear_solver_type; - evaluator_options.num_eliminate_blocks = - (options.linear_solver_ordering->NumGroups() > 0 && - IsSchurType(options.linear_solver_type)) - ? (options.linear_solver_ordering - ->group_to_elements().begin() - ->second.size()) - : 0; - evaluator_options.num_threads = options.num_threads; - evaluator_options.dynamic_sparsity = options.dynamic_sparsity; - return Evaluator::Create(evaluator_options, program, error); -} - -CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( - const Solver::Options& options, - const Program& program, - const ProblemImpl::ParameterMap& parameter_map, - Solver::Summary* summary) { - summary->inner_iterations_given = true; - - scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer( - new CoordinateDescentMinimizer); - scoped_ptr<ParameterBlockOrdering> inner_iteration_ordering; - ParameterBlockOrdering* ordering_ptr = NULL; - - if (options.inner_iteration_ordering.get() == NULL) { - // Find a recursive decomposition of the Hessian matrix as a set - // of independent sets of decreasing size and invert it. This - // seems to work better in practice, i.e., Cameras before - // points. - inner_iteration_ordering.reset(new ParameterBlockOrdering); - ComputeRecursiveIndependentSetOrdering(program, - inner_iteration_ordering.get()); - inner_iteration_ordering->Reverse(); - ordering_ptr = inner_iteration_ordering.get(); - } else { - const map<int, set<double*> >& group_to_elements = - options.inner_iteration_ordering->group_to_elements(); - - // Iterate over each group and verify that it is an independent - // set. - map<int, set<double*> >::const_iterator it = group_to_elements.begin(); - for ( ; it != group_to_elements.end(); ++it) { - if (!IsParameterBlockSetIndependent(it->second, - program.residual_blocks())) { - summary->message = - StringPrintf("The user-provided " - "parameter_blocks_for_inner_iterations does not " - "form an independent set. Group Id: %d", it->first); - return NULL; - } - } - ordering_ptr = options.inner_iteration_ordering.get(); - } - - if (!inner_iteration_minimizer->Init(program, - parameter_map, - *ordering_ptr, - &summary->message)) { - return NULL; - } - - summary->inner_iterations_used = true; - summary->inner_iteration_time_in_seconds = 0.0; - SummarizeOrdering(ordering_ptr, &(summary->inner_iteration_ordering_used)); - return inner_iteration_minimizer.release(); -} - -void SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver( - Solver::Options* options) { - if (!IsSchurType(options->linear_solver_type)) { - return; - } - - string msg = "No e_blocks remaining. Switching from "; - if (options->linear_solver_type == SPARSE_SCHUR) { - options->linear_solver_type = SPARSE_NORMAL_CHOLESKY; - msg += "SPARSE_SCHUR to SPARSE_NORMAL_CHOLESKY."; - } else if (options->linear_solver_type == DENSE_SCHUR) { - // TODO(sameeragarwal): This is probably not a great choice. - // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can - // take a BlockSparseMatrix as input. - options->linear_solver_type = DENSE_QR; - msg += "DENSE_SCHUR to DENSE_QR."; - } else if (options->linear_solver_type == ITERATIVE_SCHUR) { - options->linear_solver_type = CGNR; - if (options->preconditioner_type != IDENTITY) { - msg += StringPrintf("ITERATIVE_SCHUR with %s preconditioner " - "to CGNR with JACOBI preconditioner.", - PreconditionerTypeToString( - options->preconditioner_type)); - // CGNR currently only supports the JACOBI preconditioner. - options->preconditioner_type = JACOBI; - } else { - msg += "ITERATIVE_SCHUR with IDENTITY preconditioner" - "to CGNR with IDENTITY preconditioner."; - } - } - LOG(WARNING) << msg; -} - -bool SolverImpl::ApplyUserOrdering( - const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - const int num_parameter_blocks = program->NumParameterBlocks(); - if (parameter_block_ordering->NumElements() != num_parameter_blocks) { - *error = StringPrintf("User specified ordering does not have the same " - "number of parameters as the problem. The problem" - "has %d blocks while the ordering has %d blocks.", - num_parameter_blocks, - parameter_block_ordering->NumElements()); - return false; - } - - vector<ParameterBlock*>* parameter_blocks = - program->mutable_parameter_blocks(); - parameter_blocks->clear(); - - const map<int, set<double*> >& groups = - parameter_block_ordering->group_to_elements(); - - for (map<int, set<double*> >::const_iterator group_it = groups.begin(); - group_it != groups.end(); - ++group_it) { - const set<double*>& group = group_it->second; - for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); - parameter_block_ptr_it != group.end(); - ++parameter_block_ptr_it) { - ProblemImpl::ParameterMap::const_iterator parameter_block_it = - parameter_map.find(*parameter_block_ptr_it); - if (parameter_block_it == parameter_map.end()) { - *error = StringPrintf("User specified ordering contains a pointer " - "to a double that is not a parameter block in " - "the problem. The invalid double is in group: %d", - group_it->first); - return false; - } - parameter_blocks->push_back(parameter_block_it->second); - } - } - return true; -} - - -TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( - const Program* program) { - - // Matrix to store the block sparsity structure of the Jacobian. - TripletSparseMatrix* tsm = - new TripletSparseMatrix(program->NumParameterBlocks(), - program->NumResidualBlocks(), - 10 * program->NumResidualBlocks()); - int num_nonzeros = 0; - int* rows = tsm->mutable_rows(); - int* cols = tsm->mutable_cols(); - double* values = tsm->mutable_values(); - - const vector<ResidualBlock*>& residual_blocks = program->residual_blocks(); - for (int c = 0; c < residual_blocks.size(); ++c) { - const ResidualBlock* residual_block = residual_blocks[c]; - const int num_parameter_blocks = residual_block->NumParameterBlocks(); - ParameterBlock* const* parameter_blocks = - residual_block->parameter_blocks(); - - for (int j = 0; j < num_parameter_blocks; ++j) { - if (parameter_blocks[j]->IsConstant()) { - continue; - } - - // Re-size the matrix if needed. - if (num_nonzeros >= tsm->max_num_nonzeros()) { - tsm->set_num_nonzeros(num_nonzeros); - tsm->Reserve(2 * num_nonzeros); - rows = tsm->mutable_rows(); - cols = tsm->mutable_cols(); - values = tsm->mutable_values(); - } - CHECK_LT(num_nonzeros, tsm->max_num_nonzeros()); - - const int r = parameter_blocks[j]->index(); - rows[num_nonzeros] = r; - cols[num_nonzeros] = c; - values[num_nonzeros] = 1.0; - ++num_nonzeros; - } - } - - tsm->set_num_nonzeros(num_nonzeros); - return tsm; -} - -bool SolverImpl::ReorderProgramForSchurTypeLinearSolver( - const LinearSolverType linear_solver_type, - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - if (parameter_block_ordering->NumGroups() == 1) { - // If the user supplied an parameter_block_ordering with just one - // group, it is equivalent to the user supplying NULL as an - // parameter_block_ordering. Ceres is completely free to choose the - // parameter block ordering as it sees fit. For Schur type solvers, - // this means that the user wishes for Ceres to identify the - // e_blocks, which we do by computing a maximal independent set. - vector<ParameterBlock*> schur_ordering; - const int num_eliminate_blocks = - ComputeStableSchurOrdering(*program, &schur_ordering); - - CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - // Update the parameter_block_ordering object. - for (int i = 0; i < schur_ordering.size(); ++i) { - double* parameter_block = schur_ordering[i]->mutable_user_state(); - const int group_id = (i < num_eliminate_blocks) ? 0 : 1; - parameter_block_ordering->AddElementToGroup(parameter_block, group_id); - } - - // We could call ApplyUserOrdering but this is cheaper and - // simpler. - swap(*program->mutable_parameter_blocks(), schur_ordering); - } else { - // The user provided an ordering with more than one elimination - // group. Trust the user and apply the ordering. - if (!ApplyUserOrdering(parameter_map, - parameter_block_ordering, - program, - error)) { - return false; - } - } - - // Pre-order the columns corresponding to the schur complement if - // possible. -#if !defined(CERES_NO_SUITESPARSE) && !defined(CERES_NO_CAMD) - if (linear_solver_type == SPARSE_SCHUR && - sparse_linear_algebra_library_type == SUITE_SPARSE) { - vector<int> constraints; - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); - - for (int i = 0; i < parameter_blocks.size(); ++i) { - constraints.push_back( - parameter_block_ordering->GroupId( - parameter_blocks[i]->mutable_user_state())); - } - - // Renumber the entries of constraints to be contiguous integers - // as camd requires that the group ids be in the range [0, - // parameter_blocks.size() - 1]. - SolverImpl::CompactifyArray(&constraints); - - // Set the offsets and index for CreateJacobianSparsityTranspose. - program->SetParameterOffsetsAndIndex(); - // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( - SolverImpl::CreateJacobianBlockSparsityTranspose(program)); - - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - - vector<int> ordering(parameter_blocks.size(), 0); - ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, - &constraints[0], - &ordering[0]); - ss.Free(block_jacobian_transpose); - - const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); - for (int i = 0; i < program->NumParameterBlocks(); ++i) { - parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; - } - } -#endif - - program->SetParameterOffsetsAndIndex(); - // Schur type solvers also require that their residual blocks be - // lexicographically ordered. - const int num_eliminate_blocks = - parameter_block_ordering->group_to_elements().begin()->second.size(); - return LexicographicallyOrderResidualBlocks(num_eliminate_blocks, - program, - error); -} - -bool SolverImpl::ReorderProgramForSparseNormalCholesky( - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - // Set the offsets and index for CreateJacobianSparsityTranspose. - program->SetParameterOffsetsAndIndex(); - // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( - SolverImpl::CreateJacobianBlockSparsityTranspose(program)); - - vector<int> ordering(program->NumParameterBlocks(), 0); - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); - - if (sparse_linear_algebra_library_type == SUITE_SPARSE) { -#ifdef CERES_NO_SUITESPARSE - *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITE_SPARSE because " - "SuiteSparse was not enabled when Ceres was built."; - return false; -#else - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - -# ifdef CERES_NO_CAMD - // No cholmod_camd, so ignore user's parameter_block_ordering and - // use plain old AMD. - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); -# else - if (parameter_block_ordering->NumGroups() > 1) { - // If the user specified more than one elimination groups use them - // to constrain the ordering. - vector<int> constraints; - for (int i = 0; i < parameter_blocks.size(); ++i) { - constraints.push_back( - parameter_block_ordering->GroupId( - parameter_blocks[i]->mutable_user_state())); - } - ss.ConstrainedApproximateMinimumDegreeOrdering( - block_jacobian_transpose, - &constraints[0], - &ordering[0]); - } else { - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, - &ordering[0]); - } -# endif // CERES_NO_CAMD - - ss.Free(block_jacobian_transpose); -#endif // CERES_NO_SUITESPARSE - - } else if (sparse_linear_algebra_library_type == CX_SPARSE) { -#ifndef CERES_NO_CXSPARSE - - // CXSparse works with J'J instead of J'. So compute the block - // sparsity for J'J and compute an approximate minimum degree - // ordering. - CXSparse cxsparse; - cs_di* block_jacobian_transpose; - block_jacobian_transpose = - cxsparse.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose); - cs_di* block_hessian = - cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian); - cxsparse.Free(block_jacobian); - cxsparse.Free(block_jacobian_transpose); - - cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, &ordering[0]); - cxsparse.Free(block_hessian); -#else // CERES_NO_CXSPARSE - *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because " - "CXSparse was not enabled when Ceres was built."; - return false; -#endif // CERES_NO_CXSPARSE - } else { - *error = "Unknown sparse linear algebra library."; - return false; - } - - // Apply ordering. - const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); - for (int i = 0; i < program->NumParameterBlocks(); ++i) { - parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; - } - - program->SetParameterOffsetsAndIndex(); - return true; -} - -void SolverImpl::CompactifyArray(vector<int>* array_ptr) { - vector<int>& array = *array_ptr; - const set<int> unique_group_ids(array.begin(), array.end()); - map<int, int> group_id_map; - for (set<int>::const_iterator it = unique_group_ids.begin(); - it != unique_group_ids.end(); - ++it) { - InsertOrDie(&group_id_map, *it, group_id_map.size()); - } - - for (int i = 0; i < array.size(); ++i) { - array[i] = group_id_map[array[i]]; - } -} - -} // namespace internal -} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h deleted file mode 100644 index 631add59bdb..00000000000 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h +++ /dev/null @@ -1,228 +0,0 @@ -// 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) - -#ifndef CERES_INTERNAL_SOLVER_IMPL_H_ -#define CERES_INTERNAL_SOLVER_IMPL_H_ - -#include <set> -#include <string> -#include <vector> -#include "ceres/internal/port.h" -#include "ceres/ordered_groups.h" -#include "ceres/problem_impl.h" -#include "ceres/solver.h" - -namespace ceres { -namespace internal { - -class CoordinateDescentMinimizer; -class Evaluator; -class LinearSolver; -class Program; -class TripletSparseMatrix; - -class SolverImpl { - public: - // Mirrors the interface in solver.h, but exposes implementation - // details for testing internally. - static void Solve(const Solver::Options& options, - ProblemImpl* problem_impl, - Solver::Summary* summary); - - static void TrustRegionSolve(const Solver::Options& options, - ProblemImpl* problem_impl, - Solver::Summary* summary); - - // Run the TrustRegionMinimizer for the given evaluator and configuration. - static void TrustRegionMinimize( - const Solver::Options &options, - Program* program, - CoordinateDescentMinimizer* inner_iteration_minimizer, - Evaluator* evaluator, - LinearSolver* linear_solver, - Solver::Summary* summary); - - static void LineSearchSolve(const Solver::Options& options, - ProblemImpl* problem_impl, - Solver::Summary* summary); - - // Run the LineSearchMinimizer for the given evaluator and configuration. - static void LineSearchMinimize(const Solver::Options &options, - Program* program, - Evaluator* evaluator, - Solver::Summary* summary); - - // Create the transformed Program, which has all the fixed blocks - // and residuals eliminated, and in the case of automatic schur - // ordering, has the E blocks first in the resulting program, with - // options.num_eliminate_blocks set appropriately. - // - // If fixed_cost is not NULL, the residual blocks that are removed - // are evaluated and the sum of their cost is returned in fixed_cost. - static Program* CreateReducedProgram(Solver::Options* options, - ProblemImpl* problem_impl, - double* fixed_cost, - string* message); - - // Create the appropriate linear solver, taking into account any - // config changes decided by CreateTransformedProgram(). The - // selected linear solver, which may be different from what the user - // selected; consider the case that the remaining elimininated - // blocks is zero after removing fixed blocks. - static LinearSolver* CreateLinearSolver(Solver::Options* options, - string* message); - - // Reorder the residuals for program, if necessary, so that the - // residuals involving e block (i.e., the first num_eliminate_block - // parameter blocks) occur together. This is a necessary condition - // for the Schur eliminator. - static bool LexicographicallyOrderResidualBlocks( - const int num_eliminate_blocks, - Program* program, - string* message); - - // Create the appropriate evaluator for the transformed program. - static Evaluator* CreateEvaluator( - const Solver::Options& options, - const ProblemImpl::ParameterMap& parameter_map, - Program* program, - string* message); - - // Remove the fixed or unused parameter blocks and residuals - // depending only on fixed parameters from the program. - // - // If either linear_solver_ordering or inner_iteration_ordering are - // not NULL, the constant parameter blocks are removed from them - // too. - // - // If fixed_cost is not NULL, the residual blocks that are removed - // are evaluated and the sum of their cost is returned in - // fixed_cost. - // - // If a failure is encountered, the function returns false with a - // description of the failure in message. - static bool RemoveFixedBlocksFromProgram( - Program* program, - ParameterBlockOrdering* linear_solver_ordering, - ParameterBlockOrdering* inner_iteration_ordering, - double* fixed_cost, - string* message); - - static bool IsOrderingValid(const Solver::Options& options, - const ProblemImpl* problem_impl, - string* message); - - static bool IsParameterBlockSetIndependent( - const set<double*>& parameter_block_ptrs, - const vector<ResidualBlock*>& residual_blocks); - - static CoordinateDescentMinimizer* CreateInnerIterationMinimizer( - const Solver::Options& options, - const Program& program, - const ProblemImpl::ParameterMap& parameter_map, - Solver::Summary* summary); - - // If the linear solver is of Schur type, then replace it with the - // closest equivalent linear solver. This is done when the user - // requested a Schur type solver but the problem structure makes it - // impossible to use one. - // - // If the linear solver is not of Schur type, the function is a - // no-op. - static void AlternateLinearSolverForSchurTypeLinearSolver( - Solver::Options* options); - - // Create a TripletSparseMatrix which contains the zero-one - // structure corresponding to the block sparsity of the transpose of - // the Jacobian matrix. - // - // Caller owns the result. - static TripletSparseMatrix* CreateJacobianBlockSparsityTranspose( - const Program* program); - - // Reorder the parameter blocks in program using the ordering - static bool ApplyUserOrdering( - const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* message); - - // Sparse cholesky factorization routines when doing the sparse - // cholesky factorization of the Jacobian matrix, reorders its - // columns to reduce the fill-in. Compute this permutation and - // re-order the parameter blocks. - // - // If the parameter_block_ordering contains more than one - // elimination group and support for constrained fill-reducing - // ordering is available in the sparse linear algebra library - // (SuiteSparse version >= 4.2.0) then the fill reducing - // ordering will take it into account, otherwise it will be ignored. - static bool ReorderProgramForSparseNormalCholesky( - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* message); - - // Schur type solvers require that all parameter blocks eliminated - // by the Schur eliminator occur before others and the residuals be - // sorted in lexicographic order of their parameter blocks. - // - // If the parameter_block_ordering only contains one elimination - // group then a maximal independent set is computed and used as the - // first elimination group, otherwise the user's ordering is used. - // - // If the linear solver type is SPARSE_SCHUR and support for - // constrained fill-reducing ordering is available in the sparse - // linear algebra library (SuiteSparse version >= 4.2.0) then - // columns of the schur complement matrix are ordered to reduce the - // fill-in the Cholesky factorization. - // - // Upon return, ordering contains the parameter block ordering that - // was used to order the program. - static bool ReorderProgramForSchurTypeLinearSolver( - const LinearSolverType linear_solver_type, - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* message); - - // array contains a list of (possibly repeating) non-negative - // integers. Let us assume that we have constructed another array - // `p` by sorting and uniqueing the entries of array. - // CompactifyArray replaces each entry in "array" with its position - // in `p`. - static void CompactifyArray(vector<int>* array); -}; - -} // namespace internal -} // namespace ceres - -#endif // CERES_INTERNAL_SOLVER_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_utils.cc b/extern/libmv/third_party/ceres/internal/ceres/solver_utils.cc new file mode 100644 index 00000000000..bd304e7ac5d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_utils.cc @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include <string> + +#include "ceres/internal/port.h" +#include "ceres/version.h" + +namespace ceres { +namespace internal { + +string VersionString() { + string value = string(CERES_VERSION_STRING); + +#ifdef CERES_NO_LAPACK + value += "-no_lapack"; +#else + value += "-lapack"; +#endif + +#ifndef CERES_NO_SUITESPARSE + value += "-suitesparse"; +#endif + +#ifndef CERES_NO_CXSPARSE + value += "-cxsparse"; +#endif + +#ifdef CERES_USE_EIGEN_SPARSE + value += "-eigensparse"; +#endif + +#ifdef CERES_RESTRUCT_SCHUR_SPECIALIZATIONS + value += "-no_schur_specializations"; +#endif + +#ifdef CERES_USE_OPENMP + value += "-openmp"; +#else + value += "-no_openmp"; +#endif + +#ifdef CERES_NO_CUSTOM_BLAS + value += "-no_custom_blas"; +#endif + + return value; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_utils.h b/extern/libmv/third_party/ceres/internal/ceres/solver_utils.h new file mode 100644 index 00000000000..41c8a6e1292 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_utils.h @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include <algorithm> +#include <string> + +namespace ceres { +namespace internal { + +template <typename SummaryType> +bool IsSolutionUsable(const SummaryType& summary) { + return (summary.termination_type == CONVERGENCE || + summary.termination_type == NO_CONVERGENCE || + summary.termination_type == USER_SUCCESS); +} + +template <typename SummaryType> +void SetSummaryFinalCost(SummaryType* summary) { + summary->final_cost = summary->initial_cost; + // We need the loop here, instead of just looking at the last + // iteration because the minimizer maybe making non-monotonic steps. + for (int i = 0; i < summary->iterations.size(); ++i) { + const IterationSummary& iteration_summary = summary->iterations[i]; + summary->final_cost = min(iteration_summary.cost, summary->final_cost); + } +} + +string VersionString(); + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc index cf5bb235b46..94f7e5803c4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -28,11 +28,6 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -// This include must come before any #ifndef check on Ceres compile options. -#include "ceres/internal/port.h" - -#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE) - #include "ceres/sparse_normal_cholesky_solver.h" #include <algorithm> @@ -48,9 +43,67 @@ #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "ceres/wall_time.h" +#include "Eigen/SparseCore" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/SparseCholesky" +#endif namespace ceres { namespace internal { +namespace { + +#ifdef CERES_USE_EIGEN_SPARSE +// A templated factorized and solve function, which allows us to use +// the same code independent of whether a AMD or a Natural ordering is +// used. +template <typename SimplicialCholeskySolver> +LinearSolver::Summary SimplicialLDLTSolve( + Eigen::MappedSparseMatrix<double, Eigen::ColMajor>& lhs, + const bool do_symbolic_analysis, + SimplicialCholeskySolver* solver, + double* rhs_and_solution, + EventLogger* event_logger) { + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + if (do_symbolic_analysis) { + solver->analyzePattern(lhs); + event_logger->AddEvent("Analyze"); + if (solver->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "Eigen failure. Unable to find symbolic factorization."; + return summary; + } + } + + solver->factorize(lhs); + event_logger->AddEvent("Factorize"); + if (solver->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to find numeric factorization."; + return summary; + } + + const Vector rhs = VectorRef(rhs_and_solution, lhs.cols()); + + VectorRef(rhs_and_solution, lhs.cols()) = solver->solve(rhs); + event_logger->AddEvent("Solve"); + if (solver->info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to do triangular solve."; + return summary; + } + + return summary; +} + +#endif // CERES_USE_EIGEN_SPARSE + +} // namespace SparseNormalCholeskySolver::SparseNormalCholeskySolver( const LinearSolver::Options& options) @@ -60,19 +113,15 @@ SparseNormalCholeskySolver::SparseNormalCholeskySolver( } void SparseNormalCholeskySolver::FreeFactorization() { -#ifndef CERES_NO_SUITESPARSE if (factor_ != NULL) { ss_.Free(factor_); factor_ = NULL; } -#endif // CERES_NO_SUITESPARSE -#ifndef CERES_NO_CXSPARSE if (cxsparse_factor_ != NULL) { cxsparse_.Free(cxsparse_factor_); cxsparse_factor_ = NULL; } -#endif // CERES_NO_CXSPARSE } SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { @@ -111,6 +160,9 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( case CX_SPARSE: summary = SolveImplUsingCXSparse(A, per_solve_options, x); break; + case EIGEN_SPARSE: + summary = SolveImplUsingEigen(A, per_solve_options, x); + break; default: LOG(FATAL) << "Unknown sparse linear algebra library : " << options_.sparse_linear_algebra_library_type; @@ -123,11 +175,120 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( return summary; } -#ifndef CERES_NO_CXSPARSE +LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( + CompressedRowSparseMatrix* A, + const LinearSolver::PerSolveOptions& per_solve_options, + double * rhs_and_solution) { +#ifndef CERES_USE_EIGEN_SPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE " + "because Ceres was not built with support for " + "Eigen's SimplicialLDLT decomposition. " + "This requires enabling building with -DEIGENSPARSE=ON."; + return summary; + +#else + + EventLogger event_logger("SparseNormalCholeskySolver::Eigen::Solve"); + // Compute the normal equations. J'J delta = J'f and solve them + // using a sparse Cholesky factorization. Notice that when compared + // to SuiteSparse we have to explicitly compute the normal equations + // before they can be factorized. CHOLMOD/SuiteSparse on the other + // hand can just work off of Jt to compute the Cholesky + // factorization of the normal equations. + // + // TODO(sameeragarwal): See note about how this maybe a bad idea for + // dynamic sparsity. + if (outer_product_.get() == NULL) { + outer_product_.reset( + CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( + *A, &pattern_)); + } + + CompressedRowSparseMatrix::ComputeOuterProduct( + *A, pattern_, outer_product_.get()); + + // Map to an upper triangular column major matrix. + // + // outer_product_ is a compressed row sparse matrix and in lower + // triangular form, when mapped to a compressed column sparse + // matrix, it becomes an upper triangular matrix. + Eigen::MappedSparseMatrix<double, Eigen::ColMajor> AtA( + outer_product_->num_rows(), + outer_product_->num_rows(), + outer_product_->num_nonzeros(), + outer_product_->mutable_rows(), + outer_product_->mutable_cols(), + outer_product_->mutable_values()); + + // Dynamic sparsity means that we cannot depend on a static analysis + // of sparsity structure of the jacobian, so we compute a new + // symbolic factorization every time. + if (options_.dynamic_sparsity) { + amd_ldlt_.reset(NULL); + } + + bool do_symbolic_analysis = false; + + // If using post ordering or dynamic sparsity, or an old version of + // Eigen, we cannot depend on a preordered jacobian, so we work with + // a SimplicialLDLT decomposition with AMD ordering. + if (options_.use_postordering || + options_.dynamic_sparsity || + !EIGEN_VERSION_AT_LEAST(3, 2, 2)) { + if (amd_ldlt_.get() == NULL) { + amd_ldlt_.reset(new SimplicialLDLTWithAMDOrdering); + do_symbolic_analysis = true; + } + + return SimplicialLDLTSolve(AtA, + do_symbolic_analysis, + amd_ldlt_.get(), + rhs_and_solution, + &event_logger); + } + +#if EIGEN_VERSION_AT_LEAST(3,2,2) + // The common case + if (natural_ldlt_.get() == NULL) { + natural_ldlt_.reset(new SimplicialLDLTWithNaturalOrdering); + do_symbolic_analysis = true; + } + + return SimplicialLDLTSolve(AtA, + do_symbolic_analysis, + natural_ldlt_.get(), + rhs_and_solution, + &event_logger); +#endif + +#endif // EIGEN_USE_EIGEN_SPARSE +} + + + LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( CompressedRowSparseMatrix* A, const LinearSolver::PerSolveOptions& per_solve_options, double * rhs_and_solution) { +#ifdef CERES_NO_CXSPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE " + "because Ceres was not built with support for CXSparse. " + "This requires enabling building with -DCXSPARSE=ON."; + + return summary; + +#else + EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve"); LinearSolver::Summary summary; @@ -137,11 +298,14 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( // Compute the normal equations. J'J delta = J'f and solve them // using a sparse Cholesky factorization. Notice that when compared - // to SuiteSparse we have to explicitly compute the transpose of Jt, - // and then the normal equations before they can be - // factorized. CHOLMOD/SuiteSparse on the other hand can just work - // off of Jt to compute the Cholesky factorization of the normal - // equations. + // to SuiteSparse we have to explicitly compute the normal equations + // before they can be factorized. CHOLMOD/SuiteSparse on the other + // hand can just work off of Jt to compute the Cholesky + // factorization of the normal equations. + // + // TODO(sameeragarwal): If dynamic sparsity is enabled, then this is + // not a good idea performance wise, since the jacobian has far too + // many entries and the program will go crazy with memory. if (outer_product_.get() == NULL) { outer_product_.reset( CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( @@ -179,30 +343,35 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; summary.message = "CXSparse failure. Unable to find symbolic factorization."; - } else if (!cxsparse_.SolveCholesky(AtA, cxsparse_factor_, rhs_and_solution)) { + } else if (!cxsparse_.SolveCholesky(AtA, + cxsparse_factor_, + rhs_and_solution)) { summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "CXSparse::SolveCholesky failed."; } event_logger.AddEvent("Solve"); return summary; -} -#else -LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( - CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& per_solve_options, - double * rhs_and_solution) { - LOG(FATAL) << "No CXSparse support in Ceres."; - - // Unreachable but MSVC does not know this. - return LinearSolver::Summary(); -} #endif +} -#ifndef CERES_NO_SUITESPARSE LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( CompressedRowSparseMatrix* A, const LinearSolver::PerSolveOptions& per_solve_options, double * rhs_and_solution) { +#ifdef CERES_NO_SUITESPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE " + "because Ceres was not built with support for SuiteSparse. " + "This requires enabling building with -DSUITESPARSE=ON."; + return summary; + +#else + EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve"); LinearSolver::Summary summary; summary.termination_type = LINEAR_SOLVER_SUCCESS; @@ -226,7 +395,8 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( if (options_.dynamic_sparsity) { factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message); } else { - factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, &summary.message); + factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, + &summary.message); } } } @@ -234,6 +404,8 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( if (factor_ == NULL) { summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + // No need to set message as it has already been set by the + // symbolic analysis routines above. return summary; } @@ -242,7 +414,9 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( return summary; } - cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution, num_cols, num_cols); + cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution, + num_cols, + num_cols); cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message); event_logger.AddEvent("Solve"); @@ -251,25 +425,15 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution)); ss_.Free(solution); } else { + // No need to set message as it has already been set by the + // numeric factorization routine above. summary.termination_type = LINEAR_SOLVER_FAILURE; } event_logger.AddEvent("Teardown"); return summary; -} -#else -LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( - CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& per_solve_options, - double * rhs_and_solution) { - LOG(FATAL) << "No SuiteSparse support in Ceres."; - - // Unreachable but MSVC does not know this. - return LinearSolver::Summary(); -} #endif +} } // namespace internal } // namespace ceres - -#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE) diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h index ed777a118ae..12c05245075 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h @@ -34,15 +34,19 @@ #ifndef CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ #define CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ +#include <vector> + // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" -#if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE) - -#include "ceres/cxsparse.h" #include "ceres/internal/macros.h" #include "ceres/linear_solver.h" #include "ceres/suitesparse.h" +#include "ceres/cxsparse.h" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/SparseCholesky" +#endif namespace ceres { namespace internal { @@ -74,6 +78,12 @@ class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { const LinearSolver::PerSolveOptions& options, double* rhs_and_solution); + // Crashes if CERES_USE_EIGEN_SPARSE is not defined. + LinearSolver::Summary SolveImplUsingEigen( + CompressedRowSparseMatrix* A, + const LinearSolver::PerSolveOptions& options, + double* rhs_and_solution); + void FreeFactorization(); SuiteSparse ss_; @@ -83,6 +93,32 @@ class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { CXSparse cxsparse_; // Cached factorization cs_dis* cxsparse_factor_; + +#ifdef CERES_USE_EIGEN_SPARSE + + // The preprocessor gymnastics here are dealing with the fact that + // before version 3.2.2, Eigen did not support a third template + // parameter to specify the ordering. +#if EIGEN_VERSION_AT_LEAST(3,2,2) + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper, + Eigen::NaturalOrdering<int> > + SimplicialLDLTWithNaturalOrdering; + scoped_ptr<SimplicialLDLTWithNaturalOrdering> natural_ldlt_; + + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper, + Eigen::AMDOrdering<int> > + SimplicialLDLTWithAMDOrdering; + scoped_ptr<SimplicialLDLTWithAMDOrdering> amd_ldlt_; + +#else + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper> + SimplicialLDLTWithAMDOrdering; + + scoped_ptr<SimplicialLDLTWithAMDOrdering> amd_ldlt_; +#endif + +#endif + scoped_ptr<CompressedRowSparseMatrix> outer_product_; vector<int> pattern_; const LinearSolver::Options options_; @@ -92,5 +128,4 @@ class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { } // namespace internal } // namespace ceres -#endif // !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARSE) #endif // CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h index 0a7ea97f2d7..baab8998b29 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h @@ -283,9 +283,24 @@ class SuiteSparse { #else // CERES_NO_SUITESPARSE -class SuiteSparse {}; typedef void cholmod_factor; +class SuiteSparse { + public: + // Defining this static function even when SuiteSparse is not + // available, allows client code to check for the presence of CAMD + // without checking for the absence of the CERES_NO_CAMD symbol. + // + // This is safer because the symbol maybe missing due to a user + // accidently not including suitesparse.h in their code when + // checking for the symbol. + static bool IsConstrainedApproximateMinimumDegreeOrderingAvailable() { + return false; + } + + void Free(void*) {}; +}; + #endif // CERES_NO_SUITESPARSE #endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc b/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc index 4be561960c9..926bced6226 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_minimizer.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -40,6 +40,7 @@ #include "Eigen/Core" #include "ceres/array_utils.h" +#include "ceres/coordinate_descent_minimizer.h" #include "ceres/evaluator.h" #include "ceres/file.h" #include "ceres/internal/eigen.h" @@ -128,9 +129,10 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, double iteration_start_time = start_time; Init(options); - Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator); - SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian); - TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy); + Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator.get()); + SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian.get()); + TrustRegionStrategy* strategy = + CHECK_NOTNULL(options_.trust_region_strategy.get()); const bool is_not_silent = !options.is_silent; @@ -253,7 +255,8 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, double candidate_cost = cost; double accumulated_candidate_model_cost_change = 0.0; int num_consecutive_invalid_steps = 0; - bool inner_iterations_are_enabled = options.inner_iteration_minimizer != NULL; + bool inner_iterations_are_enabled = + options.inner_iteration_minimizer.get() != NULL; while (true) { bool inner_iterations_were_useful = false; if (!RunCallbacks(options, iteration_summary, summary)) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_preprocessor.cc b/extern/libmv/third_party/ceres/internal/ceres/trust_region_preprocessor.cc new file mode 100644 index 00000000000..22ea1ec8c80 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_preprocessor.cc @@ -0,0 +1,358 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/trust_region_preprocessor.h" + +#include <numeric> +#include <string> +#include "ceres/callbacks.h" +#include "ceres/evaluator.h" +#include "ceres/linear_solver.h" +#include "ceres/minimizer.h" +#include "ceres/parameter_block.h" +#include "ceres/preconditioner.h" +#include "ceres/preprocessor.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/reorder_program.h" +#include "ceres/suitesparse.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { +namespace { + +ParameterBlockOrdering* CreateDefaultLinearSolverOrdering( + const Program& program) { + ParameterBlockOrdering* ordering = new ParameterBlockOrdering; + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ordering->AddElementToGroup( + const_cast<double*>(parameter_blocks[i]->user_state()), 0); + } + return ordering; +} + +// Check if all the user supplied values in the parameter blocks are +// sane or not, and if the program is feasible or not. +bool IsProgramValid(const Program& program, string* error) { + return (program.ParameterBlocksAreFinite(error) && + program.IsFeasible(error)); +} + +void AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( + Solver::Options* options) { + if (!IsSchurType(options->linear_solver_type)) { + return; + } + + const LinearSolverType linear_solver_type_given = options->linear_solver_type; + const PreconditionerType preconditioner_type_given = + options->preconditioner_type; + options->linear_solver_type = LinearSolver::LinearSolverForZeroEBlocks( + linear_solver_type_given); + + string message; + if (linear_solver_type_given == ITERATIVE_SCHUR) { + options->preconditioner_type = Preconditioner::PreconditionerForZeroEBlocks( + preconditioner_type_given); + + message = + StringPrintf( + "No E blocks. Switching from %s(%s) to %s(%s).", + LinearSolverTypeToString(linear_solver_type_given), + PreconditionerTypeToString(preconditioner_type_given), + LinearSolverTypeToString(options->linear_solver_type), + PreconditionerTypeToString(options->preconditioner_type)); + } else { + message = + StringPrintf( + "No E blocks. Switching from %s to %s.", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(options->linear_solver_type)); + } + + VLOG_IF(1, options->logging_type != SILENT) << message; +} + +// For Schur type and SPARSE_NORMAL_CHOLESKY linear solvers, reorder +// the program to reduce fill-in and increase cache coherency. +bool ReorderProgram(PreprocessedProblem* pp) { + Solver::Options& options = pp->options; + if (IsSchurType(options.linear_solver_type)) { + return ReorderProgramForSchurTypeLinearSolver( + options.linear_solver_type, + options.sparse_linear_algebra_library_type, + pp->problem->parameter_map(), + options.linear_solver_ordering.get(), + pp->reduced_program.get(), + &pp->error); + } + + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY && + !options.dynamic_sparsity) { + return ReorderProgramForSparseNormalCholesky( + options.sparse_linear_algebra_library_type, + *options.linear_solver_ordering, + pp->reduced_program.get(), + &pp->error); + } + + return true; +} + +// Configure and create a linear solver object. In doing so, if a +// sparse direct factorization based linear solver is being used, then +// find a fill reducing ordering and reorder the program as needed +// too. +bool SetupLinearSolver(PreprocessedProblem* pp) { + Solver::Options& options = pp->options; + if (options.linear_solver_ordering.get() == NULL) { + // If the user has not supplied a linear solver ordering, then we + // assume that they are giving all the freedom to us in choosing + // the best possible ordering. This intent can be indicated by + // putting all the parameter blocks in the same elimination group. + options.linear_solver_ordering.reset( + CreateDefaultLinearSolverOrdering(*pp->reduced_program)); + } else { + // If the user supplied an ordering, then check if the first + // elimination group is still non-empty after the reduced problem + // has been constructed. + // + // This is important for Schur type linear solvers, where the + // first elimination group is special -- it needs to be an + // independent set. + // + // If the first elimination group is empty, then we cannot use the + // user's requested linear solver (and a preconditioner as the + // case may be) so we must use a different one. + ParameterBlockOrdering* ordering = options.linear_solver_ordering.get(); + const int min_group_id = ordering->MinNonZeroGroup(); + ordering->Remove(pp->removed_parameter_blocks); + if (IsSchurType(options.linear_solver_type) && + min_group_id != ordering->MinNonZeroGroup()) { + AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( + &options); + } + } + + // Reorder the program to reduce fill in and improve cache coherency + // of the Jacobian. + if (!ReorderProgram(pp)) { + return false; + } + + // Configure the linear solver. + pp->linear_solver_options = LinearSolver::Options(); + pp->linear_solver_options.min_num_iterations = + options.min_linear_solver_iterations; + pp->linear_solver_options.max_num_iterations = + options.max_linear_solver_iterations; + pp->linear_solver_options.type = options.linear_solver_type; + pp->linear_solver_options.preconditioner_type = options.preconditioner_type; + pp->linear_solver_options.visibility_clustering_type = + options.visibility_clustering_type; + pp->linear_solver_options.sparse_linear_algebra_library_type = + options.sparse_linear_algebra_library_type; + pp->linear_solver_options.dense_linear_algebra_library_type = + options.dense_linear_algebra_library_type; + pp->linear_solver_options.use_explicit_schur_complement = + options.use_explicit_schur_complement; + pp->linear_solver_options.dynamic_sparsity = options.dynamic_sparsity; + pp->linear_solver_options.num_threads = options.num_linear_solver_threads; + + // Ignore user's postordering preferences and force it to be true if + // cholmod_camd is not available. This ensures that the linear + // solver does not assume that a fill-reducing pre-ordering has been + // done. + pp->linear_solver_options.use_postordering = options.use_postordering; + if (options.linear_solver_type == SPARSE_SCHUR && + options.sparse_linear_algebra_library_type == SUITE_SPARSE && + !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + pp->linear_solver_options.use_postordering = true; + } + + OrderingToGroupSizes(options.linear_solver_ordering.get(), + &pp->linear_solver_options.elimination_groups); + + // Schur type solvers expect at least two elimination groups. If + // there is only one elimination group, then it is guaranteed that + // this group only contains e_blocks. Thus we add a dummy + // elimination group with zero blocks in it. + if (IsSchurType(pp->linear_solver_options.type) && + pp->linear_solver_options.elimination_groups.size() == 1) { + pp->linear_solver_options.elimination_groups.push_back(0); + } + + pp->linear_solver.reset(LinearSolver::Create(pp->linear_solver_options)); + return (pp->linear_solver.get() != NULL); +} + +// Configure and create the evaluator. +bool SetupEvaluator(PreprocessedProblem* pp) { + const Solver::Options& options = pp->options; + pp->evaluator_options = Evaluator::Options(); + pp->evaluator_options.linear_solver_type = options.linear_solver_type; + pp->evaluator_options.num_eliminate_blocks = 0; + if (IsSchurType(options.linear_solver_type)) { + pp->evaluator_options.num_eliminate_blocks = + options + .linear_solver_ordering + ->group_to_elements().begin() + ->second.size(); + } + + pp->evaluator_options.num_threads = options.num_threads; + pp->evaluator_options.dynamic_sparsity = options.dynamic_sparsity; + pp->evaluator.reset(Evaluator::Create(pp->evaluator_options, + pp->reduced_program.get(), + &pp->error)); + + return (pp->evaluator.get() != NULL); +} + +// If the user requested inner iterations, then find an inner +// iteration ordering as needed and configure and create a +// CoordinateDescentMinimizer object to perform the inner iterations. +bool SetupInnerIterationMinimizer(PreprocessedProblem* pp) { + Solver::Options& options = pp->options; + if (!options.use_inner_iterations) { + return true; + } + + // With just one parameter block, the outer iteration of the trust + // region method and inner iterations are doing exactly the same + // thing, and thus inner iterations are not needed. + if (pp->reduced_program->NumParameterBlocks() == 1) { + LOG(WARNING) << "Reduced problem only contains one parameter block." + << "Disabling inner iterations."; + return true; + } + + if (options.inner_iteration_ordering.get() != NULL) { + // If the user supplied an ordering, then remove the set of + // inactive parameter blocks from it + options.inner_iteration_ordering->Remove(pp->removed_parameter_blocks); + if (options.inner_iteration_ordering->NumElements() == 0) { + LOG(WARNING) << "No remaining elements in the inner iteration ordering."; + return true; + } + + // Validate the reduced ordering. + if (!CoordinateDescentMinimizer::IsOrderingValid( + *pp->reduced_program, + *options.inner_iteration_ordering, + &pp->error)) { + return false; + } + } else { + // The user did not supply an ordering, so create one. + options.inner_iteration_ordering.reset( + CoordinateDescentMinimizer::CreateOrdering(*pp->reduced_program)); + } + + pp->inner_iteration_minimizer.reset(new CoordinateDescentMinimizer); + return pp->inner_iteration_minimizer->Init(*pp->reduced_program, + pp->problem->parameter_map(), + *options.inner_iteration_ordering, + &pp->error); +} + +// Configure and create a TrustRegionMinimizer object. +void SetupMinimizerOptions(PreprocessedProblem* pp) { + const Solver::Options& options = pp->options; + + SetupCommonMinimizerOptions(pp); + pp->minimizer_options.is_constrained = + pp->reduced_program->IsBoundsConstrained(); + pp->minimizer_options.jacobian.reset(pp->evaluator->CreateJacobian()); + pp->minimizer_options.inner_iteration_minimizer = + pp->inner_iteration_minimizer; + + TrustRegionStrategy::Options strategy_options; + strategy_options.linear_solver = pp->linear_solver.get(); + strategy_options.initial_radius = + options.initial_trust_region_radius; + strategy_options.max_radius = options.max_trust_region_radius; + strategy_options.min_lm_diagonal = options.min_lm_diagonal; + strategy_options.max_lm_diagonal = options.max_lm_diagonal; + strategy_options.trust_region_strategy_type = + options.trust_region_strategy_type; + strategy_options.dogleg_type = options.dogleg_type; + pp->minimizer_options.trust_region_strategy.reset( + CHECK_NOTNULL(TrustRegionStrategy::Create(strategy_options))); +} + +} // namespace + +TrustRegionPreprocessor::~TrustRegionPreprocessor() { +} + +bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* pp) { + CHECK_NOTNULL(pp); + pp->options = options; + ChangeNumThreadsIfNeeded(&pp->options); + + pp->problem = problem; + Program* program = problem->mutable_program(); + if (!IsProgramValid(*program, &pp->error)) { + return false; + } + + pp->reduced_program.reset( + program->CreateReducedProgram(&pp->removed_parameter_blocks, + &pp->fixed_cost, + &pp->error)); + + if (pp->reduced_program.get() == NULL) { + return false; + } + + if (pp->reduced_program->NumParameterBlocks() == 0) { + // The reduced problem has no parameter or residual blocks. There + // is nothing more to do. + return true; + } + + if (!SetupLinearSolver(pp) || + !SetupEvaluator(pp) || + !SetupInnerIterationMinimizer(pp)) { + return false; + } + + SetupMinimizerOptions(pp); + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_preprocessor.h b/extern/libmv/third_party/ceres/internal/ceres/trust_region_preprocessor.h new file mode 100644 index 00000000000..ba70ff92e73 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_preprocessor.h @@ -0,0 +1,50 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ +#define CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ + +#include "ceres/preprocessor.h" + +namespace ceres { +namespace internal { + +class TrustRegionPreprocessor : public Preprocessor { + public: + virtual ~TrustRegionPreprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* preprocessed_problem); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/types.cc b/extern/libmv/third_party/ceres/internal/ceres/types.cc index 5a344ea43d7..47102616ee8 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/types.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/types.cc @@ -96,6 +96,7 @@ const char* SparseLinearAlgebraLibraryTypeToString( switch (type) { CASESTR(SUITE_SPARSE); CASESTR(CX_SPARSE); + CASESTR(EIGEN_SPARSE); default: return "UNKNOWN"; } @@ -107,6 +108,7 @@ bool StringToSparseLinearAlgebraLibraryType( UpperCase(&value); STRENUM(SUITE_SPARSE); STRENUM(CX_SPARSE); + STRENUM(EIGEN_SPARSE); return false; } @@ -240,7 +242,7 @@ const char* NonlinearConjugateGradientTypeToString( NonlinearConjugateGradientType type) { switch (type) { CASESTR(FLETCHER_REEVES); - CASESTR(POLAK_RIBIRERE); + CASESTR(POLAK_RIBIERE); CASESTR(HESTENES_STIEFEL); default: return "UNKNOWN"; @@ -252,7 +254,7 @@ bool StringToNonlinearConjugateGradientType( NonlinearConjugateGradientType* type) { UpperCase(&value); STRENUM(FLETCHER_REEVES); - STRENUM(POLAK_RIBIRERE); + STRENUM(POLAK_RIBIERE); STRENUM(HESTENES_STIEFEL); return false; } @@ -261,8 +263,8 @@ const char* CovarianceAlgorithmTypeToString( CovarianceAlgorithmType type) { switch (type) { CASESTR(DENSE_SVD); - CASESTR(SPARSE_CHOLESKY); - CASESTR(SPARSE_QR); + CASESTR(EIGEN_SPARSE_QR); + CASESTR(SUITE_SPARSE_QR); default: return "UNKNOWN"; } @@ -273,8 +275,8 @@ bool StringToCovarianceAlgorithmType( CovarianceAlgorithmType* type) { UpperCase(&value); STRENUM(DENSE_SVD); - STRENUM(SPARSE_CHOLESKY); - STRENUM(SPARSE_QR); + STRENUM(EIGEN_SPARSE_QR); + STRENUM(SUITE_SPARSE_QR); return false; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc index b3ee185581f..da8beedc663 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc @@ -76,7 +76,8 @@ void ComputeVisibility(const CompressedRowBlockStructure& block_structure, } } -Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { +WeightedGraph<int>* CreateSchurComplementGraph( + const vector<set<int> >& visibility) { const time_t start_time = time(NULL); // Compute the number of e_blocks/point blocks. Since the visibility // set for each e_block/camera contains the set of e_blocks/points @@ -122,7 +123,7 @@ Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { } } - Graph<int>* graph = new Graph<int>(); + WeightedGraph<int>* graph = new WeightedGraph<int>; // Add vertices and initialize the pairs for self edges so that self // edges are guaranteed. This is needed for the Canonical views diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.h b/extern/libmv/third_party/ceres/internal/ceres/visibility.h index 5ddd3a56082..322efe9bea4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility.h +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.h @@ -72,9 +72,10 @@ void ComputeVisibility(const CompressedRowBlockStructure& block_structure, // matrix/Schur complement matrix obtained by eliminating the e_blocks // from the normal equations. // -// Caller acquires ownership of the returned Graph pointer +// Caller acquires ownership of the returned WeightedGraph pointer // (heap-allocated). -Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility); +WeightedGraph<int>* CreateSchurComplementGraph( + const vector<set<int> >& visibility); } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc index 695eedcc8d9..c7ed0493d8c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc @@ -167,9 +167,9 @@ void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( // maximum spanning forest of this graph. vector<set<int> > cluster_visibility; ComputeClusterVisibility(visibility, &cluster_visibility); - scoped_ptr<Graph<int> > cluster_graph( + scoped_ptr<WeightedGraph<int> > cluster_graph( CHECK_NOTNULL(CreateClusterGraph(cluster_visibility))); - scoped_ptr<Graph<int> > forest( + scoped_ptr<WeightedGraph<int> > forest( CHECK_NOTNULL(Degree2MaximumSpanningForest(*cluster_graph))); ForestToClusterPairs(*forest, &cluster_pairs_); } @@ -189,7 +189,7 @@ void VisibilityBasedPreconditioner::InitStorage( // memberships for each camera block. void VisibilityBasedPreconditioner::ClusterCameras( const vector<set<int> >& visibility) { - scoped_ptr<Graph<int> > schur_complement_graph( + scoped_ptr<WeightedGraph<int> > schur_complement_graph( CHECK_NOTNULL(CreateSchurComplementGraph(visibility))); HashMap<int, int> membership; @@ -498,7 +498,7 @@ bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal( // Convert a graph into a list of edges that includes self edges for // each vertex. void VisibilityBasedPreconditioner::ForestToClusterPairs( - const Graph<int>& forest, + const WeightedGraph<int>& forest, HashSet<pair<int, int> >* cluster_pairs) const { CHECK_NOTNULL(cluster_pairs)->clear(); const HashSet<int>& vertices = forest.vertices(); @@ -541,9 +541,9 @@ void VisibilityBasedPreconditioner::ComputeClusterVisibility( // Construct a graph whose vertices are the clusters, and the edge // weights are the number of 3D points visible to cameras in both the // vertices. -Graph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( +WeightedGraph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( const vector<set<int> >& cluster_visibility) const { - Graph<int>* cluster_graph = new Graph<int>; + WeightedGraph<int>* cluster_graph = new WeightedGraph<int>; for (int i = 0; i < num_clusters_; ++i) { cluster_graph->AddVertex(i); diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h index 70cea83bf56..2f6922dce54 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h @@ -156,8 +156,9 @@ class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { vector<int>* membership_vector) const; void ComputeClusterVisibility(const vector<set<int> >& visibility, vector<set<int> >* cluster_visibility) const; - Graph<int>* CreateClusterGraph(const vector<set<int> >& visibility) const; - void ForestToClusterPairs(const Graph<int>& forest, + WeightedGraph<int>* CreateClusterGraph( + const vector<set<int> >& visibility) const; + void ForestToClusterPairs(const WeightedGraph<int>& forest, HashSet<pair<int, int> >* cluster_pairs) const; void ComputeBlockPairsInPreconditioner(const CompressedRowBlockStructure& bs); bool IsBlockPairInPreconditioner(int block1, int block2) const; |