diff options
Diffstat (limited to 'extern/libmv/third_party/ceres/internal')
92 files changed, 5752 insertions, 1801 deletions
diff --git a/extern/libmv/third_party/ceres/internal/ceres/blas.cc b/extern/libmv/third_party/ceres/internal/ceres/blas.cc new file mode 100644 index 00000000000..f79b1ebfae1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/blas.cc @@ -0,0 +1,78 @@ +// 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) + +#include "ceres/blas.h" +#include "glog/logging.h" + +extern "C" void dsyrk_(char* uplo, + char* trans, + int* n, + int* k, + double* alpha, + double* a, + int* lda, + double* beta, + double* c, + int* ldc); + +namespace ceres { +namespace internal { + +void BLAS::SymmetricRankKUpdate(int num_rows, + int num_cols, + const double* a, + bool transpose, + double alpha, + double beta, + double* c) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a BLAS library."; +#else + char uplo = 'L'; + char trans = transpose ? 'T' : 'N'; + int n = transpose ? num_cols : num_rows; + int k = transpose ? num_rows : num_cols; + int lda = k; + int ldc = n; + dsyrk_(&uplo, + &trans, + &n, + &k, + &alpha, + const_cast<double*>(a), + &lda, + &beta, + c, + &ldc); +#endif +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/blas.h b/extern/libmv/third_party/ceres/internal/ceres/blas.h index 9629b3da550..2ab666395b9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/blas.h +++ b/extern/libmv/third_party/ceres/internal/ceres/blas.h @@ -28,377 +28,28 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) // -// Simple blas functions for use in the Schur Eliminator. These are -// fairly basic implementations which already yield a significant -// speedup in the eliminator performance. +// Wrapper functions around BLAS functions. #ifndef CERES_INTERNAL_BLAS_H_ #define CERES_INTERNAL_BLAS_H_ -#include "ceres/internal/eigen.h" -#include "glog/logging.h" - 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) \ - template<int kRowA, int kColA, int kRowB, int kColB, int kOperation> \ - inline void name(const double* A, \ - const int num_row_a, \ - const int num_col_a, \ - const double* B, \ - const int num_row_b, \ - const int num_col_b, \ - double* C, \ - const int start_row_c, \ - const int start_col_c, \ - const int row_stride_c, \ - const int col_stride_c) - -#define CERES_GEMM_NAIVE_HEADER \ - DCHECK_GT(num_row_a, 0); \ - DCHECK_GT(num_col_a, 0); \ - DCHECK_GT(num_row_b, 0); \ - DCHECK_GT(num_col_b, 0); \ - DCHECK_GE(start_row_c, 0); \ - DCHECK_GE(start_col_c, 0); \ - DCHECK_GT(row_stride_c, 0); \ - DCHECK_GT(col_stride_c, 0); \ - DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); \ - DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); \ - DCHECK((kRowB == Eigen::Dynamic) || (kRowB == num_row_b)); \ - DCHECK((kColB == Eigen::Dynamic) || (kColB == num_col_b)); \ - const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); \ - const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); \ - const int NUM_ROW_B = (kColB != Eigen::Dynamic ? kRowB : num_row_b); \ - const int NUM_COL_B = (kColB != Eigen::Dynamic ? kColB : num_col_b); - -#define CERES_GEMM_EIGEN_HEADER \ - const typename EigenTypes<kRowA, kColA>::ConstMatrixRef \ - Aref(A, num_row_a, num_col_a); \ - const typename EigenTypes<kRowB, kColB>::ConstMatrixRef \ - Bref(B, num_row_b, num_col_b); \ - MatrixRef Cref(C, row_stride_c, col_stride_c); \ - -#define CERES_CALL_GEMM(name) \ - name<kRowA, kColA, kRowB, kColB, kOperation>( \ - A, num_row_a, num_col_a, \ - B, num_row_b, num_col_b, \ - C, start_row_c, start_col_c, row_stride_c, col_stride_c); - - -// For the matrix-matrix functions below, there are three variants for -// each functionality. Foo, FooNaive and FooEigen. Foo is the one to -// be called by the user. FooNaive is a basic loop based -// implementation and FooEigen uses Eigen's implementation. Foo -// chooses between FooNaive and FooEigen depending on how many of the -// template arguments are fixed at compile time. Currently, FooEigen -// is called if all matrix dimensions are compile time -// constants. FooNaive is called otherwise. This leads to the best -// performance currently. -// -// The MatrixMatrixMultiply variants compute: -// -// C op A * B; -// -// The MatrixTransposeMatrixMultiply variants compute: -// -// C op A' * B -// -// where op can be +=, -=, or =. -// -// The template parameters (kRowA, kColA, kRowB, kColB) allow -// specialization of the loop at compile time. If this information is -// not available, then Eigen::Dynamic should be used as the template -// argument. -// -// kOperation = 1 -> C += A * B -// kOperation = -1 -> C -= A * B -// kOperation = 0 -> C = A * B -// -// The functions can write into matrices C which are larger than the -// matrix A * B. This is done by specifying the true size of C via -// row_stride_c and col_stride_c, and then indicating where A * B -// should be written into by start_row_c and start_col_c. -// -// Graphically if row_stride_c = 10, col_stride_c = 12, start_row_c = -// 4 and start_col_c = 5, then if A = 3x2 and B = 2x4, we get -// -// ------------ -// ------------ -// ------------ -// ------------ -// -----xxxx--- -// -----xxxx--- -// -----xxxx--- -// ------------ -// ------------ -// ------------ -// -CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen) { - CERES_GEMM_EIGEN_HEADER - Eigen::Block<MatrixRef, kRowA, kColB> - block(Cref, start_row_c, start_col_c, num_row_a, num_col_b); - - if (kOperation > 0) { - block CERES_MAYBE_NOALIAS += Aref * Bref; - } else if (kOperation < 0) { - block CERES_MAYBE_NOALIAS -= Aref * Bref; - } else { - block CERES_MAYBE_NOALIAS = Aref * Bref; - } -} - -CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { - CERES_GEMM_NAIVE_HEADER - DCHECK_EQ(NUM_COL_A, NUM_ROW_B); - - const int NUM_ROW_C = NUM_ROW_A; - const int NUM_COL_C = NUM_COL_B; - DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); - DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); - - for (int row = 0; row < NUM_ROW_C; ++row) { - for (int col = 0; col < NUM_COL_C; ++col) { - double tmp = 0.0; - for (int k = 0; k < NUM_COL_A; ++k) { - tmp += A[row * NUM_COL_A + k] * B[k * NUM_COL_B + col]; - } - - const int index = (row + start_row_c) * col_stride_c + start_col_c + col; - if (kOperation > 0) { - C[index] += tmp; - } else if (kOperation < 0) { - C[index] -= tmp; - } else { - C[index] = tmp; - } - } - } -} - -CERES_GEMM_BEGIN(MatrixMatrixMultiply) { -#ifdef CERES_NO_CUSTOM_BLAS - - CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) - return; - -#else - - if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && - kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { - CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) - } else { - CERES_CALL_GEMM(MatrixMatrixMultiplyNaive) - } - -#endif -} - -CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen) { - CERES_GEMM_EIGEN_HEADER - Eigen::Block<MatrixRef, kColA, kColB> block(Cref, - start_row_c, start_col_c, - num_col_a, num_col_b); - if (kOperation > 0) { - block CERES_MAYBE_NOALIAS += Aref.transpose() * Bref; - } else if (kOperation < 0) { - block CERES_MAYBE_NOALIAS -= Aref.transpose() * Bref; - } else { - block CERES_MAYBE_NOALIAS = Aref.transpose() * Bref; - } -} - -CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { - CERES_GEMM_NAIVE_HEADER - DCHECK_EQ(NUM_ROW_A, NUM_ROW_B); - - const int NUM_ROW_C = NUM_COL_A; - const int NUM_COL_C = NUM_COL_B; - DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); - DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); - - for (int row = 0; row < NUM_ROW_C; ++row) { - for (int col = 0; col < NUM_COL_C; ++col) { - double tmp = 0.0; - for (int k = 0; k < NUM_ROW_A; ++k) { - tmp += A[k * NUM_COL_A + row] * B[k * NUM_COL_B + col]; - } - - const int index = (row + start_row_c) * col_stride_c + start_col_c + col; - if (kOperation > 0) { - C[index]+= tmp; - } else if (kOperation < 0) { - C[index]-= tmp; - } else { - C[index]= tmp; - } - } - } -} - -CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply) { -#ifdef CERES_NO_CUSTOM_BLAS - - CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) - return; - -#else - - if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && - kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { - CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) - } else { - CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyNaive) - } - -#endif -} - -// Matrix-Vector multiplication -// -// c op A * b; -// -// where op can be +=, -=, or =. -// -// The template parameters (kRowA, kColA) allow specialization of the -// loop at compile time. If this information is not available, then -// Eigen::Dynamic should be used as the template argument. -// -// kOperation = 1 -> c += A' * b -// kOperation = -1 -> c -= A' * b -// kOperation = 0 -> c = A' * b -template<int kRowA, int kColA, int kOperation> -inline void MatrixVectorMultiply(const double* A, - const int num_row_a, - const int num_col_a, - const double* b, - double* c) { -#ifdef CERES_NO_CUSTOM_BLAS - const typename EigenTypes<kRowA, kColA>::ConstMatrixRef - Aref(A, num_row_a, num_col_a); - const typename EigenTypes<kColA>::ConstVectorRef bref(b, num_col_a); - typename EigenTypes<kRowA>::VectorRef cref(c, num_row_a); - - // lazyProduct works better than .noalias() for matrix-vector - // products. - if (kOperation > 0) { - cref += Aref.lazyProduct(bref); - } else if (kOperation < 0) { - cref -= Aref.lazyProduct(bref); - } else { - cref = Aref.lazyProduct(bref); - } -#else - - DCHECK_GT(num_row_a, 0); - DCHECK_GT(num_col_a, 0); - DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); - DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); - - const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); - const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); - - for (int row = 0; row < NUM_ROW_A; ++row) { - double tmp = 0.0; - for (int col = 0; col < NUM_COL_A; ++col) { - tmp += A[row * NUM_COL_A + col] * b[col]; - } - - if (kOperation > 0) { - c[row] += tmp; - } else if (kOperation < 0) { - c[row] -= tmp; - } else { - c[row] = tmp; - } - } -#endif // CERES_NO_CUSTOM_BLAS -} - -// Similar to MatrixVectorMultiply, except that A is transposed, i.e., -// -// c op A' * b; -template<int kRowA, int kColA, int kOperation> -inline void MatrixTransposeVectorMultiply(const double* A, - const int num_row_a, - const int num_col_a, - const double* b, - double* c) { -#ifdef CERES_NO_CUSTOM_BLAS - const typename EigenTypes<kRowA, kColA>::ConstMatrixRef - Aref(A, num_row_a, num_col_a); - const typename EigenTypes<kRowA>::ConstVectorRef bref(b, num_row_a); - typename EigenTypes<kColA>::VectorRef cref(c, num_col_a); - - // lazyProduct works better than .noalias() for matrix-vector - // products. - if (kOperation > 0) { - cref += Aref.transpose().lazyProduct(bref); - } else if (kOperation < 0) { - cref -= Aref.transpose().lazyProduct(bref); - } else { - cref = Aref.transpose().lazyProduct(bref); - } -#else - - DCHECK_GT(num_row_a, 0); - DCHECK_GT(num_col_a, 0); - DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); - DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); - - const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); - const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); - - for (int row = 0; row < NUM_COL_A; ++row) { - double tmp = 0.0; - for (int col = 0; col < NUM_ROW_A; ++col) { - tmp += A[col * NUM_COL_A + row] * b[col]; - } - - if (kOperation > 0) { - c[row] += tmp; - } else if (kOperation < 0) { - c[row] -= tmp; - } else { - c[row] = tmp; - } - } -#endif // CERES_NO_CUSTOM_BLAS -} - - -#undef CERES_MAYBE_NOALIAS -#undef CERES_GEMM_BEGIN -#undef CERES_GEMM_EIGEN_HEADER -#undef CERES_GEMM_NAIVE_HEADER -#undef CERES_CALL_GEMM +class BLAS { + public: + // transpose = true : c = alpha * a'a + beta * c; + // transpose = false : c = alpha * aa' + beta * c; + // + // Assumes column major matrices. + static void SymmetricRankKUpdate(int num_rows, + int num_cols, + const double* a, + bool transpose, + double alpha, + double beta, + double* c); +}; } // 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 1d5f9d77ab0..29974d45bc9 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 @@ -41,7 +41,7 @@ namespace ceres { namespace internal { BlockJacobiPreconditioner::BlockJacobiPreconditioner( - const BlockSparseMatrixBase& A) + const BlockSparseMatrix& A) : num_rows_(A.num_rows()), block_structure_(*A.block_structure()) { // Calculate the amount of storage needed. @@ -66,19 +66,19 @@ BlockJacobiPreconditioner::BlockJacobiPreconditioner( BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} -bool BlockJacobiPreconditioner::Update(const BlockSparseMatrixBase& A, - const double* D) { +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; - const double* row_values = A.RowBlockValues(r); for (int c = 0; c < cells.size(); ++c) { const int col_block_size = bs->cols[cells[c].block_id].size; - ConstMatrixRef m(row_values + cells[c].position, + ConstMatrixRef m(values + cells[c].position, row_block_size, col_block_size); @@ -111,7 +111,7 @@ bool BlockJacobiPreconditioner::Update(const BlockSparseMatrixBase& A, } block = block.selfadjointView<Eigen::Upper>() - .ldlt() + .llt() .solve(Matrix::Identity(size, size)); } return true; 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 ed5eebc8dc6..3505a01248b 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 @@ -37,7 +37,7 @@ namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; struct CompressedRowBlockStructure; class LinearOperator; @@ -51,20 +51,21 @@ class LinearOperator; // update the matrix by running Update(A, D). The values of the matrix A are // inspected to construct the preconditioner. The vector D is applied as the // D^TD diagonal term. -class BlockJacobiPreconditioner : public Preconditioner { +class BlockJacobiPreconditioner : public BlockSparseMatrixPreconditioner { public: // A must remain valid while the BlockJacobiPreconditioner is. - explicit BlockJacobiPreconditioner(const BlockSparseMatrixBase& A); + explicit BlockJacobiPreconditioner(const BlockSparseMatrix& A); virtual ~BlockJacobiPreconditioner(); // Preconditioner interface - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); 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_; } private: + virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); + std::vector<double*> blocks_; std::vector<double> block_storage_; int num_rows_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.cc new file mode 100644 index 00000000000..5b008e2c3d8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.cc @@ -0,0 +1,170 @@ +// 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) + +#include "ceres/block_random_access_crs_matrix.h" + +#include <algorithm> +#include <set> +#include <utility> +#include <vector> +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/mutex.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessCRSMatrix::BlockRandomAccessCRSMatrix( + const vector<int>& blocks, + const set<pair<int, int> >& block_pairs) + : kMaxRowBlocks(10 * 1000 * 1000), + blocks_(blocks) { + CHECK_LT(blocks.size(), kMaxRowBlocks); + + col_layout_.resize(blocks_.size(), 0); + row_strides_.resize(blocks_.size(), 0); + + // Build the row/column layout vector and count the number of scalar + // rows/columns. + int num_cols = 0; + for (int i = 0; i < blocks_.size(); ++i) { + col_layout_[i] = num_cols; + num_cols += blocks_[i]; + } + + // Walk the sparsity pattern and count the number of non-zeros. + int num_nonzeros = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + num_nonzeros += row_block_size * col_block_size; + } + + VLOG(2) << "Matrix Size [" << num_cols + << "," << num_cols + << "] " << num_nonzeros; + + crsm_.reset(new CompressedRowSparseMatrix(num_cols, num_cols, num_nonzeros)); + int* rows = crsm_->mutable_rows(); + int* cols = crsm_->mutable_cols(); + double* values = crsm_->mutable_values(); + + // Iterate over the sparsity pattern and fill the scalar sparsity + // pattern of the underlying compressed sparse row matrix. Along the + // way also fill out the Layout object which will allow random + // access into the CRS Matrix. + set<pair<int, int> >::const_iterator it = block_pairs.begin(); + vector<int> col_blocks; + int row_pos = 0; + rows[0] = 0; + while (it != block_pairs.end()) { + // Add entries to layout_ for all the blocks for this row. + col_blocks.clear(); + const int row_block_id = it->first; + const int row_block_size = blocks_[row_block_id]; + int num_cols = 0; + while ((it != block_pairs.end()) && (it->first == row_block_id)) { + layout_[IntPairToLong(it->first, it->second)] = + new CellInfo(values + num_cols); + col_blocks.push_back(it->second); + num_cols += blocks_[it->second]; + ++it; + }; + + // Count the number of non-zeros in the row block. + for (int j = 0; j < row_block_size; ++j) { + rows[row_pos + j + 1] = rows[row_pos + j] + num_cols; + } + + // Fill out the sparsity pattern for each row. + int col_pos = 0; + for (int j = 0; j < col_blocks.size(); ++j) { + const int col_block_id = col_blocks[j]; + const int col_block_size = blocks_[col_block_id]; + for (int r = 0; r < row_block_size; ++r) { + const int column_block_begin = rows[row_pos + r] + col_pos; + for (int c = 0; c < col_block_size; ++c) { + cols[column_block_begin + c] = col_layout_[col_block_id] + c; + } + } + col_pos += col_block_size; + } + + row_pos += row_block_size; + values += row_block_size * num_cols; + row_strides_[row_block_id] = num_cols; + } +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessCRSMatrix::~BlockRandomAccessCRSMatrix() { + // TODO(sameeragarwal) this should be rationalized going forward and + // perhaps moved into BlockRandomAccessMatrix. + for (LayoutType::iterator it = layout_.begin(); + it != layout_.end(); + ++it) { + delete it->second; + } +} + +CellInfo* BlockRandomAccessCRSMatrix::GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + const LayoutType::iterator it = + layout_.find(IntPairToLong(row_block_id, col_block_id)); + if (it == layout_.end()) { + return NULL; + } + + *row = 0; + *col = 0; + *row_stride = blocks_[row_block_id]; + *col_stride = row_strides_[row_block_id]; + return it->second; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessCRSMatrix::SetZero() { + crsm_->SetZero(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.h new file mode 100644 index 00000000000..11a203b8f4d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_crs_matrix.h @@ -0,0 +1,108 @@ +// 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_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/integral_types.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A square BlockRandomAccessMatrix where the underlying storage is a +// compressed row sparse matrix. The matrix need not be symmetric. +class BlockRandomAccessCRSMatrix : public BlockRandomAccessMatrix { + public: + // blocks is an array of block sizes. block_pairs is a set of + // <row_block_id, col_block_id> pairs to identify the non-zero cells + // of this matrix. + BlockRandomAccessCRSMatrix(const vector<int>& blocks, + const set<pair<int, int> >& block_pairs); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessCRSMatrix(); + + // BlockRandomAccessMatrix Interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + + // Since the matrix is square, num_rows() == num_cols(). + virtual int num_rows() const { return crsm_->num_rows(); } + virtual int num_cols() const { return crsm_->num_cols(); } + + // Access to the underlying matrix object. + const CompressedRowSparseMatrix* matrix() const { return crsm_.get(); } + CompressedRowSparseMatrix* mutable_matrix() { return crsm_.get(); } + + private: + int64 IntPairToLong(int a, int b) { + return a * kMaxRowBlocks + b; + } + + const int64 kMaxRowBlocks; + // row/column block sizes. + const vector<int> blocks_; + vector<int> col_layout_; + vector<int> row_strides_; + + // 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_; + + scoped_ptr<CompressedRowSparseMatrix> crsm_; + friend class BlockRandomAccessCRSMatrixTest; + CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessCRSMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_CRS_MATRIX_H_ 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 48a00437cf6..a6b5f39a985 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 @@ -74,7 +74,6 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { // This is not a thread safe method, it assumes that no cell is // locked. virtual void SetZero(); - virtual bool IsThreadSafe() const { return true; } // Since the matrix is square, num_rows() == num_cols(). virtual int num_rows() const { return tsm_->num_rows(); } diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc index ae36d60c900..a4872626114 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc @@ -33,10 +33,9 @@ #include <cstddef> #include <algorithm> #include <vector> -#include "ceres/blas.h" #include "ceres/block_structure.h" #include "ceres/internal/eigen.h" -#include "ceres/matrix_proto.h" +#include "ceres/small_blas.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -82,31 +81,6 @@ BlockSparseMatrix::BlockSparseMatrix( CHECK_NOTNULL(values_.get()); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -BlockSparseMatrix::BlockSparseMatrix(const SparseMatrixProto& outer_proto) { - CHECK(outer_proto.has_block_matrix()); - - const BlockSparseMatrixProto& proto = outer_proto.block_matrix(); - CHECK(proto.has_num_rows()); - CHECK(proto.has_num_cols()); - CHECK_EQ(proto.num_nonzeros(), proto.values_size()); - - num_rows_ = proto.num_rows(); - num_cols_ = proto.num_cols(); - num_nonzeros_ = proto.num_nonzeros(); - - // Copy out the values into *this. - values_.reset(new double[num_nonzeros_]); - for (int i = 0; i < proto.num_nonzeros(); ++i) { - values_[i] = proto.values(i); - } - - // Create the block structure according to the proto. - block_structure_.reset(new CompressedRowBlockStructure); - ProtoToBlockStructure(proto.block_structure(), block_structure_.get()); -} -#endif - void BlockSparseMatrix::SetZero() { fill(values_.get(), values_.get() + num_nonzeros_, 0.0); } @@ -243,21 +217,6 @@ const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() return block_structure_.get(); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void BlockSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { - outer_proto->Clear(); - - BlockSparseMatrixProto* proto = outer_proto->mutable_block_matrix(); - proto->set_num_rows(num_rows_); - proto->set_num_cols(num_cols_); - proto->set_num_nonzeros(num_nonzeros_); - for (int i = 0; i < num_nonzeros_; ++i) { - proto->add_values(values_[i]); - } - BlockStructureToProto(*block_structure_, proto->mutable_block_structure()); -} -#endif - void BlockSparseMatrix::ToTextFile(FILE* file) const { CHECK_NOTNULL(file); for (int i = 0; i < block_structure_->rows.size(); ++i) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h index 513d398c54d..e17d12a706e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h @@ -43,40 +43,8 @@ namespace ceres { namespace internal { -class SparseMatrixProto; class TripletSparseMatrix; -// A further extension of the SparseMatrix interface to support block-oriented -// matrices. The key addition is the RowBlockValues() accessor, which enables -// the lazy block sparse matrix implementation. -class BlockSparseMatrixBase : public SparseMatrix { - public: - BlockSparseMatrixBase() {} - virtual ~BlockSparseMatrixBase() {} - - // Convert this matrix into a triplet sparse matrix. - virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const = 0; - - // Returns a pointer to the block structure. Does not transfer - // ownership. - virtual const CompressedRowBlockStructure* block_structure() const = 0; - - // Returns a pointer to a row of the matrix. The returned array is only valid - // until the next call to RowBlockValues. The caller does not own the result. - // - // The returned array is laid out such that cells on the specified row are - // contiguous in the returned array, though neighbouring cells in row order - // may not be contiguous in the row values. The cell values for cell - // (row_block, cell_block) are found at offset - // - // block_structure()->rows[row_block].cells[cell_block].position - // - virtual const double* RowBlockValues(int row_block_index) const = 0; - - private: - CERES_DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrixBase); -}; - // This class implements the SparseMatrix interface for storing and // manipulating block sparse matrices. The block structure is stored // in the CompressedRowBlockStructure object and one is needed to @@ -85,7 +53,7 @@ class BlockSparseMatrixBase : public SparseMatrix { // // internal/ceres/block_structure.h // -class BlockSparseMatrix : public BlockSparseMatrixBase { +class BlockSparseMatrix : public SparseMatrix { public: // Construct a block sparse matrix with a fully initialized // CompressedRowBlockStructure objected. The matrix takes over @@ -95,11 +63,6 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { // CompressedRowBlockStructure objects. explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); - // Construct a block sparse matrix from a protocol buffer. -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit BlockSparseMatrix(const SparseMatrixProto& proto); -#endif - BlockSparseMatrix(); virtual ~BlockSparseMatrix(); @@ -110,9 +73,6 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { virtual void SquaredColumnNorm(double* x) const; virtual void ScaleColumns(const double* scale); virtual void ToDenseMatrix(Matrix* dense_matrix) const; -#ifndef CERES_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto* proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const { return num_rows_; } @@ -121,12 +81,8 @@ class BlockSparseMatrix : public BlockSparseMatrixBase { virtual const double* values() const { return values_.get(); } virtual double* mutable_values() { return values_.get(); } - // Implementation of BlockSparseMatrixBase interface. - virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; - virtual const CompressedRowBlockStructure* block_structure() const; - virtual const double* RowBlockValues(int row_block_index) const { - return values_.get(); - } + void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; + const CompressedRowBlockStructure* block_structure() const; private: int num_rows_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc index e61131192af..5a1a5e18336 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc @@ -29,7 +29,6 @@ // Author: sameeragarwal@google.com (Sameer Agarwal) #include "ceres/block_structure.h" -#include "ceres/matrix_proto.h" namespace ceres { namespace internal { @@ -38,55 +37,5 @@ bool CellLessThan(const Cell& lhs, const Cell& rhs) { return (lhs.block_id < rhs.block_id); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void ProtoToBlockStructure(const BlockStructureProto &proto, - CompressedRowBlockStructure *block_structure) { - // Decode the column blocks. - block_structure->cols.resize(proto.cols_size()); - for (int i = 0; i < proto.cols_size(); ++i) { - block_structure->cols[i].size = proto.cols(i).size(); - block_structure->cols[i].position = - proto.cols(i).position(); - } - // Decode the row structure. - block_structure->rows.resize(proto.rows_size()); - for (int i = 0; i < proto.rows_size(); ++i) { - const CompressedRowProto &row = proto.rows(i); - block_structure->rows[i].block.size = row.block().size(); - block_structure->rows[i].block.position = row.block().position(); - - // Copy the cells within the row. - block_structure->rows[i].cells.resize(row.cells_size()); - for (int j = 0; j < row.cells_size(); ++j) { - const CellProto &cell = row.cells(j); - block_structure->rows[i].cells[j].block_id = cell.block_id(); - block_structure->rows[i].cells[j].position = cell.position(); - } - } -} - -void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, - BlockStructureProto *proto) { - // Encode the column blocks. - for (int i = 0; i < block_structure.cols.size(); ++i) { - BlockProto *block = proto->add_cols(); - block->set_size(block_structure.cols[i].size); - block->set_position(block_structure.cols[i].position); - } - // Encode the row structure. - for (int i = 0; i < block_structure.rows.size(); ++i) { - CompressedRowProto *row = proto->add_rows(); - BlockProto *block = row->mutable_block(); - block->set_size(block_structure.rows[i].block.size); - block->set_position(block_structure.rows[i].block.position); - for (int j = 0; j < block_structure.rows[i].cells.size(); ++j) { - CellProto *cell = row->add_cells(); - cell->set_block_id(block_structure.rows[i].cells[j].block_id); - cell->set_position(block_structure.rows[i].cells[j].position); - } - } -} -#endif - } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/c_api.cc b/extern/libmv/third_party/ceres/internal/ceres/c_api.cc new file mode 100644 index 00000000000..1fd01c9f0bd --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/c_api.cc @@ -0,0 +1,188 @@ +// 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: mierle@gmail.com (Keir Mierle) +// +// An incomplete C API for Ceres. +// +// TODO(keir): Figure out why logging does not seem to work. + +#include "ceres/c_api.h" + +#include <vector> +#include <iostream> +#include <string> +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" +#include "ceres/problem.h" +#include "ceres/solver.h" +#include "ceres/types.h" // for std +#include "glog/logging.h" + +using ceres::Problem; + +void ceres_init() { + // This is not ideal, but it's not clear what to do if there is no gflags and + // no access to command line arguments. + char message[] = "<unknown>"; + google::InitGoogleLogging(message); +} + +ceres_problem_t* ceres_create_problem() { + return reinterpret_cast<ceres_problem_t*>(new Problem); +} + +void ceres_free_problem(ceres_problem_t* problem) { + delete reinterpret_cast<Problem*>(problem); +} + +// This cost function wraps a C-level function pointer from the user, to bridge +// between C and C++. +class CallbackCostFunction : public ceres::CostFunction { + public: + CallbackCostFunction(ceres_cost_function_t cost_function, + void* user_data, + int num_residuals, + int num_parameter_blocks, + int* parameter_block_sizes) + : cost_function_(cost_function), + user_data_(user_data) { + set_num_residuals(num_residuals); + for (int i = 0; i < num_parameter_blocks; ++i) { + mutable_parameter_block_sizes()->push_back(parameter_block_sizes[i]); + } + } + + virtual ~CallbackCostFunction() {} + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + return (*cost_function_)(user_data_, + const_cast<double**>(parameters), + residuals, + jacobians); + } + + private: + ceres_cost_function_t cost_function_; + void* user_data_; +}; + +// This loss function wraps a C-level function pointer from the user, to bridge +// between C and C++. +class CallbackLossFunction : public ceres::LossFunction { + public: + explicit CallbackLossFunction(ceres_loss_function_t loss_function, + void* user_data) + : loss_function_(loss_function), user_data_(user_data) {} + virtual void Evaluate(double sq_norm, double* rho) const { + (*loss_function_)(user_data_, sq_norm, rho); + } + + private: + ceres_loss_function_t loss_function_; + void* user_data_; +}; + +// Wrappers for the stock loss functions. +void* ceres_create_huber_loss_function_data(double a) { + return new ceres::HuberLoss(a); +} +void* ceres_create_softl1_loss_function_data(double a) { + return new ceres::SoftLOneLoss(a); +} +void* ceres_create_cauchy_loss_function_data(double a) { + return new ceres::CauchyLoss(a); +} +void* ceres_create_arctan_loss_function_data(double a) { + return new ceres::ArctanLoss(a); +} +void* ceres_create_tolerant_loss_function_data(double a, double b) { + return new ceres::TolerantLoss(a, b); +} + +void ceres_free_stock_loss_function_data(void* loss_function_data) { + delete reinterpret_cast<ceres::LossFunction*>(loss_function_data); +} + +void ceres_stock_loss_function(void* user_data, + double squared_norm, + double out[3]) { + reinterpret_cast<ceres::LossFunction*>(user_data) + ->Evaluate(squared_norm, out); +} + +ceres_residual_block_id_t* ceres_problem_add_residual_block( + ceres_problem_t* problem, + ceres_cost_function_t cost_function, + void* cost_function_data, + ceres_loss_function_t loss_function, + void* loss_function_data, + int num_residuals, + int num_parameter_blocks, + int* parameter_block_sizes, + double** parameters) { + Problem* ceres_problem = reinterpret_cast<Problem*>(problem); + + ceres::CostFunction* callback_cost_function = + new CallbackCostFunction(cost_function, + cost_function_data, + num_residuals, + num_parameter_blocks, + parameter_block_sizes); + + ceres::LossFunction* callback_loss_function = NULL; + if (loss_function != NULL) { + callback_loss_function = new CallbackLossFunction(loss_function, + loss_function_data); + } + + std::vector<double*> parameter_blocks(parameters, + parameters + num_parameter_blocks); + return reinterpret_cast<ceres_residual_block_id_t*>( + ceres_problem->AddResidualBlock(callback_cost_function, + callback_loss_function, + parameter_blocks)); +} + +void ceres_solve(ceres_problem_t* c_problem) { + Problem* problem = reinterpret_cast<Problem*>(c_problem); + + // TODO(keir): Obviously, this way of setting options won't scale or last. + // Instead, figure out a way to specify some of the options without + // duplicating everything. + ceres::Solver::Options options; + options.max_num_iterations = 100; + options.linear_solver_type = ceres::DENSE_QR; + options.minimizer_progress_to_stdout = true; + + ceres::Solver::Summary summary; + ceres::Solve(options, problem, &summary); + std::cout << summary.FullReport() << "\n"; +} diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc index e2e799fe607..9b8f9808cc9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc @@ -46,7 +46,7 @@ CgnrSolver::CgnrSolver(const LinearSolver::Options& options) } LinearSolver::Summary CgnrSolver::SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h index d560a9de58d..c63484c628b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h @@ -48,11 +48,11 @@ class BlockJacobiPreconditioner; // // as required for solving for x in the least squares sense. Currently only // block diagonal preconditioning is supported. -class CgnrSolver : public BlockSparseMatrixBaseSolver { +class CgnrSolver : public BlockSparseMatrixSolver { public: explicit CgnrSolver(const LinearSolver::Options& options); virtual Summary SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x); diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc new file mode 100644 index 00000000000..b62a6ed3830 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc @@ -0,0 +1,118 @@ +// 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) + +#include "ceres/compressed_col_sparse_matrix_utils.h" + +#include <vector> +#include <algorithm> +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +void CompressedColumnScalarMatrixToBlockMatrix(const int* scalar_rows, + const int* scalar_cols, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* block_rows, + vector<int>* block_cols) { + CHECK_NOTNULL(block_rows)->clear(); + CHECK_NOTNULL(block_cols)->clear(); + const int num_row_blocks = row_blocks.size(); + const int num_col_blocks = col_blocks.size(); + + vector<int> row_block_starts(num_row_blocks); + for (int i = 0, cursor = 0; i < num_row_blocks; ++i) { + row_block_starts[i] = cursor; + cursor += row_blocks[i]; + } + + // This loop extracts the block sparsity of the scalar sparse matrix + // It does so by iterating over the columns, but only considering + // the columns corresponding to the first element of each column + // block. Within each column, the inner loop iterates over the rows, + // and detects the presence of a row block by checking for the + // presence of a non-zero entry corresponding to its first element. + block_cols->push_back(0); + int c = 0; + for (int col_block = 0; col_block < num_col_blocks; ++col_block) { + int column_size = 0; + for (int idx = scalar_cols[c]; idx < scalar_cols[c + 1]; ++idx) { + vector<int>::const_iterator it = lower_bound(row_block_starts.begin(), + row_block_starts.end(), + scalar_rows[idx]); + // Since we are using lower_bound, it will return the row id + // where the row block starts. For everything but the first row + // of the block, where these values will be the same, we can + // skip, as we only need the first row to detect the presence of + // the block. + // + // For rows all but the first row in the last row block, + // lower_bound will return row_block_starts.end(), but those can + // be skipped like the rows in other row blocks too. + if (it == row_block_starts.end() || *it != scalar_rows[idx]) { + continue; + } + + block_rows->push_back(it - row_block_starts.begin()); + ++column_size; + } + block_cols->push_back(block_cols->back() + column_size); + c += col_blocks[col_block]; + } +} + +void BlockOrderingToScalarOrdering(const vector<int>& blocks, + const vector<int>& block_ordering, + vector<int>* scalar_ordering) { + CHECK_EQ(blocks.size(), block_ordering.size()); + const int num_blocks = blocks.size(); + + // block_starts = [0, block1, block1 + block2 ..] + vector<int> block_starts(num_blocks); + for (int i = 0, cursor = 0; i < num_blocks ; ++i) { + block_starts[i] = cursor; + cursor += blocks[i]; + } + + scalar_ordering->resize(block_starts.back() + blocks.back()); + int cursor = 0; + for (int i = 0; i < num_blocks; ++i) { + const int block_id = block_ordering[i]; + const int block_size = blocks[block_id]; + int block_position = block_starts[block_id]; + for (int j = 0; j < block_size; ++j) { + (*scalar_ordering)[cursor++] = block_position++; + } + } +} +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h new file mode 100644 index 00000000000..c8de2a1591a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_col_sparse_matrix_utils.h @@ -0,0 +1,142 @@ +// 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_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ +#define CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +// Extract the block sparsity pattern of the scalar compressed columns +// matrix and return it in compressed column form. The compressed +// column form is stored in two vectors block_rows, and block_cols, +// which correspond to the row and column arrays in a compressed +// column sparse matrix. +// +// If c_ij is the block in the matrix A corresponding to row block i +// and column block j, then it is expected that A contains at least +// one non-zero entry corresponding to the top left entry of c_ij, +// as that entry is used to detect the presence of a non-zero c_ij. +void CompressedColumnScalarMatrixToBlockMatrix(const int* scalar_rows, + const int* scalar_cols, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* block_rows, + vector<int>* block_cols); + +// Given a set of blocks and a permutation of these blocks, compute +// the corresponding "scalar" ordering, where the scalar ordering of +// size sum(blocks). +void BlockOrderingToScalarOrdering(const vector<int>& blocks, + const vector<int>& block_ordering, + vector<int>* scalar_ordering); + +// Solve the linear system +// +// R * solution = rhs +// +// Where R is an upper triangular compressed column sparse matrix. +template <typename IntegerType> +void SolveUpperTriangularInPlace(IntegerType num_cols, + const IntegerType* rows, + const IntegerType* cols, + const double* values, + double* rhs_and_solution) { + for (IntegerType c = num_cols - 1; c >= 0; --c) { + rhs_and_solution[c] /= values[cols[c + 1] - 1]; + for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { + const IntegerType r = rows[idx]; + const double v = values[idx]; + rhs_and_solution[r] -= v * rhs_and_solution[c]; + } + } +} + +// Solve the linear system +// +// R' * solution = rhs +// +// Where R is an upper triangular compressed column sparse matrix. +template <typename IntegerType> +void SolveUpperTriangularTransposeInPlace(IntegerType num_cols, + const IntegerType* rows, + const IntegerType* cols, + const double* values, + double* rhs_and_solution) { + for (IntegerType c = 0; c < num_cols; ++c) { + for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { + const IntegerType r = rows[idx]; + const double v = values[idx]; + rhs_and_solution[c] -= v * rhs_and_solution[r]; + } + rhs_and_solution[c] = rhs_and_solution[c] / values[cols[c + 1] - 1]; + } +} + +// Given a upper triangular matrix R in compressed column form, solve +// the linear system, +// +// R'R x = b +// +// Where b is all zeros except for rhs_nonzero_index, where it is +// equal to one. +// +// The function exploits this knowledge to reduce the number of +// floating point operations. +template <typename IntegerType> +void SolveRTRWithSparseRHS(IntegerType num_cols, + const IntegerType* rows, + const IntegerType* cols, + const double* values, + const int rhs_nonzero_index, + double* solution) { + fill(solution, solution + num_cols, 0.0); + solution[rhs_nonzero_index] = 1.0 / values[cols[rhs_nonzero_index + 1] - 1]; + + for (IntegerType c = rhs_nonzero_index + 1; c < num_cols; ++c) { + for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { + const IntegerType r = rows[idx]; + if (r < rhs_nonzero_index) continue; + const double v = values[idx]; + solution[c] -= v * solution[r]; + } + solution[c] = solution[c] / values[cols[c + 1] - 1]; + } + + SolveUpperTriangularInPlace(num_cols, rows, cols, values, solution); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc index 1b61468aaae..e200c928509 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc @@ -34,7 +34,8 @@ #include <vector> #include "ceres/crs_matrix.h" #include "ceres/internal/port.h" -#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -72,28 +73,27 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, int max_num_nonzeros) { num_rows_ = num_rows; num_cols_ = num_cols; - max_num_nonzeros_ = max_num_nonzeros; + rows_.resize(num_rows + 1, 0); + cols_.resize(max_num_nonzeros, 0); + values_.resize(max_num_nonzeros, 0.0); - VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ - << " max_num_nonzeros: " << max_num_nonzeros_ - << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(double); // NOLINT - - rows_.reset(new int[num_rows_ + 1]); - cols_.reset(new int[max_num_nonzeros_]); - values_.reset(new double[max_num_nonzeros_]); - fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); - fill(cols_.get(), cols_.get() + max_num_nonzeros_, 0); - fill(values_.get(), values_.get() + max_num_nonzeros_, 0); + VLOG(1) << "# of rows: " << num_rows_ + << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << cols_.size() + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + cols_.size() * sizeof(int) + // NOLINT + cols_.size() * sizeof(double); // NOLINT } CompressedRowSparseMatrix::CompressedRowSparseMatrix( const TripletSparseMatrix& m) { num_rows_ = m.num_rows(); num_cols_ = m.num_cols(); - max_num_nonzeros_ = m.max_num_nonzeros(); + + rows_.resize(num_rows_ + 1, 0); + cols_.resize(m.num_nonzeros(), 0); + values_.resize(m.max_num_nonzeros(), 0.0); // index is the list of indices into the TripletSparseMatrix m. vector<int> index(m.num_nonzeros(), 0); @@ -105,18 +105,13 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix( // are broken by column. sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); - VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ - << " max_num_nonzeros: " << max_num_nonzeros_ - << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(int) + // NOLINT - max_num_nonzeros_ * sizeof(double); // NOLINT - - rows_.reset(new int[num_rows_ + 1]); - cols_.reset(new int[max_num_nonzeros_]); - values_.reset(new double[max_num_nonzeros_]); - - // rows_ = 0 - fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + VLOG(1) << "# of rows: " << num_rows_ + << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << cols_.size() + << ". Allocating " + << ((num_rows_ + 1) * sizeof(int) + // NOLINT + cols_.size() * sizeof(int) + // NOLINT + cols_.size() * sizeof(double)); // NOLINT // Copy the contents of the cols and values array in the order given // by index and count the number of entries in each row. @@ -135,49 +130,15 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix( CHECK_EQ(num_nonzeros(), m.num_nonzeros()); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -CompressedRowSparseMatrix::CompressedRowSparseMatrix( - const SparseMatrixProto& outer_proto) { - CHECK(outer_proto.has_compressed_row_matrix()); - - const CompressedRowSparseMatrixProto& proto = - outer_proto.compressed_row_matrix(); - - num_rows_ = proto.num_rows(); - num_cols_ = proto.num_cols(); - - rows_.reset(new int[proto.rows_size()]); - cols_.reset(new int[proto.cols_size()]); - values_.reset(new double[proto.values_size()]); - - for (int i = 0; i < proto.rows_size(); ++i) { - rows_[i] = proto.rows(i); - } - - CHECK_EQ(proto.rows_size(), num_rows_ + 1); - CHECK_EQ(proto.cols_size(), proto.values_size()); - CHECK_EQ(proto.cols_size(), rows_[num_rows_]); - - for (int i = 0; i < proto.cols_size(); ++i) { - cols_[i] = proto.cols(i); - values_[i] = proto.values(i); - } - - max_num_nonzeros_ = proto.cols_size(); -} -#endif - CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, int num_rows) { CHECK_NOTNULL(diagonal); num_rows_ = num_rows; num_cols_ = num_rows; - max_num_nonzeros_ = num_rows; - - rows_.reset(new int[num_rows_ + 1]); - cols_.reset(new int[num_rows_]); - values_.reset(new double[num_rows_]); + rows_.resize(num_rows + 1); + cols_.resize(num_rows); + values_.resize(num_rows); rows_[0] = 0; for (int i = 0; i < num_rows_; ++i) { @@ -193,7 +154,7 @@ CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { } void CompressedRowSparseMatrix::SetZero() { - fill(values_.get(), values_.get() + num_nonzeros(), 0.0); + fill(values_.begin(), values_.end(), 0); } void CompressedRowSparseMatrix::RightMultiply(const double* x, @@ -248,83 +209,35 @@ void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { } } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { - CHECK_NOTNULL(outer_proto); - - outer_proto->Clear(); - CompressedRowSparseMatrixProto* proto - = outer_proto->mutable_compressed_row_matrix(); - - proto->set_num_rows(num_rows_); - proto->set_num_cols(num_cols_); - - for (int r = 0; r < num_rows_ + 1; ++r) { - proto->add_rows(rows_[r]); - } - - for (int idx = 0; idx < rows_[num_rows_]; ++idx) { - proto->add_cols(cols_[idx]); - proto->add_values(values_[idx]); - } -} -#endif - void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { CHECK_GE(delta_rows, 0); CHECK_LE(delta_rows, num_rows_); - int new_num_rows = num_rows_ - delta_rows; - - num_rows_ = new_num_rows; - int* new_rows = new int[num_rows_ + 1]; - copy(rows_.get(), rows_.get() + num_rows_ + 1, new_rows); - rows_.reset(new_rows); + num_rows_ -= delta_rows; + rows_.resize(num_rows_ + 1); } void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { CHECK_EQ(m.num_cols(), num_cols_); - // Check if there is enough space. If not, then allocate new arrays - // to hold the combined matrix and copy the contents of this matrix - // into it. - if (max_num_nonzeros_ < num_nonzeros() + m.num_nonzeros()) { - int new_max_num_nonzeros = num_nonzeros() + m.num_nonzeros(); - - VLOG(1) << "Reallocating " << sizeof(int) * new_max_num_nonzeros; // NOLINT - - int* new_cols = new int[new_max_num_nonzeros]; - copy(cols_.get(), cols_.get() + max_num_nonzeros_, new_cols); - cols_.reset(new_cols); - - double* new_values = new double[new_max_num_nonzeros]; - copy(values_.get(), values_.get() + max_num_nonzeros_, new_values); - values_.reset(new_values); - - max_num_nonzeros_ = new_max_num_nonzeros; + if (cols_.size() < num_nonzeros() + m.num_nonzeros()) { + cols_.resize(num_nonzeros() + m.num_nonzeros()); + values_.resize(num_nonzeros() + m.num_nonzeros()); } // Copy the contents of m into this matrix. - copy(m.cols(), m.cols() + m.num_nonzeros(), cols_.get() + num_nonzeros()); - copy(m.values(), - m.values() + m.num_nonzeros(), - values_.get() + num_nonzeros()); - - // Create the new rows array to hold the enlarged matrix. - int* new_rows = new int[num_rows_ + m.num_rows() + 1]; - // The first num_rows_ entries are the same - copy(rows_.get(), rows_.get() + num_rows_, new_rows); - + copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]); + copy(m.values(), m.values() + m.num_nonzeros(), &values_[num_nonzeros()]); + rows_.resize(num_rows_ + m.num_rows() + 1); // new_rows = [rows_, m.row() + rows_[num_rows_]] - fill(new_rows + num_rows_, - new_rows + num_rows_ + m.num_rows() + 1, + fill(rows_.begin() + num_rows_, + rows_.begin() + num_rows_ + m.num_rows() + 1, rows_[num_rows_]); for (int r = 0; r < m.num_rows() + 1; ++r) { - new_rows[num_rows_ + r] += m.rows()[r]; + rows_[num_rows_ + r] += m.rows()[r]; } - rows_.reset(new_rows); num_rows_ += m.num_rows(); } @@ -332,23 +245,122 @@ void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { CHECK_NOTNULL(file); for (int r = 0; r < num_rows_; ++r) { for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { - fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]); + fprintf(file, + "% 10d % 10d %17f\n", + r, + cols_[idx], + values_[idx]); } } } void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const { - matrix->num_rows = num_rows(); - matrix->num_cols = num_cols(); + matrix->num_rows = num_rows_; + matrix->num_cols = num_cols_; + matrix->rows = rows_; + matrix->cols = cols_; + matrix->values = values_; + // Trim. matrix->rows.resize(matrix->num_rows + 1); - matrix->cols.resize(num_nonzeros()); - matrix->values.resize(num_nonzeros()); + matrix->cols.resize(matrix->rows[matrix->num_rows]); + matrix->values.resize(matrix->rows[matrix->num_rows]); +} - copy(rows_.get(), rows_.get() + matrix->num_rows + 1, matrix->rows.begin()); - copy(cols_.get(), cols_.get() + num_nonzeros(), matrix->cols.begin()); - copy(values_.get(), values_.get() + num_nonzeros(), matrix->values.begin()); +void CompressedRowSparseMatrix::SolveLowerTriangularInPlace( + double* solution) const { + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1] - 1; ++idx) { + solution[r] -= values_[idx] * solution[cols_[idx]]; + } + solution[r] /= values_[rows_[r + 1] - 1]; + } } +void CompressedRowSparseMatrix::SolveLowerTriangularTransposeInPlace( + double* solution) const { + for (int r = num_rows_ - 1; r >= 0; --r) { + solution[r] /= values_[rows_[r + 1] - 1]; + for (int idx = rows_[r + 1] - 2; idx >= rows_[r]; --idx) { + solution[cols_[idx]] -= values_[idx] * solution[r]; + } + } +} + +CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + const double* diagonal, + const vector<int>& blocks) { + int num_rows = 0; + int num_nonzeros = 0; + for (int i = 0; i < blocks.size(); ++i) { + num_rows += blocks[i]; + num_nonzeros += blocks[i] * blocks[i]; + } + + CompressedRowSparseMatrix* matrix = + new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros); + + int* rows = matrix->mutable_rows(); + int* cols = matrix->mutable_cols(); + double* values = matrix->mutable_values(); + fill(values, values + num_nonzeros, 0.0); + + int idx_cursor = 0; + int col_cursor = 0; + for (int i = 0; i < blocks.size(); ++i) { + const int block_size = blocks[i]; + for (int r = 0; r < block_size; ++r) { + *(rows++) = idx_cursor; + values[idx_cursor + r] = diagonal[col_cursor + r]; + for (int c = 0; c < block_size; ++c, ++idx_cursor) { + *(cols++) = col_cursor + c; + } + } + col_cursor += block_size; + } + *rows = idx_cursor; + + *matrix->mutable_row_blocks() = blocks; + *matrix->mutable_col_blocks() = blocks; + + CHECK_EQ(idx_cursor, num_nonzeros); + CHECK_EQ(col_cursor, num_rows); + return matrix; +} + +CompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const { + CompressedRowSparseMatrix* transpose = + new CompressedRowSparseMatrix(num_cols_, num_rows_, num_nonzeros()); + + int* transpose_rows = transpose->mutable_rows(); + int* transpose_cols = transpose->mutable_cols(); + double* transpose_values = transpose->mutable_values(); + + for (int idx = 0; idx < num_nonzeros(); ++idx) { + ++transpose_rows[cols_[idx] + 1]; + } + + for (int i = 1; i < transpose->num_rows() + 1; ++i) { + transpose_rows[i] += transpose_rows[i - 1]; + } + + for (int r = 0; r < num_rows(); ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + const int c = cols_[idx]; + const int transpose_idx = transpose_rows[c]++; + transpose_cols[transpose_idx] = r; + transpose_values[transpose_idx] = values_[idx]; + } + } + + for (int i = transpose->num_rows() - 1; i > 0 ; --i) { + transpose_rows[i] = transpose_rows[i - 1]; + } + transpose_rows[0] = 0; + + return transpose; +} + + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h index c9c904bf63c..c5721eb888a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h @@ -32,12 +32,9 @@ #define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ #include <vector> - -#include "ceres/internal/eigen.h" #include "ceres/internal/macros.h" #include "ceres/internal/port.h" #include "ceres/sparse_matrix.h" -#include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "glog/logging.h" @@ -47,7 +44,7 @@ struct CRSMatrix; namespace internal { -class SparseMatrixProto; +class TripletSparseMatrix; class CompressedRowSparseMatrix : public SparseMatrix { public: @@ -58,9 +55,6 @@ class CompressedRowSparseMatrix : public SparseMatrix { // // We assume that m does not have any repeated entries. explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit CompressedRowSparseMatrix(const SparseMatrixProto& proto); -#endif // Use this constructor only if you know what you are doing. This // creates a "blank" matrix with the appropriate amount of memory @@ -91,15 +85,12 @@ class CompressedRowSparseMatrix : public SparseMatrix { virtual void ScaleColumns(const double* scale); virtual void ToDenseMatrix(Matrix* dense_matrix) const; -#ifndef CERES_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto* proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const { return num_rows_; } virtual int num_cols() const { return num_cols_; } virtual int num_nonzeros() const { return rows_[num_rows_]; } - virtual const double* values() const { return values_.get(); } - virtual double* mutable_values() { return values_.get(); } + virtual const double* values() const { return &values_[0]; } + virtual double* mutable_values() { return &values_[0]; } // Delete the bottom delta_rows. // num_rows -= delta_rows @@ -112,11 +103,11 @@ class CompressedRowSparseMatrix : public SparseMatrix { void ToCRSMatrix(CRSMatrix* matrix) const; // Low level access methods that expose the structure of the matrix. - const int* cols() const { return cols_.get(); } - int* mutable_cols() { return cols_.get(); } + const int* cols() const { return &cols_[0]; } + int* mutable_cols() { return &cols_[0]; } - const int* rows() const { return rows_.get(); } - int* mutable_rows() { return rows_.get(); } + const int* rows() const { return &rows_[0]; } + int* mutable_rows() { return &rows_[0]; } const vector<int>& row_blocks() const { return row_blocks_; } vector<int>* mutable_row_blocks() { return &row_blocks_; } @@ -124,14 +115,25 @@ class CompressedRowSparseMatrix : public SparseMatrix { const vector<int>& col_blocks() const { return col_blocks_; } vector<int>* mutable_col_blocks() { return &col_blocks_; } - private: - scoped_array<int> cols_; - scoped_array<int> rows_; - scoped_array<double> values_; + // Non-destructive array resizing method. + void set_num_rows(const int num_rows) { num_rows_ = num_rows; } + void set_num_cols(const int num_cols) { num_cols_ = num_cols; } + + void SolveLowerTriangularInPlace(double* solution) const; + void SolveLowerTriangularTransposeInPlace(double* solution) const; + CompressedRowSparseMatrix* Transpose() const; + + static CompressedRowSparseMatrix* CreateBlockDiagonalMatrix( + const double* diagonal, + const vector<int>& blocks); + + private: int num_rows_; int num_cols_; - int max_num_nonzeros_; + vector<int> rows_; + vector<int> cols_; + vector<double> values_; // If the matrix has an underlying block structure, then it can also // carry with it row and column block sizes. This is auxilliary and 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 3dcf8faee59..424acda94ae 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 @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ #define CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ +#include <string> #include <vector> #include "ceres/evaluator.h" diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc index c3858abd2f4..60269a6a4b9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc @@ -32,7 +32,6 @@ #include <cstddef> #include <cmath> -#include "ceres/internal/eigen.h" #include "glog/logging.h" namespace ceres { @@ -90,7 +89,7 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. // // Start by calculating the discriminant D. - const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1]; + const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; // Since both rho[1] and rho[2] are guaranteed to be positive at // this point, we know that D > 1.0. @@ -102,29 +101,43 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { alpha_sq_norm_ = alpha / sq_norm; } -void Corrector::CorrectResiduals(int nrow, double* residuals) { +void Corrector::CorrectResiduals(int num_rows, double* residuals) { DCHECK(residuals != NULL); - VectorRef r_ref(residuals, nrow); // Equation 11 in BANS. - r_ref *= residual_scaling_; + for (int r = 0; r < num_rows; ++r) { + residuals[r] *= residual_scaling_; + } } -void Corrector::CorrectJacobian(int nrow, int ncol, - double* residuals, double* jacobian) { +void Corrector::CorrectJacobian(int num_rows, + int num_cols, + double* residuals, + double* jacobian) { DCHECK(residuals != NULL); DCHECK(jacobian != NULL); + // Equation 11 in BANS. + // + // J = sqrt(rho) * (J - alpha^2 r * r' J) + // + // In days gone by this loop used to be a single Eigen expression of + // the form + // + // J = sqrt_rho1_ * (J - alpha_sq_norm_ * r* (r.transpose() * J)); + // + // Which turns out to about 17x slower on bal problems. The reason + // is that Eigen is unable to figure out that this expression can be + // evaluated columnwise and ends up creating a temporary. + for (int c = 0; c < num_cols; ++c) { + double r_transpose_j = 0.0; + for (int r = 0; r < num_rows; ++r) { + r_transpose_j += jacobian[r * num_cols + c] * residuals[r]; + } - if (nrow == 1) { - // Specialization for the case where the residual is a scalar. - VectorRef j_ref(jacobian, ncol); - j_ref *= sqrt_rho1_ * (1.0 - alpha_sq_norm_ * pow(*residuals, 2)); - } else { - ConstVectorRef r_ref(residuals, nrow); - MatrixRef j_ref(jacobian, nrow, ncol); - - // Equation 11 in BANS. - j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ * - r_ref * (r_ref.transpose() * j_ref)); + for (int r = 0; r < num_rows; ++r) { + jacobian[r * num_cols + c] = sqrt_rho1_ * + (jacobian[r * num_cols + c] - + alpha_sq_norm_ * residuals[r] * r_transpose_j); + } } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.h b/extern/libmv/third_party/ceres/internal/ceres/corrector.h index 9914641cb01..2137221784e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/corrector.h +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.h @@ -66,7 +66,7 @@ class Corrector { explicit Corrector(double sq_norm, const double rho[3]); // residuals *= sqrt(rho[1]) / (1 - alpha) - void CorrectResiduals(int nrow, double* residuals); + void CorrectResiduals(int num_rows, double* residuals); // jacobian = sqrt(rho[1]) * jacobian - // sqrt(rho[1]) * alpha / sq_norm * residuals residuals' * jacobian. @@ -74,8 +74,10 @@ class Corrector { // The method assumes that the jacobian has row-major storage. It is // the caller's responsibility to ensure that the pointer to // jacobian is not null. - void CorrectJacobian(int nrow, int ncol, - double* residuals, double* jacobian); + void CorrectJacobian(int num_rows, + int num_cols, + double* residuals, + double* jacobian); private: double sqrt_rho1_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h b/extern/libmv/third_party/ceres/internal/ceres/covariance.cc index 94b3076e3d7..35146c582b2 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// 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 @@ -26,15 +26,37 @@ // 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) -// -// A portability header to make optional protocol buffer support less intrusive. +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/covariance.h" + +#include <utility> +#include <vector> +#include "ceres/covariance_impl.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.h" + +namespace ceres { + +Covariance::Covariance(const Covariance::Options& options) { + impl_.reset(new internal::CovarianceImpl(options)); +} + +Covariance::~Covariance() { +} -#ifndef CERES_INTERNAL_MATRIX_PROTO_H_ -#define CERES_INTERNAL_MATRIX_PROTO_H_ +bool Covariance::Compute( + const vector<pair<const double*, const double*> >& covariance_blocks, + Problem* problem) { + return impl_->Compute(covariance_blocks, problem->problem_impl_.get()); +} -#ifndef CERES_NO_PROTOCOL_BUFFERS -#include "ceres/matrix.pb.h" -#endif +bool Covariance::GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const { + return impl_->GetCovarianceBlock(parameter_block1, + parameter_block2, + covariance_block); +} -#endif // CERES_INTERNAL_MATRIX_PROTO_H_ +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc new file mode 100644 index 00000000000..19d545cc2d3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.cc @@ -0,0 +1,845 @@ +// 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) + +#include "ceres/covariance_impl.h" + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include <algorithm> +#include <utility> +#include <vector> +#include "Eigen/SVD" +#include "ceres/compressed_col_sparse_matrix_utils.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/covariance.h" +#include "ceres/crs_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/suitesparse.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +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; + +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; +} + +CovarianceImpl::~CovarianceImpl() { +} + +bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, + ProblemImpl* problem) { + problem_ = problem; + parameter_block_to_row_index_.clear(); + covariance_matrix_.reset(NULL); + is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) && + ComputeCovarianceValues()); + is_computed_ = true; + return is_valid_; +} + +bool CovarianceImpl::GetCovarianceBlock(const double* original_parameter_block1, + const double* original_parameter_block2, + double* covariance_block) const { + CHECK(is_computed_) + << "Covariance::GetCovarianceBlock called before Covariance::Compute"; + CHECK(is_valid_) + << "Covariance::GetCovarianceBlock called when Covariance::Compute " + << "returned false."; + + // If either of the two parameter blocks is constant, then the + // covariance block is also zero. + if (constant_parameter_blocks_.count(original_parameter_block1) > 0 || + constant_parameter_blocks_.count(original_parameter_block2) > 0) { + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + ParameterBlock* block1 = + FindOrDie(parameter_map, + const_cast<double*>(original_parameter_block1)); + + ParameterBlock* block2 = + FindOrDie(parameter_map, + const_cast<double*>(original_parameter_block2)); + const int block1_size = block1->Size(); + const int block2_size = block2->Size(); + MatrixRef(covariance_block, block1_size, block2_size).setZero(); + return true; + } + + const double* parameter_block1 = original_parameter_block1; + const double* parameter_block2 = original_parameter_block2; + const bool transpose = parameter_block1 > parameter_block2; + if (transpose) { + std::swap(parameter_block1, parameter_block2); + } + + // Find where in the covariance matrix the block is located. + const int row_begin = + FindOrDie(parameter_block_to_row_index_, parameter_block1); + const int col_begin = + FindOrDie(parameter_block_to_row_index_, parameter_block2); + const int* rows = covariance_matrix_->rows(); + const int* cols = covariance_matrix_->cols(); + const int row_size = rows[row_begin + 1] - rows[row_begin]; + const int* cols_begin = cols + rows[row_begin]; + + // The only part that requires work is walking the compressed column + // vector to determine where the set of columns correspnding to the + // covariance block begin. + int offset = 0; + while (cols_begin[offset] != col_begin && offset < row_size) { + ++offset; + } + + if (offset == row_size) { + LOG(WARNING) << "Unable to find covariance block for " + << original_parameter_block1 << " " + << original_parameter_block2; + return false; + } + + const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map(); + ParameterBlock* block1 = + FindOrDie(parameter_map, const_cast<double*>(parameter_block1)); + ParameterBlock* block2 = + FindOrDie(parameter_map, const_cast<double*>(parameter_block2)); + const LocalParameterization* local_param1 = block1->local_parameterization(); + const LocalParameterization* local_param2 = block2->local_parameterization(); + const int block1_size = block1->Size(); + const int block1_local_size = block1->LocalSize(); + const int block2_size = block2->Size(); + const int block2_local_size = block2->LocalSize(); + + ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin], + block1_size, + row_size); + + // Fast path when there are no local parameterizations. + if (local_param1 == NULL && local_param2 == NULL) { + if (transpose) { + MatrixRef(covariance_block, block2_size, block1_size) = + cov.block(0, offset, block1_size, block2_size).transpose(); + } else { + MatrixRef(covariance_block, block1_size, block2_size) = + cov.block(0, offset, block1_size, block2_size); + } + return true; + } + + // If local parameterizations are used then the covariance that has + // been computed is in the tangent space and it needs to be lifted + // back to the ambient space. + // + // This is given by the formula + // + // C'_12 = J_1 C_12 J_2' + // + // Where C_12 is the local tangent space covariance for parameter + // blocks 1 and 2. J_1 and J_2 are respectively the local to global + // jacobians for parameter blocks 1 and 2. + // + // See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition) + // for a proof. + // + // TODO(sameeragarwal): Add caching of local parameterization, so + // that they are computed just once per parameter block. + Matrix block1_jacobian(block1_size, block1_local_size); + if (local_param1 == NULL) { + block1_jacobian.setIdentity(); + } else { + local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data()); + } + + Matrix block2_jacobian(block2_size, block2_local_size); + // Fast path if the user is requesting a diagonal block. + if (parameter_block1 == parameter_block2) { + block2_jacobian = block1_jacobian; + } else { + if (local_param2 == NULL) { + block2_jacobian.setIdentity(); + } else { + local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data()); + } + } + + if (transpose) { + MatrixRef(covariance_block, block2_size, block1_size) = + block2_jacobian * + cov.block(0, offset, block1_local_size, block2_local_size).transpose() * + block1_jacobian.transpose(); + } else { + MatrixRef(covariance_block, block1_size, block2_size) = + block1_jacobian * + cov.block(0, offset, block1_local_size, block2_local_size) * + block2_jacobian.transpose(); + } + + return true; +} + +// Determine the sparsity pattern of the covariance matrix based on +// the block pairs requested by the user. +bool CovarianceImpl::ComputeCovarianceSparsity( + const CovarianceBlocks& original_covariance_blocks, + ProblemImpl* problem) { + EventLogger event_logger("CovarianceImpl::ComputeCovarianceSparsity"); + + // Determine an ordering for the parameter block, by sorting the + // parameter blocks by their pointers. + vector<double*> all_parameter_blocks; + problem->GetParameterBlocks(&all_parameter_blocks); + const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map(); + constant_parameter_blocks_.clear(); + vector<double*>& active_parameter_blocks = evaluate_options_.parameter_blocks; + active_parameter_blocks.clear(); + for (int i = 0; i < all_parameter_blocks.size(); ++i) { + double* parameter_block = all_parameter_blocks[i]; + + ParameterBlock* block = FindOrDie(parameter_map, parameter_block); + if (block->IsConstant()) { + constant_parameter_blocks_.insert(parameter_block); + } else { + active_parameter_blocks.push_back(parameter_block); + } + } + + sort(active_parameter_blocks.begin(), active_parameter_blocks.end()); + + // Compute the number of rows. Map each parameter block to the + // first row corresponding to it in the covariance matrix using the + // ordering of parameter blocks just constructed. + int num_rows = 0; + parameter_block_to_row_index_.clear(); + for (int i = 0; i < active_parameter_blocks.size(); ++i) { + double* parameter_block = active_parameter_blocks[i]; + const int parameter_block_size = + problem->ParameterBlockLocalSize(parameter_block); + parameter_block_to_row_index_[parameter_block] = num_rows; + num_rows += parameter_block_size; + } + + // Compute the number of non-zeros in the covariance matrix. Along + // the way flip any covariance blocks which are in the lower + // triangular part of the matrix. + int num_nonzeros = 0; + CovarianceBlocks covariance_blocks; + for (int i = 0; i < original_covariance_blocks.size(); ++i) { + const pair<const double*, const double*>& block_pair = + original_covariance_blocks[i]; + if (constant_parameter_blocks_.count(block_pair.first) > 0 || + constant_parameter_blocks_.count(block_pair.second) > 0) { + continue; + } + + int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first); + int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second); + const int size1 = problem->ParameterBlockLocalSize(block_pair.first); + const int size2 = problem->ParameterBlockLocalSize(block_pair.second); + num_nonzeros += size1 * size2; + + // Make sure we are constructing a block upper triangular matrix. + if (index1 > index2) { + covariance_blocks.push_back(make_pair(block_pair.second, + block_pair.first)); + } else { + covariance_blocks.push_back(block_pair); + } + } + + if (covariance_blocks.size() == 0) { + VLOG(2) << "No non-zero covariance blocks found"; + covariance_matrix_.reset(NULL); + return true; + } + + // Sort the block pairs. As a consequence we get the covariance + // blocks as they will occur in the CompressedRowSparseMatrix that + // will store the covariance. + sort(covariance_blocks.begin(), covariance_blocks.end()); + + // Fill the sparsity pattern of the covariance matrix. + covariance_matrix_.reset( + new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros)); + + int* rows = covariance_matrix_->mutable_rows(); + int* cols = covariance_matrix_->mutable_cols(); + + // Iterate over parameter blocks and in turn over the rows of the + // covariance matrix. For each parameter block, look in the upper + // triangular part of the covariance matrix to see if there are any + // blocks requested by the user. If this is the case then fill out a + // set of compressed rows corresponding to this parameter block. + // + // The key thing that makes this loop work is the fact that the + // row/columns of the covariance matrix are ordered by the pointer + // values of the parameter blocks. Thus iterating over the keys of + // parameter_block_to_row_index_ corresponds to iterating over the + // rows of the covariance matrix in order. + int i = 0; // index into covariance_blocks. + int cursor = 0; // index into the covariance matrix. + for (map<const double*, int>::const_iterator it = + parameter_block_to_row_index_.begin(); + it != parameter_block_to_row_index_.end(); + ++it) { + const double* row_block = it->first; + const int row_block_size = problem->ParameterBlockLocalSize(row_block); + int row_begin = it->second; + + // Iterate over the covariance blocks contained in this row block + // and count the number of columns in this row block. + int num_col_blocks = 0; + int num_columns = 0; + for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) { + const pair<const double*, const double*>& block_pair = + covariance_blocks[j]; + if (block_pair.first != row_block) { + break; + } + num_columns += problem->ParameterBlockLocalSize(block_pair.second); + } + + // Fill out all the compressed rows for this parameter block. + for (int r = 0; r < row_block_size; ++r) { + rows[row_begin + r] = cursor; + for (int c = 0; c < num_col_blocks; ++c) { + const double* col_block = covariance_blocks[i + c].second; + const int col_block_size = problem->ParameterBlockLocalSize(col_block); + int col_begin = FindOrDie(parameter_block_to_row_index_, col_block); + for (int k = 0; k < col_block_size; ++k) { + cols[cursor++] = col_begin++; + } + } + } + + i+= num_col_blocks; + } + + rows[num_rows] = cursor; + return true; +} + +bool CovarianceImpl::ComputeCovarianceValues() { + switch (options_.algorithm_type) { + case (DENSE_SVD): + return ComputeCovarianceValuesUsingDenseSVD(); +#ifndef CERES_NO_SUITESPARSE + case (SPARSE_CHOLESKY): + return ComputeCovarianceValuesUsingSparseCholesky(); + case (SPARSE_QR): + return ComputeCovarianceValuesUsingSparseQR(); +#endif + default: + LOG(ERROR) << "Unsupported covariance estimation algorithm type: " + << CovarianceAlgorithmTypeToString(options_.algorithm_type); + return false; + } + 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; + + cholmod_factor* factor = ss.AnalyzeCholesky(&cholmod_jacobian_view); + event_logger.AddEvent("Symbolic Factorization"); + bool factorization_succeeded = ss.Cholesky(&cholmod_jacobian_view, factor); + if (factorization_succeeded) { + const double reciprocal_condition_number = + cholmod_rcond(factor, ss.mutable_cc()); + if (reciprocal_condition_number < + options_.min_reciprocal_condition_number) { + LOG(WARNING) << "Cholesky factorization of J'J is not reliable. " + << "Reciprocal condition number: " + << reciprocal_condition_number << " " + << "min_reciprocal_condition_number : " + << options_.min_reciprocal_condition_number; + factorization_succeeded = false; + } + } + + event_logger.AddEvent("Numeric Factorization"); + if (!factorization_succeeded) { + ss.Free(factor); + LOG(WARNING) << "Cholesky factorization failed."; + 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); + 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() { + EventLogger event_logger( + "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR"); + +#ifndef CERES_NO_SUITESPARSE + 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"); + + // Construct a compressed column form of the Jacobian. + const int num_rows = jacobian.num_rows; + const int num_cols = jacobian.num_cols; + const int num_nonzeros = jacobian.values.size(); + + vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0); + vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0); + vector<double> transpose_values(num_nonzeros, 0); + + for (int idx = 0; idx < num_nonzeros; ++idx) { + transpose_rows[jacobian.cols[idx] + 1] += 1; + } + + for (int i = 1; i < transpose_rows.size(); ++i) { + transpose_rows[i] += transpose_rows[i - 1]; + } + + for (int r = 0; r < num_rows; ++r) { + for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) { + const int c = jacobian.cols[idx]; + const int transpose_idx = transpose_rows[c]; + transpose_cols[transpose_idx] = r; + transpose_values[transpose_idx] = jacobian.values[idx]; + ++transpose_rows[c]; + } + } + + for (int i = transpose_rows.size() - 1; i > 0 ; --i) { + transpose_rows[i] = transpose_rows[i - 1]; + } + transpose_rows[0] = 0; + + cholmod_sparse cholmod_jacobian; + cholmod_jacobian.nrow = num_rows; + cholmod_jacobian.ncol = num_cols; + cholmod_jacobian.nzmax = num_nonzeros; + cholmod_jacobian.nz = NULL; + cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]); + cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]); + cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]); + cholmod_jacobian.z = NULL; + cholmod_jacobian.stype = 0; // Matrix is not symmetric. + cholmod_jacobian.itype = CHOLMOD_LONG; + cholmod_jacobian.xtype = CHOLMOD_REAL; + cholmod_jacobian.dtype = CHOLMOD_DOUBLE; + cholmod_jacobian.sorted = 1; + cholmod_jacobian.packed = 1; + + cholmod_common cc; + cholmod_l_start(&cc); + + cholmod_sparse* R = NULL; + SuiteSparse_long* permutation = NULL; + + // Compute a Q-less QR factorization of the Jacobian. Since we are + // only interested in inverting J'J = R'R, we do not need Q. This + // saves memory and gives us R as a permuted compressed column + // sparse matrix. + // + // TODO(sameeragarwal): Currently the symbolic factorization and the + // numeric factorization is done at the same time, and this does not + // explicitly account for the block column and row structure in the + // matrix. When using AMD, we have observed in the past that + // computing the ordering with the block matrix is significantly + // more efficient, both in runtime as well as the quality of + // ordering computed. So, it maybe worth doing that analysis + // separately. + const SuiteSparse_long rank = + SuiteSparseQR<double>(SPQR_ORDERING_BESTAMD, + SPQR_DEFAULT_TOL, + cholmod_jacobian.ncol, + &cholmod_jacobian, + &R, + &permutation, + &cc); + event_logger.AddEvent("Numeric Factorization"); + CHECK_NOTNULL(permutation); + CHECK_NOTNULL(R); + + if (rank < cholmod_jacobian.ncol) { + LOG(WARNING) << "Jacobian matrix is rank deficient." + << "Number of columns: " << cholmod_jacobian.ncol + << " rank: " << rank; + delete []permutation; + cholmod_l_free_sparse(&R, &cc); + cholmod_l_finish(&cc); + return false; + } + + vector<int> inverse_permutation(num_cols); + for (SuiteSparse_long i = 0; i < num_cols; ++i) { + inverse_permutation[permutation[i]] = i; + } + + 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. + 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<SuiteSparse_long>( + num_cols, + static_cast<SuiteSparse_long*>(R->i), + static_cast<SuiteSparse_long*>(R->p), + static_cast<double*>(R->x), + inverse_permutation[r], + solution); + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution[inverse_permutation[c]]; + } + } + + delete []permutation; + cholmod_l_free_sparse(&R, &cc); + cholmod_l_finish(&cc); + event_logger.AddEvent("Inversion"); + return true; + +#else // CERES_NO_SUITESPARSE + + return false; + +#endif // CERES_NO_SUITESPARSE +} + +bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { + EventLogger event_logger( + "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD"); + 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"); + + Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols); + dense_jacobian.setZero(); + for (int r = 0; r < jacobian.num_rows; ++r) { + for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) { + const int c = jacobian.cols[idx]; + dense_jacobian(r, c) = jacobian.values[idx]; + } + } + event_logger.AddEvent("ConvertToDenseMatrix"); + + Eigen::JacobiSVD<Matrix> svd(dense_jacobian, + Eigen::ComputeThinU | Eigen::ComputeThinV); + + event_logger.AddEvent("SingularValueDecomposition"); + + const Vector singular_values = svd.singularValues(); + const int num_singular_values = singular_values.rows(); + Vector inverse_squared_singular_values(num_singular_values); + inverse_squared_singular_values.setZero(); + + const double max_singular_value = singular_values[0]; + const double min_singular_value_ratio = + sqrt(options_.min_reciprocal_condition_number); + + const bool automatic_truncation = (options_.null_space_rank < 0); + const int max_rank = min(num_singular_values, + num_singular_values - options_.null_space_rank); + + // Compute the squared inverse of the singular values. Truncate the + // computation based on min_singular_value_ratio and + // null_space_rank. When either of these two quantities are active, + // the resulting covariance matrix is a Moore-Penrose inverse + // instead of a regular inverse. + for (int i = 0; i < max_rank; ++i) { + const double singular_value_ratio = singular_values[i] / max_singular_value; + if (singular_value_ratio < min_singular_value_ratio) { + // Since the singular values are in decreasing order, if + // automatic truncation is enabled, then from this point on + // all values will fail the ratio test and there is nothing to + // do in this loop. + if (automatic_truncation) { + break; + } else { + LOG(WARNING) << "Cholesky factorization of J'J is not reliable. " + << "Reciprocal condition number: " + << singular_value_ratio * singular_value_ratio << " " + << "min_reciprocal_condition_number : " + << options_.min_reciprocal_condition_number; + return false; + } + } + + inverse_squared_singular_values[i] = + 1.0 / (singular_values[i] * singular_values[i]); + } + + Matrix dense_covariance = + svd.matrixV() * + inverse_squared_singular_values.asDiagonal() * + svd.matrixV().transpose(); + event_logger.AddEvent("PseudoInverse"); + + 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(); + + for (int r = 0; r < num_rows; ++r) { + for (int idx = rows[r]; idx < rows[r + 1]; ++idx) { + const int c = cols[idx]; + values[idx] = dense_covariance(r, c); + } + } + event_logger.AddEvent("CopyToCovarianceMatrix"); + 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 new file mode 100644 index 00000000000..0e7e2173079 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/covariance_impl.h @@ -0,0 +1,89 @@ +// 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_COVARIANCE_IMPL_H_ +#define CERES_INTERNAL_COVARIANCE_IMPL_H_ + +#include <map> +#include <set> +#include <utility> +#include <vector> +#include "ceres/covariance.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem_impl.h" +#include "ceres/suitesparse.h" + +namespace ceres { + +namespace internal { + +class CompressedRowSparseMatrix; + +class CovarianceImpl { + public: + explicit CovarianceImpl(const Covariance::Options& options); + ~CovarianceImpl(); + + bool Compute( + const vector<pair<const double*, const double*> >& covariance_blocks, + ProblemImpl* problem); + + bool GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + bool ComputeCovarianceSparsity( + const vector<pair<const double*, const double*> >& covariance_blocks, + ProblemImpl* problem); + + bool ComputeCovarianceValues(); + bool ComputeCovarianceValuesUsingSparseCholesky(); + bool ComputeCovarianceValuesUsingSparseQR(); + bool ComputeCovarianceValuesUsingDenseSVD(); + + const CompressedRowSparseMatrix* covariance_matrix() const { + return covariance_matrix_.get(); + } + + private: + ProblemImpl* problem_; + Covariance::Options options_; + Problem::EvaluateOptions evaluate_options_; + bool is_computed_; + bool is_valid_; + map<const double*, int> parameter_block_to_row_index_; + set<const double*> constant_parameter_blocks_; + scoped_ptr<CompressedRowSparseMatrix> covariance_matrix_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COVARIANCE_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc index 3fbc2717f64..c6d77439653 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.cc @@ -32,7 +32,10 @@ #include "ceres/cxsparse.h" +#include <vector> +#include "ceres/compressed_col_sparse_matrix_utils.h" #include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/internal/port.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -44,46 +47,46 @@ CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) { CXSparse::~CXSparse() { if (scratch_size_ > 0) { - cs_free(scratch_); + cs_di_free(scratch_); } } + bool CXSparse::SolveCholesky(cs_di* A, cs_dis* symbolic_factorization, double* b) { // Make sure we have enough scratch space available. if (scratch_size_ < A->n) { if (scratch_size_ > 0) { - cs_free(scratch_); + cs_di_free(scratch_); } - scratch_ = reinterpret_cast<CS_ENTRY*>(cs_malloc(A->n, sizeof(CS_ENTRY))); + scratch_ = + reinterpret_cast<CS_ENTRY*>(cs_di_malloc(A->n, sizeof(CS_ENTRY))); scratch_size_ = A->n; } // Solve using Cholesky factorization - csn* numeric_factorization = cs_chol(A, symbolic_factorization); + csn* numeric_factorization = cs_di_chol(A, symbolic_factorization); if (numeric_factorization == NULL) { LOG(WARNING) << "Cholesky factorization failed."; return false; } - // When the Cholesky factorization succeeded, these methods are guaranteed to - // succeeded as well. In the comments below, "x" refers to the scratch space. + // When the Cholesky factorization succeeded, these methods are + // guaranteed to succeeded as well. In the comments below, "x" + // refers to the scratch space. // // Set x = P * b. - cs_ipvec(symbolic_factorization->pinv, b, scratch_, A->n); - + cs_di_ipvec(symbolic_factorization->pinv, b, scratch_, A->n); // Set x = L \ x. - cs_lsolve(numeric_factorization->L, scratch_); - + cs_di_lsolve(numeric_factorization->L, scratch_); // Set x = L' \ x. - cs_ltsolve(numeric_factorization->L, scratch_); - + cs_di_ltsolve(numeric_factorization->L, scratch_); // Set b = P' * x. - cs_pvec(symbolic_factorization->pinv, scratch_, b, A->n); + cs_di_pvec(symbolic_factorization->pinv, scratch_, b, A->n); // Free Cholesky factorization. - cs_nfree(numeric_factorization); + cs_di_nfree(numeric_factorization); return true; } @@ -92,6 +95,72 @@ cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) { return cs_schol(1, A); } +cs_dis* CXSparse::AnalyzeCholeskyWithNaturalOrdering(cs_di* A) { + // order = 0 for Natural ordering. + return cs_schol(0, A); +} + +cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks) { + const int num_row_blocks = row_blocks.size(); + const int num_col_blocks = col_blocks.size(); + + vector<int> block_rows; + vector<int> block_cols; + CompressedColumnScalarMatrixToBlockMatrix(A->i, + A->p, + row_blocks, + col_blocks, + &block_rows, + &block_cols); + cs_di block_matrix; + block_matrix.m = num_row_blocks; + block_matrix.n = num_col_blocks; + block_matrix.nz = -1; + block_matrix.nzmax = block_rows.size(); + block_matrix.p = &block_cols[0]; + block_matrix.i = &block_rows[0]; + block_matrix.x = NULL; + + int* ordering = cs_amd(1, &block_matrix); + vector<int> block_ordering(num_row_blocks, -1); + copy(ordering, ordering + num_row_blocks, &block_ordering[0]); + cs_free(ordering); + + vector<int> scalar_ordering; + BlockOrderingToScalarOrdering(row_blocks, block_ordering, &scalar_ordering); + + cs_dis* symbolic_factorization = + reinterpret_cast<cs_dis*>(cs_calloc(1, sizeof(cs_dis))); + symbolic_factorization->pinv = cs_pinv(&scalar_ordering[0], A->n); + cs* permuted_A = cs_symperm(A, symbolic_factorization->pinv, 0); + + symbolic_factorization->parent = cs_etree(permuted_A, 0); + int* postordering = cs_post(symbolic_factorization->parent, A->n); + int* column_counts = cs_counts(permuted_A, + symbolic_factorization->parent, + postordering, + 0); + cs_free(postordering); + cs_spfree(permuted_A); + + symbolic_factorization->cp = (int*) cs_malloc(A->n+1, sizeof(int)); + symbolic_factorization->lnz = cs_cumsum(symbolic_factorization->cp, + column_counts, + A->n); + symbolic_factorization->unz = symbolic_factorization->lnz; + + cs_free(column_counts); + + if (symbolic_factorization->lnz < 0) { + cs_sfree(symbolic_factorization); + symbolic_factorization = NULL; + } + + return symbolic_factorization; +} + cs_di CXSparse::CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A) { cs_di At; At.m = A->num_cols(); @@ -117,6 +186,20 @@ cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) { return cs_compress(&tsm_wrapper); } +void CXSparse::ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering) { + int* cs_ordering = cs_amd(1, A); + copy(cs_ordering, cs_ordering + A->m, ordering); + cs_free(cs_ordering); +} + +cs_di* CXSparse::TransposeMatrix(cs_di* A) { + return cs_di_transpose(A, 1); +} + +cs_di* CXSparse::MatrixMatrixMultiply(cs_di* A, cs_di* B) { + return cs_di_multiply(A, B); +} + void CXSparse::Free(cs_di* sparse_matrix) { cs_di_spfree(sparse_matrix); } diff --git a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h index dd5eadc8da8..cd87908a43d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h +++ b/extern/libmv/third_party/ceres/internal/ceres/cxsparse.h @@ -33,7 +33,9 @@ #ifndef CERES_NO_CXSPARSE +#include <vector> #include "cs.h" +#include "ceres/internal/port.h" namespace ceres { namespace internal { @@ -68,10 +70,49 @@ class CXSparse { // with Free. May return NULL if the compression or allocation fails. cs_di* CreateSparseMatrix(TripletSparseMatrix* A); + // B = A' + // + // The returned matrix should be deallocated with Free when not used + // anymore. + cs_di* TransposeMatrix(cs_di* A); + + // C = A * B + // + // The returned matrix should be deallocated with Free when not used + // anymore. + cs_di* MatrixMatrixMultiply(cs_di* A, cs_di* B); + // Computes a symbolic factorization of A that can be used in SolveCholesky. + // // The returned matrix should be deallocated with Free when not used anymore. cs_dis* AnalyzeCholesky(cs_di* A); + // Computes a symbolic factorization of A that can be used in + // SolveCholesky, but does not compute a fill-reducing ordering. + // + // The returned matrix should be deallocated with Free when not used anymore. + cs_dis* AnalyzeCholeskyWithNaturalOrdering(cs_di* A); + + // Computes a symbolic factorization of A that can be used in + // SolveCholesky. The difference from AnalyzeCholesky is that this + // function first detects the block sparsity of the matrix using + // information about the row and column blocks and uses this block + // sparse matrix to find a fill-reducing ordering. This ordering is + // then used to find a symbolic factorization. This can result in a + // significant performance improvement AnalyzeCholesky on block + // sparse matrices. + // + // The returned matrix should be deallocated with Free when not used + // anymore. + cs_dis* BlockAnalyzeCholesky(cs_di* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks); + + // Compute an fill-reducing approximate minimum degree ordering of + // the matrix A. ordering should be non-NULL and should point to + // enough memory to hold the ordering for the rows of A. + void ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering); + void Free(cs_di* sparse_matrix); void Free(cs_dis* symbolic_factorization); @@ -84,6 +125,11 @@ class CXSparse { } // namespace internal } // namespace ceres +#else // CERES_NO_CXSPARSE + +class CXSparse {}; +typedef void cs_dis; + #endif // CERES_NO_CXSPARSE #endif // CERES_INTERNAL_CXSPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.cc index 96f55115a67..fbf3cbec9d2 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.cc @@ -33,9 +33,11 @@ #include <cstddef> #include "Eigen/Dense" +#include "ceres/blas.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -52,6 +54,18 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { + if (options_.dense_linear_algebra_library_type == EIGEN) { + return SolveUsingEigen(A, b, per_solve_options, x); + } else { + return SolveUsingLAPACK(A, b, per_solve_options, x); + } +} + +LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { EventLogger event_logger("DenseNormalCholeskySolver::Solve"); const int num_rows = A->num_rows(); @@ -62,6 +76,7 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( lhs.setZero(); event_logger.AddEvent("Setup"); + // lhs += A'A // // Using rankUpdate instead of GEMM, exposes the fact that its the @@ -76,16 +91,66 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl( ConstVectorRef D(per_solve_options.D, num_cols); lhs += D.array().square().matrix().asDiagonal(); } + event_logger.AddEvent("Product"); LinearSolver::Summary summary; summary.num_iterations = 1; summary.termination_type = TOLERANCE; VectorRef(x, num_cols) = - lhs.selfadjointView<Eigen::Upper>().ldlt().solve(rhs); + lhs.selfadjointView<Eigen::Upper>().llt().solve(rhs); event_logger.AddEvent("Solve"); - return summary; } +LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + EventLogger event_logger("DenseNormalCholeskySolver::Solve"); + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo + // it before returning the matrix to the user. + A->AppendDiagonal(per_solve_options.D); + } + + const int num_cols = A->num_cols(); + Matrix lhs(num_cols, num_cols); + event_logger.AddEvent("Setup"); + + // lhs = A'A + // + // Note: This is a bit delicate, it assumes that the stride on this + // matrix is the same as the number of rows. + BLAS::SymmetricRankKUpdate(A->num_rows(), + num_cols, + A->values(), + true, + 1.0, + 0.0, + lhs.data()); + + if (per_solve_options.D != NULL) { + // Undo the modifications to the matrix A. + A->RemoveDiagonal(); + } + + // TODO(sameeragarwal): Replace this with a gemv call for true blasness. + // rhs = A'b + VectorRef(x, num_cols) = + A->matrix().transpose() * ConstVectorRef(b, A->num_rows()); + event_logger.AddEvent("Product"); + + const int info = LAPACK::SolveInPlaceUsingCholesky(num_cols, lhs.data(), x); + event_logger.AddEvent("Solve"); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = info == 0 ? TOLERANCE : FAILURE; + + event_logger.AddEvent("TearDown"); + return summary; +} } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.h b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.h index de47740583d..e35053f5234 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_normal_cholesky_solver.h @@ -85,6 +85,18 @@ class DenseNormalCholeskySolver: public DenseSparseMatrixSolver { const LinearSolver::PerSolveOptions& per_solve_options, double* x); + LinearSolver::Summary SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + LinearSolver::Summary SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + const LinearSolver::Options options_; CERES_DISALLOW_COPY_AND_ASSIGN(DenseNormalCholeskySolver); }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc index 1fb9709b42a..d76d58b51b5 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc @@ -30,12 +30,13 @@ #include "ceres/dense_qr_solver.h" -#include <cstddef> +#include <cstddef> #include "Eigen/Dense" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -44,13 +45,87 @@ namespace ceres { namespace internal { DenseQRSolver::DenseQRSolver(const LinearSolver::Options& options) - : options_(options) {} + : options_(options) { + work_.resize(1); +} LinearSolver::Summary DenseQRSolver::SolveImpl( DenseSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { + if (options_.dense_linear_algebra_library_type == EIGEN) { + return SolveUsingEigen(A, b, per_solve_options, x); + } else { + return SolveUsingLAPACK(A, b, per_solve_options, x); + } +} +LinearSolver::Summary DenseQRSolver::SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + EventLogger event_logger("DenseQRSolver::Solve"); + + const int num_rows = A->num_rows(); + const int num_cols = A->num_cols(); + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo + // it before returning the matrix to the user. + A->AppendDiagonal(per_solve_options.D); + } + + // TODO(sameeragarwal): Since we are copying anyways, the diagonal + // can be appended to the matrix instead of doing it on A. + lhs_ = A->matrix(); + + if (per_solve_options.D != NULL) { + // Undo the modifications to the matrix A. + A->RemoveDiagonal(); + } + + // rhs = [b;0] to account for the additional rows in the lhs. + if (rhs_.rows() != lhs_.rows()) { + rhs_.resize(lhs_.rows()); + } + rhs_.setZero(); + rhs_.head(num_rows) = ConstVectorRef(b, num_rows); + + if (work_.rows() == 1) { + const int work_size = + LAPACK::EstimateWorkSizeForQR(lhs_.rows(), lhs_.cols()); + VLOG(3) << "Working memory for Dense QR factorization: " + << work_size * sizeof(double); + work_.resize(work_size); + } + + const int info = LAPACK::SolveUsingQR(lhs_.rows(), + lhs_.cols(), + lhs_.data(), + work_.rows(), + work_.data(), + rhs_.data()); + event_logger.AddEvent("Solve"); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + if (info == 0) { + VectorRef(x, num_cols) = rhs_.head(num_cols); + summary.termination_type = TOLERANCE; + } else { + summary.termination_type = FAILURE; + } + + event_logger.AddEvent("TearDown"); + return summary; +} + +LinearSolver::Summary DenseQRSolver::SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { EventLogger event_logger("DenseQRSolver::Solve"); const int num_rows = A->num_rows(); @@ -73,7 +148,7 @@ LinearSolver::Summary DenseQRSolver::SolveImpl( event_logger.AddEvent("Setup"); // Solve the system. - VectorRef(x, num_cols) = A->matrix().colPivHouseholderQr().solve(rhs_); + VectorRef(x, num_cols) = A->matrix().householderQr().solve(rhs_); event_logger.AddEvent("Solve"); if (per_solve_options.D != NULL) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h index f78fa72c5f3..e745c63cb44 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h @@ -90,8 +90,22 @@ class DenseQRSolver: public DenseSparseMatrixSolver { const LinearSolver::PerSolveOptions& per_solve_options, double* x); + LinearSolver::Summary SolveUsingEigen( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + LinearSolver::Summary SolveUsingLAPACK( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + const LinearSolver::Options options_; + ColMajorMatrix lhs_; Vector rhs_; + Vector work_; CERES_DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc index 9d58031ccfc..d67474fed32 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc @@ -31,10 +31,10 @@ #include "ceres/dense_sparse_matrix.h" #include <algorithm> -#include "ceres/matrix_proto.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -80,22 +80,6 @@ DenseSparseMatrix::DenseSparseMatrix(const ColMajorMatrix& m) has_diagonal_reserved_(false) { } -#ifndef CERES_NO_PROTOCOL_BUFFERS -DenseSparseMatrix::DenseSparseMatrix(const SparseMatrixProto& outer_proto) - : m_(Eigen::MatrixXd::Zero( - outer_proto.dense_matrix().num_rows(), - outer_proto.dense_matrix().num_cols())), - has_diagonal_appended_(false), - has_diagonal_reserved_(false) { - const DenseSparseMatrixProto& proto = outer_proto.dense_matrix(); - for (int i = 0; i < m_.rows(); ++i) { - for (int j = 0; j < m_.cols(); ++j) { - m_(i, j) = proto.values(m_.cols() * i + j); - } - } -} -#endif - void DenseSparseMatrix::SetZero() { m_.setZero(); } @@ -121,22 +105,6 @@ void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { *dense_matrix = m_.block(0, 0, num_rows(), num_cols()); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void DenseSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { - CHECK(!has_diagonal_appended_) << "Not supported."; - outer_proto->Clear(); - DenseSparseMatrixProto* proto = outer_proto->mutable_dense_matrix(); - - proto->set_num_rows(num_rows()); - proto->set_num_cols(num_cols()); - - int num_nnz = num_nonzeros(); - for (int i = 0; i < num_nnz; ++i) { - proto->add_values(m_.data()[i]); - } -} -#endif - void DenseSparseMatrix::AppendDiagonal(double *d) { CHECK(!has_diagonal_appended_); if (!has_diagonal_reserved_) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h index 6c7b60ade13..981e2d14562 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h @@ -33,7 +33,6 @@ #ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ #define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ -#include <glog/logging.h> #include "ceres/sparse_matrix.h" #include "ceres/internal/eigen.h" #include "ceres/internal/macros.h" @@ -43,7 +42,6 @@ namespace ceres { namespace internal { -class SparseMatrixProto; class TripletSparseMatrix; class DenseSparseMatrix : public SparseMatrix { @@ -52,9 +50,6 @@ class DenseSparseMatrix : public SparseMatrix { // m. This assumes that m does not have any repeated entries. explicit DenseSparseMatrix(const TripletSparseMatrix& m); explicit DenseSparseMatrix(const ColMajorMatrix& m); -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit DenseSparseMatrix(const SparseMatrixProto& proto); -#endif DenseSparseMatrix(int num_rows, int num_cols); DenseSparseMatrix(int num_rows, int num_cols, bool reserve_diagonal); @@ -68,9 +63,6 @@ class DenseSparseMatrix : public SparseMatrix { virtual void SquaredColumnNorm(double* x) const; virtual void ScaleColumns(const double* scale); virtual void ToDenseMatrix(Matrix* dense_matrix) const; -#ifndef CERES_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto* proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const; virtual int num_cols() const; diff --git a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc index a330ad2c7a2..c85c8e5cbf5 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.cc @@ -34,6 +34,7 @@ #include "Eigen/Dense" #include "ceres/array_utils.h" #include "ceres/internal/eigen.h" +#include "ceres/linear_least_squares_problems.h" #include "ceres/linear_solver.h" #include "ceres/polynomial.h" #include "ceres/sparse_matrix.h" @@ -52,8 +53,8 @@ DoglegStrategy::DoglegStrategy(const TrustRegionStrategy::Options& options) : linear_solver_(options.linear_solver), radius_(options.initial_radius), max_radius_(options.max_radius), - min_diagonal_(options.lm_min_diagonal), - max_diagonal_(options.lm_max_diagonal), + min_diagonal_(options.min_lm_diagonal), + max_diagonal_(options.max_lm_diagonal), mu_(kMinMu), min_mu_(kMinMu), max_mu_(kMaxMu), @@ -127,7 +128,7 @@ TrustRegionStrategy::Summary DoglegStrategy::ComputeStep( ComputeCauchyPoint(jacobian); LinearSolver::Summary linear_solver_summary = - ComputeGaussNewtonStep(jacobian, residuals); + ComputeGaussNewtonStep(per_solve_options, jacobian, residuals); TrustRegionStrategy::Summary summary; summary.residual_norm = linear_solver_summary.residual_norm; @@ -507,6 +508,7 @@ bool DoglegStrategy::FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const { } LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( + const PerSolveOptions& per_solve_options, SparseMatrix* jacobian, const double* residuals) { const int n = jacobian->num_cols(); @@ -561,6 +563,22 @@ LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( solve_options, gauss_newton_step_.data()); + if (per_solve_options.dump_format_type == CONSOLE || + (per_solve_options.dump_format_type != CONSOLE && + !per_solve_options.dump_filename_base.empty())) { + if (!DumpLinearLeastSquaresProblem(per_solve_options.dump_filename_base, + per_solve_options.dump_format_type, + jacobian, + solve_options.D, + residuals, + gauss_newton_step_.data(), + 0)) { + LOG(ERROR) << "Unable to dump trust region problem." + << " Filename base: " + << per_solve_options.dump_filename_base; + } + } + if (linear_solver_summary.termination_type == FAILURE || !IsArrayValid(n, gauss_newton_step_.data())) { mu_ *= mu_increase_factor_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h index 7131467d6ce..71c785cc3f7 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h +++ b/extern/libmv/third_party/ceres/internal/ceres/dogleg_strategy.h @@ -79,8 +79,10 @@ class DoglegStrategy : public TrustRegionStrategy { typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Vector2d; typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Matrix2d; - LinearSolver::Summary ComputeGaussNewtonStep(SparseMatrix* jacobian, - const double* residuals); + LinearSolver::Summary ComputeGaussNewtonStep( + const PerSolveOptions& per_solve_options, + SparseMatrix* jacobian, + const double* residuals); void ComputeCauchyPoint(SparseMatrix* jacobian); void ComputeGradient(SparseMatrix* jacobian, const double* residuals); void ComputeTraditionalDoglegStep(double* step); 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 2e6eec0e6d8..ca3a2fe1a88 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h +++ b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h @@ -43,11 +43,12 @@ namespace ceres { namespace internal { -// Compare two vertices of a graph by their degrees. +// Compare two vertices of a graph by their degrees, if the degrees +// are equal then order them by their ids. template <typename Vertex> -class VertexDegreeLessThan { +class VertexTotalOrdering { public: - explicit VertexDegreeLessThan(const Graph<Vertex>& graph) + explicit VertexTotalOrdering(const Graph<Vertex>& graph) : graph_(graph) {} bool operator()(const Vertex& lhs, const Vertex& rhs) const { @@ -61,6 +62,20 @@ class VertexDegreeLessThan { const Graph<Vertex>& graph_; }; +template <typename Vertex> +class VertexDegreeLessThan { + public: + explicit VertexDegreeLessThan(const Graph<Vertex>& graph) + : graph_(graph) {} + + bool operator()(const Vertex& lhs, const Vertex& rhs) const { + return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size(); + } + + private: + const Graph<Vertex>& graph_; +}; + // Order the vertices of a graph using its (approximately) largest // independent set, where an independent set of a graph is a set of // vertices that have no edges connecting them. The maximum @@ -104,8 +119,83 @@ int IndependentSetOrdering(const Graph<Vertex>& graph, sort(vertex_queue.begin(), vertex_queue.end(), - VertexDegreeLessThan<Vertex>(graph)); + VertexTotalOrdering<Vertex>(graph)); + + // Iterate over vertex_queue. Pick the first white vertex, add it + // to the independent set. Mark it black and its neighbors grey. + for (int i = 0; i < vertex_queue.size(); ++i) { + const Vertex& vertex = vertex_queue[i]; + if (vertex_color[vertex] != kWhite) { + continue; + } + + ordering->push_back(vertex); + vertex_color[vertex] = kBlack; + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex); + for (typename HashSet<Vertex>::const_iterator it = neighbors.begin(); + it != neighbors.end(); + ++it) { + vertex_color[*it] = kGrey; + } + } + + int independent_set_size = ordering->size(); + + // Iterate over the vertices and add all the grey vertices to the + // ordering. At this stage there should only be black or grey + // vertices in the graph. + for (typename vector<Vertex>::const_iterator it = vertex_queue.begin(); + it != vertex_queue.end(); + ++it) { + const Vertex vertex = *it; + DCHECK(vertex_color[vertex] != kWhite); + if (vertex_color[vertex] != kBlack) { + ordering->push_back(vertex); + } + } + + CHECK_EQ(ordering->size(), num_vertices); + return independent_set_size; +} + +// Same as above with one important difference. The ordering parameter +// is an input/output parameter which carries an initial ordering of +// the vertices of the graph. The greedy independent set algorithm +// starts by sorting the vertices in increasing order of their +// degree. The input ordering is used to stabilize this sort, i.e., if +// two vertices have the same degree then they are ordered in the same +// order in which they occur in "ordering". +// +// This is useful in eliminating non-determinism from the Schur +// ordering algorithm over all. +template <typename Vertex> +int StableIndependentSetOrdering(const Graph<Vertex>& graph, + vector<Vertex>* ordering) { + CHECK_NOTNULL(ordering); + const HashSet<Vertex>& vertices = graph.vertices(); + const int num_vertices = vertices.size(); + CHECK_EQ(vertices.size(), ordering->size()); + + // Colors for labeling the graph during the BFS. + const char kWhite = 0; + const char kGrey = 1; + const char kBlack = 2; + + vector<Vertex> vertex_queue(*ordering); + stable_sort(vertex_queue.begin(), vertex_queue.end(), + VertexDegreeLessThan<Vertex>(graph)); + + // Mark all vertices white. + HashMap<Vertex, char> vertex_color; + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + vertex_color[*it] = kWhite; + } + + ordering->clear(); + ordering->reserve(num_vertices); // Iterate over vertex_queue. Pick the first white vertex, add it // to the independent set. Mark it black and its neighbors grey. for (int i = 0; i < vertex_queue.size(); ++i) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc index 4af030a8535..32722bb6e8f 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc @@ -55,7 +55,7 @@ ImplicitSchurComplement::ImplicitSchurComplement(int num_eliminate_blocks, ImplicitSchurComplement::~ImplicitSchurComplement() { } -void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, +void ImplicitSchurComplement::Init(const BlockSparseMatrix& A, const double* D, const double* b) { // Since initialization is reasonably heavy, perhaps we can save on @@ -161,7 +161,7 @@ void ImplicitSchurComplement::AddDiagonalAndInvert( m = m .selfadjointView<Eigen::Upper>() - .ldlt() + .llt() .solve(Matrix::Identity(row_block_size, row_block_size)); } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h index b9ebaa4628e..c1bb6e19bab 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h @@ -44,7 +44,6 @@ namespace ceres { namespace internal { class BlockSparseMatrix; -class BlockSparseMatrixBase; // This class implements various linear algebraic operations related // to the Schur complement without explicitly forming it. @@ -110,7 +109,7 @@ class ImplicitSchurComplement : public LinearOperator { // is important that the matrix A have a BlockStructure object // associated with it and has a block structure that is compatible // with the SchurComplement solver. - void Init(const BlockSparseMatrixBase& A, const double* D, const double* b); + void Init(const BlockSparseMatrix& A, const double* D, const double* b); // y += Sx, where S is the Schur complement. virtual void RightMultiply(const double* x, double* y) const; diff --git a/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.cc b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.cc new file mode 100644 index 00000000000..6ba38ec8eec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.cc @@ -0,0 +1,239 @@ +// 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) + +#include "ceres/incomplete_lq_factorization.h" + +#include <vector> +#include <utility> +#include <cmath> +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Normalize a row and return it's norm. +inline double NormalizeRow(const int row, CompressedRowSparseMatrix* matrix) { + const int row_begin = matrix->rows()[row]; + const int row_end = matrix->rows()[row + 1]; + + double* values = matrix->mutable_values(); + double norm = 0.0; + for (int i = row_begin; i < row_end; ++i) { + norm += values[i] * values[i]; + } + + norm = sqrt(norm); + const double inverse_norm = 1.0 / norm; + for (int i = row_begin; i < row_end; ++i) { + values[i] *= inverse_norm; + } + + return norm; +} + +// Compute a(row_a,:) * b(row_b, :)' +inline double RowDotProduct(const CompressedRowSparseMatrix& a, + const int row_a, + const CompressedRowSparseMatrix& b, + const int row_b) { + const int* a_rows = a.rows(); + const int* a_cols = a.cols(); + const double* a_values = a.values(); + + const int* b_rows = b.rows(); + const int* b_cols = b.cols(); + const double* b_values = b.values(); + + const int row_a_end = a_rows[row_a + 1]; + const int row_b_end = b_rows[row_b + 1]; + + int idx_a = a_rows[row_a]; + int idx_b = b_rows[row_b]; + double dot_product = 0.0; + while (idx_a < row_a_end && idx_b < row_b_end) { + if (a_cols[idx_a] == b_cols[idx_b]) { + dot_product += a_values[idx_a++] * b_values[idx_b++]; + } + + while (a_cols[idx_a] < b_cols[idx_b] && idx_a < row_a_end) { + ++idx_a; + } + + while (a_cols[idx_a] > b_cols[idx_b] && idx_b < row_b_end) { + ++idx_b; + } + } + + return dot_product; +} + +struct SecondGreaterThan { + public: + bool operator()(const pair<int, double>& lhs, + const pair<int, double>& rhs) const { + return (fabs(lhs.second) > fabs(rhs.second)); + } +}; + +// In the row vector dense_row(0:num_cols), drop values smaller than +// the max_value * drop_tolerance. Of the remaining non-zero values, +// choose at most level_of_fill values and then add the resulting row +// vector to matrix. + +void DropEntriesAndAddRow(const Vector& dense_row, + const int num_entries, + const int level_of_fill, + const double drop_tolerance, + vector<pair<int, double> >* scratch, + CompressedRowSparseMatrix* matrix) { + int* rows = matrix->mutable_rows(); + int* cols = matrix->mutable_cols(); + double* values = matrix->mutable_values(); + int num_nonzeros = rows[matrix->num_rows()]; + + if (num_entries == 0) { + matrix->set_num_rows(matrix->num_rows() + 1); + rows[matrix->num_rows()] = num_nonzeros; + return; + } + + const double max_value = dense_row.head(num_entries).cwiseAbs().maxCoeff(); + const double threshold = drop_tolerance * max_value; + + int scratch_count = 0; + for (int i = 0; i < num_entries; ++i) { + if (fabs(dense_row[i]) > threshold) { + pair<int, double>& entry = (*scratch)[scratch_count]; + entry.first = i; + entry.second = dense_row[i]; + ++scratch_count; + } + } + + if (scratch_count > level_of_fill) { + nth_element(scratch->begin(), + scratch->begin() + level_of_fill, + scratch->begin() + scratch_count, + SecondGreaterThan()); + scratch_count = level_of_fill; + sort(scratch->begin(), scratch->begin() + scratch_count); + } + + for (int i = 0; i < scratch_count; ++i) { + const pair<int, double>& entry = (*scratch)[i]; + cols[num_nonzeros] = entry.first; + values[num_nonzeros] = entry.second; + ++num_nonzeros; + } + + matrix->set_num_rows(matrix->num_rows() + 1); + rows[matrix->num_rows()] = num_nonzeros; +} + +// Saad's Incomplete LQ factorization algorithm. +CompressedRowSparseMatrix* IncompleteLQFactorization( + const CompressedRowSparseMatrix& matrix, + const int l_level_of_fill, + const double l_drop_tolerance, + const int q_level_of_fill, + const double q_drop_tolerance) { + const int num_rows = matrix.num_rows(); + const int num_cols = matrix.num_cols(); + const int* rows = matrix.rows(); + const int* cols = matrix.cols(); + const double* values = matrix.values(); + + CompressedRowSparseMatrix* l = + new CompressedRowSparseMatrix(num_rows, + num_rows, + l_level_of_fill * num_rows); + l->set_num_rows(0); + + CompressedRowSparseMatrix q(num_rows, num_cols, q_level_of_fill * num_rows); + q.set_num_rows(0); + + int* l_rows = l->mutable_rows(); + int* l_cols = l->mutable_cols(); + double* l_values = l->mutable_values(); + + int* q_rows = q.mutable_rows(); + int* q_cols = q.mutable_cols(); + double* q_values = q.mutable_values(); + + Vector l_i(num_rows); + Vector q_i(num_cols); + vector<pair<int, double> > scratch(num_cols); + for (int i = 0; i < num_rows; ++i) { + // l_i = q * matrix(i,:)'); + l_i.setZero(); + for (int j = 0; j < i; ++j) { + l_i(j) = RowDotProduct(matrix, i, q, j); + } + DropEntriesAndAddRow(l_i, + i, + l_level_of_fill, + l_drop_tolerance, + &scratch, + l); + + // q_i = matrix(i,:) - q(0:i-1,:) * l_i); + q_i.setZero(); + for (int idx = rows[i]; idx < rows[i + 1]; ++idx) { + q_i(cols[idx]) = values[idx]; + } + + for (int j = l_rows[i]; j < l_rows[i + 1]; ++j) { + const int r = l_cols[j]; + const double lij = l_values[j]; + for (int idx = q_rows[r]; idx < q_rows[r + 1]; ++idx) { + q_i(q_cols[idx]) -= lij * q_values[idx]; + } + } + DropEntriesAndAddRow(q_i, + num_cols, + q_level_of_fill, + q_drop_tolerance, + &scratch, + &q); + + // lii = |qi| + l_cols[l->num_nonzeros()] = i; + l_values[l->num_nonzeros()] = NormalizeRow(i, &q); + l_rows[l->num_rows()] += 1; + } + + return l; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.h b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.h new file mode 100644 index 00000000000..e678463cf8d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/incomplete_lq_factorization.h @@ -0,0 +1,90 @@ +// 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_INCOMPLETE_LQ_FACTORIZATION_H_ +#define CERES_INTERNAL_INCOMPLETE_LQ_FACTORIZATION_H_ + +#include <vector> +#include <utility> +#include "ceres/compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +// Incomplete LQ factorization as described in +// +// Preconditioning techniques for indefinite and nonsymmetric linear +// systems. Yousef Saad, Preprint RIACS-ILQ-TR, RIACS, NASA Ames +// Research Center, Moffett Field, CA, 1987. +// +// An incomplete LQ factorization of a matrix A is a decomposition +// +// A = LQ + E +// +// Where L is a lower triangular matrix, and Q is a near orthonormal +// matrix. The extent of orthonormality depends on E. E is the "drop" +// matrix. Each row of L has a maximum of l_level_of_fill entries, and +// all non-zero entries are within l_drop_tolerance of the largest +// entry. Each row of Q has a maximum of q_level_of_fill entries and +// all non-zero entries are within q_drop_tolerance of the largest +// entry. +// +// E is the error of the incomplete factorization. +// +// The purpose of incomplete factorizations is preconditioning and +// there one only needs the L matrix, therefore this function just +// returns L. +// +// Caller owns the result. +CompressedRowSparseMatrix* IncompleteLQFactorization( + const CompressedRowSparseMatrix& matrix, + const int l_level_of_fill, + const double l_drop_tolerance, + const int q_level_of_fill, + const double q_drop_tolerance); + +// In the row vector dense_row(0:num_cols), drop values smaller than +// the max_value * drop_tolerance. Of the remaining non-zero values, +// choose at most level_of_fill values and then add the resulting row +// vector to matrix. +// +// scratch is used to prevent allocations inside this function. It is +// assumed that scratch is of size matrix->num_cols(). +void DropEntriesAndAddRow(const Vector& dense_row, + const int num_entries, + const int level_of_fill, + const double drop_tolerance, + vector<pair<int, double> >* scratch, + CompressedRowSparseMatrix* matrix); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INCOMPLETE_LQ_FACTORIZATION_H_ 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 15e0bdcd81a..1aac5657ce6 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 @@ -62,7 +62,7 @@ IterativeSchurComplementSolver::~IterativeSchurComplementSolver() { } LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { @@ -78,6 +78,17 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( } schur_complement_->Init(*A, per_solve_options.D, b); + const int num_schur_complement_blocks = + A->block_structure()->cols.size() - options_.elimination_groups[0]; + if (num_schur_complement_blocks == 0) { + VLOG(2) << "No parameter blocks left in the schur complement."; + LinearSolver::Summary cg_summary; + cg_summary.num_iterations = 0; + cg_summary.termination_type = TOLERANCE; + schur_complement_->BackSubstitute(NULL, x); + return cg_summary; + } + // Initialize the solution to the Schur complement system to zero. // // TODO(sameeragarwal): There maybe a better initialization than an @@ -97,8 +108,8 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( Preconditioner::Options preconditioner_options; preconditioner_options.type = options_.preconditioner_type; - preconditioner_options.sparse_linear_algebra_library = - options_.sparse_linear_algebra_library; + preconditioner_options.sparse_linear_algebra_library_type = + options_.sparse_linear_algebra_library_type; preconditioner_options.num_threads = options_.num_threads; preconditioner_options.row_block_size = options_.row_block_size; preconditioner_options.e_block_size = options_.e_block_size; @@ -116,16 +127,16 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( case SCHUR_JACOBI: if (preconditioner_.get() == NULL) { preconditioner_.reset( - new SchurJacobiPreconditioner( - *A->block_structure(), preconditioner_options)); + new SchurJacobiPreconditioner(*A->block_structure(), + preconditioner_options)); } break; case CLUSTER_JACOBI: case CLUSTER_TRIDIAGONAL: if (preconditioner_.get() == NULL) { preconditioner_.reset( - new VisibilityBasedPreconditioner( - *A->block_structure(), preconditioner_options)); + new VisibilityBasedPreconditioner(*A->block_structure(), + preconditioner_options)); } break; default: diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h index f8abe04c142..b056a694478 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -39,7 +39,7 @@ namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; class ImplicitSchurComplement; class Preconditioner; @@ -67,14 +67,14 @@ class Preconditioner; // a proof of this fact and others related to this solver please see // the section on Domain Decomposition Methods in Saad's book // "Iterative Methods for Sparse Linear Systems". -class IterativeSchurComplementSolver : public BlockSparseMatrixBaseSolver { +class IterativeSchurComplementSolver : public BlockSparseMatrixSolver { public: explicit IterativeSchurComplementSolver(const LinearSolver::Options& options); virtual ~IterativeSchurComplementSolver(); private: virtual LinearSolver::Summary SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& options, double* x); diff --git a/extern/libmv/third_party/ceres/internal/ceres/lapack.cc b/extern/libmv/third_party/ceres/internal/ceres/lapack.cc new file mode 100644 index 00000000000..73bfa69cbbd --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/lapack.cc @@ -0,0 +1,157 @@ +// 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) + +#include "ceres/lapack.h" +#include "glog/logging.h" + +// C interface to the LAPACK Cholesky factorization and triangular solve. +extern "C" void dpotrf_(char* uplo, + int* n, + double* a, + int* lda, + int* info); + +extern "C" void dpotrs_(char* uplo, + int* n, + int* nrhs, + double* a, + int* lda, + double* b, + int* ldb, + int* info); + +extern "C" void dgels_(char* uplo, + int* m, + int* n, + int* nrhs, + double* a, + int* lda, + double* b, + int* ldb, + double* work, + int* lwork, + int* info); + + +namespace ceres { +namespace internal { + +int LAPACK::SolveInPlaceUsingCholesky(int num_rows, + const double* in_lhs, + double* rhs_and_solution) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a BLAS library."; + return -1; +#else + char uplo = 'L'; + int n = num_rows; + int info = 0; + int nrhs = 1; + double* lhs = const_cast<double*>(in_lhs); + + dpotrf_(&uplo, &n, lhs, &n, &info); + if (info != 0) { + LOG(INFO) << "Cholesky factorization (dpotrf) failed: " << info; + return info; + } + + dpotrs_(&uplo, &n, &nrhs, lhs, &n, rhs_and_solution, &n, &info); + if (info != 0) { + LOG(INFO) << "Triangular solve (dpotrs) failed: " << info; + } + + return info; +#endif +}; + +int LAPACK::EstimateWorkSizeForQR(int num_rows, int num_cols) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a LAPACK library."; + return -1; +#else + char trans = 'N'; + int nrhs = 1; + int lwork = -1; + double work; + int info = 0; + dgels_(&trans, + &num_rows, + &num_cols, + &nrhs, + NULL, + &num_rows, + NULL, + &num_rows, + &work, + &lwork, + &info); + + CHECK_EQ(info, 0); + return work; +#endif +} + +int LAPACK::SolveUsingQR(int num_rows, + int num_cols, + const double* in_lhs, + int work_size, + double* work, + double* rhs_and_solution) { +#ifdef CERES_NO_LAPACK + LOG(FATAL) << "Ceres was built without a LAPACK library."; + return -1; +#else + char trans = 'N'; + int m = num_rows; + int n = num_cols; + int nrhs = 1; + int lda = num_rows; + int ldb = num_rows; + int info = 0; + double* lhs = const_cast<double*>(in_lhs); + + dgels_(&trans, + &m, + &n, + &nrhs, + lhs, + &lda, + rhs_and_solution, + &ldb, + work, + &work_size, + &info); + + return info; +#endif +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/lapack.h b/extern/libmv/third_party/ceres/internal/ceres/lapack.h new file mode 100644 index 00000000000..4f3a88c700a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/lapack.h @@ -0,0 +1,88 @@ +// 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_LAPACK_H_ +#define CERES_INTERNAL_LAPACK_H_ + +namespace ceres { +namespace internal { + +class LAPACK { + public: + // Solve + // + // lhs * solution = rhs + // + // using a Cholesky factorization. Here + // lhs is a symmetric positive definite matrix. It is assumed to be + // column major and only the lower triangular part of the matrix is + // referenced. + // + // This function uses the LAPACK dpotrf and dpotrs routines. + // + // The return value is zero if the solve is successful. + static int SolveInPlaceUsingCholesky(int num_rows, + const double* lhs, + double* rhs_and_solution); + + // The SolveUsingQR function requires a buffer for its temporary + // computation. This function given the size of the lhs matrix will + // return the size of the buffer needed. + static int EstimateWorkSizeForQR(int num_rows, int num_cols); + + // Solve + // + // lhs * solution = rhs + // + // using a dense QR factorization. lhs is an arbitrary (possibly + // rectangular) matrix with full column rank. + // + // work is an array of size work_size that this routine uses for its + // temporary storage. The optimal size of this array can be obtained + // by calling EstimateWorkSizeForQR. + // + // When calling, rhs_and_solution contains the rhs, and upon return + // the first num_col entries are the solution. + // + // This function uses the LAPACK dgels routine. + // + // The return value is zero if the solve is successful. + static int SolveUsingQR(int num_rows, + int num_cols, + const double* lhs, + int work_size, + double* work, + double* rhs_and_solution); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LAPACK_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc index 9e6a59e3813..fad7c1f3258 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt_strategy.cc @@ -34,6 +34,7 @@ #include "Eigen/Core" #include "ceres/array_utils.h" #include "ceres/internal/eigen.h" +#include "ceres/linear_least_squares_problems.h" #include "ceres/linear_solver.h" #include "ceres/sparse_matrix.h" #include "ceres/trust_region_strategy.h" @@ -48,8 +49,8 @@ LevenbergMarquardtStrategy::LevenbergMarquardtStrategy( : linear_solver_(options.linear_solver), radius_(options.initial_radius), max_radius_(options.max_radius), - min_diagonal_(options.lm_min_diagonal), - max_diagonal_(options.lm_max_diagonal), + min_diagonal_(options.min_lm_diagonal), + max_diagonal_(options.max_lm_diagonal), decrease_factor_(2.0), reuse_diagonal_(false) { CHECK_NOTNULL(linear_solver_); @@ -111,9 +112,24 @@ TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( } else { VectorRef(step, num_parameters) *= -1.0; } - reuse_diagonal_ = true; + if (per_solve_options.dump_format_type == CONSOLE || + (per_solve_options.dump_format_type != CONSOLE && + !per_solve_options.dump_filename_base.empty())) { + if (!DumpLinearLeastSquaresProblem(per_solve_options.dump_filename_base, + per_solve_options.dump_format_type, + jacobian, + solve_options.D, + residuals, + step, + 0)) { + LOG(ERROR) << "Unable to dump trust region problem." + << " Filename base: " << per_solve_options.dump_filename_base; + } + } + + TrustRegionStrategy::Summary summary; summary.residual_norm = linear_solver_summary.residual_norm; summary.num_iterations = linear_solver_summary.num_iterations; diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search.cc b/extern/libmv/third_party/ceres/internal/ceres/line_search.cc index 437f742607f..8323896915a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search.cc @@ -31,12 +31,12 @@ #ifndef CERES_NO_LINE_SEARCH_MINIMIZER #include "ceres/line_search.h" -#include <glog/logging.h> #include "ceres/fpclassify.h" #include "ceres/evaluator.h" #include "ceres/internal/eigen.h" #include "ceres/polynomial.h" - +#include "ceres/stringprintf.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -64,6 +64,39 @@ FunctionSample ValueAndGradientSample(const double x, } // namespace +// Convenience stream operator for pushing FunctionSamples into log messages. +std::ostream& operator<<(std::ostream &os, + const FunctionSample& sample) { + os << "[x: " << sample.x << ", value: " << sample.value + << ", gradient: " << sample.gradient << ", value_is_valid: " + << std::boolalpha << sample.value_is_valid << ", gradient_is_valid: " + << std::boolalpha << sample.gradient_is_valid << "]"; + return os; +} + +LineSearch::LineSearch(const LineSearch::Options& options) + : options_(options) {} + +LineSearch* LineSearch::Create(const LineSearchType line_search_type, + const LineSearch::Options& options, + string* error) { + LineSearch* line_search = NULL; + switch (line_search_type) { + case ceres::ARMIJO: + line_search = new ArmijoLineSearch(options); + break; + case ceres::WOLFE: + line_search = new WolfeLineSearch(options); + break; + default: + *error = string("Invalid line search algorithm type: ") + + LineSearchTypeToString(line_search_type) + + string(", unable to create line search."); + return NULL; + } + return line_search; +} + LineSearchFunction::LineSearchFunction(Evaluator* evaluator) : evaluator_(evaluator), position_(evaluator->NumParameters()), @@ -79,7 +112,7 @@ void LineSearchFunction::Init(const Vector& position, direction_ = direction; } -bool LineSearchFunction::Evaluate(const double x, double* f, double* g) { +bool LineSearchFunction::Evaluate(double x, double* f, double* g) { scaled_direction_ = x * direction_; if (!evaluator_->Plus(position_.data(), scaled_direction_.data(), @@ -104,110 +137,608 @@ bool LineSearchFunction::Evaluate(const double x, double* f, double* g) { return IsFinite(*f) && IsFinite(*g); } -void ArmijoLineSearch::Search(const LineSearch::Options& options, - const double initial_step_size, +double LineSearchFunction::DirectionInfinityNorm() const { + return direction_.lpNorm<Eigen::Infinity>(); +} + +// Returns step_size \in [min_step_size, max_step_size] which minimizes the +// polynomial of degree defined by interpolation_type which interpolates all +// of the provided samples with valid values. +double LineSearch::InterpolatingPolynomialMinimizingStepSize( + const LineSearchInterpolationType& interpolation_type, + const FunctionSample& lowerbound, + const FunctionSample& previous, + const FunctionSample& current, + const double min_step_size, + const double max_step_size) const { + if (!current.value_is_valid || + (interpolation_type == BISECTION && + max_step_size <= current.x)) { + // Either: sample is invalid; or we are using BISECTION and contracting + // the step size. + return min(max(current.x * 0.5, min_step_size), max_step_size); + } else if (interpolation_type == BISECTION) { + CHECK_GT(max_step_size, current.x); + // We are expanding the search (during a Wolfe bracketing phase) using + // BISECTION interpolation. Using BISECTION when trying to expand is + // strictly speaking an oxymoron, but we define this to mean always taking + // the maximum step size so that the Armijo & Wolfe implementations are + // agnostic to the interpolation type. + return max_step_size; + } + // Only check if lower-bound is valid here, where it is required + // to avoid replicating current.value_is_valid == false + // behaviour in WolfeLineSearch. + CHECK(lowerbound.value_is_valid) + << "Ceres bug: lower-bound sample for interpolation is invalid, " + << "please contact the developers!, interpolation_type: " + << LineSearchInterpolationTypeToString(interpolation_type) + << ", lowerbound: " << lowerbound << ", previous: " << previous + << ", current: " << current; + + // Select step size by interpolating the function and gradient values + // and minimizing the corresponding polynomial. + vector<FunctionSample> samples; + samples.push_back(lowerbound); + + if (interpolation_type == QUADRATIC) { + // Two point interpolation using function values and the + // gradient at the lower bound. + samples.push_back(ValueSample(current.x, current.value)); + + if (previous.value_is_valid) { + // Three point interpolation, using function values and the + // gradient at the lower bound. + samples.push_back(ValueSample(previous.x, previous.value)); + } + } else if (interpolation_type == CUBIC) { + // Two point interpolation using the function values and the gradients. + samples.push_back(current); + + if (previous.value_is_valid) { + // Three point interpolation using the function values and + // the gradients. + samples.push_back(previous); + } + } else { + LOG(FATAL) << "Ceres bug: No handler for interpolation_type: " + << LineSearchInterpolationTypeToString(interpolation_type) + << ", please contact the developers!"; + } + + double step_size = 0.0, unused_min_value = 0.0; + MinimizeInterpolatingPolynomial(samples, min_step_size, max_step_size, + &step_size, &unused_min_value); + return step_size; +} + +ArmijoLineSearch::ArmijoLineSearch(const LineSearch::Options& options) + : LineSearch(options) {} + +void ArmijoLineSearch::Search(const double step_size_estimate, const double initial_cost, const double initial_gradient, Summary* summary) { *CHECK_NOTNULL(summary) = LineSearch::Summary(); - Function* function = options.function; - - double previous_step_size = 0.0; - double previous_cost = 0.0; - double previous_gradient = 0.0; - bool previous_step_size_is_valid = false; - - double step_size = initial_step_size; - double cost = 0.0; - double gradient = 0.0; - bool step_size_is_valid = false; - - ++summary->num_evaluations; - step_size_is_valid = - function->Evaluate(step_size, - &cost, - options.interpolation_degree < 2 ? NULL : &gradient); - while (!step_size_is_valid || cost > (initial_cost - + options.sufficient_decrease - * initial_gradient - * step_size)) { - // If step_size_is_valid is not true we treat it as if the cost at - // that point is not large enough to satisfy the sufficient - // decrease condition. - - const double current_step_size = step_size; - // Backtracking search. Each iteration of this loop finds a new point - - if ((options.interpolation_degree == 0) || !step_size_is_valid) { - // Backtrack by halving the step_size; - step_size *= 0.5; - } else { - // Backtrack by interpolating the function and gradient values - // and minimizing the corresponding polynomial. - - vector<FunctionSample> samples; - samples.push_back(ValueAndGradientSample(0.0, - initial_cost, - initial_gradient)); - - if (options.interpolation_degree == 1) { - // Two point interpolation using function values and the - // initial gradient. - samples.push_back(ValueSample(step_size, cost)); - - if (options.use_higher_degree_interpolation_when_possible && - summary->num_evaluations > 1 && - previous_step_size_is_valid) { - // Three point interpolation, using function values and the - // initial gradient. - samples.push_back(ValueSample(previous_step_size, previous_cost)); - } - } else { - // Two point interpolation using the function values and the gradients. - samples.push_back(ValueAndGradientSample(step_size, - cost, - gradient)); - - if (options.use_higher_degree_interpolation_when_possible && - summary->num_evaluations > 1 && - previous_step_size_is_valid) { - // Three point interpolation using the function values and - // the gradients. - samples.push_back(ValueAndGradientSample(previous_step_size, - previous_cost, - previous_gradient)); - } - } + CHECK_GE(step_size_estimate, 0.0); + CHECK_GT(options().sufficient_decrease, 0.0); + CHECK_LT(options().sufficient_decrease, 1.0); + CHECK_GT(options().max_num_iterations, 0); + Function* function = options().function; + + // Note initial_cost & initial_gradient are evaluated at step_size = 0, + // not step_size_estimate, which is our starting guess. + const FunctionSample initial_position = + ValueAndGradientSample(0.0, initial_cost, initial_gradient); + + FunctionSample previous = ValueAndGradientSample(0.0, 0.0, 0.0); + previous.value_is_valid = false; + + FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0); + current.value_is_valid = false; - double min_value; - MinimizeInterpolatingPolynomial(samples, 0.0, current_step_size, - &step_size, &min_value); - step_size = - min(max(step_size, - options.min_relative_step_size_change * current_step_size), - options.max_relative_step_size_change * current_step_size); + const bool interpolation_uses_gradients = + options().interpolation_type == CUBIC; + const double descent_direction_max_norm = + static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm(); + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + while (!current.value_is_valid || + current.value > (initial_cost + + options().sufficient_decrease + * initial_gradient + * current.x)) { + // If current.value_is_valid is false, we treat it as if the cost at that + // point is not large enough to satisfy the sufficient decrease condition. + ++summary->num_iterations; + if (summary->num_iterations >= options().max_num_iterations) { + summary->error = + StringPrintf("Line search failed: Armijo failed to find a point " + "satisfying the sufficient decrease condition within " + "specified max_num_iterations: %d.", + options().max_num_iterations); + LOG(WARNING) << summary->error; + return; } - previous_step_size = current_step_size; - previous_cost = cost; - previous_gradient = gradient; + const double step_size = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + initial_position, + previous, + current, + (options().max_step_contraction * current.x), + (options().min_step_contraction * current.x)); - if (fabs(initial_gradient) * step_size < options.step_size_threshold) { - LOG(WARNING) << "Line search failed: step_size too small: " << step_size; + if (step_size * descent_direction_max_norm < options().min_step_size) { + summary->error = + StringPrintf("Line search failed: step_size too small: %.5e " + "with descent_direction_max_norm: %.5e.", step_size, + descent_direction_max_norm); + LOG(WARNING) << summary->error; return; } - ++summary->num_evaluations; - step_size_is_valid = - function->Evaluate(step_size, - &cost, - options.interpolation_degree < 2 ? NULL : &gradient); + previous = current; + current.x = step_size; + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + } + + summary->optimal_step_size = current.x; + summary->success = true; +} + +WolfeLineSearch::WolfeLineSearch(const LineSearch::Options& options) + : LineSearch(options) {} + +void WolfeLineSearch::Search(const double step_size_estimate, + const double initial_cost, + const double initial_gradient, + Summary* summary) { + *CHECK_NOTNULL(summary) = LineSearch::Summary(); + // All parameters should have been validated by the Solver, but as + // invalid values would produce crazy nonsense, hard check them here. + CHECK_GE(step_size_estimate, 0.0); + CHECK_GT(options().sufficient_decrease, 0.0); + CHECK_GT(options().sufficient_curvature_decrease, + options().sufficient_decrease); + CHECK_LT(options().sufficient_curvature_decrease, 1.0); + CHECK_GT(options().max_step_expansion, 1.0); + + // Note initial_cost & initial_gradient are evaluated at step_size = 0, + // not step_size_estimate, which is our starting guess. + const FunctionSample initial_position = + ValueAndGradientSample(0.0, initial_cost, initial_gradient); + + bool do_zoom_search = false; + // Important: The high/low in bracket_high & bracket_low refer to their + // _function_ values, not their step sizes i.e. it is _not_ required that + // bracket_low.x < bracket_high.x. + FunctionSample solution, bracket_low, bracket_high; + + // Wolfe bracketing phase: Increases step_size until either it finds a point + // that satisfies the (strong) Wolfe conditions, or an interval that brackets + // step sizes which satisfy the conditions. From Nocedal & Wright [1] p61 the + // interval: (step_size_{k-1}, step_size_{k}) contains step lengths satisfying + // the strong Wolfe conditions if one of the following conditions are met: + // + // 1. step_size_{k} violates the sufficient decrease (Armijo) condition. + // 2. f(step_size_{k}) >= f(step_size_{k-1}). + // 3. f'(step_size_{k}) >= 0. + // + // Caveat: If f(step_size_{k}) is invalid, then step_size is reduced, ignoring + // this special case, step_size monotonically increases during bracketing. + if (!this->BracketingPhase(initial_position, + step_size_estimate, + &bracket_low, + &bracket_high, + &do_zoom_search, + summary) && + summary->num_iterations < options().max_num_iterations) { + // Failed to find either a valid point or a valid bracket, but we did not + // run out of iterations. + return; + } + if (!do_zoom_search) { + // Either: Bracketing phase already found a point satisfying the strong + // Wolfe conditions, thus no Zoom required. + // + // Or: Bracketing failed to find a valid bracket or a point satisfying the + // strong Wolfe conditions within max_num_iterations. As this is an + // 'artificial' constraint, and we would otherwise fail to produce a valid + // point when ArmijoLineSearch would succeed, we return the lowest point + // found thus far which satsifies the Armijo condition (but not the Wolfe + // conditions). + CHECK(bracket_low.value_is_valid) + << "Ceres bug: Bracketing produced an invalid bracket_low, please " + << "contact the developers!, bracket_low: " << bracket_low + << ", bracket_high: " << bracket_high << ", num_iterations: " + << summary->num_iterations << ", max_num_iterations: " + << options().max_num_iterations; + summary->optimal_step_size = bracket_low.x; + summary->success = true; + return; + } + + // Wolfe Zoom phase: Called when the Bracketing phase finds an interval of + // non-zero, finite width that should bracket step sizes which satisfy the + // (strong) Wolfe conditions (before finding a step size that satisfies the + // conditions). Zoom successively decreases the size of the interval until a + // step size which satisfies the Wolfe conditions is found. The interval is + // defined by bracket_low & bracket_high, which satisfy: + // + // 1. The interval bounded by step sizes: bracket_low.x & bracket_high.x + // contains step sizes that satsify the strong Wolfe conditions. + // 2. bracket_low.x is of all the step sizes evaluated *which satisifed the + // Armijo sufficient decrease condition*, the one which generated the + // smallest function value, i.e. bracket_low.value < + // f(all other steps satisfying Armijo). + // - Note that this does _not_ (necessarily) mean that initially + // bracket_low.value < bracket_high.value (although this is typical) + // e.g. when bracket_low = initial_position, and bracket_high is the + // first sample, and which does not satisfy the Armijo condition, + // but still has bracket_high.value < initial_position.value. + // 3. bracket_high is chosen after bracket_low, s.t. + // bracket_low.gradient * (bracket_high.x - bracket_low.x) < 0. + if (!this->ZoomPhase(initial_position, + bracket_low, + bracket_high, + &solution, + summary) && !solution.value_is_valid) { + // Failed to find a valid point (given the specified decrease parameters) + // within the specified bracket. + return; } + // Ensure that if we ran out of iterations whilst zooming the bracket, or + // shrank the bracket width to < tolerance and failed to find a point which + // satisfies the strong Wolfe curvature condition, that we return the point + // amongst those found thus far, which minimizes f() and satisfies the Armijo + // condition. + solution = + solution.value_is_valid && solution.value <= bracket_low.value + ? solution : bracket_low; - summary->optimal_step_size = step_size; + summary->optimal_step_size = solution.x; summary->success = true; } +// Returns true iff bracket_low & bracket_high bound a bracket that contains +// points which satisfy the strong Wolfe conditions. Otherwise, on return false, +// if we stopped searching due to the 'artificial' condition of reaching +// max_num_iterations, bracket_low is the step size amongst all those +// tested, which satisfied the Armijo decrease condition and minimized f(). +bool WolfeLineSearch::BracketingPhase( + const FunctionSample& initial_position, + const double step_size_estimate, + FunctionSample* bracket_low, + FunctionSample* bracket_high, + bool* do_zoom_search, + Summary* summary) { + Function* function = options().function; + + FunctionSample previous = initial_position; + FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0); + current.value_is_valid = false; + + const bool interpolation_uses_gradients = + options().interpolation_type == CUBIC; + const double descent_direction_max_norm = + static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm(); + + *do_zoom_search = false; + *bracket_low = initial_position; + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + + while (true) { + ++summary->num_iterations; + + if (current.value_is_valid && + (current.value > (initial_position.value + + options().sufficient_decrease + * initial_position.gradient + * current.x) || + (previous.value_is_valid && current.value > previous.value))) { + // Bracket found: current step size violates Armijo sufficient decrease + // condition, or has stepped past an inflection point of f() relative to + // previous step size. + *do_zoom_search = true; + *bracket_low = previous; + *bracket_high = current; + break; + } + + // Irrespective of the interpolation type we are using, we now need the + // gradient at the current point (which satisfies the Armijo condition) + // in order to check the strong Wolfe conditions. + if (!interpolation_uses_gradients) { + ++summary->num_function_evaluations; + ++summary->num_gradient_evaluations; + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + ¤t.gradient); + current.gradient_is_valid = current.value_is_valid; + } + + if (current.value_is_valid && + fabs(current.gradient) <= + -options().sufficient_curvature_decrease * initial_position.gradient) { + // Current step size satisfies the strong Wolfe conditions, and is thus a + // valid termination point, therefore a Zoom not required. + *bracket_low = current; + *bracket_high = current; + break; + + } else if (current.value_is_valid && current.gradient >= 0) { + // Bracket found: current step size has stepped past an inflection point + // of f(), but Armijo sufficient decrease is still satisfied and + // f(current) is our best minimum thus far. Remember step size + // monotonically increases, thus previous_step_size < current_step_size + // even though f(previous) > f(current). + *do_zoom_search = true; + // Note inverse ordering from first bracket case. + *bracket_low = current; + *bracket_high = previous; + break; + + } else if (summary->num_iterations >= options().max_num_iterations) { + // Check num iterations bound here so that we always evaluate the + // max_num_iterations-th iteration against all conditions, and + // then perform no additional (unused) evaluations. + summary->error = + StringPrintf("Line search failed: Wolfe bracketing phase failed to " + "find a point satisfying strong Wolfe conditions, or a " + "bracket containing such a point within specified " + "max_num_iterations: %d", options().max_num_iterations); + LOG(WARNING) << summary->error; + // Ensure that bracket_low is always set to the step size amongst all + // those tested which minimizes f() and satisfies the Armijo condition + // when we terminate due to the 'artificial' max_num_iterations condition. + *bracket_low = + current.value_is_valid && current.value < bracket_low->value + ? current : *bracket_low; + return false; + } + // Either: f(current) is invalid; or, f(current) is valid, but does not + // satisfy the strong Wolfe conditions itself, or the conditions for + // being a boundary of a bracket. + + // If f(current) is valid, (but meets no criteria) expand the search by + // increasing the step size. + const double max_step_size = + current.value_is_valid + ? (current.x * options().max_step_expansion) : current.x; + + // We are performing 2-point interpolation only here, but the API of + // InterpolatingPolynomialMinimizingStepSize() allows for up to + // 3-point interpolation, so pad call with a sample with an invalid + // value that will therefore be ignored. + const FunctionSample unused_previous; + DCHECK(!unused_previous.value_is_valid); + // Contracts step size if f(current) is not valid. + const double step_size = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + previous, + unused_previous, + current, + previous.x, + max_step_size); + if (step_size * descent_direction_max_norm < options().min_step_size) { + summary->error = + StringPrintf("Line search failed: step_size too small: %.5e " + "with descent_direction_max_norm: %.5e", step_size, + descent_direction_max_norm); + LOG(WARNING) << summary->error; + return false; + } + + previous = current.value_is_valid ? current : previous; + current.x = step_size; + + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + current.value_is_valid = + function->Evaluate(current.x, + ¤t.value, + interpolation_uses_gradients + ? ¤t.gradient : NULL); + current.gradient_is_valid = + interpolation_uses_gradients && current.value_is_valid; + } + // Either we have a valid point, defined as a bracket of zero width, in which + // case no zoom is required, or a valid bracket in which to zoom. + return true; +} + +// Returns true iff solution satisfies the strong Wolfe conditions. Otherwise, +// on return false, if we stopped searching due to the 'artificial' condition of +// reaching max_num_iterations, solution is the step size amongst all those +// tested, which satisfied the Armijo decrease condition and minimized f(). +bool WolfeLineSearch::ZoomPhase(const FunctionSample& initial_position, + FunctionSample bracket_low, + FunctionSample bracket_high, + FunctionSample* solution, + Summary* summary) { + Function* function = options().function; + + CHECK(bracket_low.value_is_valid && bracket_low.gradient_is_valid) + << "Ceres bug: f_low input to Wolfe Zoom invalid, please contact " + << "the developers!, initial_position: " << initial_position + << ", bracket_low: " << bracket_low + << ", bracket_high: "<< bracket_high; + // We do not require bracket_high.gradient_is_valid as the gradient condition + // for a valid bracket is only dependent upon bracket_low.gradient, and + // in order to minimize jacobian evaluations, bracket_high.gradient may + // not have been calculated (if bracket_high.value does not satisfy the + // Armijo sufficient decrease condition and interpolation method does not + // require it). + CHECK(bracket_high.value_is_valid) + << "Ceres bug: f_high input to Wolfe Zoom invalid, please " + << "contact the developers!, initial_position: " << initial_position + << ", bracket_low: " << bracket_low + << ", bracket_high: "<< bracket_high; + CHECK_LT(bracket_low.gradient * + (bracket_high.x - bracket_low.x), 0.0) + << "Ceres bug: f_high input to Wolfe Zoom does not satisfy gradient " + << "condition combined with f_low, please contact the developers!" + << ", initial_position: " << initial_position + << ", bracket_low: " << bracket_low + << ", bracket_high: "<< bracket_high; + + const int num_bracketing_iterations = summary->num_iterations; + const bool interpolation_uses_gradients = + options().interpolation_type == CUBIC; + const double descent_direction_max_norm = + static_cast<const LineSearchFunction*>(function)->DirectionInfinityNorm(); + + while (true) { + // Set solution to bracket_low, as it is our best step size (smallest f()) + // found thus far and satisfies the Armijo condition, even though it does + // not satisfy the Wolfe condition. + *solution = bracket_low; + if (summary->num_iterations >= options().max_num_iterations) { + summary->error = + StringPrintf("Line search failed: Wolfe zoom phase failed to " + "find a point satisfying strong Wolfe conditions " + "within specified max_num_iterations: %d, " + "(num iterations taken for bracketing: %d).", + options().max_num_iterations, num_bracketing_iterations); + LOG(WARNING) << summary->error; + return false; + } + if (fabs(bracket_high.x - bracket_low.x) * descent_direction_max_norm + < options().min_step_size) { + // Bracket width has been reduced below tolerance, and no point satisfying + // the strong Wolfe conditions has been found. + summary->error = + StringPrintf("Line search failed: Wolfe zoom bracket width: %.5e " + "too small with descent_direction_max_norm: %.5e.", + fabs(bracket_high.x - bracket_low.x), + descent_direction_max_norm); + LOG(WARNING) << summary->error; + return false; + } + + ++summary->num_iterations; + // Polynomial interpolation requires inputs ordered according to step size, + // not f(step size). + const FunctionSample& lower_bound_step = + bracket_low.x < bracket_high.x ? bracket_low : bracket_high; + const FunctionSample& upper_bound_step = + bracket_low.x < bracket_high.x ? bracket_high : bracket_low; + // We are performing 2-point interpolation only here, but the API of + // InterpolatingPolynomialMinimizingStepSize() allows for up to + // 3-point interpolation, so pad call with a sample with an invalid + // value that will therefore be ignored. + const FunctionSample unused_previous; + DCHECK(!unused_previous.value_is_valid); + solution->x = + this->InterpolatingPolynomialMinimizingStepSize( + options().interpolation_type, + lower_bound_step, + unused_previous, + upper_bound_step, + lower_bound_step.x, + upper_bound_step.x); + // No check on magnitude of step size being too small here as it is + // lower-bounded by the initial bracket start point, which was valid. + ++summary->num_function_evaluations; + if (interpolation_uses_gradients) { ++summary->num_gradient_evaluations; } + solution->value_is_valid = + function->Evaluate(solution->x, + &solution->value, + interpolation_uses_gradients + ? &solution->gradient : NULL); + solution->gradient_is_valid = + interpolation_uses_gradients && solution->value_is_valid; + if (!solution->value_is_valid) { + summary->error = + StringPrintf("Line search failed: Wolfe Zoom phase found " + "step_size: %.5e, for which function is invalid, " + "between low_step: %.5e and high_step: %.5e " + "at which function is valid.", + solution->x, bracket_low.x, bracket_high.x); + LOG(WARNING) << summary->error; + return false; + } + + if ((solution->value > (initial_position.value + + options().sufficient_decrease + * initial_position.gradient + * solution->x)) || + (solution->value >= bracket_low.value)) { + // Armijo sufficient decrease not satisfied, or not better + // than current lowest sample, use as new upper bound. + bracket_high = *solution; + continue; + } + + // Armijo sufficient decrease satisfied, check strong Wolfe condition. + if (!interpolation_uses_gradients) { + // Irrespective of the interpolation type we are using, we now need the + // gradient at the current point (which satisfies the Armijo condition) + // in order to check the strong Wolfe conditions. + ++summary->num_function_evaluations; + ++summary->num_gradient_evaluations; + solution->value_is_valid = + function->Evaluate(solution->x, + &solution->value, + &solution->gradient); + solution->gradient_is_valid = solution->value_is_valid; + if (!solution->value_is_valid) { + summary->error = + StringPrintf("Line search failed: Wolfe Zoom phase found " + "step_size: %.5e, for which function is invalid, " + "between low_step: %.5e and high_step: %.5e " + "at which function is valid.", + solution->x, bracket_low.x, bracket_high.x); + LOG(WARNING) << summary->error; + return false; + } + } + if (fabs(solution->gradient) <= + -options().sufficient_curvature_decrease * initial_position.gradient) { + // Found a valid termination point satisfying strong Wolfe conditions. + break; + + } else if (solution->gradient * (bracket_high.x - bracket_low.x) >= 0) { + bracket_high = bracket_low; + } + + bracket_low = *solution; + } + // Solution contains a valid point which satisfies the strong Wolfe + // conditions. + return true; +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search.h b/extern/libmv/third_party/ceres/internal/ceres/line_search.h index 95bf56e2a6b..5f24e9fd76e 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search.h +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search.h @@ -35,15 +35,17 @@ #ifndef CERES_NO_LINE_SEARCH_MINIMIZER -#include <glog/logging.h> +#include <string> #include <vector> #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" +#include "ceres/types.h" namespace ceres { namespace internal { class Evaluator; +struct FunctionSample; // Line search is another name for a one dimensional optimization // algorithm. The name "line search" comes from the fact one @@ -61,32 +63,21 @@ class LineSearch { struct Options { Options() - : interpolation_degree(1), - use_higher_degree_interpolation_when_possible(false), + : interpolation_type(CUBIC), sufficient_decrease(1e-4), - min_relative_step_size_change(1e-3), - max_relative_step_size_change(0.6), - step_size_threshold(1e-9), + max_step_contraction(1e-3), + min_step_contraction(0.9), + min_step_size(1e-9), + max_num_iterations(20), + sufficient_curvature_decrease(0.9), + max_step_expansion(10.0), function(NULL) {} - // TODO(sameeragarwal): Replace this with enums which are common - // across various line searches. - // // Degree of the polynomial used to approximate the objective - // function. Valid values are {0, 1, 2}. - // - // For Armijo line search - // - // 0: Bisection based backtracking search. - // 1: Quadratic interpolation. - // 2: Cubic interpolation. - int interpolation_degree; - - // Usually its possible to increase the degree of the - // interpolation polynomial by storing and using an extra point. - bool use_higher_degree_interpolation_when_possible; + // function. + LineSearchInterpolationType interpolation_type; - // Armijo line search parameters. + // Armijo and Wolfe line search parameters. // Solving the line search problem exactly is computationally // prohibitive. Fortunately, line search based optimization @@ -99,19 +90,59 @@ class LineSearch { // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size double sufficient_decrease; - // In each iteration of the Armijo line search, + // In each iteration of the Armijo / Wolfe line search, + // + // new_step_size >= max_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 // - // new_step_size >= min_relative_step_size_change * step_size - double min_relative_step_size_change; + double max_step_contraction; - // In each iteration of the Armijo line search, + // In each iteration of the Armijo / Wolfe line search, // - // new_step_size <= max_relative_step_size_change * step_size - double max_relative_step_size_change; + // new_step_size <= min_step_contraction * step_size + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double min_step_contraction; // If during the line search, the step_size falls below this // value, it is truncated to zero. - double step_size_threshold; + double min_step_size; + + // Maximum number of trial step size iterations during each line search, + // if a step size satisfying the search conditions cannot be found within + // this number of trials, the line search will terminate. + int max_num_iterations; + + // Wolfe-specific line search parameters. + + // The strong Wolfe conditions consist of the Armijo sufficient + // decrease condition, and an additional requirement that the + // step-size be chosen s.t. the _magnitude_ ('strong' Wolfe + // conditions) of the gradient along the search direction + // decreases sufficiently. Precisely, this second condition + // is that we seek a step_size s.t. + // + // |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)| + // + // Where f() is the line search objective and f'() is the derivative + // of f w.r.t step_size (d f / d step_size). + double sufficient_curvature_decrease; + + // During the bracketing phase of the Wolfe search, the step size is + // increased until either a point satisfying the Wolfe conditions is + // found, or an upper bound for a bracket containing a point satisfying + // the conditions is found. Precisely, at each iteration of the + // expansion: + // + // new_step_size <= max_step_expansion * step_size. + // + // By definition for expansion, max_step_expansion > 1.0. + double max_step_expansion; // The one dimensional function that the line search algorithm // minimizes. @@ -147,18 +178,28 @@ class LineSearch { Summary() : success(false), optimal_step_size(0.0), - num_evaluations(0) {} + num_function_evaluations(0), + num_gradient_evaluations(0), + num_iterations(0) {} bool success; double optimal_step_size; - int num_evaluations; + int num_function_evaluations; + int num_gradient_evaluations; + int num_iterations; + string error; }; + explicit LineSearch(const LineSearch::Options& options); virtual ~LineSearch() {} + static LineSearch* Create(const LineSearchType line_search_type, + const LineSearch::Options& options, + string* error); + // Perform the line search. // - // initial_step_size must be a positive number. + // step_size_estimate must be a positive number. // // initial_cost and initial_gradient are the values and gradient of // the function at zero. @@ -166,11 +207,23 @@ class LineSearch { // search. // // Summary::success is true if a non-zero step size is found. - virtual void Search(const LineSearch::Options& options, - double initial_step_size, + virtual void Search(double step_size_estimate, double initial_cost, double initial_gradient, Summary* summary) = 0; + double InterpolatingPolynomialMinimizingStepSize( + const LineSearchInterpolationType& interpolation_type, + const FunctionSample& lowerbound_sample, + const FunctionSample& previous_sample, + const FunctionSample& current_sample, + const double min_step_size, + const double max_step_size) const; + + protected: + const LineSearch::Options& options() const { return options_; } + + private: + LineSearch::Options options_; }; class LineSearchFunction : public LineSearch::Function { @@ -178,7 +231,8 @@ class LineSearchFunction : public LineSearch::Function { explicit LineSearchFunction(Evaluator* evaluator); virtual ~LineSearchFunction() {} void Init(const Vector& position, const Vector& direction); - virtual bool Evaluate(const double x, double* f, double* g); + virtual bool Evaluate(double x, double* f, double* g); + double DirectionInfinityNorm() const; private: Evaluator* evaluator_; @@ -200,12 +254,42 @@ class LineSearchFunction : public LineSearch::Function { // For more details: http://www.di.ens.fr/~mschmidt/Software/minFunc.html class ArmijoLineSearch : public LineSearch { public: + explicit ArmijoLineSearch(const LineSearch::Options& options); virtual ~ArmijoLineSearch() {} - virtual void Search(const LineSearch::Options& options, - double initial_step_size, + virtual void Search(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary); +}; + +// Bracketing / Zoom Strong Wolfe condition line search. This implementation +// is based on the pseudo-code algorithm presented in Nocedal & Wright [1] +// (p60-61) with inspiration from the WolfeLineSearch which ships with the +// minFunc package by Mark Schmidt [2]. +// +// [1] Nocedal J., Wright S., Numerical Optimization, 2nd Ed., Springer, 1999. +// [2] http://www.di.ens.fr/~mschmidt/Software/minFunc.html. +class WolfeLineSearch : public LineSearch { + public: + explicit WolfeLineSearch(const LineSearch::Options& options); + virtual ~WolfeLineSearch() {} + virtual void Search(double step_size_estimate, double initial_cost, double initial_gradient, Summary* summary); + // Returns true iff either a valid point, or valid bracket are found. + bool BracketingPhase(const FunctionSample& initial_position, + const double step_size_estimate, + FunctionSample* bracket_low, + FunctionSample* bracket_high, + bool* perform_zoom_search, + Summary* summary); + // Returns true iff final_line_sample satisfies strong Wolfe conditions. + bool ZoomPhase(const FunctionSample& initial_position, + FunctionSample bracket_low, + FunctionSample bracket_high, + FunctionSample* solution, + Summary* summary); }; } // namespace internal 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 b8b582c3fb1..8ded823e5bd 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 @@ -100,14 +100,24 @@ class NonlinearConjugateGradient : public LineSearchDirection { class LBFGS : public LineSearchDirection { public: - LBFGS(const int num_parameters, const int max_lbfgs_rank) - : low_rank_inverse_hessian_(num_parameters, max_lbfgs_rank) {} + LBFGS(const int num_parameters, + const int max_lbfgs_rank, + const bool use_approximate_eigenvalue_bfgs_scaling) + : low_rank_inverse_hessian_(num_parameters, + max_lbfgs_rank, + use_approximate_eigenvalue_bfgs_scaling), + is_positive_definite_(true) {} virtual ~LBFGS() {} bool NextDirection(const LineSearchMinimizer::State& previous, const LineSearchMinimizer::State& current, Vector* search_direction) { + CHECK(is_positive_definite_) + << "Ceres bug: NextDirection() called on L-BFGS after inverse Hessian " + << "approximation has become indefinite, please contact the " + << "developers!"; + low_rank_inverse_hessian_.Update( previous.search_direction * previous.step_size, current.gradient - previous.gradient); @@ -115,11 +125,177 @@ class LBFGS : public LineSearchDirection { low_rank_inverse_hessian_.RightMultiply(current.gradient.data(), search_direction->data()); *search_direction *= -1.0; + + if (search_direction->dot(current.gradient) >= 0.0) { + LOG(WARNING) << "Numerical failure in L-BFGS update: inverse Hessian " + << "approximation is not positive definite, and thus " + << "initial gradient for search direction is positive: " + << search_direction->dot(current.gradient); + is_positive_definite_ = false; + return false; + } + return true; } private: LowRankInverseHessian low_rank_inverse_hessian_; + bool is_positive_definite_; +}; + +class BFGS : public LineSearchDirection { + public: + BFGS(const int num_parameters, + const bool use_approximate_eigenvalue_scaling) + : num_parameters_(num_parameters), + use_approximate_eigenvalue_scaling_(use_approximate_eigenvalue_scaling), + initialized_(false), + is_positive_definite_(true) { + LOG_IF(WARNING, num_parameters_ >= 1e3) + << "BFGS line search being created with: " << num_parameters_ + << " parameters, this will allocate a dense approximate inverse Hessian" + << " of size: " << num_parameters_ << " x " << num_parameters_ + << ", consider using the L-BFGS memory-efficient line search direction " + << "instead."; + // Construct inverse_hessian_ after logging warning about size s.t. if the + // allocation crashes us, the log will highlight what the issue likely was. + inverse_hessian_ = Matrix::Identity(num_parameters, num_parameters); + } + + virtual ~BFGS() {} + + bool NextDirection(const LineSearchMinimizer::State& previous, + const LineSearchMinimizer::State& current, + Vector* search_direction) { + CHECK(is_positive_definite_) + << "Ceres bug: NextDirection() called on BFGS after inverse Hessian " + << "approximation has become indefinite, please contact the " + << "developers!"; + + const Vector delta_x = previous.search_direction * previous.step_size; + const Vector delta_gradient = current.gradient - previous.gradient; + const double delta_x_dot_delta_gradient = delta_x.dot(delta_gradient); + + if (delta_x_dot_delta_gradient <= 1e-10) { + VLOG(2) << "Skipping BFGS Update, delta_x_dot_delta_gradient too " + << "small: " << delta_x_dot_delta_gradient; + } else { + // Update dense inverse Hessian approximation. + + if (!initialized_ && use_approximate_eigenvalue_scaling_) { + // Rescale the initial inverse Hessian approximation (H_0) to be + // iteratively updated so that it is of similar 'size' to the true + // inverse Hessian at the start point. As shown in [1]: + // + // \gamma = (delta_gradient_{0}' * delta_x_{0}) / + // (delta_gradient_{0}' * delta_gradient_{0}) + // + // Satisfies: + // + // (1 / \lambda_m) <= \gamma <= (1 / \lambda_1) + // + // Where \lambda_1 & \lambda_m are the smallest and largest eigenvalues + // of the true initial Hessian (not the inverse) respectively. Thus, + // \gamma is an approximate eigenvalue of the true inverse Hessian, and + // choosing: H_0 = I * \gamma will yield a starting point that has a + // similar scale to the true inverse Hessian. This technique is widely + // reported to often improve convergence, however this is not + // universally true, particularly if there are errors in the initial + // gradients, or if there are significant differences in the sensitivity + // of the problem to the parameters (i.e. the range of the magnitudes of + // the components of the gradient is large). + // + // The original origin of this rescaling trick is somewhat unclear, the + // earliest reference appears to be Oren [1], however it is widely + // discussed without specific attributation in various texts including + // [2] (p143). + // + // [1] Oren S.S., Self-scaling variable metric (SSVM) algorithms + // Part II: Implementation and experiments, Management Science, + // 20(5), 863-874, 1974. + // [2] Nocedal J., Wright S., Numerical Optimization, Springer, 1999. + inverse_hessian_ *= + delta_x_dot_delta_gradient / delta_gradient.dot(delta_gradient); + } + initialized_ = true; + + // Efficient O(num_parameters^2) BFGS update [2]. + // + // Starting from dense BFGS update detailed in Nocedal [2] p140/177 and + // using: y_k = delta_gradient, s_k = delta_x: + // + // \rho_k = 1.0 / (s_k' * y_k) + // V_k = I - \rho_k * y_k * s_k' + // H_k = (V_k' * H_{k-1} * V_k) + (\rho_k * s_k * s_k') + // + // This update involves matrix, matrix products which naively O(N^3), + // however we can exploit our knowledge that H_k is positive definite + // and thus by defn. symmetric to reduce the cost of the update: + // + // Expanding the update above yields: + // + // H_k = H_{k-1} + + // \rho_k * ( (1.0 + \rho_k * y_k' * H_k * y_k) * s_k * s_k' - + // (s_k * y_k' * H_k + H_k * y_k * s_k') ) + // + // Using: A = (s_k * y_k' * H_k), and the knowledge that H_k = H_k', the + // last term simplifies to (A + A'). Note that although A is not symmetric + // (A + A') is symmetric. For ease of construction we also define + // B = (1 + \rho_k * y_k' * H_k * y_k) * s_k * s_k', which is by defn + // symmetric due to construction from: s_k * s_k'. + // + // Now we can write the BFGS update as: + // + // H_k = H_{k-1} + \rho_k * (B - (A + A')) + + // For efficiency, as H_k is by defn. symmetric, we will only maintain the + // *lower* triangle of H_k (and all intermediary terms). + + const double rho_k = 1.0 / delta_x_dot_delta_gradient; + + // Calculate: A = s_k * y_k' * H_k + Matrix A = delta_x * (delta_gradient.transpose() * + inverse_hessian_.selfadjointView<Eigen::Lower>()); + + // Calculate scalar: (1 + \rho_k * y_k' * H_k * y_k) + const double delta_x_times_delta_x_transpose_scale_factor = + (1.0 + (rho_k * delta_gradient.transpose() * + inverse_hessian_.selfadjointView<Eigen::Lower>() * + delta_gradient)); + // Calculate: B = (1 + \rho_k * y_k' * H_k * y_k) * s_k * s_k' + Matrix B = Matrix::Zero(num_parameters_, num_parameters_); + B.selfadjointView<Eigen::Lower>(). + rankUpdate(delta_x, delta_x_times_delta_x_transpose_scale_factor); + + // Finally, update inverse Hessian approximation according to: + // H_k = H_{k-1} + \rho_k * (B - (A + A')). Note that (A + A') is + // symmetric, even though A is not. + inverse_hessian_.triangularView<Eigen::Lower>() += + rho_k * (B - A - A.transpose()); + } + + *search_direction = + inverse_hessian_.selfadjointView<Eigen::Lower>() * + (-1.0 * current.gradient); + + if (search_direction->dot(current.gradient) >= 0.0) { + LOG(WARNING) << "Numerical failure in BFGS update: inverse Hessian " + << "approximation is not positive definite, and thus " + << "initial gradient for search direction is positive: " + << search_direction->dot(current.gradient); + is_positive_definite_ = false; + return false; + } + + return true; + } + + private: + const int num_parameters_; + const bool use_approximate_eigenvalue_scaling_; + Matrix inverse_hessian_; + bool initialized_; + bool is_positive_definite_; }; LineSearchDirection* @@ -135,8 +311,16 @@ LineSearchDirection::Create(const LineSearchDirection::Options& options) { } if (options.type == ceres::LBFGS) { - return new ceres::internal::LBFGS(options.num_parameters, - options.max_lbfgs_rank); + return new ceres::internal::LBFGS( + options.num_parameters, + options.max_lbfgs_rank, + options.use_approximate_eigenvalue_bfgs_scaling); + } + + if (options.type == ceres::BFGS) { + return new ceres::internal::BFGS( + options.num_parameters, + options.use_approximate_eigenvalue_bfgs_scaling); } LOG(ERROR) << "Unknown line search direction type: " << options.type; diff --git a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h index 08747544bbe..0857cb005f9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h +++ b/extern/libmv/third_party/ceres/internal/ceres/line_search_direction.h @@ -48,7 +48,8 @@ class LineSearchDirection { type(LBFGS), nonlinear_conjugate_gradient_type(FLETCHER_REEVES), function_tolerance(1e-12), - max_lbfgs_rank(20) { + max_lbfgs_rank(20), + use_approximate_eigenvalue_bfgs_scaling(true) { } int num_parameters; @@ -56,6 +57,7 @@ class LineSearchDirection { NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; double function_tolerance; int max_lbfgs_rank; + bool use_approximate_eigenvalue_bfgs_scaling; }; static LineSearchDirection* Create(const Options& options); 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 684a7369b3a..2cc89faf4c4 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 @@ -160,17 +160,44 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, line_search_direction_options.nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; line_search_direction_options.max_lbfgs_rank = options.max_lbfgs_rank; + line_search_direction_options.use_approximate_eigenvalue_bfgs_scaling = + options.use_approximate_eigenvalue_bfgs_scaling; scoped_ptr<LineSearchDirection> line_search_direction( LineSearchDirection::Create(line_search_direction_options)); LineSearchFunction line_search_function(evaluator); + LineSearch::Options line_search_options; + line_search_options.interpolation_type = + options.line_search_interpolation_type; + line_search_options.min_step_size = options.min_line_search_step_size; + line_search_options.sufficient_decrease = + options.line_search_sufficient_function_decrease; + line_search_options.max_step_contraction = + options.max_line_search_step_contraction; + line_search_options.min_step_contraction = + options.min_line_search_step_contraction; + line_search_options.max_num_iterations = + options.max_num_line_search_step_size_iterations; + line_search_options.sufficient_curvature_decrease = + options.line_search_sufficient_curvature_decrease; + line_search_options.max_step_expansion = + options.max_line_search_step_expansion; line_search_options.function = &line_search_function; - // TODO(sameeragarwal): Make this parameterizable over different - // line searches. - ArmijoLineSearch line_search; + scoped_ptr<LineSearch> + line_search(LineSearch::Create(options.line_search_type, + line_search_options, + &summary->error)); + if (line_search.get() == NULL) { + LOG(ERROR) << "Ceres bug: Unable to create a LineSearch object, please " + << "contact the developers!, error: " << summary->error; + summary->termination_type = DID_NOT_RUN; + return; + } + LineSearch::Summary line_search_summary; + int num_line_search_direction_restarts = 0; while (true) { if (!RunCallbacks(options.callbacks, iteration_summary, summary)) { @@ -194,6 +221,8 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, iteration_summary = IterationSummary(); iteration_summary.iteration = summary->iterations.back().iteration + 1; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; bool line_search_status = true; if (iteration_summary.iteration == 1) { @@ -205,9 +234,36 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, ¤t_state.search_direction); } - if (!line_search_status) { - LOG(WARNING) << "Line search direction computation failed. " - "Resorting to steepest descent."; + if (!line_search_status && + num_line_search_direction_restarts >= + options.max_num_line_search_direction_restarts) { + // Line search direction failed to generate a new direction, and we + // have already reached our specified maximum number of restarts, + // terminate optimization. + summary->error = + StringPrintf("Line search direction failure: specified " + "max_num_line_search_direction_restarts: %d reached.", + options.max_num_line_search_direction_restarts); + LOG(WARNING) << summary->error << " terminating optimization."; + summary->termination_type = NUMERICAL_FAILURE; + break; + + } else if (!line_search_status) { + // Restart line search direction with gradient descent on first iteration + // as we have not yet reached our maximum number of restarts. + CHECK_LT(num_line_search_direction_restarts, + options.max_num_line_search_direction_restarts); + + ++num_line_search_direction_restarts; + LOG(WARNING) + << "Line search direction algorithm: " + << LineSearchDirectionTypeToString(options.line_search_direction_type) + << ", failed to produce a valid new direction at iteration: " + << iteration_summary.iteration << ". Restarting, number of " + << "restarts: " << num_line_search_direction_restarts << " / " + << options.max_num_line_search_direction_restarts << " [max]."; + line_search_direction.reset( + LineSearchDirection::Create(line_search_direction_options)); current_state.search_direction = -current_state.gradient; } @@ -217,21 +273,41 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, // TODO(sameeragarwal): Refactor this into its own object and add // explanations for the various choices. - const double initial_step_size = (iteration_summary.iteration == 1) + // + // Note that we use !line_search_status to ensure that we treat cases when + // we restarted the line search direction equivalently to the first + // iteration. + const double initial_step_size = + (iteration_summary.iteration == 1 || !line_search_status) ? min(1.0, 1.0 / current_state.gradient_max_norm) : min(1.0, 2.0 * (current_state.cost - previous_state.cost) / current_state.directional_derivative); + // By definition, we should only ever go forwards along the specified search + // direction in a line search, most likely cause for this being violated + // would be a numerical failure in the line search direction calculation. + if (initial_step_size < 0.0) { + summary->error = + StringPrintf("Numerical failure in line search, initial_step_size is " + "negative: %.5e, directional_derivative: %.5e, " + "(current_cost - previous_cost): %.5e", + initial_step_size, current_state.directional_derivative, + (current_state.cost - previous_state.cost)); + LOG(WARNING) << summary->error; + summary->termination_type = NUMERICAL_FAILURE; + break; + } - line_search.Search(line_search_options, - initial_step_size, - current_state.cost, - current_state.directional_derivative, - &line_search_summary); + line_search->Search(initial_step_size, + current_state.cost, + current_state.directional_derivative, + &line_search_summary); current_state.step_size = line_search_summary.optimal_step_size; delta = current_state.step_size * current_state.search_direction; previous_state = current_state; + iteration_summary.step_solver_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; // TODO(sameeragarwal): Collect stats. if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data()) || @@ -270,7 +346,11 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, iteration_summary.step_norm = delta.norm(); iteration_summary.step_size = current_state.step_size; iteration_summary.line_search_function_evaluations = - line_search_summary.num_evaluations; + line_search_summary.num_function_evaluations; + iteration_summary.line_search_gradient_evaluations = + line_search_summary.num_gradient_evaluations; + iteration_summary.line_search_iterations = + line_search_summary.num_iterations; iteration_summary.iteration_time_in_seconds = WallTimeInSeconds() - iteration_start_time; iteration_summary.cumulative_time_in_seconds = @@ -278,6 +358,7 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, + summary->preprocessor_time_in_seconds; summary->iterations.push_back(iteration_summary); + ++summary->num_successful_steps; } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc index 6c886a1be38..24ba565daf9 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc @@ -36,10 +36,8 @@ #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/casts.h" -#include "ceres/compressed_row_sparse_matrix.h" #include "ceres/file.h" #include "ceres/internal/scoped_ptr.h" -#include "ceres/matrix_proto.h" #include "ceres/stringprintf.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" @@ -64,74 +62,6 @@ LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { return NULL; } -#ifndef CERES_NO_PROTOCOL_BUFFERS -LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( - const string& filename) { - LinearLeastSquaresProblemProto problem_proto; - { - string serialized_proto; - ReadFileToStringOrDie(filename, &serialized_proto); - CHECK(problem_proto.ParseFromString(serialized_proto)); - } - - LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; - const SparseMatrixProto& A = problem_proto.a(); - - if (A.has_block_matrix()) { - problem->A.reset(new BlockSparseMatrix(A)); - } else if (A.has_triplet_matrix()) { - problem->A.reset(new TripletSparseMatrix(A)); - } else { - problem->A.reset(new CompressedRowSparseMatrix(A)); - } - - if (problem_proto.b_size() > 0) { - problem->b.reset(new double[problem_proto.b_size()]); - for (int i = 0; i < problem_proto.b_size(); ++i) { - problem->b[i] = problem_proto.b(i); - } - } - - if (problem_proto.d_size() > 0) { - problem->D.reset(new double[problem_proto.d_size()]); - for (int i = 0; i < problem_proto.d_size(); ++i) { - problem->D[i] = problem_proto.d(i); - } - } - - if (problem_proto.d_size() > 0) { - if (problem_proto.x_size() > 0) { - problem->x_D.reset(new double[problem_proto.x_size()]); - for (int i = 0; i < problem_proto.x_size(); ++i) { - problem->x_D[i] = problem_proto.x(i); - } - } - } else { - if (problem_proto.x_size() > 0) { - problem->x.reset(new double[problem_proto.x_size()]); - for (int i = 0; i < problem_proto.x_size(); ++i) { - problem->x[i] = problem_proto.x(i); - } - } - } - - problem->num_eliminate_blocks = 0; - if (problem_proto.has_num_eliminate_blocks()) { - problem->num_eliminate_blocks = problem_proto.num_eliminate_blocks(); - } - - return problem; -} -#else -LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( - const string& filename) { - LOG(FATAL) - << "Loading a least squares problem from disk requires " - << "Ceres to be built with Protocol Buffers support."; - return NULL; -} -#endif // CERES_NO_PROTOCOL_BUFFERS - /* A = [1 2] [3 4] @@ -574,9 +504,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { } namespace { -bool DumpLinearLeastSquaresProblemToConsole(const string& directory, - int iteration, - const SparseMatrix* A, +bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, const double* D, const double* b, const double* x, @@ -601,61 +529,6 @@ bool DumpLinearLeastSquaresProblemToConsole(const string& directory, return true; }; -#ifndef CERES_NO_PROTOCOL_BUFFERS -bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, - int iteration, - const SparseMatrix* A, - const double* D, - const double* b, - const double* x, - int num_eliminate_blocks) { - CHECK_NOTNULL(A); - LinearLeastSquaresProblemProto lsqp; - A->ToProto(lsqp.mutable_a()); - - if (D != NULL) { - for (int i = 0; i < A->num_cols(); ++i) { - lsqp.add_d(D[i]); - } - } - - if (b != NULL) { - for (int i = 0; i < A->num_rows(); ++i) { - lsqp.add_b(b[i]); - } - } - - if (x != NULL) { - for (int i = 0; i < A->num_cols(); ++i) { - lsqp.add_x(x[i]); - } - } - - lsqp.set_num_eliminate_blocks(num_eliminate_blocks); - string format_string = JoinPath(directory, - "lm_iteration_%03d.lsqp"); - string filename = - StringPrintf(format_string.c_str(), iteration); - LOG(INFO) << "Dumping least squares problem for iteration " << iteration - << " to disk. File: " << filename; - WriteStringToFileOrDie(lsqp.SerializeAsString(), filename); - return true; -} -#else -bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, - int iteration, - const SparseMatrix* A, - const double* D, - const double* b, - const double* x, - int num_eliminate_blocks) { - LOG(ERROR) << "Dumping least squares problems is only " - << "supported when Ceres is compiled with " - << "protocol buffer support."; - return false; -} -#endif - void WriteArrayToFileOrDie(const string& filename, const double* x, const int size) { @@ -669,31 +542,25 @@ void WriteArrayToFileOrDie(const string& filename, fclose(fptr); } -bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, - int iteration, +bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base, const SparseMatrix* A, const double* D, const double* b, const double* x, int num_eliminate_blocks) { CHECK_NOTNULL(A); - string format_string = JoinPath(directory, - "lm_iteration_%03d"); - string filename_prefix = - StringPrintf(format_string.c_str(), iteration); - - LOG(INFO) << "writing to: " << filename_prefix << "*"; + LOG(INFO) << "writing to: " << filename_base << "*"; string matlab_script; StringAppendF(&matlab_script, - "function lsqp = lm_iteration_%03d()\n", iteration); + "function lsqp = load_trust_region_problem()\n"); StringAppendF(&matlab_script, "lsqp.num_rows = %d;\n", A->num_rows()); StringAppendF(&matlab_script, "lsqp.num_cols = %d;\n", A->num_cols()); { - string filename = filename_prefix + "_A.txt"; + string filename = filename_base + "_A.txt"; FILE* fptr = fopen(filename.c_str(), "w"); CHECK_NOTNULL(fptr); A->ToTextFile(fptr); @@ -709,34 +576,33 @@ bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, if (D != NULL) { - string filename = filename_prefix + "_D.txt"; + string filename = filename_base + "_D.txt"; WriteArrayToFileOrDie(filename, D, A->num_cols()); StringAppendF(&matlab_script, "lsqp.D = load('%s', '-ascii');\n", filename.c_str()); } if (b != NULL) { - string filename = filename_prefix + "_b.txt"; + string filename = filename_base + "_b.txt"; WriteArrayToFileOrDie(filename, b, A->num_rows()); StringAppendF(&matlab_script, "lsqp.b = load('%s', '-ascii');\n", filename.c_str()); } if (x != NULL) { - string filename = filename_prefix + "_x.txt"; + string filename = filename_base + "_x.txt"; WriteArrayToFileOrDie(filename, x, A->num_cols()); StringAppendF(&matlab_script, "lsqp.x = load('%s', '-ascii');\n", filename.c_str()); } - string matlab_filename = filename_prefix + ".m"; + string matlab_filename = filename_base + ".m"; WriteStringToFileOrDie(matlab_script, matlab_filename); return true; } } // namespace -bool DumpLinearLeastSquaresProblem(const string& directory, - int iteration, +bool DumpLinearLeastSquaresProblem(const string& filename_base, DumpFormatType dump_format_type, const SparseMatrix* A, const double* D, @@ -745,19 +611,10 @@ bool DumpLinearLeastSquaresProblem(const string& directory, int num_eliminate_blocks) { switch (dump_format_type) { case CONSOLE: - return DumpLinearLeastSquaresProblemToConsole(directory, - iteration, - A, D, b, x, + return DumpLinearLeastSquaresProblemToConsole(A, D, b, x, num_eliminate_blocks); - case PROTOBUF: - return DumpLinearLeastSquaresProblemToProtocolBuffer( - directory, - iteration, - A, D, b, x, - num_eliminate_blocks); case TEXTFILE: - return DumpLinearLeastSquaresProblemToTextFile(directory, - iteration, + return DumpLinearLeastSquaresProblemToTextFile(filename_base, A, D, b, x, num_eliminate_blocks); default: diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h index c76ae91c7d8..fdeed70de62 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h @@ -63,8 +63,6 @@ struct LinearLeastSquaresProblem { // Factories for linear least squares problem. LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id); -LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( - const string& filename); LinearLeastSquaresProblem* LinearLeastSquaresProblem0(); LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); @@ -73,8 +71,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); // Write the linear least squares problem to disk. The exact format // depends on dump_format_type. -bool DumpLinearLeastSquaresProblem(const string& directory, - int iteration, +bool DumpLinearLeastSquaresProblem(const string& filename_base, DumpFormatType dump_format_type, const SparseMatrix* A, const double* D, 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 ca10faa24b4..22691b33e44 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h @@ -36,6 +36,7 @@ #include <cstddef> #include <map> +#include <string> #include <vector> #include "ceres/block_sparse_matrix.h" #include "ceres/casts.h" @@ -73,7 +74,8 @@ class LinearSolver { Options() : type(SPARSE_NORMAL_CHOLESKY), preconditioner_type(JACOBI), - sparse_linear_algebra_library(SUITE_SPARSE), + dense_linear_algebra_library_type(EIGEN), + sparse_linear_algebra_library_type(SUITE_SPARSE), use_postordering(false), min_num_iterations(1), max_num_iterations(1), @@ -88,7 +90,8 @@ class LinearSolver { PreconditionerType preconditioner_type; - SparseLinearAlgebraLibraryType sparse_linear_algebra_library; + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; // See solver.h for information about this flag. bool use_postordering; @@ -316,7 +319,6 @@ class TypedLinearSolver : public LinearSolver { // Linear solvers that depend on acccess to the low level structure of // a SparseMatrix. typedef TypedLinearSolver<BlockSparseMatrix> BlockSparseMatrixSolver; // NOLINT -typedef TypedLinearSolver<BlockSparseMatrixBase> BlockSparseMatrixBaseSolver; // NOLINT typedef TypedLinearSolver<CompressedRowSparseMatrix> CompressedRowSparseMatrixSolver; // NOLINT typedef TypedLinearSolver<DenseSparseMatrix> DenseSparseMatrixSolver; // NOLINT typedef TypedLinearSolver<TripletSparseMatrix> TripletSparseMatrixSolver; // NOLINT diff --git a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc index 3fe113f1afb..372165f9523 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.cc @@ -35,12 +35,15 @@ namespace ceres { namespace internal { -LowRankInverseHessian::LowRankInverseHessian(int num_parameters, - int max_num_corrections) +LowRankInverseHessian::LowRankInverseHessian( + int num_parameters, + int max_num_corrections, + bool use_approximate_eigenvalue_scaling) : num_parameters_(num_parameters), max_num_corrections_(max_num_corrections), + use_approximate_eigenvalue_scaling_(use_approximate_eigenvalue_scaling), num_corrections_(0), - diagonal_(1.0), + approximate_eigenvalue_scale_(1.0), delta_x_history_(num_parameters, max_num_corrections), delta_gradient_history_(num_parameters, max_num_corrections), delta_x_dot_delta_gradient_(max_num_corrections) { @@ -50,7 +53,8 @@ bool LowRankInverseHessian::Update(const Vector& delta_x, const Vector& delta_gradient) { const double delta_x_dot_delta_gradient = delta_x.dot(delta_gradient); if (delta_x_dot_delta_gradient <= 1e-10) { - VLOG(2) << "Skipping LBFGS Update. " << delta_x_dot_delta_gradient; + VLOG(2) << "Skipping LBFGS Update, delta_x_dot_delta_gradient too small: " + << delta_x_dot_delta_gradient; return false; } @@ -58,16 +62,16 @@ bool LowRankInverseHessian::Update(const Vector& delta_x, // TODO(sameeragarwal): This can be done more efficiently using // a circular buffer/indexing scheme, but for simplicity we will // do the expensive copy for now. - delta_x_history_.block(0, 0, num_parameters_, max_num_corrections_ - 2) = + delta_x_history_.block(0, 0, num_parameters_, max_num_corrections_ - 1) = delta_x_history_ .block(0, 1, num_parameters_, max_num_corrections_ - 1); delta_gradient_history_ - .block(0, 0, num_parameters_, max_num_corrections_ - 2) = + .block(0, 0, num_parameters_, max_num_corrections_ - 1) = delta_gradient_history_ .block(0, 1, num_parameters_, max_num_corrections_ - 1); - delta_x_dot_delta_gradient_.head(num_corrections_ - 2) = + delta_x_dot_delta_gradient_.head(num_corrections_ - 1) = delta_x_dot_delta_gradient_.tail(num_corrections_ - 1); } else { ++num_corrections_; @@ -77,7 +81,8 @@ bool LowRankInverseHessian::Update(const Vector& delta_x, delta_gradient_history_.col(num_corrections_ - 1) = delta_gradient; delta_x_dot_delta_gradient_(num_corrections_ - 1) = delta_x_dot_delta_gradient; - diagonal_ = delta_x_dot_delta_gradient / delta_gradient.squaredNorm(); + approximate_eigenvalue_scale_ = + delta_x_dot_delta_gradient / delta_gradient.squaredNorm(); return true; } @@ -96,7 +101,39 @@ void LowRankInverseHessian::RightMultiply(const double* x_ptr, search_direction -= alpha(i) * delta_gradient_history_.col(i); } - search_direction *= diagonal_; + if (use_approximate_eigenvalue_scaling_) { + // Rescale the initial inverse Hessian approximation (H_0) to be iteratively + // updated so that it is of similar 'size' to the true inverse Hessian along + // the most recent search direction. As shown in [1]: + // + // \gamma_k = (delta_gradient_{k-1}' * delta_x_{k-1}) / + // (delta_gradient_{k-1}' * delta_gradient_{k-1}) + // + // Satisfies: + // + // (1 / \lambda_m) <= \gamma_k <= (1 / \lambda_1) + // + // Where \lambda_1 & \lambda_m are the smallest and largest eigenvalues of + // the true Hessian (not the inverse) along the most recent search direction + // respectively. Thus \gamma is an approximate eigenvalue of the true + // inverse Hessian, and choosing: H_0 = I * \gamma will yield a starting + // point that has a similar scale to the true inverse Hessian. This + // technique is widely reported to often improve convergence, however this + // is not universally true, particularly if there are errors in the initial + // jacobians, or if there are significant differences in the sensitivity + // of the problem to the parameters (i.e. the range of the magnitudes of + // the components of the gradient is large). + // + // The original origin of this rescaling trick is somewhat unclear, the + // earliest reference appears to be Oren [1], however it is widely discussed + // without specific attributation in various texts including [2] (p143/178). + // + // [1] Oren S.S., Self-scaling variable metric (SSVM) algorithms Part II: + // Implementation and experiments, Management Science, + // 20(5), 863-874, 1974. + // [2] Nocedal J., Wright S., Numerical Optimization, Springer, 1999. + search_direction *= approximate_eigenvalue_scale_; + } for (int i = 0; i < num_corrections_; ++i) { const double beta = delta_gradient_history_.col(i).dot(search_direction) / diff --git a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h index 6f3fc0c9d00..7d293d09422 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h +++ b/extern/libmv/third_party/ceres/internal/ceres/low_rank_inverse_hessian.h @@ -61,10 +61,16 @@ class LowRankInverseHessian : public LinearOperator { public: // num_parameters is the row/column size of the Hessian. // max_num_corrections is the rank of the Hessian approximation. + // use_approximate_eigenvalue_scaling controls whether the initial + // inverse Hessian used during Right/LeftMultiply() is scaled by + // the approximate eigenvalue of the true inverse Hessian at the + // current operating point. // The approximation uses: // 2 * max_num_corrections * num_parameters + max_num_corrections // doubles. - LowRankInverseHessian(int num_parameters, int max_num_corrections); + LowRankInverseHessian(int num_parameters, + int max_num_corrections, + bool use_approximate_eigenvalue_scaling); virtual ~LowRankInverseHessian() {} // Update the low rank approximation. delta_x is the change in the @@ -86,8 +92,9 @@ class LowRankInverseHessian : public LinearOperator { private: const int num_parameters_; const int max_num_corrections_; + const bool use_approximate_eigenvalue_scaling_; int num_corrections_; - double diagonal_; + double approximate_eigenvalue_scale_; Matrix delta_x_history_; Matrix delta_gradient_history_; Vector delta_x_dot_delta_gradient_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h index 040ddd96fbb..622e9cee1d0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_MINIMIZER_H_ #define CERES_INTERNAL_MINIMIZER_H_ +#include <string> #include <vector> #include "ceres/internal/port.h" #include "ceres/iteration_callback.h" @@ -73,9 +74,12 @@ class Minimizer { use_nonmonotonic_steps = options.use_nonmonotonic_steps; max_consecutive_nonmonotonic_steps = options.max_consecutive_nonmonotonic_steps; - lsqp_dump_directory = options.lsqp_dump_directory; - lsqp_iterations_to_dump = options.lsqp_iterations_to_dump; - lsqp_dump_format_type = options.lsqp_dump_format_type; + trust_region_problem_dump_directory = + options.trust_region_problem_dump_directory; + trust_region_minimizer_iterations_to_dump = + options.trust_region_minimizer_iterations_to_dump; + trust_region_problem_dump_format_type = + options.trust_region_problem_dump_format_type; max_num_consecutive_invalid_steps = options.max_num_consecutive_invalid_steps; min_trust_region_radius = options.min_trust_region_radius; @@ -84,11 +88,31 @@ class Minimizer { nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; max_lbfgs_rank = options.max_lbfgs_rank; + use_approximate_eigenvalue_bfgs_scaling = + options.use_approximate_eigenvalue_bfgs_scaling; + line_search_interpolation_type = + options.line_search_interpolation_type; + min_line_search_step_size = options.min_line_search_step_size; + line_search_sufficient_function_decrease = + options.line_search_sufficient_function_decrease; + max_line_search_step_contraction = + options.max_line_search_step_contraction; + min_line_search_step_contraction = + options.min_line_search_step_contraction; + max_num_line_search_step_size_iterations = + options.max_num_line_search_step_size_iterations; + max_num_line_search_direction_restarts = + options.max_num_line_search_direction_restarts; + line_search_sufficient_curvature_decrease = + options.line_search_sufficient_curvature_decrease; + max_line_search_step_expansion = + options.max_line_search_step_expansion; evaluator = NULL; trust_region_strategy = NULL; jacobian = NULL; callbacks = options.callbacks; inner_iteration_minimizer = NULL; + inner_iteration_tolerance = options.inner_iteration_tolerance; } int max_num_iterations; @@ -109,15 +133,26 @@ class Minimizer { bool jacobi_scaling; bool use_nonmonotonic_steps; int max_consecutive_nonmonotonic_steps; - vector<int> lsqp_iterations_to_dump; - DumpFormatType lsqp_dump_format_type; - string lsqp_dump_directory; + vector<int> trust_region_minimizer_iterations_to_dump; + DumpFormatType trust_region_problem_dump_format_type; + string trust_region_problem_dump_directory; int max_num_consecutive_invalid_steps; double min_trust_region_radius; LineSearchDirectionType line_search_direction_type; LineSearchType line_search_type; NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; int max_lbfgs_rank; + bool use_approximate_eigenvalue_bfgs_scaling; + LineSearchInterpolationType line_search_interpolation_type; + double min_line_search_step_size; + double line_search_sufficient_function_decrease; + double max_line_search_step_contraction; + double min_line_search_step_contraction; + int max_num_line_search_step_size_iterations; + int max_num_line_search_direction_restarts; + double line_search_sufficient_curvature_decrease; + double max_line_search_step_expansion; + // List of callbacks that are executed by the Minimizer at the end // of each iteration. @@ -141,6 +176,7 @@ class Minimizer { SparseMatrix* jacobian; Minimizer* inner_iteration_minimizer; + double inner_iteration_tolerance; }; static bool RunCallbacks(const vector<IterationCallback*> callbacks, diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h index b1e8d938b8a..695fa6ff97b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h @@ -173,8 +173,8 @@ class ParameterBlock { new double[local_parameterization_->GlobalSize() * local_parameterization_->LocalSize()]); CHECK(UpdateLocalParameterizationJacobian()) - "Local parameterization Jacobian computation failed" - "for x: " << ConstVectorRef(state_, Size()).transpose(); + << "Local parameterization Jacobian computation failed for x: " + << ConstVectorRef(state_, Size()).transpose(); } else { // Ignore the case that the parameterizations match. } 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 e8f626f8e80..190715bee43 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 @@ -42,6 +42,32 @@ namespace ceres { namespace internal { +int ComputeStableSchurOrdering(const Program& program, + vector<ParameterBlock*>* ordering) { + CHECK_NOTNULL(ordering)->clear(); + + scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + const HashSet<ParameterBlock*>& vertices = graph->vertices(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + if (vertices.count(parameter_blocks[i]) > 0) { + ordering->push_back(parameter_blocks[i]); + } + } + + int independent_set_size = StableIndependentSetOrdering(*graph, ordering); + + // Add the excluded blocks to back of the ordering vector. + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (parameter_block->IsConstant()) { + ordering->push_back(parameter_block); + } + } + + return independent_set_size; +} + int ComputeSchurOrdering(const Program& program, vector<ParameterBlock*>* ordering) { CHECK_NOTNULL(ordering)->clear(); 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 a5277a44c70..4675cb8dc7c 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 @@ -58,6 +58,12 @@ class ParameterBlock; int ComputeSchurOrdering(const Program& program, vector<ParameterBlock* >* ordering); +// Same as above, except that ties while computing the independent set +// ordering are resolved in favour of the order in which the parameter +// blocks occur in the program. +int ComputeStableSchurOrdering(const Program& program, + vector<ParameterBlock* >* ordering); + // Use an approximate independent set ordering to decompose the // parameter blocks of a problem in a sequence of independent // sets. The ordering covers all the non-constant parameter blocks in diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc index c488184ac93..59eaff8ec1b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc @@ -35,17 +35,17 @@ #include <algorithm> #include <cstring> #include <vector> -#include "ceres/blas.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/internal/eigen.h" +#include "ceres/small_blas.h" #include "glog/logging.h" namespace ceres { namespace internal { PartitionedMatrixView::PartitionedMatrixView( - const BlockSparseMatrixBase& matrix, + const BlockSparseMatrix& matrix, int num_col_blocks_a) : matrix_(matrix), num_col_blocks_e_(num_col_blocks_a) { @@ -96,8 +96,8 @@ void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { // Iterate over the first num_row_blocks_e_ row blocks, and multiply // by the first cell in each row block. + const double* values = matrix_.values(); for (int r = 0; r < num_row_blocks_e_; ++r) { - const double* row_values = matrix_.RowBlockValues(r); const Cell& cell = bs->rows[r].cells[0]; const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; @@ -105,7 +105,7 @@ void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, x + col_block_pos, y + row_block_pos); } @@ -119,17 +119,17 @@ void PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const { // E. If the row block is not in E (i.e its in the bottom // num_row_blocks - num_row_blocks_e row blocks), then all the cells // are of type F and multiply by them all. + const double* values = matrix_.values(); for (int r = 0; r < bs->rows.size(); ++r) { const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const vector<Cell>& cells = bs->rows[r].cells; for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { - const double* row_values = matrix_.RowBlockValues(r); const int col_block_id = cells[c].block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, x + col_block_pos - num_cols_e(), y + row_block_pos); } @@ -141,16 +141,16 @@ void PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const { // Iterate over the first num_row_blocks_e_ row blocks, and multiply // by the first cell in each row block. + const double* values = matrix_.values(); for (int r = 0; r < num_row_blocks_e_; ++r) { const Cell& cell = bs->rows[r].cells[0]; - const double* row_values = matrix_.RowBlockValues(r); const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const int col_block_id = cell.block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, x + row_block_pos, y + col_block_pos); } @@ -164,17 +164,17 @@ void PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const { // E. If the row block is not in E (i.e its in the bottom // num_row_blocks - num_row_blocks_e row blocks), then all the cells // are of type F and multiply by them all. + const double* values = matrix_.values(); for (int r = 0; r < bs->rows.size(); ++r) { const int row_block_pos = bs->rows[r].block.position; const int row_block_size = bs->rows[r].block.size; const vector<Cell>& cells = bs->rows[r].cells; for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { - const double* row_values = matrix_.RowBlockValues(r); const int col_block_id = cells[c].block_id; const int col_block_pos = bs->cols[col_block_id].position; const int col_block_size = bs->cols[col_block_id].size; MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, x + row_block_pos, y + col_block_pos - num_cols_e()); } @@ -248,9 +248,8 @@ void PartitionedMatrixView::UpdateBlockDiagonalEtE( block_diagonal->block_structure(); block_diagonal->SetZero(); - + const double* values = matrix_.values(); for (int r = 0; r < num_row_blocks_e_ ; ++r) { - const double* row_values = matrix_.RowBlockValues(r); const Cell& cell = bs->rows[r].cells[0]; const int row_block_size = bs->rows[r].block.size; const int block_id = cell.block_id; @@ -260,8 +259,8 @@ void PartitionedMatrixView::UpdateBlockDiagonalEtE( MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cell.position, row_block_size, col_block_size, - row_values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, + values + cell.position, row_block_size, col_block_size, block_diagonal->mutable_values() + cell_position, 0, 0, col_block_size, col_block_size); } @@ -279,10 +278,10 @@ void PartitionedMatrixView::UpdateBlockDiagonalFtF( block_diagonal->block_structure(); block_diagonal->SetZero(); + const double* values = matrix_.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; - const double* row_values = matrix_.RowBlockValues(r); for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { const int col_block_id = cells[c].block_id; const int col_block_size = bs->cols[col_block_id].size; @@ -292,8 +291,8 @@ void PartitionedMatrixView::UpdateBlockDiagonalFtF( MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + cells[c].position, row_block_size, col_block_size, - row_values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, + values + cells[c].position, row_block_size, col_block_size, block_diagonal->mutable_values() + cell_position, 0, 0, col_block_size, col_block_size); } diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h index cfe4de5b436..ebfbe403189 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h @@ -60,7 +60,7 @@ class PartitionedMatrixView { public: // matrix = [E F], where the matrix E contains the first // num_col_blocks_a column blocks. - PartitionedMatrixView(const BlockSparseMatrixBase& matrix, + PartitionedMatrixView(const BlockSparseMatrix& matrix, int num_col_blocks_a); ~PartitionedMatrixView(); @@ -107,7 +107,7 @@ class PartitionedMatrixView { BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, int end_col_block) const; - const BlockSparseMatrixBase& matrix_; + const BlockSparseMatrix& matrix_; int num_row_blocks_e_; int num_col_blocks_e_; int num_col_blocks_f_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc index 05e539f9fb1..505a47d3d61 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.cc @@ -45,8 +45,8 @@ SparseMatrixPreconditionerWrapper::SparseMatrixPreconditionerWrapper( SparseMatrixPreconditionerWrapper::~SparseMatrixPreconditionerWrapper() { } -bool SparseMatrixPreconditionerWrapper::Update(const BlockSparseMatrixBase& A, - const double* D) { +bool SparseMatrixPreconditionerWrapper::UpdateImpl(const SparseMatrix& A, + const double* D) { return true; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h index d7c88293687..af64e3c9a44 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h +++ b/extern/libmv/third_party/ceres/internal/ceres/preconditioner.h @@ -32,13 +32,15 @@ #define CERES_INTERNAL_PRECONDITIONER_H_ #include <vector> +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" #include "ceres/linear_operator.h" #include "ceres/sparse_matrix.h" namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; class SparseMatrix; class Preconditioner : public LinearOperator { @@ -46,7 +48,7 @@ class Preconditioner : public LinearOperator { struct Options { Options() : type(JACOBI), - sparse_linear_algebra_library(SUITE_SPARSE), + sparse_linear_algebra_library_type(SUITE_SPARSE), num_threads(1), row_block_size(Eigen::Dynamic), e_block_size(Eigen::Dynamic), @@ -55,7 +57,7 @@ class Preconditioner : public LinearOperator { PreconditionerType type; - SparseLinearAlgebraLibraryType sparse_linear_algebra_library; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; // If possible, how many threads the preconditioner can use. int num_threads; @@ -105,7 +107,7 @@ class Preconditioner : public LinearOperator { // // D can be NULL, in which case its interpreted as a diagonal matrix // of size zero. - virtual bool Update(const BlockSparseMatrixBase& A, const double* D) = 0; + virtual bool Update(const LinearOperator& A, const double* D) = 0; // LinearOperator interface. Since the operator is symmetric, // LeftMultiply and num_cols are just calls to RightMultiply and @@ -122,19 +124,40 @@ class Preconditioner : public LinearOperator { } }; +// This templated subclass of Preconditioner serves as a base class for +// other preconditioners that depend on the particular matrix layout of +// the underlying linear operator. +template <typename MatrixType> +class TypedPreconditioner : public Preconditioner { + public: + virtual ~TypedPreconditioner() {} + virtual bool Update(const LinearOperator& A, const double* D) { + return UpdateImpl(*down_cast<const MatrixType*>(&A), D); + } + + private: + virtual bool UpdateImpl(const MatrixType& A, const double* D) = 0; +}; + +// Preconditioners that depend on acccess to the low level structure +// of a SparseMatrix. +typedef TypedPreconditioner<SparseMatrix> SparseMatrixPreconditioner; // NOLINT +typedef TypedPreconditioner<BlockSparseMatrix> BlockSparseMatrixPreconditioner; // NOLINT +typedef TypedPreconditioner<CompressedRowSparseMatrix> CompressedRowSparseMatrixPreconditioner; // NOLINT + // Wrap a SparseMatrix object as a preconditioner. -class SparseMatrixPreconditionerWrapper : public Preconditioner { +class SparseMatrixPreconditionerWrapper : public SparseMatrixPreconditioner { public: // Wrapper does NOT take ownership of the matrix pointer. explicit SparseMatrixPreconditionerWrapper(const SparseMatrix* matrix); virtual ~SparseMatrixPreconditionerWrapper(); // Preconditioner interface - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); virtual void RightMultiply(const double* x, double* y) const; virtual int num_rows() const; private: + virtual bool UpdateImpl(const SparseMatrix& A, const double* D); const SparseMatrix* matrix_; }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem.cc b/extern/libmv/third_party/ceres/internal/ceres/problem.cc index b483932b2c1..403e96a3ade 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/problem.cc @@ -206,11 +206,11 @@ int Problem::NumResiduals() const { return problem_impl_->NumResiduals(); } -int Problem::ParameterBlockSize(double* parameter_block) const { +int Problem::ParameterBlockSize(const double* parameter_block) const { return problem_impl_->ParameterBlockSize(parameter_block); }; -int Problem::ParameterBlockLocalSize(double* parameter_block) const { +int Problem::ParameterBlockLocalSize(const double* parameter_block) const { return problem_impl_->ParameterBlockLocalSize(parameter_block); }; 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 34c37857538..830270269c3 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc @@ -711,13 +711,14 @@ int ProblemImpl::NumResiduals() const { return program_->NumResiduals(); } -int ProblemImpl::ParameterBlockSize(double* parameter_block) const { - return FindParameterBlockOrDie(parameter_block_map_, parameter_block)->Size(); +int ProblemImpl::ParameterBlockSize(const double* parameter_block) const { + return FindParameterBlockOrDie(parameter_block_map_, + const_cast<double*>(parameter_block))->Size(); }; -int ProblemImpl::ParameterBlockLocalSize(double* parameter_block) const { - return FindParameterBlockOrDie(parameter_block_map_, - parameter_block)->LocalSize(); +int ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const { + return FindParameterBlockOrDie( + parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize(); }; void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_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 2609389645a..ace27f56bb1 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h @@ -139,8 +139,8 @@ class ProblemImpl { int NumResidualBlocks() const; int NumResiduals() const; - int ParameterBlockSize(double* parameter_block) const; - int ParameterBlockLocalSize(double* parameter_block) const; + int ParameterBlockSize(const double* parameter_block) const; + int ParameterBlockLocalSize(const double* parameter_block) const; void GetParameterBlocks(vector<double*>* parameter_blocks) const; const Program& program() const { return *program_; } diff --git a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h index de56ac25ff6..8aa2a3977c4 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h +++ b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h @@ -84,6 +84,7 @@ #endif #include <map> +#include <string> #include <vector> #include "ceres/execution_summary.h" #include "ceres/internal/eigen.h" @@ -91,6 +92,7 @@ #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" +#include "ceres/small_blas.h" namespace ceres { namespace internal { @@ -230,14 +232,13 @@ class ProgramEvaluator : public Evaluator { if (parameter_block->IsConstant()) { continue; } - MatrixRef block_jacobian(block_jacobians[j], - num_residuals, - parameter_block->LocalSize()); - VectorRef block_gradient(scratch->gradient.get() + - parameter_block->delta_offset(), - parameter_block->LocalSize()); - VectorRef block_residual(block_residuals, num_residuals); - block_gradient += block_residual.transpose() * block_jacobian; + + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + block_jacobians[j], + num_residuals, + parameter_block->LocalSize(), + block_residuals, + scratch->gradient.get() + parameter_block->delta_offset()); } } } diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc index 649f3f714c2..621082ac0ea 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc @@ -34,8 +34,6 @@ #include <algorithm> #include <cstddef> #include <vector> - -#include "ceres/blas.h" #include "ceres/corrector.h" #include "ceres/parameter_block.h" #include "ceres/residual_block_utils.h" @@ -44,6 +42,7 @@ #include "ceres/internal/fixed_array.h" #include "ceres/local_parameterization.h" #include "ceres/loss_function.h" +#include "ceres/small_blas.h" using Eigen::Dynamic; 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 8afb1215015..b192aa1172b 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 @@ -33,20 +33,18 @@ #include <set> #include <vector> -#ifndef CERES_NO_CXSPARSE -#include "cs.h" -#endif // CERES_NO_CXSPARSE - #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/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" #include "ceres/schur_complement_solver.h" #include "ceres/suitesparse.h" @@ -58,7 +56,7 @@ namespace ceres { namespace internal { LinearSolver::Summary SchurComplementSolver::SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { @@ -130,29 +128,31 @@ bool DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { return true; } - // TODO(sameeragarwal): Add proper error handling; this completely ignores - // the quality of the solution to the solve. - VectorRef(solution, num_rows) = - ConstMatrixRef(m->values(), num_rows, num_rows) - .selfadjointView<Eigen::Upper>() - .ldlt() - .solve(ConstVectorRef(rhs(), num_rows)); + if (options().dense_linear_algebra_library_type == EIGEN) { + // TODO(sameeragarwal): Add proper error handling; this completely ignores + // the quality of the solution to the solve. + VectorRef(solution, num_rows) = + ConstMatrixRef(m->values(), num_rows, num_rows) + .selfadjointView<Eigen::Upper>() + .llt() + .solve(ConstVectorRef(rhs(), num_rows)); + return true; + } - return true; + VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); + const int info = LAPACK::SolveInPlaceUsingCholesky(num_rows, + m->values(), + solution); + return (info == 0); } #if !defined(CERES_NO_SUITESPARSE) || !defined(CERES_NO_CXSPARE) SparseSchurComplementSolver::SparseSchurComplementSolver( const LinearSolver::Options& options) - : SchurComplementSolver(options) { -#ifndef CERES_NO_SUITESPARSE - factor_ = NULL; -#endif // CERES_NO_SUITESPARSE - -#ifndef CERES_NO_CXSPARSE - cxsparse_factor_ = NULL; -#endif // CERES_NO_CXSPARSE + : SchurComplementSolver(options), + factor_(NULL), + cxsparse_factor_(NULL) { } SparseSchurComplementSolver::~SparseSchurComplementSolver() { @@ -243,18 +243,18 @@ void SparseSchurComplementSolver::InitStorage( } bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { - switch (options().sparse_linear_algebra_library) { + switch (options().sparse_linear_algebra_library_type) { case SUITE_SPARSE: return SolveReducedLinearSystemUsingSuiteSparse(solution); case CX_SPARSE: return SolveReducedLinearSystemUsingCXSparse(solution); default: LOG(FATAL) << "Unknown sparse linear algebra library : " - << options().sparse_linear_algebra_library; + << options().sparse_linear_algebra_library_type; } LOG(FATAL) << "Unknown sparse linear algebra library : " - << options().sparse_linear_algebra_library; + << options().sparse_linear_algebra_library_type; return false; } @@ -276,26 +276,42 @@ bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( return true; } - cholmod_sparse* cholmod_lhs = ss_.CreateSparseMatrix(tsm); - // The matrix is symmetric, and the upper triangular part of the - // matrix contains the values. - cholmod_lhs->stype = 1; + cholmod_sparse* cholmod_lhs = NULL; + if (options().use_postordering) { + // If we are going to do a full symbolic analysis of the schur + // complement matrix from scratch and not rely on the + // pre-ordering, then the fastest path in cholmod_factorize is the + // one corresponding to upper triangular matrices. - cholmod_dense* cholmod_rhs = - ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); + // Create a upper triangular symmetric matrix. + cholmod_lhs = ss_.CreateSparseMatrix(tsm); + cholmod_lhs->stype = 1; - // Symbolic factorization is computed if we don't already have one handy. - if (factor_ == NULL) { - factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, blocks_, blocks_); + if (factor_ == NULL) { + factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, blocks_, blocks_); + } + } else { + // If we are going to use the natural ordering (i.e. rely on the + // pre-ordering computed by solver_impl.cc), then the fastest + // path in cholmod_factorize is the one corresponding to lower + // triangular matrices. + + // Create a upper triangular symmetric matrix. + cholmod_lhs = ss_.CreateSparseMatrixTranspose(tsm); + cholmod_lhs->stype = -1; + + if (factor_ == NULL) { + factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(cholmod_lhs); + } } + cholmod_dense* cholmod_rhs = + ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); cholmod_dense* cholmod_solution = ss_.SolveCholesky(cholmod_lhs, factor_, cholmod_rhs); ss_.Free(cholmod_lhs); - cholmod_lhs = NULL; ss_.Free(cholmod_rhs); - cholmod_rhs = NULL; if (cholmod_solution == NULL) { LOG(WARNING) << "CHOLMOD solve failed."; @@ -339,7 +355,8 @@ bool SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( // Compute symbolic factorization if not available. if (cxsparse_factor_ == NULL) { - cxsparse_factor_ = CHECK_NOTNULL(cxsparse_.AnalyzeCholesky(lhs)); + cxsparse_factor_ = + CHECK_NOTNULL(cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_)); } // Solve the linear system. 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 7c8d2e7ce38..b5a1c74ab1a 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 @@ -48,7 +48,7 @@ namespace ceres { namespace internal { -class BlockSparseMatrixBase; +class BlockSparseMatrix; // Base class for Schur complement based linear least squares // solvers. It assumes that the input linear system Ax = b can be @@ -100,7 +100,7 @@ class BlockSparseMatrixBase; // set to DENSE_SCHUR and SPARSE_SCHUR // respectively. LinearSolver::Options::elimination_groups[0] should be // at least 1. -class SchurComplementSolver : public BlockSparseMatrixBaseSolver { +class SchurComplementSolver : public BlockSparseMatrixSolver { public: explicit SchurComplementSolver(const LinearSolver::Options& options) : options_(options) { @@ -111,7 +111,7 @@ class SchurComplementSolver : public BlockSparseMatrixBaseSolver { // LinearSolver methods virtual ~SchurComplementSolver() {} virtual LinearSolver::Summary SolveImpl( - BlockSparseMatrixBase* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x); @@ -167,18 +167,14 @@ class SparseSchurComplementSolver : public SchurComplementSolver { // Size of the blocks in the Schur complement. vector<int> blocks_; -#ifndef CERES_NO_SUITESPARSE SuiteSparse ss_; // Symbolic factorization of the reduced linear system. Precomputed // once and reused in subsequent calls. cholmod_factor* factor_; -#endif // CERES_NO_SUITESPARSE -#ifndef CERES_NO_CXSPARSE CXSparse cxsparse_; // Cached factorization cs_dis* cxsparse_factor_; -#endif // CERES_NO_CXSPARSE CERES_DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h index f2c247a5adb..8fe8b9c88b7 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h @@ -170,7 +170,7 @@ class SchurEliminatorBase { // also the caller's responsibilty to ensure that the // CompressedRowBlockStructure object passed to this method is the // same one (or is equivalent to) the one associated with the - // BlockSparseMatrixBase objects below. + // BlockSparseMatrix objects below. virtual void Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) = 0; @@ -185,7 +185,7 @@ class SchurEliminatorBase { // // Since the Schur complement is a symmetric matrix, only the upper // triangular part of the Schur complement is computed. - virtual void Eliminate(const BlockSparseMatrixBase* A, + virtual void Eliminate(const BlockSparseMatrix* A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, @@ -194,7 +194,7 @@ class SchurEliminatorBase { // Given values for the variables z in the F block of A, solve for // the optimal values of the variables y corresponding to the E // block in A. - virtual void BackSubstitute(const BlockSparseMatrixBase* A, + virtual void BackSubstitute(const BlockSparseMatrix* A, const double* b, const double* D, const double* z, @@ -226,12 +226,12 @@ class SchurEliminator : public SchurEliminatorBase { virtual ~SchurEliminator(); virtual void Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs); - virtual void Eliminate(const BlockSparseMatrixBase* A, + virtual void Eliminate(const BlockSparseMatrix* A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, double* rhs); - virtual void BackSubstitute(const BlockSparseMatrixBase* A, + virtual void BackSubstitute(const BlockSparseMatrix* A, const double* b, const double* D, const double* z, @@ -273,7 +273,7 @@ class SchurEliminator : public SchurEliminatorBase { void ChunkDiagonalBlockAndGradient( const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* eet, @@ -282,7 +282,7 @@ class SchurEliminator : public SchurEliminatorBase { BlockRandomAccessMatrix* lhs); void UpdateRhs(const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, const double* inverse_ete_g, @@ -293,18 +293,18 @@ class SchurEliminator : public SchurEliminatorBase { const double* buffer, const BufferLayoutType& buffer_layout, BlockRandomAccessMatrix* lhs); - void EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + void EBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs); - void NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + void NoEBlockRowsUpdate(const BlockSparseMatrix* A, const double* b, int row_block_counter, BlockRandomAccessMatrix* lhs, double* rhs); - void NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + void NoEBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs); diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h index 835f879caf6..c09b7fb3a77 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h @@ -51,8 +51,6 @@ #include <algorithm> #include <map> - -#include "ceres/blas.h" #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" @@ -61,6 +59,7 @@ #include "ceres/internal/scoped_ptr.h" #include "ceres/map_util.h" #include "ceres/schur_eliminator.h" +#include "ceres/small_blas.h" #include "ceres/stl_util.h" #include "Eigen/Dense" #include "glog/logging.h" @@ -168,7 +167,7 @@ Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -Eliminate(const BlockSparseMatrixBase* A, +Eliminate(const BlockSparseMatrix* A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, @@ -299,7 +298,7 @@ Eliminate(const BlockSparseMatrixBase* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -BackSubstitute(const BlockSparseMatrixBase* A, +BackSubstitute(const BlockSparseMatrix* A, const double* b, const double* D, const double* z, @@ -324,9 +323,9 @@ BackSubstitute(const BlockSparseMatrixBase* A, ete.setZero(); } + const double* values = A->values(); for (int j = 0; j < chunk.size; ++j) { const CompressedRow& row = bs->rows[chunk.start + j]; - const double* row_values = A->RowBlockValues(chunk.start + j); const Cell& e_cell = row.cells.front(); DCHECK_EQ(e_block_id, e_cell.block_id); @@ -342,20 +341,20 @@ BackSubstitute(const BlockSparseMatrixBase* A, const int r_block = f_block_id - num_eliminate_blocks_; MatrixVectorMultiply<kRowBlockSize, kFBlockSize, -1>( - row_values + row.cells[c].position, row.block.size, f_block_size, + values + row.cells[c].position, row.block.size, f_block_size, z + lhs_row_layout_[r_block], sj.get()); } MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, sj.get(), y_ptr); MatrixTransposeMatrixMultiply <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, ete.data(), 0, 0, e_block_size, e_block_size); } @@ -370,7 +369,7 @@ template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: UpdateRhs(const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, const double* inverse_ete_g, @@ -380,9 +379,9 @@ UpdateRhs(const Chunk& chunk, const int e_block_size = bs->cols[e_block_id].size; int b_pos = bs->rows[row_block_counter].block.position; + const double* values = A->values(); for (int j = 0; j < chunk.size; ++j) { const CompressedRow& row = bs->rows[row_block_counter + j]; - const double *row_values = A->RowBlockValues(row_block_counter + j); const Cell& e_cell = row.cells.front(); typename EigenTypes<kRowBlockSize>::Vector sj = @@ -390,7 +389,7 @@ UpdateRhs(const Chunk& chunk, (b + b_pos, row.block.size); MatrixVectorMultiply<kRowBlockSize, kEBlockSize, -1>( - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, inverse_ete_g, sj.data()); for (int c = 1; c < row.cells.size(); ++c) { @@ -399,7 +398,7 @@ UpdateRhs(const Chunk& chunk, const int block = block_id - num_eliminate_blocks_; CeresMutexLock l(rhs_locks_[block]); MatrixTransposeVectorMultiply<kRowBlockSize, kFBlockSize, 1>( - row_values + row.cells[c].position, + values + row.cells[c].position, row.block.size, block_size, sj.data(), rhs + lhs_row_layout_[block]); } @@ -431,7 +430,7 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: ChunkDiagonalBlockAndGradient( const Chunk& chunk, - const BlockSparseMatrixBase* A, + const BlockSparseMatrix* A, const double* b, int row_block_counter, typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete, @@ -447,9 +446,9 @@ ChunkDiagonalBlockAndGradient( // contribution of its F blocks to the Schur complement, the // contribution of its E block to the matrix EE' (ete), and the // corresponding block in the gradient vector. + const double* values = A->values(); for (int j = 0; j < chunk.size; ++j) { const CompressedRow& row = bs->rows[row_block_counter + j]; - const double *row_values = A->RowBlockValues(row_block_counter + j); if (row.cells.size() > 1) { EBlockRowOuterProduct(A, row_block_counter + j, lhs); @@ -459,13 +458,13 @@ ChunkDiagonalBlockAndGradient( const Cell& e_cell = row.cells.front(); MatrixTransposeMatrixMultiply <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, ete->data(), 0, 0, e_block_size, e_block_size); // g += E_i' b_i MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, + values + e_cell.position, row.block.size, e_block_size, b + b_pos, g); @@ -479,8 +478,8 @@ ChunkDiagonalBlockAndGradient( buffer + FindOrDie(chunk.buffer_layout, f_block_id); MatrixTransposeMatrixMultiply <kRowBlockSize, kEBlockSize, kRowBlockSize, kFBlockSize, 1>( - row_values + e_cell.position, row.block.size, e_block_size, - row_values + row.cells[c].position, row.block.size, f_block_size, + values + e_cell.position, row.block.size, e_block_size, + values + row.cells[c].position, row.block.size, f_block_size, buffer_ptr, 0, 0, e_block_size, f_block_size); } b_pos += row.block.size; @@ -551,21 +550,21 @@ ChunkOuterProduct(const CompressedRowBlockStructure* bs, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, +NoEBlockRowsUpdate(const BlockSparseMatrix* A, const double* b, int row_block_counter, BlockRandomAccessMatrix* lhs, double* rhs) { const CompressedRowBlockStructure* bs = A->block_structure(); + const double* values = A->values(); for (; row_block_counter < bs->rows.size(); ++row_block_counter) { const CompressedRow& row = bs->rows[row_block_counter]; - const double *row_values = A->RowBlockValues(row_block_counter); for (int c = 0; c < row.cells.size(); ++c) { const int block_id = row.cells[c].block_id; const int block_size = bs->cols[block_id].size; const int block = block_id - num_eliminate_blocks_; MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + row.cells[c].position, row.block.size, block_size, + values + row.cells[c].position, row.block.size, block_size, b + row.block.position, rhs + lhs_row_layout_[block]); } @@ -591,12 +590,12 @@ NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, +NoEBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs) { const CompressedRowBlockStructure* bs = A->block_structure(); const CompressedRow& row = bs->rows[row_block_index]; - const double *row_values = A->RowBlockValues(row_block_index); + const double* values = A->values(); for (int i = 0; i < row.cells.size(); ++i) { const int block1 = row.cells[i].block_id - num_eliminate_blocks_; DCHECK_GE(block1, 0); @@ -612,8 +611,8 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, // symmetric outer product. MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, cell_info->values, r, c, row_stride, col_stride); } @@ -630,8 +629,8 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, CeresMutexLock l(&cell_info->m); MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[j].position, row.block.size, block2_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[j].position, row.block.size, block2_size, cell_info->values, r, c, row_stride, col_stride); } } @@ -644,12 +643,12 @@ NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -EBlockRowOuterProduct(const BlockSparseMatrixBase* A, +EBlockRowOuterProduct(const BlockSparseMatrix* A, int row_block_index, BlockRandomAccessMatrix* lhs) { const CompressedRowBlockStructure* bs = A->block_structure(); const CompressedRow& row = bs->rows[row_block_index]; - const double *row_values = A->RowBlockValues(row_block_index); + const double* values = A->values(); for (int i = 1; i < row.cells.size(); ++i) { const int block1 = row.cells[i].block_id - num_eliminate_blocks_; DCHECK_GE(block1, 0); @@ -664,8 +663,8 @@ EBlockRowOuterProduct(const BlockSparseMatrixBase* A, // block += b1.transpose() * b1; MatrixTransposeMatrixMultiply <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[i].position, row.block.size, block1_size, cell_info->values, r, c, row_stride, col_stride); } @@ -683,8 +682,8 @@ EBlockRowOuterProduct(const BlockSparseMatrixBase* A, CeresMutexLock l(&cell_info->m); MatrixTransposeMatrixMultiply <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( - row_values + row.cells[i].position, row.block.size, block1_size, - row_values + row.cells[j].position, row.block.size, block2_size, + values + row.cells[i].position, row.block.size, block1_size, + values + row.cells[j].position, row.block.size, block2_size, cell_info->values, r, c, row_stride, col_stride); } } 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 33a666ed037..338df715c0a 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 @@ -91,8 +91,8 @@ void SchurJacobiPreconditioner::InitEliminator( } // Update the values of the preconditioner matrix and factorize it. -bool SchurJacobiPreconditioner::Update(const BlockSparseMatrixBase& A, - const double* D) { +bool SchurJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { const int num_rows = m_->num_rows(); CHECK_GT(num_rows, 0); @@ -128,7 +128,7 @@ void SchurJacobiPreconditioner::RightMultiply(const double* x, VectorRef(y, block_size) = block .selfadjointView<Eigen::Upper>() - .ldlt() + .llt() .solve(ConstVectorRef(x, block_size)); x += block_size; 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 3addd73abd2..f6e7b0d37ef 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 @@ -50,7 +50,7 @@ namespace ceres { namespace internal { class BlockRandomAccessSparseMatrix; -class BlockSparseMatrixBase; +class BlockSparseMatrix; struct CompressedRowBlockStructure; class SchurEliminatorBase; @@ -73,7 +73,7 @@ class SchurEliminatorBase; // preconditioner.Update(A, NULL); // preconditioner.RightMultiply(x, y); // -class SchurJacobiPreconditioner : public Preconditioner { +class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { public: // Initialize the symbolic structure of the preconditioner. bs is // the block structure of the linear system to be solved. It is used @@ -86,12 +86,12 @@ class SchurJacobiPreconditioner : public Preconditioner { virtual ~SchurJacobiPreconditioner(); // Preconditioner interface. - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); virtual void RightMultiply(const double* x, double* y) const; virtual int num_rows() const; private: void InitEliminator(const CompressedRowBlockStructure& bs); + virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); Preconditioner::Options options_; diff --git a/extern/libmv/third_party/ceres/internal/ceres/small_blas.h b/extern/libmv/third_party/ceres/internal/ceres/small_blas.h new file mode 100644 index 00000000000..e14e664b7fa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/small_blas.h @@ -0,0 +1,406 @@ +// 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) +// +// Simple blas functions for use in the Schur Eliminator. These are +// fairly basic implementations which already yield a significant +// speedup in the eliminator performance. + +#ifndef CERES_INTERNAL_SMALL_BLAS_H_ +#define CERES_INTERNAL_SMALL_BLAS_H_ + +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +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) \ + template<int kRowA, int kColA, int kRowB, int kColB, int kOperation> \ + inline void name(const double* A, \ + const int num_row_a, \ + const int num_col_a, \ + const double* B, \ + const int num_row_b, \ + const int num_col_b, \ + double* C, \ + const int start_row_c, \ + const int start_col_c, \ + const int row_stride_c, \ + const int col_stride_c) + +#define CERES_GEMM_NAIVE_HEADER \ + DCHECK_GT(num_row_a, 0); \ + DCHECK_GT(num_col_a, 0); \ + DCHECK_GT(num_row_b, 0); \ + DCHECK_GT(num_col_b, 0); \ + DCHECK_GE(start_row_c, 0); \ + DCHECK_GE(start_col_c, 0); \ + DCHECK_GT(row_stride_c, 0); \ + DCHECK_GT(col_stride_c, 0); \ + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); \ + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); \ + DCHECK((kRowB == Eigen::Dynamic) || (kRowB == num_row_b)); \ + DCHECK((kColB == Eigen::Dynamic) || (kColB == num_col_b)); \ + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); \ + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); \ + const int NUM_ROW_B = (kColB != Eigen::Dynamic ? kRowB : num_row_b); \ + const int NUM_COL_B = (kColB != Eigen::Dynamic ? kColB : num_col_b); + +#define CERES_GEMM_EIGEN_HEADER \ + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef \ + Aref(A, num_row_a, num_col_a); \ + const typename EigenTypes<kRowB, kColB>::ConstMatrixRef \ + Bref(B, num_row_b, num_col_b); \ + MatrixRef Cref(C, row_stride_c, col_stride_c); \ + +#define CERES_CALL_GEMM(name) \ + name<kRowA, kColA, kRowB, kColB, kOperation>( \ + A, num_row_a, num_col_a, \ + B, num_row_b, num_col_b, \ + C, start_row_c, start_col_c, row_stride_c, col_stride_c); + + +// For the matrix-matrix functions below, there are three variants for +// each functionality. Foo, FooNaive and FooEigen. Foo is the one to +// be called by the user. FooNaive is a basic loop based +// implementation and FooEigen uses Eigen's implementation. Foo +// chooses between FooNaive and FooEigen depending on how many of the +// template arguments are fixed at compile time. Currently, FooEigen +// is called if all matrix dimensions are compile time +// constants. FooNaive is called otherwise. This leads to the best +// performance currently. +// +// The MatrixMatrixMultiply variants compute: +// +// C op A * B; +// +// The MatrixTransposeMatrixMultiply variants compute: +// +// C op A' * B +// +// where op can be +=, -=, or =. +// +// The template parameters (kRowA, kColA, kRowB, kColB) allow +// specialization of the loop at compile time. If this information is +// not available, then Eigen::Dynamic should be used as the template +// argument. +// +// kOperation = 1 -> C += A * B +// kOperation = -1 -> C -= A * B +// kOperation = 0 -> C = A * B +// +// The functions can write into matrices C which are larger than the +// matrix A * B. This is done by specifying the true size of C via +// row_stride_c and col_stride_c, and then indicating where A * B +// should be written into by start_row_c and start_col_c. +// +// Graphically if row_stride_c = 10, col_stride_c = 12, start_row_c = +// 4 and start_col_c = 5, then if A = 3x2 and B = 2x4, we get +// +// ------------ +// ------------ +// ------------ +// ------------ +// -----xxxx--- +// -----xxxx--- +// -----xxxx--- +// ------------ +// ------------ +// ------------ +// +CERES_GEMM_BEGIN(MatrixMatrixMultiplyEigen) { + CERES_GEMM_EIGEN_HEADER + Eigen::Block<MatrixRef, kRowA, kColB> + block(Cref, start_row_c, start_col_c, num_row_a, num_col_b); + + if (kOperation > 0) { + block CERES_MAYBE_NOALIAS += Aref * Bref; + } else if (kOperation < 0) { + block CERES_MAYBE_NOALIAS -= Aref * Bref; + } else { + block CERES_MAYBE_NOALIAS = Aref * Bref; + } +} + +CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { + CERES_GEMM_NAIVE_HEADER + DCHECK_EQ(NUM_COL_A, NUM_ROW_B); + + const int NUM_ROW_C = NUM_ROW_A; + const int NUM_COL_C = NUM_COL_B; + DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); + DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); + + for (int row = 0; row < NUM_ROW_C; ++row) { + for (int col = 0; col < NUM_COL_C; ++col) { + double tmp = 0.0; + for (int k = 0; k < NUM_COL_A; ++k) { + tmp += A[row * NUM_COL_A + k] * B[k * NUM_COL_B + col]; + } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + if (kOperation > 0) { + C[index] += tmp; + } else if (kOperation < 0) { + C[index] -= tmp; + } else { + C[index] = tmp; + } + } + } +} + +CERES_GEMM_BEGIN(MatrixMatrixMultiply) { +#ifdef CERES_NO_CUSTOM_BLAS + + CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) + return; + +#else + + if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && + kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { + CERES_CALL_GEMM(MatrixMatrixMultiplyEigen) + } else { + CERES_CALL_GEMM(MatrixMatrixMultiplyNaive) + } + +#endif +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyEigen) { + CERES_GEMM_EIGEN_HEADER + Eigen::Block<MatrixRef, kColA, kColB> block(Cref, + start_row_c, start_col_c, + num_col_a, num_col_b); + if (kOperation > 0) { + block CERES_MAYBE_NOALIAS += Aref.transpose() * Bref; + } else if (kOperation < 0) { + block CERES_MAYBE_NOALIAS -= Aref.transpose() * Bref; + } else { + block CERES_MAYBE_NOALIAS = Aref.transpose() * Bref; + } +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { + CERES_GEMM_NAIVE_HEADER + DCHECK_EQ(NUM_ROW_A, NUM_ROW_B); + + const int NUM_ROW_C = NUM_COL_A; + const int NUM_COL_C = NUM_COL_B; + DCHECK_LE(start_row_c + NUM_ROW_C, row_stride_c); + DCHECK_LE(start_col_c + NUM_COL_C, col_stride_c); + + for (int row = 0; row < NUM_ROW_C; ++row) { + for (int col = 0; col < NUM_COL_C; ++col) { + double tmp = 0.0; + for (int k = 0; k < NUM_ROW_A; ++k) { + tmp += A[k * NUM_COL_A + row] * B[k * NUM_COL_B + col]; + } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + if (kOperation > 0) { + C[index]+= tmp; + } else if (kOperation < 0) { + C[index]-= tmp; + } else { + C[index]= tmp; + } + } + } +} + +CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply) { +#ifdef CERES_NO_CUSTOM_BLAS + + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) + return; + +#else + + if (kRowA != Eigen::Dynamic && kColA != Eigen::Dynamic && + kRowB != Eigen::Dynamic && kColB != Eigen::Dynamic) { + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyEigen) + } else { + CERES_CALL_GEMM(MatrixTransposeMatrixMultiplyNaive) + } + +#endif +} + +// Matrix-Vector multiplication +// +// c op A * b; +// +// where op can be +=, -=, or =. +// +// The template parameters (kRowA, kColA) allow specialization of the +// loop at compile time. If this information is not available, then +// Eigen::Dynamic should be used as the template argument. +// +// kOperation = 1 -> c += A' * b +// kOperation = -1 -> c -= A' * b +// kOperation = 0 -> c = A' * b +template<int kRowA, int kColA, int kOperation> +inline void MatrixVectorMultiply(const double* A, + const int num_row_a, + const int num_col_a, + const double* b, + double* c) { +#ifdef CERES_NO_CUSTOM_BLAS + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef + Aref(A, num_row_a, num_col_a); + const typename EigenTypes<kColA>::ConstVectorRef bref(b, num_col_a); + typename EigenTypes<kRowA>::VectorRef cref(c, num_row_a); + + // lazyProduct works better than .noalias() for matrix-vector + // products. + if (kOperation > 0) { + cref += Aref.lazyProduct(bref); + } else if (kOperation < 0) { + cref -= Aref.lazyProduct(bref); + } else { + cref = Aref.lazyProduct(bref); + } +#else + + DCHECK_GT(num_row_a, 0); + DCHECK_GT(num_col_a, 0); + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); + + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); + + for (int row = 0; row < NUM_ROW_A; ++row) { + double tmp = 0.0; + for (int col = 0; col < NUM_COL_A; ++col) { + tmp += A[row * NUM_COL_A + col] * b[col]; + } + + if (kOperation > 0) { + c[row] += tmp; + } else if (kOperation < 0) { + c[row] -= tmp; + } else { + c[row] = tmp; + } + } +#endif // CERES_NO_CUSTOM_BLAS +} + +// Similar to MatrixVectorMultiply, except that A is transposed, i.e., +// +// c op A' * b; +template<int kRowA, int kColA, int kOperation> +inline void MatrixTransposeVectorMultiply(const double* A, + const int num_row_a, + const int num_col_a, + const double* b, + double* c) { +#ifdef CERES_NO_CUSTOM_BLAS + const typename EigenTypes<kRowA, kColA>::ConstMatrixRef + Aref(A, num_row_a, num_col_a); + const typename EigenTypes<kRowA>::ConstVectorRef bref(b, num_row_a); + typename EigenTypes<kColA>::VectorRef cref(c, num_col_a); + + // lazyProduct works better than .noalias() for matrix-vector + // products. + if (kOperation > 0) { + cref += Aref.transpose().lazyProduct(bref); + } else if (kOperation < 0) { + cref -= Aref.transpose().lazyProduct(bref); + } else { + cref = Aref.transpose().lazyProduct(bref); + } +#else + + DCHECK_GT(num_row_a, 0); + DCHECK_GT(num_col_a, 0); + DCHECK((kRowA == Eigen::Dynamic) || (kRowA == num_row_a)); + DCHECK((kColA == Eigen::Dynamic) || (kColA == num_col_a)); + + const int NUM_ROW_A = (kRowA != Eigen::Dynamic ? kRowA : num_row_a); + const int NUM_COL_A = (kColA != Eigen::Dynamic ? kColA : num_col_a); + + for (int row = 0; row < NUM_COL_A; ++row) { + double tmp = 0.0; + for (int col = 0; col < NUM_ROW_A; ++col) { + tmp += A[col * NUM_COL_A + row] * b[col]; + } + + if (kOperation > 0) { + c[row] += tmp; + } else if (kOperation < 0) { + c[row] -= tmp; + } else { + c[row] = tmp; + } + } +#endif // CERES_NO_CUSTOM_BLAS +} + + +#undef CERES_MAYBE_NOALIAS +#undef CERES_GEMM_BEGIN +#undef CERES_GEMM_EIGEN_HEADER +#undef CERES_GEMM_NAIVE_HEADER +#undef CERES_CALL_GEMM + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SMALL_BLAS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver.cc b/extern/libmv/third_party/ceres/internal/ceres/solver.cc index ea9ff1f488b..3b67746044c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/solver.cc @@ -91,6 +91,7 @@ Solver::Summary::Summary() fixed_cost(-1.0), num_successful_steps(-1), num_unsuccessful_steps(-1), + num_inner_iteration_steps(-1), preprocessor_time_in_seconds(-1.0), minimizer_time_in_seconds(-1.0), postprocessor_time_in_seconds(-1.0), @@ -98,6 +99,7 @@ Solver::Summary::Summary() linear_solver_time_in_seconds(-1.0), residual_evaluation_time_in_seconds(-1.0), jacobian_evaluation_time_in_seconds(-1.0), + inner_iteration_time_in_seconds(-1.0), num_parameter_blocks(-1), num_parameters(-1), num_effective_parameters(-1), @@ -114,10 +116,12 @@ Solver::Summary::Summary() num_linear_solver_threads_used(-1), linear_solver_type_given(SPARSE_NORMAL_CHOLESKY), linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), + inner_iterations_given(false), + inner_iterations_used(false), preconditioner_type(IDENTITY), trust_region_strategy_type(LEVENBERG_MARQUARDT), - inner_iterations(false), - sparse_linear_algebra_library(SUITE_SPARSE), + dense_linear_algebra_library_type(EIGEN), + sparse_linear_algebra_library_type(SUITE_SPARSE), line_search_direction_type(LBFGS), line_search_type(ARMIJO) { } @@ -149,6 +153,7 @@ string Solver::Summary::BriefReport() const { }; using internal::StringAppendF; +using internal::StringPrintf; string Solver::Summary::FullReport() const { string report = @@ -184,22 +189,30 @@ string Solver::Summary::FullReport() const { num_residuals, num_residuals_reduced); } - // TODO(sameeragarwal): Refactor this into separate functions. - if (minimizer_type == TRUST_REGION) { + // TRUST_SEARCH HEADER StringAppendF(&report, "\nMinimizer %19s\n", "TRUST_REGION"); + + if (linear_solver_type_used == DENSE_NORMAL_CHOLESKY || + linear_solver_type_used == DENSE_SCHUR || + linear_solver_type_used == DENSE_QR) { + StringAppendF(&report, "\nDense linear algebra library %15s\n", + DenseLinearAlgebraLibraryTypeToString( + dense_linear_algebra_library_type)); + } + 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))) { - StringAppendF(&report, "\nSparse Linear Algebra Library %15s\n", + StringAppendF(&report, "\nSparse linear algebra library %15s\n", SparseLinearAlgebraLibraryTypeToString( - sparse_linear_algebra_library)); + sparse_linear_algebra_library_type)); } - StringAppendF(&report, "Trust Region Strategy %19s", + StringAppendF(&report, "Trust region strategy %19s", TrustRegionStrategyTypeToString( trust_region_strategy_type)); if (trust_region_strategy_type == DOGLEG) { @@ -222,12 +235,9 @@ string Solver::Summary::FullReport() const { StringAppendF(&report, "Preconditioner %25s%25s\n", PreconditionerTypeToString(preconditioner_type), PreconditionerTypeToString(preconditioner_type)); - } else { - StringAppendF(&report, "Preconditioner %25s%25s\n", - "N/A", "N/A"); } - StringAppendF(&report, "Threads: % 25d% 25d\n", + StringAppendF(&report, "Threads % 25d% 25d\n", num_threads_given, num_threads_used); StringAppendF(&report, "Linear solver threads % 23d% 25d\n", num_linear_solver_threads_given, @@ -244,7 +254,14 @@ string Solver::Summary::FullReport() const { used.c_str()); } - if (inner_iterations) { + if (inner_iterations_given) { + StringAppendF(&report, + "Use inner iterations %20s %20s\n", + inner_iterations_given ? "True" : "False", + inner_iterations_used ? "True" : "False"); + } + + if (inner_iterations_used) { string given; StringifyOrdering(inner_iteration_ordering_given, &given); string used; @@ -254,119 +271,107 @@ string Solver::Summary::FullReport() const { given.c_str(), used.c_str()); } - - if (termination_type == DID_NOT_RUN) { - CHECK(!error.empty()) - << "Solver terminated with DID_NOT_RUN but the solver did not " - << "return a reason. This is a Ceres error. Please report this " - << "to the Ceres team"; - StringAppendF(&report, "Termination: %20s\n", - "DID_NOT_RUN"); - StringAppendF(&report, "Reason: %s\n", error.c_str()); - return report; - } - - StringAppendF(&report, "\nCost:\n"); - StringAppendF(&report, "Initial % 30e\n", initial_cost); - if (termination_type != NUMERICAL_FAILURE && - termination_type != USER_ABORT) { - StringAppendF(&report, "Final % 30e\n", final_cost); - StringAppendF(&report, "Change % 30e\n", - initial_cost - final_cost); - } - - StringAppendF(&report, "\nNumber of iterations:\n"); - StringAppendF(&report, "Successful % 20d\n", - num_successful_steps); - StringAppendF(&report, "Unsuccessful % 20d\n", - num_unsuccessful_steps); - StringAppendF(&report, "Total % 20d\n", - num_successful_steps + num_unsuccessful_steps); - - StringAppendF(&report, "\nTime (in seconds):\n"); - StringAppendF(&report, "Preprocessor %25.3f\n", - preprocessor_time_in_seconds); - StringAppendF(&report, "\n Residual Evaluations %22.3f\n", - residual_evaluation_time_in_seconds); - StringAppendF(&report, " Jacobian Evaluations %22.3f\n", - jacobian_evaluation_time_in_seconds); - StringAppendF(&report, " Linear Solver %23.3f\n", - linear_solver_time_in_seconds); - StringAppendF(&report, "Minimizer %25.3f\n\n", - minimizer_time_in_seconds); - - StringAppendF(&report, "Postprocessor %24.3f\n", - postprocessor_time_in_seconds); - - StringAppendF(&report, "Total %25.3f\n\n", - total_time_in_seconds); - - StringAppendF(&report, "Termination: %25s\n", - SolverTerminationTypeToString(termination_type)); } else { - // LINE_SEARCH + // LINE_SEARCH HEADER StringAppendF(&report, "\nMinimizer %19s\n", "LINE_SEARCH"); + + + string line_search_direction_string; if (line_search_direction_type == LBFGS) { - StringAppendF(&report, "Line search direction %19s(%d)\n", - LineSearchDirectionTypeToString(line_search_direction_type), - max_lbfgs_rank); + 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 { - StringAppendF(&report, "Line search direction %19s\n", - LineSearchDirectionTypeToString( - line_search_direction_type)); + line_search_direction_string = + LineSearchDirectionTypeToString(line_search_direction_type); } - StringAppendF(&report, "Line search type %19s\n", - LineSearchTypeToString(line_search_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, "%45s %21s\n", "Given", "Used"); - StringAppendF(&report, "Threads: % 25d% 25d\n", + StringAppendF(&report, "Threads % 25d% 25d\n", num_threads_given, num_threads_used); + } - if (termination_type == DID_NOT_RUN) { - CHECK(!error.empty()) - << "Solver terminated with DID_NOT_RUN but the solver did not " - << "return a reason. This is a Ceres error. Please report this " - << "to the Ceres team"; - StringAppendF(&report, "Termination: %20s\n", - "DID_NOT_RUN"); - StringAppendF(&report, "Reason: %s\n", error.c_str()); - return report; - } + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + StringAppendF(&report, "Termination: %20s\n", + "DID_NOT_RUN"); + StringAppendF(&report, "Reason: %s\n", error.c_str()); + return report; + } - StringAppendF(&report, "\nCost:\n"); - StringAppendF(&report, "Initial % 30e\n", initial_cost); - if (termination_type != NUMERICAL_FAILURE && - termination_type != USER_ABORT) { - StringAppendF(&report, "Final % 30e\n", final_cost); - StringAppendF(&report, "Change % 30e\n", - initial_cost - final_cost); - } + StringAppendF(&report, "\nCost:\n"); + StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != NUMERICAL_FAILURE && + termination_type != USER_ABORT) { + StringAppendF(&report, "Final % 30e\n", final_cost); + StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } - StringAppendF(&report, "\nNumber of iterations: % 20d\n", - static_cast<int>(iterations.size() - 1)); + StringAppendF(&report, "\nMinimizer iterations % 16d\n", + num_successful_steps + num_unsuccessful_steps); - StringAppendF(&report, "\nTime (in seconds):\n"); - StringAppendF(&report, "Preprocessor %25.3f\n", - preprocessor_time_in_seconds); - StringAppendF(&report, "\n Residual Evaluations %22.3f\n", - residual_evaluation_time_in_seconds); - StringAppendF(&report, " Jacobian Evaluations %22.3f\n", - jacobian_evaluation_time_in_seconds); - StringAppendF(&report, "Minimizer %25.3f\n\n", - minimizer_time_in_seconds); + // Successful/Unsuccessful steps only matter in the case of the + // trust region solver. Line search terminates when it encounters + // the first unsuccessful step. + if (minimizer_type == TRUST_REGION) { + StringAppendF(&report, "Successful steps % 14d\n", + num_successful_steps); + StringAppendF(&report, "Unsuccessful steps % 14d\n", + num_unsuccessful_steps); + } + if (inner_iterations_used) { + StringAppendF(&report, "Steps with inner iterations % 14d\n", + num_inner_iteration_steps); + } - StringAppendF(&report, "Postprocessor %24.3f\n", - postprocessor_time_in_seconds); + StringAppendF(&report, "\nTime (in seconds):\n"); + StringAppendF(&report, "Preprocessor %25.3f\n", + preprocessor_time_in_seconds); - StringAppendF(&report, "Total %25.3f\n\n", - total_time_in_seconds); + StringAppendF(&report, "\n Residual evaluation %23.3f\n", + residual_evaluation_time_in_seconds); + StringAppendF(&report, " Jacobian evaluation %23.3f\n", + jacobian_evaluation_time_in_seconds); - StringAppendF(&report, "Termination: %25s\n", - SolverTerminationTypeToString(termination_type)); + if (minimizer_type == TRUST_REGION) { + StringAppendF(&report, " Linear solver %23.3f\n", + linear_solver_time_in_seconds); + } + + if (inner_iterations_used) { + StringAppendF(&report, " Inner iterations %23.3f\n", + inner_iteration_time_in_seconds); } + StringAppendF(&report, "Minimizer %25.3f\n\n", + minimizer_time_in_seconds); + + StringAppendF(&report, "Postprocessor %24.3f\n", + postprocessor_time_in_seconds); + + StringAppendF(&report, "Total %25.3f\n\n", + total_time_in_seconds); + + StringAppendF(&report, "Termination: %25s\n", + SolverTerminationTypeToString(termination_type)); return report; }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc index 43c0be6180d..83faa0510c0 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc @@ -33,7 +33,9 @@ #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" @@ -253,8 +255,8 @@ void SolverImpl::TrustRegionMinimize( 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.lm_min_diagonal = options.lm_min_diagonal; - trust_region_strategy_options.lm_max_diagonal = options.lm_max_diagonal; + 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; @@ -315,6 +317,16 @@ void SolverImpl::LineSearchMinimize( 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."; + if (options.minimizer_type == TRUST_REGION) { TrustRegionSolve(options, problem_impl, summary); } else { @@ -389,9 +401,13 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, summary->num_threads_given = original_options.num_threads; summary->num_threads_used = options.num_threads; - if (options.lsqp_iterations_to_dump.size() > 0) { - LOG(WARNING) << "Dumping linear least squares problems to disk is" - " currently broken. Ignoring Solver::Options::lsqp_iterations_to_dump"; + 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->error = + "Solver::Options::trust_region_problem_dump_directory is empty."; + LOG(ERROR) << summary->error; + return; } event_logger.AddEvent("Init"); @@ -500,8 +516,10 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, original_options.num_linear_solver_threads; summary->num_linear_solver_threads_used = options.num_linear_solver_threads; - summary->sparse_linear_algebra_library = - options.sparse_linear_algebra_library; + 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; @@ -534,8 +552,7 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, } } } - - event_logger.AddEvent("CreateIIM"); + event_logger.AddEvent("CreateInnerIterationMinimizer"); // The optimizer works on contiguous parameter vectors; allocate some. Vector parameters(reduced_program->NumParameters()); @@ -619,10 +636,98 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, 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->num_parameter_blocks = problem_impl->NumParameterBlocks(); - summary->num_parameters = problem_impl->NumParameters(); - summary->num_residual_blocks = problem_impl->NumResidualBlocks(); - summary->num_residuals = problem_impl->NumResiduals(); + summary->line_search_interpolation_type = + original_options.line_search_interpolation_type; + summary->nonlinear_conjugate_gradient_type = + original_options.nonlinear_conjugate_gradient_type; + + summary->num_parameter_blocks = original_program->NumParameterBlocks(); + summary->num_parameters = original_program->NumParameters(); + summary->num_residual_blocks = original_program->NumResidualBlocks(); + summary->num_residuals = original_program->NumResiduals(); + summary->num_effective_parameters = + original_program->NumEffectiveParameters(); + + // Validate values for configuration parameters supplied by user. + if ((original_options.line_search_direction_type == ceres::BFGS || + original_options.line_search_direction_type == ceres::LBFGS) && + original_options.line_search_type != ceres::WOLFE) { + summary->error = + string("Invalid configuration: require line_search_type == " + "ceres::WOLFE when using (L)BFGS to ensure that underlying " + "assumptions are guaranteed to be satisfied."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.max_lbfgs_rank <= 0) { + summary->error = + string("Invalid configuration: require max_lbfgs_rank > 0"); + LOG(ERROR) << summary->error; + return; + } + if (original_options.min_line_search_step_size <= 0.0) { + summary->error = "Invalid configuration: min_line_search_step_size <= 0.0."; + LOG(ERROR) << summary->error; + return; + } + if (original_options.line_search_sufficient_function_decrease <= 0.0) { + summary->error = + string("Invalid configuration: require ") + + string("line_search_sufficient_function_decrease <= 0.0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.max_line_search_step_contraction <= 0.0 || + original_options.max_line_search_step_contraction >= 1.0) { + summary->error = string("Invalid configuration: require ") + + string("0.0 < max_line_search_step_contraction < 1.0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.min_line_search_step_contraction <= + original_options.max_line_search_step_contraction || + original_options.min_line_search_step_contraction > 1.0) { + summary->error = string("Invalid configuration: require ") + + string("max_line_search_step_contraction < ") + + string("min_line_search_step_contraction <= 1.0."); + LOG(ERROR) << summary->error; + return; + } + // 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, + (original_options.line_search_interpolation_type == ceres::BISECTION && + (original_options.max_line_search_step_contraction > 0.5 || + original_options.min_line_search_step_contraction < 0.5))) + << "Line search interpolation type is BISECTION, but specified " + << "max_line_search_step_contraction: " + << original_options.max_line_search_step_contraction << ", and " + << "min_line_search_step_contraction: " + << original_options.min_line_search_step_contraction + << ", prevent bisection (0.5) scaling, continuing with solve regardless."; + if (original_options.max_num_line_search_step_size_iterations <= 0) { + summary->error = string("Invalid configuration: require ") + + string("max_num_line_search_step_size_iterations > 0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.line_search_sufficient_curvature_decrease <= + original_options.line_search_sufficient_function_decrease || + original_options.line_search_sufficient_curvature_decrease > 1.0) { + summary->error = string("Invalid configuration: require ") + + string("line_search_sufficient_function_decrease < ") + + string("line_search_sufficient_curvature_decrease < 1.0."); + LOG(ERROR) << summary->error; + return; + } + if (original_options.max_line_search_step_expansion <= 1.0) { + summary->error = string("Invalid configuration: require ") + + string("max_line_search_step_expansion > 1.0."); + LOG(ERROR) << summary->error; + return; + } // Empty programs are usually a user error. if (summary->num_parameter_blocks == 0) { @@ -712,6 +817,8 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); summary->num_parameters_reduced = reduced_program->NumParameters(); summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); + summary->num_effective_parameters_reduced = + reduced_program->NumEffectiveParameters(); summary->num_residuals_reduced = reduced_program->NumResiduals(); if (summary->num_parameter_blocks_reduced == 0) { @@ -972,6 +1079,16 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, 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."; @@ -995,18 +1112,27 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, } if (IsSchurType(options->linear_solver_type)) { - if (!ReorderProgramForSchurTypeLinearSolver(problem_impl->parameter_map(), - linear_solver_ordering, - transformed_program.get(), - error)) { + 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->sparse_linear_algebra_library == SUITE_SPARSE) { - ReorderProgramForSparseNormalCholesky(transformed_program.get()); + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + if (!ReorderProgramForSparseNormalCholesky( + options->sparse_linear_algebra_library_type, + linear_solver_ordering, + transformed_program.get(), + error)) { + return NULL; + } + return transformed_program.release(); } @@ -1030,9 +1156,32 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, } } +#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 == SUITE_SPARSE) { + 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; @@ -1053,7 +1202,7 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, #ifdef CERES_NO_CXSPARSE if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && - options->sparse_linear_algebra_library == CX_SPARSE) { + 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; @@ -1068,31 +1217,45 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, } #endif - if (options->linear_solver_max_num_iterations <= 0) { - *error = "Solver::Options::linear_solver_max_num_iterations is 0."; + if (options->max_linear_solver_iterations <= 0) { + *error = "Solver::Options::max_linear_solver_iterations is not positive."; return NULL; } - if (options->linear_solver_min_num_iterations <= 0) { - *error = "Solver::Options::linear_solver_min_num_iterations is 0."; + if (options->min_linear_solver_iterations <= 0) { + *error = "Solver::Options::min_linear_solver_iterations is not positive."; return NULL; } - if (options->linear_solver_min_num_iterations > - options->linear_solver_max_num_iterations) { - *error = "Solver::Options::linear_solver_min_num_iterations > " - "Solver::Options::linear_solver_max_num_iterations."; + 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->linear_solver_min_num_iterations; + options->min_linear_solver_iterations; linear_solver_options.max_num_iterations = - options->linear_solver_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.sparse_linear_algebra_library = - options->sparse_linear_algebra_library; + 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; + + // 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; @@ -1115,48 +1278,6 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, return LinearSolver::Create(linear_solver_options); } -bool SolverImpl::ApplyUserOrdering( - const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* ordering, - Program* program, - string* error) { - if (ordering->NumElements() != program->NumParameterBlocks()) { - *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.", - program->NumParameterBlocks(), - 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; -} // Find the minimum index of any parameter block to the given residual. // Parameter blocks that have indices greater than num_eliminate_blocks are @@ -1283,6 +1404,8 @@ CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( 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; @@ -1325,9 +1448,9 @@ CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( return NULL; } - summary->inner_iterations = true; + 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(); } @@ -1357,75 +1480,62 @@ void SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver( // CGNR currently only supports the JACOBI preconditioner. options->preconditioner_type = JACOBI; } else { - msg += StringPrintf("ITERATIVE_SCHUR with IDENTITY preconditioner " - "to CGNR with IDENTITY preconditioner."); + msg += "ITERATIVE_SCHUR with IDENTITY preconditioner" + "to CGNR with IDENTITY preconditioner."; } } LOG(WARNING) << msg; } -bool SolverImpl::ReorderProgramForSchurTypeLinearSolver( +bool SolverImpl::ApplyUserOrdering( const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* ordering, + const ParameterBlockOrdering* parameter_block_ordering, Program* program, string* error) { - // At this point one of two things is true. - // - // 1. The user did not specify an ordering - ordering has one - // group containined all the parameter blocks. - - // 2. The user specified an ordering, and the first group has - // non-zero elements. - // - // We handle these two cases in turn. - if (ordering->NumGroups() == 1) { - // If the user supplied an ordering with just one - // group, it is equivalent to the user supplying NULL as an - // 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 = ComputeSchurOrdering(*program, - &schur_ordering); + 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; + } - CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + parameter_blocks->clear(); - // Update the 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; - ordering->AddElementToGroup(parameter_block, group_id); - } + const map<int, set<double*> >& groups = + parameter_block_ordering->group_to_elements(); - // Apply the parameter block re-ordering. Technically we could - // call ApplyUserOrdering, but this is cheaper and simpler. - swap(*program->mutable_parameter_blocks(), schur_ordering); - } else { - // The user supplied an ordering. - if (!ApplyUserOrdering(parameter_map, ordering, program, error)) { - return false; + 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); } } - - program->SetParameterOffsetsAndIndex(); - - const int num_eliminate_blocks = - ordering->group_to_elements().begin()->second.size(); - - // Schur type solvers also require that their residual blocks be - // lexicographically ordered. - return LexicographicallyOrderResidualBlocks(num_eliminate_blocks, - program, - error); + return true; } + TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( const Program* program) { - // Matrix to store the block sparsity structure of + // Matrix to store the block sparsity structure of the Jacobian. TripletSparseMatrix* tsm = new TripletSparseMatrix(program->NumParameterBlocks(), program->NumResidualBlocks(), @@ -1449,6 +1559,7 @@ TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( // 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(); @@ -1468,34 +1579,205 @@ TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( return tsm; } -void SolverImpl::ReorderProgramForSparseNormalCholesky(Program* program) { -#ifndef CERES_NO_SUITESPARSE - // Set the offsets and index for CreateJacobianSparsityTranspose. +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)); - // Order rows using AMD. - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); + vector<int> ordering(program->NumParameterBlocks(), 0); + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); - vector<int> ordering(program->NumParameterBlocks(), -1); - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); - ss.Free(block_jacobian_transpose); + 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. - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); 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(); + 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 diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h index 22ca6229b81..2b7ca3e3310 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h @@ -103,15 +103,6 @@ class SolverImpl { static LinearSolver* CreateLinearSolver(Solver::Options* options, string* error); - // Reorder the parameter blocks in program using the ordering. A - // return value of true indicates success and false indicates an - // error was encountered whose cause is logged to LOG(ERROR). - static bool ApplyUserOrdering(const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* ordering, - Program* program, - string* error); - - // 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 @@ -163,36 +154,67 @@ class SolverImpl { 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* 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. + // + // 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* 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 ordering has atleast two groups, then apply the ordering, - // otherwise compute a new ordering using a Maximal Independent Set - // algorithm and apply it. + // 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* ordering, + ParameterBlockOrdering* parameter_block_ordering, Program* program, string* error); - // CHOLMOD 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. - // - static void ReorderProgramForSparseNormalCholesky(Program* program); - - // 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); + // 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 diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h index 1b19f887946..f3b96712a70 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h @@ -41,8 +41,6 @@ namespace ceres { namespace internal { -class SparseMatrixProto; - // This class defines the interface for storing and manipulating // sparse matrices. The key property that differentiates different // sparse matrices is how they are organized in memory and how the @@ -86,11 +84,6 @@ class SparseMatrix : public LinearOperator { // sparse matrix. virtual void ToDenseMatrix(Matrix* dense_matrix) const = 0; -#ifndef CERES_NO_PROTOCOL_BUFFERS - // Dump the sparse matrix to a proto. Destroys the contents of proto. - virtual void ToProto(SparseMatrixProto* proto) const = 0; -#endif - // Write out the matrix as a sequence of (i,j,s) triplets. This // format is useful for loading the matrix into MATLAB/octave as a // sparse matrix. 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 bc1f98334ae..f1a52378e2b 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 @@ -36,11 +36,8 @@ #include <cstring> #include <ctime> -#ifndef CERES_NO_CXSPARSE -#include "cs.h" -#endif - #include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/cxsparse.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/linear_solver.h" @@ -54,14 +51,9 @@ namespace internal { SparseNormalCholeskySolver::SparseNormalCholeskySolver( const LinearSolver::Options& options) - : options_(options) { -#ifndef CERES_NO_SUITESPARSE - factor_ = NULL; -#endif - -#ifndef CERES_NO_CXSPARSE - cxsparse_factor_ = NULL; -#endif // CERES_NO_CXSPARSE + : factor_(NULL), + cxsparse_factor_(NULL), + options_(options) { } SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { @@ -85,18 +77,18 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double * x) { - switch (options_.sparse_linear_algebra_library) { + switch (options_.sparse_linear_algebra_library_type) { case SUITE_SPARSE: return SolveImplUsingSuiteSparse(A, b, per_solve_options, x); case CX_SPARSE: return SolveImplUsingCXSparse(A, b, per_solve_options, x); default: LOG(FATAL) << "Unknown sparse linear algebra library : " - << options_.sparse_linear_algebra_library; + << options_.sparse_linear_algebra_library_type; } LOG(FATAL) << "Unknown sparse linear algebra library : " - << options_.sparse_linear_algebra_library; + << options_.sparse_linear_algebra_library_type; return LinearSolver::Summary(); } @@ -133,34 +125,37 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( // factorized. CHOLMOD/SuiteSparse on the other hand can just work // off of Jt to compute the Cholesky factorization of the normal // equations. - cs_di* A2 = cs_transpose(&At, 1); - cs_di* AtA = cs_multiply(&At, A2); + cs_di* A2 = cxsparse_.TransposeMatrix(&At); + cs_di* AtA = cxsparse_.MatrixMatrixMultiply(&At, A2); cxsparse_.Free(A2); if (per_solve_options.D != NULL) { A->DeleteRows(num_cols); } - event_logger.AddEvent("Setup"); // Compute symbolic factorization if not available. if (cxsparse_factor_ == NULL) { - cxsparse_factor_ = CHECK_NOTNULL(cxsparse_.AnalyzeCholesky(AtA)); + if (options_.use_postordering) { + cxsparse_factor_ = + CHECK_NOTNULL(cxsparse_.BlockAnalyzeCholesky(AtA, + A->col_blocks(), + A->col_blocks())); + } else { + cxsparse_factor_ = + CHECK_NOTNULL(cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA)); + } } - event_logger.AddEvent("Analysis"); - // Solve the linear system. if (cxsparse_.SolveCholesky(AtA, cxsparse_factor_, Atb.data())) { VectorRef(x, Atb.rows()) = Atb; summary.termination_type = TOLERANCE; } - event_logger.AddEvent("Solve"); cxsparse_.Free(AtA); - event_logger.AddEvent("Teardown"); return summary; } @@ -205,11 +200,13 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( if (factor_ == NULL) { if (options_.use_postordering) { - factor_ = ss_.BlockAnalyzeCholesky(&lhs, - A->col_blocks(), - A->row_blocks()); + factor_ = + CHECK_NOTNULL(ss_.BlockAnalyzeCholesky(&lhs, + A->col_blocks(), + A->row_blocks())); } else { - factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs); + factor_ = + CHECK_NOTNULL(ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs)); } } 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 ebb32e61939..61111b41b49 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 @@ -73,17 +73,13 @@ class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { const LinearSolver::PerSolveOptions& options, double* x); -#ifndef CERES_NO_SUITESPARSE SuiteSparse ss_; // Cached factorization cholmod_factor* factor_; -#endif // CERES_NO_SUITESPARSE -#ifndef CERES_NO_CXSPARSE CXSparse cxsparse_; // Cached factorization cs_dis* cxsparse_factor_; -#endif // CERES_NO_CXSPARSE const LinearSolver::Options options_; CERES_DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); diff --git a/extern/libmv/third_party/ceres/internal/ceres/split.h b/extern/libmv/third_party/ceres/internal/ceres/split.h index 4df48c3a7cd..2334d26037f 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/split.h +++ b/extern/libmv/third_party/ceres/internal/ceres/split.h @@ -1,4 +1,31 @@ -// Copyright 2011 Google Inc. All Rights Reserved. +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2011 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_SPLIT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc index 5138b522d09..9de32fd76ad 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc @@ -33,6 +33,7 @@ #include <vector> #include "cholmod.h" +#include "ceres/compressed_col_sparse_matrix_utils.h" #include "ceres/compressed_row_sparse_matrix.h" #include "ceres/triplet_sparse_matrix.h" @@ -172,7 +173,8 @@ cholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering( return factor; } -cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering(cholmod_sparse* A) { +cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering( + cholmod_sparse* A) { cc_.nmethods = 1; cc_.method[0].ordering = CHOLMOD_NATURAL; cc_.postorder = 0; @@ -201,11 +203,12 @@ bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A, vector<int> block_cols; vector<int> block_rows; - ScalarMatrixToBlockMatrix(A, - row_blocks, - col_blocks, - &block_rows, - &block_cols); + CompressedColumnScalarMatrixToBlockMatrix(reinterpret_cast<const int*>(A->i), + reinterpret_cast<const int*>(A->p), + row_blocks, + col_blocks, + &block_rows, + &block_cols); cholmod_sparse_struct block_matrix; block_matrix.nrow = num_row_blocks; @@ -230,88 +233,6 @@ bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A, return true; } -void SuiteSparse::ScalarMatrixToBlockMatrix(const cholmod_sparse* A, - const vector<int>& row_blocks, - const vector<int>& col_blocks, - vector<int>* block_rows, - vector<int>* block_cols) { - CHECK_NOTNULL(block_rows)->clear(); - CHECK_NOTNULL(block_cols)->clear(); - const int num_row_blocks = row_blocks.size(); - const int num_col_blocks = col_blocks.size(); - - vector<int> row_block_starts(num_row_blocks); - for (int i = 0, cursor = 0; i < num_row_blocks; ++i) { - row_block_starts[i] = cursor; - cursor += row_blocks[i]; - } - - // The reinterpret_cast is needed here because CHOLMOD stores arrays - // as void*. - const int* scalar_cols = reinterpret_cast<const int*>(A->p); - const int* scalar_rows = reinterpret_cast<const int*>(A->i); - - // This loop extracts the block sparsity of the scalar sparse matrix - // A. It does so by iterating over the columns, but only considering - // the columns corresponding to the first element of each column - // block. Within each column, the inner loop iterates over the rows, - // and detects the presence of a row block by checking for the - // presence of a non-zero entry corresponding to its first element. - block_cols->push_back(0); - int c = 0; - for (int col_block = 0; col_block < num_col_blocks; ++col_block) { - int column_size = 0; - for (int idx = scalar_cols[c]; idx < scalar_cols[c + 1]; ++idx) { - vector<int>::const_iterator it = lower_bound(row_block_starts.begin(), - row_block_starts.end(), - scalar_rows[idx]); - // Since we are using lower_bound, it will return the row id - // where the row block starts. For everything but the first row - // of the block, where these values will be the same, we can - // skip, as we only need the first row to detect the presence of - // the block. - // - // For rows all but the first row in the last row block, - // lower_bound will return row_block_starts.end(), but those can - // be skipped like the rows in other row blocks too. - if (it == row_block_starts.end() || *it != scalar_rows[idx]) { - continue; - } - - block_rows->push_back(it - row_block_starts.begin()); - ++column_size; - } - block_cols->push_back(block_cols->back() + column_size); - c += col_blocks[col_block]; - } -} - -void SuiteSparse::BlockOrderingToScalarOrdering( - const vector<int>& blocks, - const vector<int>& block_ordering, - vector<int>* scalar_ordering) { - CHECK_EQ(blocks.size(), block_ordering.size()); - const int num_blocks = blocks.size(); - - // block_starts = [0, block1, block1 + block2 ..] - vector<int> block_starts(num_blocks); - for (int i = 0, cursor = 0; i < num_blocks ; ++i) { - block_starts[i] = cursor; - cursor += blocks[i]; - } - - scalar_ordering->resize(block_starts.back() + blocks.back()); - int cursor = 0; - for (int i = 0; i < num_blocks; ++i) { - const int block_id = block_ordering[i]; - const int block_size = blocks[block_id]; - int block_position = block_starts[block_id]; - for (int j = 0; j < block_size; ++j) { - (*scalar_ordering)[cursor++] = block_position++; - } - } -} - bool SuiteSparse::Cholesky(cholmod_sparse* A, cholmod_factor* L) { CHECK_NOTNULL(A); CHECK_NOTNULL(L); @@ -402,6 +323,21 @@ void SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, cholmod_amd(matrix, NULL, 0, ordering, &cc_); } +void SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering( + cholmod_sparse* matrix, + int* constraints, + int* ordering) { +#ifndef CERES_NO_CAMD + cholmod_camd(matrix, NULL, 0, constraints, ordering, &cc_); +#else + LOG(FATAL) << "Congratulations you have found a bug in Ceres." + << "Ceres Solver was compiled with SuiteSparse " + << "version 4.1.0 or less. Calling this function " + << "in that case is a bug. Please contact the" + << "the Ceres Solver developers."; +#endif +} + } // namespace internal } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h index a1a4f355d76..16f298ea79c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h @@ -33,6 +33,7 @@ #ifndef CERES_INTERNAL_SUITESPARSE_H_ #define CERES_INTERNAL_SUITESPARSE_H_ + #ifndef CERES_NO_SUITESPARSE #include <cstring> @@ -42,6 +43,29 @@ #include "ceres/internal/port.h" #include "cholmod.h" #include "glog/logging.h" +#include "SuiteSparseQR.hpp" + +// Before SuiteSparse version 4.2.0, cholmod_camd was only enabled +// if SuiteSparse was compiled with Metis support. This makes +// calling and linking into cholmod_camd problematic even though it +// has nothing to do with Metis. This has been fixed reliably in +// 4.2.0. +// +// The fix was actually committed in 4.1.0, but there is +// some confusion about a silent update to the tar ball, so we are +// being conservative and choosing the next minor version where +// things are stable. +#if (SUITESPARSE_VERSION < 4002) +#define CERES_NO_CAMD +#endif + +// UF_long is deprecated but SuiteSparse_long is only available in +// newer versions of SuiteSparse. So for older versions of +// SuiteSparse, we define SuiteSparse_long to be the same as UF_long, +// which is what recent versions of SuiteSparse do anyways. +#ifndef SuiteSparse_long +#define SuiteSparse_long UF_long +#endif namespace ceres { namespace internal { @@ -184,34 +208,43 @@ class SuiteSparse { const vector<int>& col_blocks, vector<int>* ordering); - // Given a set of blocks and a permutation of these blocks, compute - // the corresponding "scalar" ordering, where the scalar ordering of - // size sum(blocks). - static void BlockOrderingToScalarOrdering(const vector<int>& blocks, - const vector<int>& block_ordering, - vector<int>* scalar_ordering); - - // Extract the block sparsity pattern of the scalar sparse matrix - // A and return it in compressed column form. The compressed column - // form is stored in two vectors block_rows, and block_cols, which - // correspond to the row and column arrays in a compressed column sparse - // matrix. - // - // If c_ij is the block in the matrix A corresponding to row block i - // and column block j, then it is expected that A contains at least - // one non-zero entry corresponding to the top left entry of c_ij, - // as that entry is used to detect the presence of a non-zero c_ij. - static void ScalarMatrixToBlockMatrix(const cholmod_sparse* A, - const vector<int>& row_blocks, - const vector<int>& col_blocks, - vector<int>* block_rows, - vector<int>* block_cols); - // Find a fill reducing approximate minimum degree // ordering. ordering is expected to be large enough to hold the // ordering. void ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, int* ordering); + + // Before SuiteSparse version 4.2.0, cholmod_camd was only enabled + // if SuiteSparse was compiled with Metis support. This makes + // calling and linking into cholmod_camd problematic even though it + // has nothing to do with Metis. This has been fixed reliably in + // 4.2.0. + // + // The fix was actually committed in 4.1.0, but there is + // some confusion about a silent update to the tar ball, so we are + // being conservative and choosing the next minor version where + // things are stable. + static bool IsConstrainedApproximateMinimumDegreeOrderingAvailable() { + return (SUITESPARSE_VERSION>4001); + } + + // Find a fill reducing approximate minimum degree + // ordering. constraints is an array which associates with each + // column of the matrix an elimination group. i.e., all columns in + // group 0 are eliminated first, all columns in group 1 are + // eliminated next etc. This function finds a fill reducing ordering + // that obeys these constraints. + // + // Calling ApproximateMinimumDegreeOrdering is equivalent to calling + // ConstrainedApproximateMinimumDegreeOrdering with a constraint + // array that puts all columns in the same elimination group. + // + // If CERES_NO_CAMD is defined then calling this function will + // result in a crash. + void ConstrainedApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, + int* constraints, + int* ordering); + void Free(cholmod_sparse* m) { cholmod_free_sparse(&m, &cc_); } void Free(cholmod_dense* m) { cholmod_free_dense(&m, &cc_); } void Free(cholmod_factor* m) { cholmod_free_factor(&m, &cc_); } @@ -237,6 +270,11 @@ class SuiteSparse { } // namespace internal } // namespace ceres +#else // CERES_NO_SUITESPARSE + +class SuiteSparse {}; +typedef void cholmod_factor; + #endif // CERES_NO_SUITESPARSE #endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc index a09f38ee24e..824b123bc28 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -35,7 +35,6 @@ #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" -#include "ceres/matrix_proto.h" #include "ceres/types.h" #include "glog/logging.h" @@ -83,32 +82,6 @@ TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) CopyData(orig); } -#ifndef CERES_NO_PROTOCOL_BUFFERS -TripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) { - CHECK(outer_proto.has_triplet_matrix()); - - const TripletSparseMatrixProto& proto = outer_proto.triplet_matrix(); - CHECK(proto.has_num_rows()); - CHECK(proto.has_num_cols()); - CHECK_EQ(proto.rows_size(), proto.cols_size()); - CHECK_EQ(proto.cols_size(), proto.values_size()); - - // Initialize the matrix with the appropriate size and capacity. - max_num_nonzeros_ = 0; - set_num_nonzeros(0); - Reserve(proto.num_nonzeros()); - Resize(proto.num_rows(), proto.num_cols()); - set_num_nonzeros(proto.num_nonzeros()); - - // Copy the entries in. - for (int i = 0; i < proto.num_nonzeros(); ++i) { - rows_[i] = proto.rows(i); - cols_[i] = proto.cols(i); - values_[i] = proto.values(i); - } -} -#endif - TripletSparseMatrix& TripletSparseMatrix::operator=( const TripletSparseMatrix& rhs) { num_rows_ = rhs.num_rows_; @@ -215,22 +188,6 @@ void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { } } -#ifndef CERES_NO_PROTOCOL_BUFFERS -void TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const { - proto->Clear(); - - TripletSparseMatrixProto* tsm_proto = proto->mutable_triplet_matrix(); - tsm_proto->set_num_rows(num_rows_); - tsm_proto->set_num_cols(num_cols_); - tsm_proto->set_num_nonzeros(num_nonzeros_); - for (int i = 0; i < num_nonzeros_; ++i) { - tsm_proto->add_rows(rows_[i]); - tsm_proto->add_cols(cols_[i]); - tsm_proto->add_values(values_[i]); - } -} -#endif - void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { CHECK_EQ(B.num_cols(), num_cols_); Reserve(num_nonzeros_ + B.num_nonzeros_); diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h index 89a645bd879..4d7cde7fe9c 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h @@ -39,8 +39,6 @@ namespace ceres { namespace internal { -class SparseMatrixProto; - // An implementation of the SparseMatrix interface to store and // manipulate sparse matrices in triplet (i,j,s) form. This object is // inspired by the design of the cholmod_triplet struct used in the @@ -50,9 +48,6 @@ class TripletSparseMatrix : public SparseMatrix { TripletSparseMatrix(); TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); explicit TripletSparseMatrix(const TripletSparseMatrix& orig); -#ifndef CERES_NO_PROTOCOL_BUFFERS - explicit TripletSparseMatrix(const SparseMatrixProto& proto); -#endif TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); @@ -65,9 +60,6 @@ class TripletSparseMatrix : public SparseMatrix { virtual void SquaredColumnNorm(double* x) const; virtual void ScaleColumns(const double* scale); virtual void ToDenseMatrix(Matrix* dense_matrix) const; -#ifndef CERES_NO_PROTOCOL_BUFFERS - virtual void ToProto(SparseMatrixProto *proto) const; -#endif virtual void ToTextFile(FILE* file) const; virtual int num_rows() const { return num_rows_; } virtual int num_cols() const { return num_cols_; } 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 981c60a12e7..03d6c8e6b94 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 @@ -41,6 +41,7 @@ #include "Eigen/Core" #include "ceres/array_utils.h" #include "ceres/evaluator.h" +#include "ceres/file.h" #include "ceres/internal/eigen.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/linear_least_squares_problems.h" @@ -70,25 +71,8 @@ void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian, void TrustRegionMinimizer::Init(const Minimizer::Options& options) { options_ = options; - sort(options_.lsqp_iterations_to_dump.begin(), - options_.lsqp_iterations_to_dump.end()); -} - -bool TrustRegionMinimizer::MaybeDumpLinearLeastSquaresProblem( - const int iteration, - const SparseMatrix* jacobian, - const double* residuals, - const double* step) const { - // TODO(sameeragarwal): Since the use of trust_region_radius has - // moved inside TrustRegionStrategy, its not clear how we dump the - // regularization vector/matrix anymore. - // - // Also num_eliminate_blocks is not visible to the trust region - // minimizer either. - // - // Both of these indicate that this is the wrong place for this - // code, and going forward this should needs fixing/refactoring. - return true; + sort(options_.trust_region_minimizer_iterations_to_dump.begin(), + options_.trust_region_minimizer_iterations_to_dump.end()); } void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, @@ -139,15 +123,16 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, // Do initial cost and Jacobian evaluation. double cost = 0.0; - if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), NULL, jacobian)) { + if (!evaluator->Evaluate(x.data(), + &cost, + residuals.data(), + gradient.data(), + jacobian)) { LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; summary->termination_type = NUMERICAL_FAILURE; return; } - summary->initial_cost = cost + summary->fixed_cost; - iteration_summary.cost = cost + summary->fixed_cost; - int num_consecutive_nonmonotonic_steps = 0; double minimum_cost = cost; double reference_cost = cost; @@ -155,17 +140,10 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, double candidate_cost = cost; double accumulated_candidate_model_cost_change = 0.0; - gradient.setZero(); - jacobian->LeftMultiply(residuals.data(), gradient.data()); + summary->initial_cost = cost + summary->fixed_cost; + iteration_summary.cost = cost + summary->fixed_cost; iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); - if (options_.jacobi_scaling) { - EstimateScale(*jacobian, scale.data()); - jacobian->ScaleColumns(scale.data()); - } else { - scale.setOnes(); - } - // The initial gradient max_norm is bounded from below so that we do // not divide by zero. const double initial_gradient_max_norm = @@ -189,8 +167,17 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, + summary->preprocessor_time_in_seconds; summary->iterations.push_back(iteration_summary); + if (options_.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + int num_consecutive_invalid_steps = 0; + bool inner_iterations_are_enabled = options.inner_iteration_minimizer != NULL; while (true) { + bool inner_iterations_were_useful = false; if (!RunCallbacks(options.callbacks, iteration_summary, summary)) { return; } @@ -210,33 +197,38 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, break; } - iteration_summary = IterationSummary(); - iteration_summary = summary->iterations.back(); - iteration_summary.iteration = summary->iterations.back().iteration + 1; - iteration_summary.step_is_valid = false; - iteration_summary.step_is_successful = false; - const double strategy_start_time = WallTimeInSeconds(); TrustRegionStrategy::PerSolveOptions per_solve_options; per_solve_options.eta = options_.eta; + if (find(options_.trust_region_minimizer_iterations_to_dump.begin(), + options_.trust_region_minimizer_iterations_to_dump.end(), + iteration_summary.iteration) != + options_.trust_region_minimizer_iterations_to_dump.end()) { + per_solve_options.dump_format_type = + options_.trust_region_problem_dump_format_type; + per_solve_options.dump_filename_base = + JoinPath(options_.trust_region_problem_dump_directory, + StringPrintf("ceres_solver_iteration_%03d", + iteration_summary.iteration)); + } else { + per_solve_options.dump_format_type = TEXTFILE; + per_solve_options.dump_filename_base.clear(); + } + TrustRegionStrategy::Summary strategy_summary = strategy->ComputeStep(per_solve_options, jacobian, residuals.data(), trust_region_step.data()); + iteration_summary = IterationSummary(); + iteration_summary.iteration = summary->iterations.back().iteration + 1; iteration_summary.step_solver_time_in_seconds = WallTimeInSeconds() - strategy_start_time; iteration_summary.linear_solver_iterations = strategy_summary.num_iterations; - - if (!MaybeDumpLinearLeastSquaresProblem(iteration_summary.iteration, - jacobian, - residuals.data(), - trust_region_step.data())) { - LOG(FATAL) << "Tried writing linear least squares problem: " - << options.lsqp_dump_directory << "but failed."; - } + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; double model_cost_change = 0.0; if (strategy_summary.termination_type != FAILURE) { @@ -249,8 +241,8 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, // = -f'J * step - step' * J' * J * step / 2 model_residuals.setZero(); jacobian->RightMultiply(trust_region_step.data(), model_residuals.data()); - model_cost_change = -(residuals.dot(model_residuals) + - model_residuals.squaredNorm() / 2.0); + model_cost_change = + - model_residuals.dot(residuals + model_residuals / 2.0); if (model_cost_change < 0.0) { VLOG(1) << "Invalid step: current_cost: " << cost @@ -316,7 +308,9 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, new_cost = numeric_limits<double>::max(); } else { // Check if performing an inner iteration will make it better. - if (options.inner_iteration_minimizer != NULL) { + if (inner_iterations_are_enabled) { + ++summary->num_inner_iteration_steps; + double inner_iteration_start_time = WallTimeInSeconds(); const double x_plus_delta_cost = new_cost; Vector inner_iteration_x = x_plus_delta; Solver::Summary inner_iteration_summary; @@ -336,7 +330,23 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, VLOG(2) << "Inner iteration succeeded; current cost: " << cost << " x_plus_delta_cost: " << x_plus_delta_cost << " new_cost: " << new_cost; + const double inner_iteration_relative_progress = + 1.0 - new_cost / x_plus_delta_cost; + inner_iterations_are_enabled = + (inner_iteration_relative_progress > + options.inner_iteration_tolerance); + + inner_iterations_were_useful = new_cost < cost; + + // Disable inner iterations once the relative improvement + // drops below tolerance. + if (!inner_iterations_are_enabled) { + VLOG(2) << "Disabling inner iterations. Progress : " + << inner_iteration_relative_progress; + } } + summary->inner_iteration_time_in_seconds += + WallTimeInSeconds() - inner_iteration_start_time; } } @@ -355,7 +365,6 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, return; } - VLOG(2) << "old cost: " << cost << " new cost: " << new_cost; iteration_summary.cost_change = cost - new_cost; const double absolute_function_tolerance = options_.function_tolerance * cost; @@ -392,13 +401,51 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, ? max(relative_decrease, historical_relative_decrease) : relative_decrease; + // Normally, the quality of a trust region step is measured by + // the ratio + // + // cost_change + // r = ----------------- + // model_cost_change + // + // All the change in the nonlinear objective is due to the trust + // region step so this ratio is a good measure of the quality of + // the trust region radius. However, when inner iterations are + // being used, cost_change includes the contribution of the + // inner iterations and its not fair to credit it all to the + // trust region algorithm. So we change the ratio to be + // + // cost_change + // r = ------------------------------------------------ + // (model_cost_change + inner_iteration_cost_change) + // + // In most cases this is fine, but it can be the case that the + // change in solution quality due to inner iterations is so large + // and the trust region step is so bad, that this ratio can become + // quite small. + // + // This can cause the trust region loop to reject this step. To + // get around this, we expicitly check if the inner iterations + // led to a net decrease in the objective function value. If + // they did, we accept the step even if the trust region ratio + // is small. + // + // Notice that we do not just check that cost_change is positive + // which is a weaker condition and would render the + // min_relative_decrease threshold useless. Instead, we keep + // track of inner_iterations_were_useful, which is true only + // when inner iterations lead to a net decrease in the cost. iteration_summary.step_is_successful = - iteration_summary.relative_decrease > options_.min_relative_decrease; + (inner_iterations_were_useful || + iteration_summary.relative_decrease > + options_.min_relative_decrease); if (iteration_summary.step_is_successful) { accumulated_candidate_model_cost_change += model_cost_change; accumulated_reference_model_cost_change += model_cost_change; - if (relative_decrease <= options_.min_relative_decrease) { + + if (!inner_iterations_were_useful && + relative_decrease <= options_.min_relative_decrease) { iteration_summary.step_is_nonmonotonic = true; VLOG(2) << "Non-monotonic step! " << " relative_decrease: " << relative_decrease @@ -419,7 +466,7 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), - NULL, + gradient.data(), jacobian)) { summary->termination_type = NUMERICAL_FAILURE; summary->error = @@ -428,8 +475,6 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, return; } - gradient.setZero(); - jacobian->LeftMultiply(residuals.data(), gradient.data()); iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { diff --git a/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h b/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h index f150594bbd2..0dcdbfef016 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h +++ b/extern/libmv/third_party/ceres/internal/ceres/trust_region_strategy.h @@ -31,6 +31,8 @@ #ifndef CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ #define CERES_INTERNAL_TRUST_REGION_STRATEGY_H_ +#include <string> +#include "ceres/internal/port.h" #include "ceres/types.h" namespace ceres { @@ -58,8 +60,8 @@ class TrustRegionStrategy { : trust_region_strategy_type(LEVENBERG_MARQUARDT), initial_radius(1e4), max_radius(1e32), - lm_min_diagonal(1e-6), - lm_max_diagonal(1e32), + min_lm_diagonal(1e-6), + max_lm_diagonal(1e32), dogleg_type(TRADITIONAL_DOGLEG) { } @@ -73,8 +75,8 @@ class TrustRegionStrategy { // by LevenbergMarquardtStrategy. The DoglegStrategy also uses // these bounds to construct a regularizing diagonal to ensure // that the Gauss-Newton step computation is of full rank. - double lm_min_diagonal; - double lm_max_diagonal; + double min_lm_diagonal; + double max_lm_diagonal; // Further specify which dogleg method to use DoglegType dogleg_type; @@ -82,8 +84,22 @@ class TrustRegionStrategy { // Per solve options. struct PerSolveOptions { + PerSolveOptions() + : eta(0), + dump_filename_base(""), + dump_format_type(TEXTFILE) { + } + // Forcing sequence for inexact solves. double eta; + + // If non-empty and dump_format_type is not CONSOLE, the trust + // regions strategy will write the linear system to file(s) with + // name starting with dump_filename_base. If dump_format_type is + // CONSOLE then dump_filename_base will be ignored and the linear + // system will be written to the standard error. + string dump_filename_base; + DumpFormatType dump_format_type; }; struct Summary { diff --git a/extern/libmv/third_party/ceres/internal/ceres/types.cc b/extern/libmv/third_party/ceres/internal/ceres/types.cc index 2e19322cc76..a97f1a55e6b 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/types.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/types.cc @@ -101,7 +101,6 @@ const char* SparseLinearAlgebraLibraryTypeToString( } } - bool StringToSparseLinearAlgebraLibraryType( string value, SparseLinearAlgebraLibraryType* type) { @@ -111,6 +110,25 @@ bool StringToSparseLinearAlgebraLibraryType( return false; } +const char* DenseLinearAlgebraLibraryTypeToString( + DenseLinearAlgebraLibraryType type) { + switch (type) { + CASESTR(EIGEN); + CASESTR(LAPACK); + default: + return "UNKNOWN"; + } +} + +bool StringToDenseLinearAlgebraLibraryType( + string value, + DenseLinearAlgebraLibraryType* type) { + UpperCase(&value); + STRENUM(EIGEN); + STRENUM(LAPACK); + return false; +} + const char* TrustRegionStrategyTypeToString(TrustRegionStrategyType type) { switch (type) { CASESTR(LEVENBERG_MARQUARDT); @@ -165,6 +183,7 @@ const char* LineSearchDirectionTypeToString(LineSearchDirectionType type) { CASESTR(STEEPEST_DESCENT); CASESTR(NONLINEAR_CONJUGATE_GRADIENT); CASESTR(LBFGS); + CASESTR(BFGS); default: return "UNKNOWN"; } @@ -176,12 +195,14 @@ bool StringToLineSearchDirectionType(string value, STRENUM(STEEPEST_DESCENT); STRENUM(NONLINEAR_CONJUGATE_GRADIENT); STRENUM(LBFGS); + STRENUM(BFGS); return false; } const char* LineSearchTypeToString(LineSearchType type) { switch (type) { CASESTR(ARMIJO); + CASESTR(WOLFE); default: return "UNKNOWN"; } @@ -190,6 +211,28 @@ const char* LineSearchTypeToString(LineSearchType type) { bool StringToLineSearchType(string value, LineSearchType* type) { UpperCase(&value); STRENUM(ARMIJO); + STRENUM(WOLFE); + return false; +} + +const char* LineSearchInterpolationTypeToString( + LineSearchInterpolationType type) { + switch (type) { + CASESTR(BISECTION); + CASESTR(QUADRATIC); + CASESTR(CUBIC); + default: + return "UNKNOWN"; + } +} + +bool StringToLineSearchInterpolationType( + string value, + LineSearchInterpolationType* type) { + UpperCase(&value); + STRENUM(BISECTION); + STRENUM(QUADRATIC); + STRENUM(CUBIC); return false; } @@ -214,6 +257,27 @@ bool StringToNonlinearConjugateGradientType( return false; } +const char* CovarianceAlgorithmTypeToString( + CovarianceAlgorithmType type) { + switch (type) { + CASESTR(DENSE_SVD); + CASESTR(SPARSE_CHOLESKY); + CASESTR(SPARSE_QR); + default: + return "UNKNOWN"; + } +} + +bool StringToCovarianceAlgorithmType( + string value, + CovarianceAlgorithmType* type) { + UpperCase(&value); + STRENUM(DENSE_SVD); + STRENUM(SPARSE_CHOLESKY); + STRENUM(SPARSE_QR); + return false; +} + const char* SolverTerminationTypeToString(SolverTerminationType type) { switch (type) { CASESTR(NO_CONVERGENCE); @@ -272,4 +336,21 @@ bool IsSparseLinearAlgebraLibraryTypeAvailable( return false; } +bool IsDenseLinearAlgebraLibraryTypeAvailable( + DenseLinearAlgebraLibraryType type) { + if (type == EIGEN) { + return true; + } + if (type == LAPACK) { +#ifdef CERES_NO_LAPACK + return false; +#else + return true; +#endif + } + + LOG(WARNING) << "Unknown dense linear algebra library " << type; + return false; +} + } // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc index fcd793c00a8..acfa45b863a 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc @@ -153,4 +153,4 @@ Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { } // namespace internal } // namespace ceres -#endif +#endif // CERES_NO_SUITESPARSE 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 4b1e26af6d2..7af133905b3 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 @@ -324,8 +324,8 @@ void VisibilityBasedPreconditioner::InitEliminator( } // Update the values of the preconditioner matrix and factorize it. -bool VisibilityBasedPreconditioner::Update(const BlockSparseMatrixBase& A, - const double* D) { +bool VisibilityBasedPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { const time_t start_time = time(NULL); const int num_rows = m_->num_rows(); CHECK_GT(num_rows, 0); 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 dae498730aa..c58b1a7a90a 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 @@ -62,7 +62,7 @@ namespace ceres { namespace internal { class BlockRandomAccessSparseMatrix; -class BlockSparseMatrixBase; +class BlockSparseMatrix; struct CompressedRowBlockStructure; class SchurEliminatorBase; @@ -123,7 +123,7 @@ class SchurEliminatorBase; // preconditioner.RightMultiply(x, y); // #ifndef CERES_NO_SUITESPARSE -class VisibilityBasedPreconditioner : public Preconditioner { +class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { public: // Initialize the symbolic structure of the preconditioner. bs is // the block structure of the linear system to be solved. It is used @@ -136,12 +136,13 @@ class VisibilityBasedPreconditioner : public Preconditioner { virtual ~VisibilityBasedPreconditioner(); // Preconditioner interface - virtual bool Update(const BlockSparseMatrixBase& A, const double* D); virtual void RightMultiply(const double* x, double* y) const; virtual int num_rows() const; friend class VisibilityBasedPreconditionerTest; + private: + virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); void ComputeClusterJacobiSparsity(const CompressedRowBlockStructure& bs); void ComputeClusterTridiagonalSparsity(const CompressedRowBlockStructure& bs); void InitStorage(const CompressedRowBlockStructure& bs); @@ -203,7 +204,7 @@ class VisibilityBasedPreconditioner : public Preconditioner { #else // SuiteSparse // If SuiteSparse is not compiled in, the preconditioner is not // available. -class VisibilityBasedPreconditioner : public Preconditioner { +class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { public: VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, const Preconditioner::Options& options) { @@ -215,7 +216,9 @@ class VisibilityBasedPreconditioner : public Preconditioner { virtual void LeftMultiply(const double* x, double* y) const {} virtual int num_rows() const { return -1; } virtual int num_cols() const { return -1; } - bool Update(const BlockSparseMatrixBase& A, const double* D) { + + private: + bool UpdateImpl(const BlockSparseMatrix& A, const double* D) { return false; } }; diff --git a/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc b/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc index e63d20c0ab1..85c4417552d 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc +++ b/extern/libmv/third_party/ceres/internal/ceres/wall_time.cc @@ -86,7 +86,7 @@ void EventLogger::AddEvent(const string& event_name) { last_event_time_ = current_time; StringAppendF(&events_, - " %25s : %10.5f %10.5f\n", + " %30s : %10.5f %10.5f\n", event_name.c_str(), relative_time_delta, absolute_time_delta); diff --git a/extern/libmv/third_party/ceres/internal/ceres/wall_time.h b/extern/libmv/third_party/ceres/internal/ceres/wall_time.h index 45f65ca1aa5..37f5568a125 100644 --- a/extern/libmv/third_party/ceres/internal/ceres/wall_time.h +++ b/extern/libmv/third_party/ceres/internal/ceres/wall_time.h @@ -32,7 +32,7 @@ #define CERES_INTERNAL_WALL_TIME_H_ #include <map> - +#include <string> #include "ceres/internal/port.h" #include "ceres/stringprintf.h" #include "glog/logging.h" |