diff options
author | Sergey Sharybin <sergey.vfx@gmail.com> | 2020-06-18 11:12:01 +0300 |
---|---|---|
committer | Sergey Sharybin <sergey.vfx@gmail.com> | 2020-06-19 13:02:21 +0300 |
commit | 31ae83381106a7aaee5303710b818c13f42ceddc (patch) | |
tree | 6935ebe7e84db7e54c689ea3308fb6df67e03d73 /extern/ceres/internal | |
parent | 171c4fb238a2a65291540ac5406187bc69f3a6bc (diff) |
Ceres: Update to the latest upstream version
Using latest master because of various compilation error fixes.
Brings a lot of recent development. From most interesting parts:
- New threading model.
- Tiny solver.
- Compatibility with C++17.
Diffstat (limited to 'extern/ceres/internal')
232 files changed, 12006 insertions, 5417 deletions
diff --git a/extern/ceres/internal/ceres/accelerate_sparse.cc b/extern/ceres/internal/ceres/accelerate_sparse.cc new file mode 100644 index 00000000000..eb04e7113d7 --- /dev/null +++ b/extern/ceres/internal/ceres/accelerate_sparse.cc @@ -0,0 +1,289 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: alexs.mac@gmail.com (Alex Stewart) + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_NO_ACCELERATE_SPARSE + +#include "ceres/accelerate_sparse.h" + +#include <algorithm> +#include <string> +#include <vector> + +#include "ceres/compressed_col_sparse_matrix_utils.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "glog/logging.h" + +#define CASESTR(x) case x: return #x + +namespace ceres { +namespace internal { + +namespace { +const char* SparseStatusToString(SparseStatus_t status) { + switch (status) { + CASESTR(SparseStatusOK); + CASESTR(SparseFactorizationFailed); + CASESTR(SparseMatrixIsSingular); + CASESTR(SparseInternalError); + CASESTR(SparseParameterError); + CASESTR(SparseStatusReleased); + default: + return "UKNOWN"; + } +} +} // namespace. + +// Resizes workspace as required to contain at least required_size bytes +// aligned to kAccelerateRequiredAlignment and returns a pointer to the +// aligned start. +void* ResizeForAccelerateAlignment(const size_t required_size, + std::vector<uint8_t> *workspace) { + // As per the Accelerate documentation, all workspace memory passed to the + // sparse solver functions must be 16-byte aligned. + constexpr int kAccelerateRequiredAlignment = 16; + // Although malloc() on macOS should always be 16-byte aligned, it is unclear + // if this holds for new(), or on other Apple OSs (phoneOS, watchOS etc). + // As such we assume it is not and use std::align() to create a (potentially + // offset) 16-byte aligned sub-buffer of the specified size within workspace. + workspace->resize(required_size + kAccelerateRequiredAlignment); + size_t size_from_aligned_start = workspace->size(); + void* aligned_solve_workspace_start = + reinterpret_cast<void*>(workspace->data()); + aligned_solve_workspace_start = + std::align(kAccelerateRequiredAlignment, + required_size, + aligned_solve_workspace_start, + size_from_aligned_start); + CHECK(aligned_solve_workspace_start != nullptr) + << "required_size: " << required_size + << ", workspace size: " << workspace->size(); + return aligned_solve_workspace_start; +} + +template<typename Scalar> +void AccelerateSparse<Scalar>::Solve(NumericFactorization* numeric_factor, + DenseVector* rhs_and_solution) { + // From SparseSolve() documentation in Solve.h + const int required_size = + numeric_factor->solveWorkspaceRequiredStatic + + numeric_factor->solveWorkspaceRequiredPerRHS; + SparseSolve(*numeric_factor, *rhs_and_solution, + ResizeForAccelerateAlignment(required_size, &solve_workspace_)); +} + +template<typename Scalar> +typename AccelerateSparse<Scalar>::ASSparseMatrix +AccelerateSparse<Scalar>::CreateSparseMatrixTransposeView( + CompressedRowSparseMatrix* A) { + // Accelerate uses CSC as its sparse storage format whereas Ceres uses CSR. + // As this method returns the transpose view we can flip rows/cols to map + // from CSR to CSC^T. + // + // Accelerate's columnStarts is a long*, not an int*. These types might be + // different (e.g. ARM on iOS) so always make a copy. + column_starts_.resize(A->num_rows() +1); // +1 for final column length. + std::copy_n(A->rows(), column_starts_.size(), &column_starts_[0]); + + ASSparseMatrix At; + At.structure.rowCount = A->num_cols(); + At.structure.columnCount = A->num_rows(); + At.structure.columnStarts = &column_starts_[0]; + At.structure.rowIndices = A->mutable_cols(); + At.structure.attributes.transpose = false; + At.structure.attributes.triangle = SparseUpperTriangle; + At.structure.attributes.kind = SparseSymmetric; + At.structure.attributes._reserved = 0; + At.structure.attributes._allocatedBySparse = 0; + At.structure.blockSize = 1; + if (std::is_same<Scalar, double>::value) { + At.data = reinterpret_cast<Scalar*>(A->mutable_values()); + } else { + values_ = + ConstVectorRef(A->values(), A->num_nonzeros()).template cast<Scalar>(); + At.data = values_.data(); + } + return At; +} + +template<typename Scalar> +typename AccelerateSparse<Scalar>::SymbolicFactorization +AccelerateSparse<Scalar>::AnalyzeCholesky(ASSparseMatrix* A) { + return SparseFactor(SparseFactorizationCholesky, A->structure); +} + +template<typename Scalar> +typename AccelerateSparse<Scalar>::NumericFactorization +AccelerateSparse<Scalar>::Cholesky(ASSparseMatrix* A, + SymbolicFactorization* symbolic_factor) { + return SparseFactor(*symbolic_factor, *A); +} + +template<typename Scalar> +void AccelerateSparse<Scalar>::Cholesky(ASSparseMatrix* A, + NumericFactorization* numeric_factor) { + // From SparseRefactor() documentation in Solve.h + const int required_size = std::is_same<Scalar, double>::value + ? numeric_factor->symbolicFactorization.workspaceSize_Double + : numeric_factor->symbolicFactorization.workspaceSize_Float; + return SparseRefactor(*A, numeric_factor, + ResizeForAccelerateAlignment(required_size, + &factorization_workspace_)); +} + +// Instantiate only for the specific template types required/supported s/t the +// definition can be in the .cc file. +template class AccelerateSparse<double>; +template class AccelerateSparse<float>; + +template<typename Scalar> +std::unique_ptr<SparseCholesky> +AppleAccelerateCholesky<Scalar>::Create(OrderingType ordering_type) { + return std::unique_ptr<SparseCholesky>( + new AppleAccelerateCholesky<Scalar>(ordering_type)); +} + +template<typename Scalar> +AppleAccelerateCholesky<Scalar>::AppleAccelerateCholesky( + const OrderingType ordering_type) + : ordering_type_(ordering_type) {} + +template<typename Scalar> +AppleAccelerateCholesky<Scalar>::~AppleAccelerateCholesky() { + FreeSymbolicFactorization(); + FreeNumericFactorization(); +} + +template<typename Scalar> +CompressedRowSparseMatrix::StorageType +AppleAccelerateCholesky<Scalar>::StorageType() const { + return CompressedRowSparseMatrix::LOWER_TRIANGULAR; +} + +template<typename Scalar> +LinearSolverTerminationType +AppleAccelerateCholesky<Scalar>::Factorize(CompressedRowSparseMatrix* lhs, + std::string* message) { + CHECK_EQ(lhs->storage_type(), StorageType()); + if (lhs == NULL) { + *message = "Failure: Input lhs is NULL."; + return LINEAR_SOLVER_FATAL_ERROR; + } + typename SparseTypesTrait<Scalar>::SparseMatrix as_lhs = + as_.CreateSparseMatrixTransposeView(lhs); + + if (!symbolic_factor_) { + symbolic_factor_.reset( + new typename SparseTypesTrait<Scalar>::SymbolicFactorization( + as_.AnalyzeCholesky(&as_lhs))); + if (symbolic_factor_->status != SparseStatusOK) { + *message = StringPrintf( + "Apple Accelerate Failure : Symbolic factorisation failed: %s", + SparseStatusToString(symbolic_factor_->status)); + FreeSymbolicFactorization(); + return LINEAR_SOLVER_FATAL_ERROR; + } + } + + if (!numeric_factor_) { + numeric_factor_.reset( + new typename SparseTypesTrait<Scalar>::NumericFactorization( + as_.Cholesky(&as_lhs, symbolic_factor_.get()))); + } else { + // Recycle memory from previous numeric factorization. + as_.Cholesky(&as_lhs, numeric_factor_.get()); + } + if (numeric_factor_->status != SparseStatusOK) { + *message = StringPrintf( + "Apple Accelerate Failure : Numeric factorisation failed: %s", + SparseStatusToString(numeric_factor_->status)); + FreeNumericFactorization(); + return LINEAR_SOLVER_FAILURE; + } + + return LINEAR_SOLVER_SUCCESS; +} + +template<typename Scalar> +LinearSolverTerminationType +AppleAccelerateCholesky<Scalar>::Solve(const double* rhs, + double* solution, + std::string* message) { + CHECK_EQ(numeric_factor_->status, SparseStatusOK) + << "Solve called without a call to Factorize first (" + << SparseStatusToString(numeric_factor_->status) << ")."; + const int num_cols = numeric_factor_->symbolicFactorization.columnCount; + + typename SparseTypesTrait<Scalar>::DenseVector as_rhs_and_solution; + as_rhs_and_solution.count = num_cols; + if (std::is_same<Scalar, double>::value) { + as_rhs_and_solution.data = reinterpret_cast<Scalar*>(solution); + std::copy_n(rhs, num_cols, solution); + } else { + scalar_rhs_and_solution_ = + ConstVectorRef(rhs, num_cols).template cast<Scalar>(); + as_rhs_and_solution.data = scalar_rhs_and_solution_.data(); + } + as_.Solve(numeric_factor_.get(), &as_rhs_and_solution); + if (!std::is_same<Scalar, double>::value) { + VectorRef(solution, num_cols) = + scalar_rhs_and_solution_.template cast<double>(); + } + return LINEAR_SOLVER_SUCCESS; +} + +template<typename Scalar> +void AppleAccelerateCholesky<Scalar>::FreeSymbolicFactorization() { + if (symbolic_factor_) { + SparseCleanup(*symbolic_factor_); + symbolic_factor_.reset(); + } +} + +template<typename Scalar> +void AppleAccelerateCholesky<Scalar>::FreeNumericFactorization() { + if (numeric_factor_) { + SparseCleanup(*numeric_factor_); + numeric_factor_.reset(); + } +} + +// Instantiate only for the specific template types required/supported s/t the +// definition can be in the .cc file. +template class AppleAccelerateCholesky<double>; +template class AppleAccelerateCholesky<float>; + +} +} + +#endif // CERES_NO_ACCELERATE_SPARSE diff --git a/extern/ceres/internal/ceres/accelerate_sparse.h b/extern/ceres/internal/ceres/accelerate_sparse.h new file mode 100644 index 00000000000..43b4ea5fd70 --- /dev/null +++ b/extern/ceres/internal/ceres/accelerate_sparse.h @@ -0,0 +1,147 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: alexs.mac@gmail.com (Alex Stewart) + +#ifndef CERES_INTERNAL_ACCELERATE_SPARSE_H_ +#define CERES_INTERNAL_ACCELERATE_SPARSE_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_NO_ACCELERATE_SPARSE + +#include <memory> +#include <string> +#include <vector> + +#include "ceres/linear_solver.h" +#include "ceres/sparse_cholesky.h" +#include "Accelerate.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; +class TripletSparseMatrix; + +template<typename Scalar> +struct SparseTypesTrait { +}; + +template<> +struct SparseTypesTrait<double> { + typedef DenseVector_Double DenseVector; + typedef SparseMatrix_Double SparseMatrix; + typedef SparseOpaqueSymbolicFactorization SymbolicFactorization; + typedef SparseOpaqueFactorization_Double NumericFactorization; +}; + +template<> +struct SparseTypesTrait<float> { + typedef DenseVector_Float DenseVector; + typedef SparseMatrix_Float SparseMatrix; + typedef SparseOpaqueSymbolicFactorization SymbolicFactorization; + typedef SparseOpaqueFactorization_Float NumericFactorization; +}; + +template<typename Scalar> +class AccelerateSparse { + public: + using DenseVector = typename SparseTypesTrait<Scalar>::DenseVector; + // Use ASSparseMatrix to avoid collision with ceres::internal::SparseMatrix. + using ASSparseMatrix = typename SparseTypesTrait<Scalar>::SparseMatrix; + using SymbolicFactorization = typename SparseTypesTrait<Scalar>::SymbolicFactorization; + using NumericFactorization = typename SparseTypesTrait<Scalar>::NumericFactorization; + + // Solves a linear system given its symbolic (reference counted within + // NumericFactorization) and numeric factorization. + void Solve(NumericFactorization* numeric_factor, + DenseVector* rhs_and_solution); + + // Note: Accelerate's API passes/returns its objects by value, but as the + // objects contain pointers to the underlying data these copies are + // all shallow (in some cases Accelerate also reference counts the + // objects internally). + ASSparseMatrix CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + // Computes a symbolic factorisation of A that can be used in Solve(). + SymbolicFactorization AnalyzeCholesky(ASSparseMatrix* A); + // Compute the numeric Cholesky factorization of A, given its + // symbolic factorization. + NumericFactorization Cholesky(ASSparseMatrix* A, + SymbolicFactorization* symbolic_factor); + // Reuse the NumericFactorization from a previous matrix with the same + // symbolic factorization to represent a new numeric factorization. + void Cholesky(ASSparseMatrix* A, NumericFactorization* numeric_factor); + + private: + std::vector<long> column_starts_; + std::vector<uint8_t> solve_workspace_; + std::vector<uint8_t> factorization_workspace_; + // Storage for the values of A if Scalar != double (necessitating a copy). + Eigen::Matrix<Scalar, Eigen::Dynamic, 1> values_; +}; + +// An implementation of SparseCholesky interface using Apple's Accelerate +// framework. +template<typename Scalar> +class AppleAccelerateCholesky : public SparseCholesky { + public: + // Factory + static std::unique_ptr<SparseCholesky> Create(OrderingType ordering_type); + + // SparseCholesky interface. + virtual ~AppleAccelerateCholesky(); + CompressedRowSparseMatrix::StorageType StorageType() const; + LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, + std::string* message) final; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) final ; + + private: + AppleAccelerateCholesky(const OrderingType ordering_type); + void FreeSymbolicFactorization(); + void FreeNumericFactorization(); + + const OrderingType ordering_type_; + AccelerateSparse<Scalar> as_; + std::unique_ptr<typename AccelerateSparse<Scalar>::SymbolicFactorization> + symbolic_factor_; + std::unique_ptr<typename AccelerateSparse<Scalar>::NumericFactorization> + numeric_factor_; + // Copy of rhs/solution if Scalar != double (necessitating a copy). + Eigen::Matrix<Scalar, Eigen::Dynamic, 1> scalar_rhs_and_solution_; +}; + +} +} + +#endif // CERES_NO_ACCELERATE_SPARSE + +#endif // CERES_INTERNAL_ACCELERATE_SPARSE_H_ diff --git a/extern/ceres/internal/ceres/array_utils.cc b/extern/ceres/internal/ceres/array_utils.cc index 7be3c78ce24..32459e6dcd9 100644 --- a/extern/ceres/internal/ceres/array_utils.cc +++ b/extern/ceres/internal/ceres/array_utils.cc @@ -35,25 +35,17 @@ #include <cstddef> #include <string> #include <vector> -#include "ceres/fpclassify.h" #include "ceres/stringprintf.h" - +#include "ceres/types.h" namespace ceres { namespace internal { using std::string; -// It is a near impossibility that user code generates this exact -// value in normal operation, thus we will use it to fill arrays -// before passing them to user code. If on return an element of the -// array still contains this value, we will assume that the user code -// did not write to that memory location. -const double kImpossibleValue = 1e302; - bool IsArrayValid(const int size, const double* x) { if (x != NULL) { for (int i = 0; i < size; ++i) { - if (!IsFinite(x[i]) || (x[i] == kImpossibleValue)) { + if (!std::isfinite(x[i]) || (x[i] == kImpossibleValue)) { return false; } } @@ -67,7 +59,7 @@ int FindInvalidValue(const int size, const double* x) { } for (int i = 0; i < size; ++i) { - if (!IsFinite(x[i]) || (x[i] == kImpossibleValue)) { + if (!std::isfinite(x[i]) || (x[i] == kImpossibleValue)) { return i; } } diff --git a/extern/ceres/internal/ceres/array_utils.h b/extern/ceres/internal/ceres/array_utils.h index 2d2ffca8809..1d55733b7b8 100644 --- a/extern/ceres/internal/ceres/array_utils.h +++ b/extern/ceres/internal/ceres/array_utils.h @@ -66,11 +66,9 @@ int FindInvalidValue(const int size, const double* x); // array pointer is NULL, it is treated as an array of zeros. void AppendArrayToString(const int size, const double* x, std::string* result); -extern const double kImpossibleValue; - // This routine takes an array of integer values, sorts and uniques // them and then maps each value in the array to its position in the -// sorted+uniqued array. By doing this, if there are are k unique +// sorted+uniqued array. By doing this, if there are k unique // values in the array, each value is replaced by an integer in the // range [0, k-1], while preserving their relative order. // diff --git a/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc b/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc index 22d4b351c51..772c7af2ba5 100644 --- a/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc +++ b/extern/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -34,7 +34,6 @@ #include "ceres/block_structure.h" #include "ceres/block_random_access_diagonal_matrix.h" #include "ceres/casts.h" -#include "ceres/integral_types.h" #include "ceres/internal/eigen.h" namespace ceres { diff --git a/extern/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/ceres/internal/ceres/block_jacobi_preconditioner.h index 14007295823..856b506e073 100644 --- a/extern/ceres/internal/ceres/block_jacobi_preconditioner.h +++ b/extern/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -31,9 +31,8 @@ #ifndef CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ #define CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ -#include <vector> +#include <memory> #include "ceres/block_random_access_diagonal_matrix.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/preconditioner.h" namespace ceres { @@ -56,18 +55,21 @@ class BlockJacobiPreconditioner : public BlockSparseMatrixPreconditioner { public: // A must remain valid while the BlockJacobiPreconditioner is. explicit BlockJacobiPreconditioner(const BlockSparseMatrix& A); + BlockJacobiPreconditioner(const BlockJacobiPreconditioner&) = delete; + void operator=(const BlockJacobiPreconditioner&) = delete; + virtual ~BlockJacobiPreconditioner(); // Preconditioner interface - virtual void RightMultiply(const double* x, double* y) const; - virtual int num_rows() const { return m_->num_rows(); } - virtual int num_cols() const { return m_->num_rows(); } - + void RightMultiply(const double* x, double* y) const final; + int num_rows() const final { return m_->num_rows(); } + int num_cols() const final { return m_->num_rows(); } const BlockRandomAccessDiagonalMatrix& matrix() const { return *m_; } + private: - virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); + bool UpdateImpl(const BlockSparseMatrix& A, const double* D) final; - scoped_ptr<BlockRandomAccessDiagonalMatrix> m_; + std::unique_ptr<BlockRandomAccessDiagonalMatrix> m_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/block_jacobian_writer.cc b/extern/ceres/internal/ceres/block_jacobian_writer.cc index 7a3fee4fbdf..6998bd66e61 100644 --- a/extern/ceres/internal/ceres/block_jacobian_writer.cc +++ b/extern/ceres/internal/ceres/block_jacobian_writer.cc @@ -37,7 +37,6 @@ #include "ceres/residual_block.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" namespace ceres { namespace internal { @@ -206,7 +205,7 @@ SparseMatrix* BlockJacobianWriter::CreateJacobian() const { } BlockSparseMatrix* jacobian = new BlockSparseMatrix(bs); - CHECK_NOTNULL(jacobian); + CHECK(jacobian != nullptr); return jacobian; } diff --git a/extern/ceres/internal/ceres/block_jacobian_writer.h b/extern/ceres/internal/ceres/block_jacobian_writer.h index 8e6f45130a4..c94a0d3f909 100644 --- a/extern/ceres/internal/ceres/block_jacobian_writer.h +++ b/extern/ceres/internal/ceres/block_jacobian_writer.h @@ -49,6 +49,7 @@ class BlockEvaluatePreparer; class Program; class SparseMatrix; +// TODO(sameeragarwal): This class needs documemtation. class BlockJacobianWriter { public: BlockJacobianWriter(const Evaluator::Options& options, diff --git a/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc b/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc index 61748ef6f7f..f567aa5816b 100644 --- a/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc +++ b/extern/ceres/internal/ceres/block_random_access_dense_matrix.cc @@ -32,7 +32,6 @@ #include <vector> #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "glog/logging.h" namespace ceres { diff --git a/extern/ceres/internal/ceres/block_random_access_dense_matrix.h b/extern/ceres/internal/ceres/block_random_access_dense_matrix.h index 89689082561..8c5e2527ec1 100644 --- a/extern/ceres/internal/ceres/block_random_access_dense_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_dense_matrix.h @@ -33,11 +33,10 @@ #include "ceres/block_random_access_matrix.h" +#include <memory> #include <vector> -#include "ceres/internal/macros.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" namespace ceres { namespace internal { @@ -57,27 +56,29 @@ class BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { // blocks is a vector of block sizes. The resulting matrix has // blocks.size() * blocks.size() cells. explicit BlockRandomAccessDenseMatrix(const std::vector<int>& blocks); + BlockRandomAccessDenseMatrix(const BlockRandomAccessDenseMatrix&) = delete; + void operator=(const BlockRandomAccessDenseMatrix&) = delete; // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. virtual ~BlockRandomAccessDenseMatrix(); // BlockRandomAccessMatrix interface. - virtual CellInfo* GetCell(int row_block_id, - int col_block_id, - int* row, - int* col, - int* row_stride, - int* col_stride); + CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) final; // This is not a thread safe method, it assumes that no cell is // locked. - virtual void SetZero(); + void SetZero() final; // Since the matrix is square with the same row and column block // structure, num_rows() = num_cols(). - virtual int num_rows() const { return num_rows_; } - virtual int num_cols() const { return num_rows_; } + int num_rows() const final { return num_rows_; } + int num_cols() const final { return num_rows_; } // The underlying matrix storing the cells. const double* values() const { return values_.get(); } @@ -86,10 +87,8 @@ class BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { private: int num_rows_; std::vector<int> block_layout_; - scoped_array<double> values_; - scoped_array<CellInfo> cell_infos_; - - CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDenseMatrix); + std::unique_ptr<double[]> values_; + std::unique_ptr<CellInfo[]> cell_infos_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc index 052690d18be..526d173e4b0 100644 --- a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc +++ b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.cc @@ -34,9 +34,9 @@ #include <set> #include <utility> #include <vector> + #include "Eigen/Dense" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/stl_util.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" @@ -137,8 +137,8 @@ void BlockRandomAccessDiagonalMatrix::Invert() { void BlockRandomAccessDiagonalMatrix::RightMultiply(const double* x, double* y) const { - CHECK_NOTNULL(x); - CHECK_NOTNULL(y); + CHECK(x != nullptr); + CHECK(y != nullptr); const double* values = tsm_->values(); for (int i = 0; i < blocks_.size(); ++i) { const int block_size = blocks_[i]; diff --git a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h index 07ffc9d4a0d..3bda7d19074 100644 --- a/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_diagonal_matrix.h @@ -31,17 +31,14 @@ #ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_ #define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DIAGONAL_MATRIX_H_ +#include <memory> #include <set> -#include <vector> #include <utility> -#include "ceres/mutex.h" +#include <vector> + #include "ceres/block_random_access_matrix.h" -#include "ceres/collections_port.h" -#include "ceres/triplet_sparse_matrix.h" -#include "ceres/integral_types.h" -#include "ceres/internal/macros.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" namespace ceres { @@ -53,22 +50,24 @@ class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { public: // blocks is an array of block sizes. explicit BlockRandomAccessDiagonalMatrix(const std::vector<int>& blocks); + BlockRandomAccessDiagonalMatrix(const BlockRandomAccessDiagonalMatrix&) = delete; + void operator=(const BlockRandomAccessDiagonalMatrix&) = delete; // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. virtual ~BlockRandomAccessDiagonalMatrix(); // BlockRandomAccessMatrix Interface. - virtual CellInfo* GetCell(int row_block_id, - int col_block_id, - int* row, - int* col, - int* row_stride, - int* col_stride); + CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) final; // This is not a thread safe method, it assumes that no cell is // locked. - virtual void SetZero(); + void SetZero() final; // Invert the matrix assuming that each block is positive definite. void Invert(); @@ -77,8 +76,8 @@ class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { void RightMultiply(const double* x, double* y) const; // Since the matrix is square, num_rows() == num_cols(). - virtual int num_rows() const { return tsm_->num_rows(); } - virtual int num_cols() const { return tsm_->num_cols(); } + int num_rows() const final { return tsm_->num_rows(); } + int num_cols() const final { return tsm_->num_cols(); } const TripletSparseMatrix* matrix() const { return tsm_.get(); } TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } @@ -89,10 +88,9 @@ class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { std::vector<CellInfo*> layout_; // The underlying matrix object which actually stores the cells. - scoped_ptr<TripletSparseMatrix> tsm_; + std::unique_ptr<TripletSparseMatrix> tsm_; friend class BlockRandomAccessDiagonalMatrixTest; - CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDiagonalMatrix); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/block_random_access_matrix.h b/extern/ceres/internal/ceres/block_random_access_matrix.h index 34c8bf5cd4d..6fcf0dc8a7c 100644 --- a/extern/ceres/internal/ceres/block_random_access_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_matrix.h @@ -33,7 +33,7 @@ #ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ #define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ -#include "ceres/mutex.h" +#include <mutex> namespace ceres { namespace internal { @@ -77,23 +77,18 @@ namespace internal { // // if (cell != NULL) { // MatrixRef m(cell->values, row_stride, col_stride); -// CeresMutexLock l(&cell->m); +// std::lock_guard<std::mutex> l(&cell->m); // m.block(row, col, row_block_size, col_block_size) = ... // } // Structure to carry a pointer to the array containing a cell and the -// Mutex guarding it. +// mutex guarding it. struct CellInfo { - CellInfo() - : values(NULL) { - } - - explicit CellInfo(double* ptr) - : values(ptr) { - } + CellInfo() : values(nullptr) {} + explicit CellInfo(double* values) : values(values) {} double* values; - Mutex m; + std::mutex m; }; class BlockRandomAccessMatrix { diff --git a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc index 5432ec1064a..9c164546635 100644 --- a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.cc @@ -31,12 +31,12 @@ #include "ceres/block_random_access_sparse_matrix.h" #include <algorithm> +#include <memory> #include <set> #include <utility> #include <vector> + #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" @@ -51,7 +51,7 @@ using std::vector; BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( const vector<int>& blocks, - const set<pair<int, int> >& block_pairs) + const set<pair<int, int>>& block_pairs) : kMaxRowBlocks(10 * 1000 * 1000), blocks_(blocks) { CHECK_LT(blocks.size(), kMaxRowBlocks); @@ -69,11 +69,9 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( // object for looking into the values array of the // TripletSparseMatrix. 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]; + for (const auto& block_pair : block_pairs) { + const int row_block_size = blocks_[block_pair.first]; + const int col_block_size = blocks_[block_pair.second]; num_nonzeros += row_block_size * col_block_size; } @@ -88,24 +86,19 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( double* values = tsm_->mutable_values(); int pos = 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]; - cell_values_.push_back(make_pair(make_pair(it->first, it->second), - values + pos)); - layout_[IntPairToLong(it->first, it->second)] = + for (const auto& block_pair : block_pairs) { + const int row_block_size = blocks_[block_pair.first]; + const int col_block_size = blocks_[block_pair.second]; + cell_values_.push_back(make_pair(block_pair, values + pos)); + layout_[IntPairToLong(block_pair.first, block_pair.second)] = new CellInfo(values + pos); pos += row_block_size * col_block_size; } // Fill the sparsity pattern of the underlying matrix. - for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); - it != block_pairs.end(); - ++it) { - const int row_block_id = it->first; - const int col_block_id = it->second; + for (const auto& block_pair : block_pairs) { + const int row_block_id = block_pair.first; + const int col_block_id = block_pair.second; const int row_block_size = blocks_[row_block_id]; const int col_block_size = blocks_[col_block_id]; int pos = @@ -125,10 +118,8 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( // Assume that the user does not hold any locks on any cell blocks // when they are calling SetZero. BlockRandomAccessSparseMatrix::~BlockRandomAccessSparseMatrix() { - for (LayoutType::iterator it = layout_.begin(); - it != layout_.end(); - ++it) { - delete it->second; + for (const auto& entry : layout_) { + delete entry.second; } } @@ -163,19 +154,17 @@ void BlockRandomAccessSparseMatrix::SetZero() { void BlockRandomAccessSparseMatrix::SymmetricRightMultiply(const double* x, double* y) const { - vector< pair<pair<int, int>, double*> >::const_iterator it = - cell_values_.begin(); - for (; it != cell_values_.end(); ++it) { - const int row = it->first.first; + for (const auto& cell_position_and_data : cell_values_) { + const int row = cell_position_and_data.first.first; const int row_block_size = blocks_[row]; const int row_block_pos = block_positions_[row]; - const int col = it->first.second; + const int col = cell_position_and_data.first.second; const int col_block_size = blocks_[col]; const int col_block_pos = block_positions_[col]; MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - it->second, row_block_size, col_block_size, + cell_position_and_data.second, row_block_size, col_block_size, x + col_block_pos, y + row_block_pos); @@ -185,7 +174,7 @@ void BlockRandomAccessSparseMatrix::SymmetricRightMultiply(const double* x, // triangular multiply also. if (row != col) { MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - it->second, row_block_size, col_block_size, + cell_position_and_data.second, row_block_size, col_block_size, x + row_block_pos, y + col_block_pos); } diff --git a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h index 2b3c7fdabae..d542a3d64e3 100644 --- a/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h +++ b/extern/ceres/internal/ceres/block_random_access_sparse_matrix.h @@ -31,17 +31,16 @@ #ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ #define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ +#include <cstdint> +#include <memory> #include <set> -#include <vector> +#include <unordered_map> #include <utility> -#include "ceres/mutex.h" +#include <vector> + #include "ceres/block_random_access_matrix.h" -#include "ceres/collections_port.h" #include "ceres/triplet_sparse_matrix.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" #include "ceres/small_blas.h" @@ -59,23 +58,25 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { // of this matrix. BlockRandomAccessSparseMatrix( const std::vector<int>& blocks, - const std::set<std::pair<int, int> >& block_pairs); + const std::set<std::pair<int, int>>& block_pairs); + BlockRandomAccessSparseMatrix(const BlockRandomAccessSparseMatrix&) = delete; + void operator=(const BlockRandomAccessSparseMatrix&) = delete; // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. virtual ~BlockRandomAccessSparseMatrix(); // BlockRandomAccessMatrix Interface. - virtual CellInfo* GetCell(int row_block_id, - int col_block_id, - int* row, - int* col, - int* row_stride, - int* col_stride); + CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) final; // This is not a thread safe method, it assumes that no cell is // locked. - virtual void SetZero(); + void SetZero() final; // Assume that the matrix is symmetric and only one half of the // matrix is stored. @@ -84,24 +85,24 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { void SymmetricRightMultiply(const double* x, double* y) const; // Since the matrix is square, num_rows() == num_cols(). - virtual int num_rows() const { return tsm_->num_rows(); } - virtual int num_cols() const { return tsm_->num_cols(); } + int num_rows() const final { return tsm_->num_rows(); } + int num_cols() const final { return tsm_->num_cols(); } // Access to the underlying matrix object. const TripletSparseMatrix* matrix() const { return tsm_.get(); } TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } private: - int64 IntPairToLong(int row, int col) const { + int64_t IntPairToLong(int row, int col) const { return row * kMaxRowBlocks + col; } - void LongToIntPair(int64 index, int* row, int* col) const { + void LongToIntPair(int64_t index, int* row, int* col) const { *row = index / kMaxRowBlocks; *col = index % kMaxRowBlocks; } - const int64 kMaxRowBlocks; + const int64_t kMaxRowBlocks; // row/column block sizes. const std::vector<int> blocks_; @@ -109,18 +110,17 @@ class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { // 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; + typedef std::unordered_map<long int, CellInfo* > LayoutType; LayoutType layout_; // In order traversal of contents of the matrix. This allows us to // implement a matrix-vector which is 20% faster than using the // iterator in the Layout object instead. - std::vector<std::pair<std::pair<int, int>, double*> > cell_values_; + std::vector<std::pair<std::pair<int, int>, double*>> cell_values_; // The underlying matrix object which actually stores the cells. - scoped_ptr<TripletSparseMatrix> tsm_; + std::unique_ptr<TripletSparseMatrix> tsm_; friend class BlockRandomAccessSparseMatrixTest; - CERES_DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessSparseMatrix); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/block_sparse_matrix.cc b/extern/ceres/internal/ceres/block_sparse_matrix.cc index 68d0780156c..8f50f3561e2 100644 --- a/extern/ceres/internal/ceres/block_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/block_sparse_matrix.cc @@ -35,6 +35,7 @@ #include <vector> #include "ceres/block_structure.h" #include "ceres/internal/eigen.h" +#include "ceres/random.h" #include "ceres/small_blas.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -51,9 +52,8 @@ BlockSparseMatrix::BlockSparseMatrix( : num_rows_(0), num_cols_(0), num_nonzeros_(0), - values_(NULL), block_structure_(block_structure) { - CHECK_NOTNULL(block_structure_.get()); + CHECK(block_structure_ != nullptr); // Count the number of columns in the matrix. for (int i = 0; i < block_structure_->cols.size(); ++i) { @@ -80,7 +80,8 @@ BlockSparseMatrix::BlockSparseMatrix( VLOG(2) << "Allocating values array with " << num_nonzeros_ * sizeof(double) << " bytes."; // NOLINT values_.reset(new double[num_nonzeros_]); - CHECK_NOTNULL(values_.get()); + max_num_nonzeros_ = num_nonzeros_; + CHECK(values_ != nullptr); } void BlockSparseMatrix::SetZero() { @@ -88,8 +89,8 @@ void BlockSparseMatrix::SetZero() { } void BlockSparseMatrix::RightMultiply(const double* x, double* y) const { - CHECK_NOTNULL(x); - CHECK_NOTNULL(y); + CHECK(x != nullptr); + CHECK(y != nullptr); for (int i = 0; i < block_structure_->rows.size(); ++i) { int row_block_pos = block_structure_->rows[i].block.position; @@ -108,8 +109,8 @@ void BlockSparseMatrix::RightMultiply(const double* x, double* y) const { } void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const { - CHECK_NOTNULL(x); - CHECK_NOTNULL(y); + CHECK(x != nullptr); + CHECK(y != nullptr); for (int i = 0; i < block_structure_->rows.size(); ++i) { int row_block_pos = block_structure_->rows[i].block.position; @@ -128,7 +129,7 @@ void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const { } void BlockSparseMatrix::SquaredColumnNorm(double* x) const { - CHECK_NOTNULL(x); + CHECK(x != nullptr); VectorRef(x, num_cols_).setZero(); for (int i = 0; i < block_structure_->rows.size(); ++i) { int row_block_size = block_structure_->rows[i].block.size; @@ -145,7 +146,7 @@ void BlockSparseMatrix::SquaredColumnNorm(double* x) const { } void BlockSparseMatrix::ScaleColumns(const double* scale) { - CHECK_NOTNULL(scale); + CHECK(scale != nullptr); for (int i = 0; i < block_structure_->rows.size(); ++i) { int row_block_size = block_structure_->rows[i].block.size; @@ -162,7 +163,7 @@ void BlockSparseMatrix::ScaleColumns(const double* scale) { } void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { - CHECK_NOTNULL(dense_matrix); + CHECK(dense_matrix != nullptr); dense_matrix->resize(num_rows_, num_cols_); dense_matrix->setZero(); @@ -185,7 +186,7 @@ void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { void BlockSparseMatrix::ToTripletSparseMatrix( TripletSparseMatrix* matrix) const { - CHECK_NOTNULL(matrix); + CHECK(matrix != nullptr); matrix->Reserve(num_nonzeros_); matrix->Resize(num_rows_, num_cols_); @@ -220,7 +221,7 @@ const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() } void BlockSparseMatrix::ToTextFile(FILE* file) const { - CHECK_NOTNULL(file); + CHECK(file != nullptr); for (int i = 0; i < block_structure_->rows.size(); ++i) { const int row_block_pos = block_structure_->rows[i].block.position; const int row_block_size = block_structure_->rows[i].block.size; @@ -242,5 +243,162 @@ void BlockSparseMatrix::ToTextFile(FILE* file) const { } } +BlockSparseMatrix* BlockSparseMatrix::CreateDiagonalMatrix( + const double* diagonal, const std::vector<Block>& column_blocks) { + // Create the block structure for the diagonal matrix. + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure(); + bs->cols = column_blocks; + int position = 0; + bs->rows.resize(column_blocks.size(), CompressedRow(1)); + for (int i = 0; i < column_blocks.size(); ++i) { + CompressedRow& row = bs->rows[i]; + row.block = column_blocks[i]; + Cell& cell = row.cells[0]; + cell.block_id = i; + cell.position = position; + position += row.block.size * row.block.size; + } + + // Create the BlockSparseMatrix with the given block structure. + BlockSparseMatrix* matrix = new BlockSparseMatrix(bs); + matrix->SetZero(); + + // Fill the values array of the block sparse matrix. + double* values = matrix->mutable_values(); + for (int i = 0; i < column_blocks.size(); ++i) { + const int size = column_blocks[i].size; + for (int j = 0; j < size; ++j) { + // (j + 1) * size is compact way of accessing the (j,j) entry. + values[j * (size + 1)] = diagonal[j]; + } + diagonal += size; + values += size * size; + } + + return matrix; +} + +void BlockSparseMatrix::AppendRows(const BlockSparseMatrix& m) { + CHECK_EQ(m.num_cols(), num_cols()); + const CompressedRowBlockStructure* m_bs = m.block_structure(); + CHECK_EQ(m_bs->cols.size(), block_structure_->cols.size()); + + const int old_num_nonzeros = num_nonzeros_; + const int old_num_row_blocks = block_structure_->rows.size(); + block_structure_->rows.resize(old_num_row_blocks + m_bs->rows.size()); + + for (int i = 0; i < m_bs->rows.size(); ++i) { + const CompressedRow& m_row = m_bs->rows[i]; + CompressedRow& row = block_structure_->rows[old_num_row_blocks + i]; + row.block.size = m_row.block.size; + row.block.position = num_rows_; + num_rows_ += m_row.block.size; + row.cells.resize(m_row.cells.size()); + for (int c = 0; c < m_row.cells.size(); ++c) { + const int block_id = m_row.cells[c].block_id; + row.cells[c].block_id = block_id; + row.cells[c].position = num_nonzeros_; + num_nonzeros_ += m_row.block.size * m_bs->cols[block_id].size; + } + } + + if (num_nonzeros_ > max_num_nonzeros_) { + double* new_values = new double[num_nonzeros_]; + std::copy(values_.get(), values_.get() + old_num_nonzeros, new_values); + values_.reset(new_values); + max_num_nonzeros_ = num_nonzeros_; + } + + std::copy(m.values(), + m.values() + m.num_nonzeros(), + values_.get() + old_num_nonzeros); +} + +void BlockSparseMatrix::DeleteRowBlocks(const int delta_row_blocks) { + const int num_row_blocks = block_structure_->rows.size(); + int delta_num_nonzeros = 0; + int delta_num_rows = 0; + const std::vector<Block>& column_blocks = block_structure_->cols; + for (int i = 0; i < delta_row_blocks; ++i) { + const CompressedRow& row = block_structure_->rows[num_row_blocks - i - 1]; + delta_num_rows += row.block.size; + for (int c = 0; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + delta_num_nonzeros += row.block.size * column_blocks[cell.block_id].size; + } + } + num_nonzeros_ -= delta_num_nonzeros; + num_rows_ -= delta_num_rows; + block_structure_->rows.resize(num_row_blocks - delta_row_blocks); +} + +BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix( + const BlockSparseMatrix::RandomMatrixOptions& options) { + CHECK_GT(options.num_row_blocks, 0); + CHECK_GT(options.min_row_block_size, 0); + CHECK_GT(options.max_row_block_size, 0); + CHECK_LE(options.min_row_block_size, options.max_row_block_size); + CHECK_GT(options.block_density, 0.0); + CHECK_LE(options.block_density, 1.0); + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure(); + if (options.col_blocks.empty()) { + CHECK_GT(options.num_col_blocks, 0); + CHECK_GT(options.min_col_block_size, 0); + CHECK_GT(options.max_col_block_size, 0); + CHECK_LE(options.min_col_block_size, options.max_col_block_size); + + // Generate the col block structure. + int col_block_position = 0; + for (int i = 0; i < options.num_col_blocks; ++i) { + // Generate a random integer in [min_col_block_size, max_col_block_size] + const int delta_block_size = + Uniform(options.max_col_block_size - options.min_col_block_size); + const int col_block_size = options.min_col_block_size + delta_block_size; + bs->cols.push_back(Block(col_block_size, col_block_position)); + col_block_position += col_block_size; + } + } else { + bs->cols = options.col_blocks; + } + + bool matrix_has_blocks = false; + while (!matrix_has_blocks) { + VLOG(1) << "Clearing"; + bs->rows.clear(); + int row_block_position = 0; + int value_position = 0; + for (int r = 0; r < options.num_row_blocks; ++r) { + + const int delta_block_size = + Uniform(options.max_row_block_size - options.min_row_block_size); + const int row_block_size = options.min_row_block_size + delta_block_size; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = row_block_size; + row.block.position = row_block_position; + row_block_position += row_block_size; + for (int c = 0; c < bs->cols.size(); ++c) { + if (RandDouble() > options.block_density) continue; + + row.cells.push_back(Cell()); + Cell& cell = row.cells.back(); + cell.block_id = c; + cell.position = value_position; + value_position += row_block_size * bs->cols[c].size; + matrix_has_blocks = true; + } + } + } + + BlockSparseMatrix* matrix = new BlockSparseMatrix(bs); + double* values = matrix->mutable_values(); + for (int i = 0; i < matrix->num_nonzeros(); ++i) { + values[i] = RandNormal(); + } + + return matrix; +} + } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/block_sparse_matrix.h b/extern/ceres/internal/ceres/block_sparse_matrix.h index 2f9afb738f8..d0c255de612 100644 --- a/extern/ceres/internal/ceres/block_sparse_matrix.h +++ b/extern/ceres/internal/ceres/block_sparse_matrix.h @@ -34,11 +34,10 @@ #ifndef CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ #define CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ +#include <memory> #include "ceres/block_structure.h" #include "ceres/sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/macros.h" -#include "ceres/internal/scoped_ptr.h" namespace ceres { namespace internal { @@ -64,34 +63,99 @@ class BlockSparseMatrix : public SparseMatrix { explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); BlockSparseMatrix(); + BlockSparseMatrix(const BlockSparseMatrix&) = delete; + void operator=(const BlockSparseMatrix&) = delete; + virtual ~BlockSparseMatrix(); // Implementation of SparseMatrix interface. - virtual void SetZero(); - virtual void RightMultiply(const double* x, double* y) const; - virtual void LeftMultiply(const double* x, double* y) const; - virtual void SquaredColumnNorm(double* x) const; - virtual void ScaleColumns(const double* scale); - virtual void ToDenseMatrix(Matrix* dense_matrix) const; - 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 num_nonzeros_; } - virtual const double* values() const { return values_.get(); } - virtual double* mutable_values() { return values_.get(); } + void SetZero() final; + void RightMultiply(const double* x, double* y) const final; + void LeftMultiply(const double* x, double* y) const final; + void SquaredColumnNorm(double* x) const final; + void ScaleColumns(const double* scale) final; + void ToDenseMatrix(Matrix* dense_matrix) const final; + void ToTextFile(FILE* file) const final; + + int num_rows() const final { return num_rows_; } + int num_cols() const final { return num_cols_; } + int num_nonzeros() const final { return num_nonzeros_; } + const double* values() const final { return values_.get(); } + double* mutable_values() final { return values_.get(); } void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; const CompressedRowBlockStructure* block_structure() const; + // Append the contents of m to the bottom of this matrix. m must + // have the same column blocks structure as this matrix. + void AppendRows(const BlockSparseMatrix& m); + + // Delete the bottom delta_rows_blocks. + void DeleteRowBlocks(int delta_row_blocks); + + static BlockSparseMatrix* CreateDiagonalMatrix( + const double* diagonal, + const std::vector<Block>& column_blocks); + + struct RandomMatrixOptions { + int num_row_blocks = 0; + int min_row_block_size = 0; + int max_row_block_size = 0; + int num_col_blocks = 0; + int min_col_block_size = 0; + int max_col_block_size = 0; + + // 0 < block_density <= 1 is the probability of a block being + // present in the matrix. A given random matrix will not have + // precisely this density. + double block_density = 0.0; + + // If col_blocks is non-empty, then the generated random matrix + // has this block structure and the column related options in this + // struct are ignored. + std::vector<Block> col_blocks; + }; + + // Create a random BlockSparseMatrix whose entries are normally + // distributed and whose structure is determined by + // RandomMatrixOptions. + // + // Caller owns the result. + static BlockSparseMatrix* CreateRandomMatrix( + const RandomMatrixOptions& options); + private: int num_rows_; int num_cols_; - int max_num_nonzeros_; int num_nonzeros_; - scoped_array<double> values_; - scoped_ptr<CompressedRowBlockStructure> block_structure_; - CERES_DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrix); + int max_num_nonzeros_; + std::unique_ptr<double[]> values_; + std::unique_ptr<CompressedRowBlockStructure> block_structure_; +}; + +// A number of algorithms like the SchurEliminator do not need +// access to the full BlockSparseMatrix interface. They only +// need read only access to the values array and the block structure. +// +// BlockSparseDataMatrix a struct that carries these two bits of +// information +class BlockSparseMatrixData { + public: + BlockSparseMatrixData(const BlockSparseMatrix& m) + : block_structure_(m.block_structure()), values_(m.values()){}; + + BlockSparseMatrixData(const CompressedRowBlockStructure* block_structure, + const double* values) + : block_structure_(block_structure), values_(values) {} + + const CompressedRowBlockStructure* block_structure() const { + return block_structure_; + } + const double* values() const { return values_; } + + private: + const CompressedRowBlockStructure* block_structure_; + const double* values_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/block_structure.h b/extern/ceres/internal/ceres/block_structure.h index 6e7003addb6..b5218c0c2cc 100644 --- a/extern/ceres/internal/ceres/block_structure.h +++ b/extern/ceres/internal/ceres/block_structure.h @@ -38,14 +38,14 @@ #ifndef CERES_INTERNAL_BLOCK_STRUCTURE_H_ #define CERES_INTERNAL_BLOCK_STRUCTURE_H_ +#include <cstdint> #include <vector> #include "ceres/internal/port.h" -#include "ceres/types.h" namespace ceres { namespace internal { -typedef int32 BlockSize; +typedef int32_t BlockSize; struct Block { Block() : size(-1), position(-1) {} @@ -70,6 +70,11 @@ struct Cell { bool CellLessThan(const Cell& lhs, const Cell& rhs); struct CompressedList { + CompressedList() {} + + // Construct a CompressedList with the cells containing num_cells + // entries. + CompressedList(int num_cells) : cells(num_cells) {} Block block; std::vector<Cell> cells; }; diff --git a/extern/ceres/internal/ceres/c_api.cc b/extern/ceres/internal/ceres/c_api.cc index ada8f3e0013..2244909131a 100644 --- a/extern/ceres/internal/ceres/c_api.cc +++ b/extern/ceres/internal/ceres/c_api.cc @@ -80,9 +80,9 @@ class CallbackCostFunction : public ceres::CostFunction { virtual ~CallbackCostFunction() {} - virtual bool Evaluate(double const* const* parameters, + bool Evaluate(double const* const* parameters, double* residuals, - double** jacobians) const { + double** jacobians) const final { return (*cost_function_)(user_data_, const_cast<double**>(parameters), residuals, @@ -101,7 +101,7 @@ class CallbackLossFunction : public ceres::LossFunction { 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 { + void Evaluate(double sq_norm, double* rho) const final { (*loss_function_)(user_data_, sq_norm, rho); } diff --git a/extern/ceres/internal/ceres/callbacks.cc b/extern/ceres/internal/ceres/callbacks.cc index 50a0ec19924..84576e40e7d 100644 --- a/extern/ceres/internal/ceres/callbacks.cc +++ b/extern/ceres/internal/ceres/callbacks.cc @@ -47,9 +47,29 @@ StateUpdatingCallback::~StateUpdatingCallback() {} CallbackReturnType StateUpdatingCallback::operator()( const IterationSummary& summary) { + program_->StateVectorToParameterBlocks(parameters_); + program_->CopyParameterBlockStateToUserState(); + return SOLVER_CONTINUE; +} + +GradientProblemSolverStateUpdatingCallback:: + GradientProblemSolverStateUpdatingCallback( + int num_parameters, + const double* internal_parameters, + double* user_parameters) + : num_parameters_(num_parameters), + internal_parameters_(internal_parameters), + user_parameters_(user_parameters) {} + +GradientProblemSolverStateUpdatingCallback:: + ~GradientProblemSolverStateUpdatingCallback() {} + +CallbackReturnType GradientProblemSolverStateUpdatingCallback::operator()( + const IterationSummary& summary) { if (summary.step_is_successful) { - program_->StateVectorToParameterBlocks(parameters_); - program_->CopyParameterBlockStateToUserState(); + std::copy(internal_parameters_, + internal_parameters_ + num_parameters_, + user_parameters_); } return SOLVER_CONTINUE; } diff --git a/extern/ceres/internal/ceres/callbacks.h b/extern/ceres/internal/ceres/callbacks.h index 33c66df5c11..d68bf7f6301 100644 --- a/extern/ceres/internal/ceres/callbacks.h +++ b/extern/ceres/internal/ceres/callbacks.h @@ -46,19 +46,34 @@ class StateUpdatingCallback : public IterationCallback { public: StateUpdatingCallback(Program* program, double* parameters); virtual ~StateUpdatingCallback(); - virtual CallbackReturnType operator()(const IterationSummary& summary); + CallbackReturnType operator()(const IterationSummary& summary) final; private: Program* program_; double* parameters_; }; +// Callback for updating the externally visible state of the +// parameters vector for GradientProblemSolver. +class GradientProblemSolverStateUpdatingCallback : public IterationCallback { + public: + GradientProblemSolverStateUpdatingCallback(int num_parameters, + const double* internal_parameters, + double* user_parameters); + virtual ~GradientProblemSolverStateUpdatingCallback(); + CallbackReturnType operator()(const IterationSummary& summary) final; + private: + int num_parameters_; + const double* internal_parameters_; + double* user_parameters_; +}; + // Callback for logging the state of the minimizer to STDERR or // STDOUT depending on the user's preferences and logging level. class LoggingCallback : public IterationCallback { public: LoggingCallback(MinimizerType minimizer_type, bool log_to_stdout); virtual ~LoggingCallback(); - virtual CallbackReturnType operator()(const IterationSummary& summary); + CallbackReturnType operator()(const IterationSummary& summary) final; private: const MinimizerType minimizer_type; diff --git a/extern/ceres/internal/ceres/canonical_views_clustering.cc b/extern/ceres/internal/ceres/canonical_views_clustering.cc new file mode 100644 index 00000000000..e927e1f91be --- /dev/null +++ b/extern/ceres/internal/ceres/canonical_views_clustering.cc @@ -0,0 +1,232 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: David Gallup (dgallup@google.com) +// Sameer Agarwal (sameeragarwal@google.com) + +#include "ceres/canonical_views_clustering.h" + +#include <unordered_set> +#include <unordered_map> + +#include "ceres/graph.h" +#include "ceres/map_util.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::vector; + +typedef std::unordered_map<int, int> IntMap; +typedef std::unordered_set<int> IntSet; + +class CanonicalViewsClustering { + public: + CanonicalViewsClustering() {} + + // Compute the canonical views clustering of the vertices of the + // graph. centers will contain the vertices that are the identified + // as the canonical views/cluster centers, and membership is a map + // from vertices to cluster_ids. The i^th cluster center corresponds + // to the i^th cluster. It is possible depending on the + // configuration of the clustering algorithm that some of the + // vertices may not be assigned to any cluster. In this case they + // are assigned to a cluster with id = kInvalidClusterId. + void ComputeClustering(const CanonicalViewsClusteringOptions& options, + const WeightedGraph<int>& graph, + vector<int>* centers, + IntMap* membership); + + private: + void FindValidViews(IntSet* valid_views) const; + double ComputeClusteringQualityDifference(const int candidate, + const vector<int>& centers) const; + void UpdateCanonicalViewAssignments(const int canonical_view); + void ComputeClusterMembership(const vector<int>& centers, + IntMap* membership) const; + + CanonicalViewsClusteringOptions options_; + const WeightedGraph<int>* graph_; + // Maps a view to its representative canonical view (its cluster + // center). + IntMap view_to_canonical_view_; + // Maps a view to its similarity to its current cluster center. + std::unordered_map<int, double> view_to_canonical_view_similarity_; +}; + +void ComputeCanonicalViewsClustering( + const CanonicalViewsClusteringOptions& options, + const WeightedGraph<int>& graph, + vector<int>* centers, + IntMap* membership) { + time_t start_time = time(NULL); + CanonicalViewsClustering cv; + cv.ComputeClustering(options, graph, centers, membership); + VLOG(2) << "Canonical views clustering time (secs): " + << time(NULL) - start_time; +} + +// Implementation of CanonicalViewsClustering +void CanonicalViewsClustering::ComputeClustering( + const CanonicalViewsClusteringOptions& options, + const WeightedGraph<int>& graph, + vector<int>* centers, + IntMap* membership) { + options_ = options; + CHECK(centers != nullptr); + CHECK(membership != nullptr); + centers->clear(); + membership->clear(); + graph_ = &graph; + + IntSet valid_views; + FindValidViews(&valid_views); + while (valid_views.size() > 0) { + // Find the next best canonical view. + double best_difference = -std::numeric_limits<double>::max(); + int best_view = 0; + + // TODO(sameeragarwal): Make this loop multi-threaded. + for (const auto& view : valid_views) { + const double difference = + ComputeClusteringQualityDifference(view, *centers); + if (difference > best_difference) { + best_difference = difference; + best_view = view; + } + } + + CHECK_GT(best_difference, -std::numeric_limits<double>::max()); + + // Add canonical view if quality improves, or if minimum is not + // yet met, otherwise break. + if ((best_difference <= 0) && + (centers->size() >= options_.min_views)) { + break; + } + + centers->push_back(best_view); + valid_views.erase(best_view); + UpdateCanonicalViewAssignments(best_view); + } + + ComputeClusterMembership(*centers, membership); +} + +// Return the set of vertices of the graph which have valid vertex +// weights. +void CanonicalViewsClustering::FindValidViews( + IntSet* valid_views) const { + const IntSet& views = graph_->vertices(); + for (const auto& view : views) { + if (graph_->VertexWeight(view) != WeightedGraph<int>::InvalidWeight()) { + valid_views->insert(view); + } + } +} + +// Computes the difference in the quality score if 'candidate' were +// added to the set of canonical views. +double CanonicalViewsClustering::ComputeClusteringQualityDifference( + const int candidate, + const vector<int>& centers) const { + // View score. + double difference = + options_.view_score_weight * graph_->VertexWeight(candidate); + + // Compute how much the quality score changes if the candidate view + // was added to the list of canonical views and its nearest + // neighbors became members of its cluster. + const IntSet& neighbors = graph_->Neighbors(candidate); + for (const auto& neighbor : neighbors) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, neighbor, 0.0); + const double new_similarity = graph_->EdgeWeight(neighbor, candidate); + if (new_similarity > old_similarity) { + difference += new_similarity - old_similarity; + } + } + + // Number of views penalty. + difference -= options_.size_penalty_weight; + + // Orthogonality. + for (int i = 0; i < centers.size(); ++i) { + difference -= options_.similarity_penalty_weight * + graph_->EdgeWeight(centers[i], candidate); + } + + return difference; +} + +// Reassign views if they're more similar to the new canonical view. +void CanonicalViewsClustering::UpdateCanonicalViewAssignments( + const int canonical_view) { + const IntSet& neighbors = graph_->Neighbors(canonical_view); + for (const auto& neighbor : neighbors) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, neighbor, 0.0); + const double new_similarity = + graph_->EdgeWeight(neighbor, canonical_view); + if (new_similarity > old_similarity) { + view_to_canonical_view_[neighbor] = canonical_view; + view_to_canonical_view_similarity_[neighbor] = new_similarity; + } + } +} + +// Assign a cluster id to each view. +void CanonicalViewsClustering::ComputeClusterMembership( + const vector<int>& centers, + IntMap* membership) const { + CHECK(membership != nullptr); + membership->clear(); + + // The i^th cluster has cluster id i. + IntMap center_to_cluster_id; + for (int i = 0; i < centers.size(); ++i) { + center_to_cluster_id[centers[i]] = i; + } + + static constexpr int kInvalidClusterId = -1; + + const IntSet& views = graph_->vertices(); + for (const auto& view : views) { + auto it = view_to_canonical_view_.find(view); + int cluster_id = kInvalidClusterId; + if (it != view_to_canonical_view_.end()) { + cluster_id = FindOrDie(center_to_cluster_id, it->second); + } + + InsertOrDie(membership, view, cluster_id); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/canonical_views_clustering.h b/extern/ceres/internal/ceres/canonical_views_clustering.h new file mode 100644 index 00000000000..630adfed717 --- /dev/null +++ b/extern/ceres/internal/ceres/canonical_views_clustering.h @@ -0,0 +1,124 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// An implementation of the Canonical Views clustering algorithm from +// "Scene Summarization for Online Image Collections", Ian Simon, Noah +// Snavely, Steven M. Seitz, ICCV 2007. +// +// More details can be found at +// http://grail.cs.washington.edu/projects/canonview/ +// +// Ceres uses this algorithm to perform view clustering for +// constructing visibility based preconditioners. + +#ifndef CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ +#define CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ + +#include <unordered_map> +#include <vector> + +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +struct CanonicalViewsClusteringOptions; + +// Compute a partitioning of the vertices of the graph using the +// canonical views clustering algorithm. +// +// In the following we will use the terms vertices and views +// interchangeably. Given a weighted Graph G(V,E), the canonical views +// of G are the set of vertices that best "summarize" the content +// of the graph. If w_ij i s the weight connecting the vertex i to +// vertex j, and C is the set of canonical views. Then the objective +// of the canonical views algorithm is +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// +// alpha is the size penalty that penalizes large number of canonical +// views. +// +// beta is the similarity penalty that penalizes canonical views that +// are too similar to other canonical views. +// +// Thus the canonical views algorithm tries to find a canonical view +// for each vertex in the graph which best explains it, while trying +// to minimize the number of canonical views and the overlap between +// them. +// +// We further augment the above objective function by allowing for per +// vertex weights, higher weights indicating a higher preference for +// being chosen as a canonical view. Thus if w_i is the vertex weight +// for vertex i, the objective function is then +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// + view_score_weight * sum_[i in C] w_i +// +// centers will contain the vertices that are the identified +// as the canonical views/cluster centers, and membership is a map +// from vertices to cluster_ids. The i^th cluster center corresponds +// to the i^th cluster. +// +// It is possible depending on the configuration of the clustering +// algorithm that some of the vertices may not be assigned to any +// cluster. In this case they are assigned to a cluster with id = -1; +void ComputeCanonicalViewsClustering( + const CanonicalViewsClusteringOptions& options, + const WeightedGraph<int>& graph, + std::vector<int>* centers, + std::unordered_map<int, int>* membership); + +struct CanonicalViewsClusteringOptions { + // The minimum number of canonical views to compute. + int min_views = 3; + + // Penalty weight for the number of canonical views. A higher + // number will result in fewer canonical views. + double size_penalty_weight = 5.75; + + // Penalty weight for the diversity (orthogonality) of the + // canonical views. A higher number will encourage less similar + // canonical views. + double similarity_penalty_weight = 100; + + // Weight for per-view scores. Lower weight places less + // confidence in the view scores. + double view_score_weight = 0.0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ diff --git a/extern/ceres/internal/ceres/cgnr_linear_operator.h b/extern/ceres/internal/ceres/cgnr_linear_operator.h index 44c07cabd01..8e8febcc934 100644 --- a/extern/ceres/internal/ceres/cgnr_linear_operator.h +++ b/extern/ceres/internal/ceres/cgnr_linear_operator.h @@ -32,8 +32,8 @@ #define CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ #include <algorithm> +#include <memory> #include "ceres/linear_operator.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/internal/eigen.h" namespace ceres { @@ -84,7 +84,7 @@ class CgnrLinearOperator : public LinearOperator { } virtual ~CgnrLinearOperator() {} - virtual void RightMultiply(const double* x, double* y) const { + void RightMultiply(const double* x, double* y) const final { std::fill(z_.get(), z_.get() + A_.num_rows(), 0.0); // z = Ax @@ -101,17 +101,17 @@ class CgnrLinearOperator : public LinearOperator { } } - virtual void LeftMultiply(const double* x, double* y) const { + void LeftMultiply(const double* x, double* y) const final { RightMultiply(x, y); } - virtual int num_rows() const { return A_.num_cols(); } - virtual int num_cols() const { return A_.num_cols(); } + int num_rows() const final { return A_.num_cols(); } + int num_cols() const final { return A_.num_cols(); } private: const LinearOperator& A_; const double* D_; - scoped_array<double> z_; + std::unique_ptr<double[]> z_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/cgnr_solver.cc b/extern/ceres/internal/ceres/cgnr_solver.cc index 61fae758d5b..9dba1cfb4a8 100644 --- a/extern/ceres/internal/ceres/cgnr_solver.cc +++ b/extern/ceres/internal/ceres/cgnr_solver.cc @@ -35,6 +35,7 @@ #include "ceres/conjugate_gradients_solver.h" #include "ceres/internal/eigen.h" #include "ceres/linear_solver.h" +#include "ceres/subset_preconditioner.h" #include "ceres/wall_time.h" #include "glog/logging.h" @@ -42,14 +43,19 @@ namespace ceres { namespace internal { CgnrSolver::CgnrSolver(const LinearSolver::Options& options) - : options_(options), - preconditioner_(NULL) { + : options_(options) { if (options_.preconditioner_type != JACOBI && - options_.preconditioner_type != IDENTITY) { - LOG(FATAL) << "CGNR only supports IDENTITY and JACOBI preconditioners."; + options_.preconditioner_type != IDENTITY && + options_.preconditioner_type != SUBSET) { + LOG(FATAL) + << "Preconditioner = " + << PreconditionerTypeToString(options_.preconditioner_type) << ". " + << "Congratulations, you found a bug in Ceres. Please report it."; } } +CgnrSolver::~CgnrSolver() {} + LinearSolver::Summary CgnrSolver::SolveImpl( BlockSparseMatrix* A, const double* b, @@ -62,16 +68,31 @@ LinearSolver::Summary CgnrSolver::SolveImpl( z.setZero(); A->LeftMultiply(b, z.data()); - // Precondition if necessary. - LinearSolver::PerSolveOptions cg_per_solve_options = per_solve_options; - if (options_.preconditioner_type == JACOBI) { - if (preconditioner_.get() == NULL) { + if (!preconditioner_) { + if (options_.preconditioner_type == JACOBI) { preconditioner_.reset(new BlockJacobiPreconditioner(*A)); + } else if (options_.preconditioner_type == SUBSET) { + Preconditioner::Options preconditioner_options; + preconditioner_options.type = SUBSET; + preconditioner_options.subset_preconditioner_start_row_block = + options_.subset_preconditioner_start_row_block; + preconditioner_options.sparse_linear_algebra_library_type = + options_.sparse_linear_algebra_library_type; + preconditioner_options.use_postordering = options_.use_postordering; + preconditioner_options.num_threads = options_.num_threads; + preconditioner_options.context = options_.context; + preconditioner_.reset( + new SubsetPreconditioner(preconditioner_options, *A)); } + } + + if (preconditioner_) { preconditioner_->Update(*A, per_solve_options.D); - cg_per_solve_options.preconditioner = preconditioner_.get(); } + LinearSolver::PerSolveOptions cg_per_solve_options = per_solve_options; + cg_per_solve_options.preconditioner = preconditioner_.get(); + // Solve (AtA + DtD)x = z (= Atb). VectorRef(x, A->num_cols()).setZero(); CgnrLinearOperator lhs(*A, per_solve_options.D); diff --git a/extern/ceres/internal/ceres/cgnr_solver.h b/extern/ceres/internal/ceres/cgnr_solver.h index f7a15736925..52927333daa 100644 --- a/extern/ceres/internal/ceres/cgnr_solver.h +++ b/extern/ceres/internal/ceres/cgnr_solver.h @@ -31,7 +31,7 @@ #ifndef CERES_INTERNAL_CGNR_SOLVER_H_ #define CERES_INTERNAL_CGNR_SOLVER_H_ -#include "ceres/internal/scoped_ptr.h" +#include <memory> #include "ceres/linear_solver.h" namespace ceres { @@ -51,16 +51,19 @@ class BlockJacobiPreconditioner; class CgnrSolver : public BlockSparseMatrixSolver { public: explicit CgnrSolver(const LinearSolver::Options& options); - virtual Summary SolveImpl( + CgnrSolver(const CgnrSolver&) = delete; + void operator=(const CgnrSolver&) = delete; + virtual ~CgnrSolver(); + + Summary SolveImpl( BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, - double* x); + double* x) final; private: const LinearSolver::Options options_; - scoped_ptr<Preconditioner> preconditioner_; - CERES_DISALLOW_COPY_AND_ASSIGN(CgnrSolver); + std::unique_ptr<Preconditioner> preconditioner_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/collections_port.h b/extern/ceres/internal/ceres/collections_port.h deleted file mode 100644 index e699a661b8b..00000000000 --- a/extern/ceres/internal/ceres/collections_port.h +++ /dev/null @@ -1,196 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// 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) -// -// Portable HashMap and HashSet, and a specialized overload for hashing pairs. - -#ifndef CERES_INTERNAL_COLLECTIONS_PORT_H_ -#define CERES_INTERNAL_COLLECTIONS_PORT_H_ - -#include "ceres/internal/port.h" - -#if defined(CERES_NO_UNORDERED_MAP) -# include <map> -# include <set> -#endif - -#if defined(CERES_TR1_UNORDERED_MAP) -# include <tr1/unordered_map> -# include <tr1/unordered_set> -# define CERES_HASH_NAMESPACE_START namespace std { namespace tr1 { -# define CERES_HASH_NAMESPACE_END } } -#endif - -#if defined(CERES_STD_UNORDERED_MAP) -# include <unordered_map> -# include <unordered_set> -# define CERES_HASH_NAMESPACE_START namespace std { -# define CERES_HASH_NAMESPACE_END } -#endif - -#if defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) -# include <unordered_map> -# include <unordered_set> -# define CERES_HASH_NAMESPACE_START namespace std { namespace tr1 { -# define CERES_HASH_NAMESPACE_END } } -#endif - -#if !defined(CERES_NO_UNORDERED_MAP) && !defined(CERES_TR1_UNORDERED_MAP) && \ - !defined(CERES_STD_UNORDERED_MAP) && !defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) // NOLINT -# error One of: CERES_NO_UNORDERED_MAP, CERES_TR1_UNORDERED_MAP,\ - CERES_STD_UNORDERED_MAP, CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE must be defined! // NOLINT -#endif - -#include <utility> -#include "ceres/integral_types.h" - -// Some systems don't have access to unordered_map/unordered_set. In -// that case, substitute the hash map/set with normal map/set. The -// price to pay is slower speed for some operations. -#if defined(CERES_NO_UNORDERED_MAP) - -namespace ceres { -namespace internal { - -template<typename K, typename V> -struct HashMap : map<K, V> {}; - -template<typename K> -struct HashSet : set<K> {}; - -} // namespace internal -} // namespace ceres - -#else - -namespace ceres { -namespace internal { - -#if defined(CERES_TR1_UNORDERED_MAP) || \ - defined(CERES_STD_UNORDERED_MAP_IN_TR1_NAMESPACE) -template<typename K, typename V> -struct HashMap : std::tr1::unordered_map<K, V> {}; -template<typename K> -struct HashSet : std::tr1::unordered_set<K> {}; -#endif - -#if defined(CERES_STD_UNORDERED_MAP) -template<typename K, typename V> -struct HashMap : std::unordered_map<K, V> {}; -template<typename K> -struct HashSet : std::unordered_set<K> {}; -#endif - -#if defined(_WIN32) && !defined(__MINGW64__) && !defined(__MINGW32__) -#define GG_LONGLONG(x) x##I64 -#define GG_ULONGLONG(x) x##UI64 -#else -#define GG_LONGLONG(x) x##LL -#define GG_ULONGLONG(x) x##ULL -#endif - -// The hash function is due to Bob Jenkins (see -// http://burtleburtle.net/bob/hash/index.html). Each mix takes 36 instructions, -// in 18 cycles if you're lucky. On x86 architectures, this requires 45 -// instructions in 27 cycles, if you're lucky. -// -// 32bit version -inline void hash_mix(uint32& a, uint32& b, uint32& c) { - a -= b; a -= c; a ^= (c>>13); - b -= c; b -= a; b ^= (a<<8); - c -= a; c -= b; c ^= (b>>13); - a -= b; a -= c; a ^= (c>>12); - b -= c; b -= a; b ^= (a<<16); - c -= a; c -= b; c ^= (b>>5); - a -= b; a -= c; a ^= (c>>3); - b -= c; b -= a; b ^= (a<<10); - c -= a; c -= b; c ^= (b>>15); -} - -// 64bit version -inline void hash_mix(uint64& a, uint64& b, uint64& c) { - a -= b; a -= c; a ^= (c>>43); - b -= c; b -= a; b ^= (a<<9); - c -= a; c -= b; c ^= (b>>8); - a -= b; a -= c; a ^= (c>>38); - b -= c; b -= a; b ^= (a<<23); - c -= a; c -= b; c ^= (b>>5); - a -= b; a -= c; a ^= (c>>35); - b -= c; b -= a; b ^= (a<<49); - c -= a; c -= b; c ^= (b>>11); -} - -inline uint32 Hash32NumWithSeed(uint32 num, uint32 c) { - // The golden ratio; an arbitrary value. - uint32 b = 0x9e3779b9UL; - hash_mix(num, b, c); - return c; -} - -inline uint64 Hash64NumWithSeed(uint64 num, uint64 c) { - // More of the golden ratio. - uint64 b = GG_ULONGLONG(0xe08c1d668b756f82); - hash_mix(num, b, c); - return c; -} - -} // namespace internal -} // namespace ceres - -// Since on some platforms this is a doubly-nested namespace (std::tr1) and -// others it is not, the entire namespace line must be in a macro. -CERES_HASH_NAMESPACE_START - -// The outrageously annoying specializations below are for portability reasons. -// In short, it's not possible to have two overloads of hash<pair<T1, T2> - -// Hasher for STL pairs. Requires hashers for both members to be defined. -template<typename T> -struct hash<pair<T, T> > { - size_t operator()(const pair<T, T>& p) const { - size_t h1 = hash<T>()(p.first); - size_t h2 = hash<T>()(p.second); - // The decision below is at compile time - return (sizeof(h1) <= sizeof(ceres::internal::uint32)) ? - ceres::internal::Hash32NumWithSeed(h1, h2) : - ceres::internal::Hash64NumWithSeed(h1, h2); - } - // Less than operator for MSVC. - bool operator()(const pair<T, T>& a, - const pair<T, T>& b) const { - return a < b; - } - static const size_t bucket_size = 4; // These are required by MSVC - static const size_t min_buckets = 8; // 4 and 8 are defaults. -}; - -CERES_HASH_NAMESPACE_END - -#endif // CERES_NO_UNORDERED_MAP -#endif // CERES_INTERNAL_COLLECTIONS_PORT_H_ diff --git a/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc index ebb2a62c544..3f6672f858c 100644 --- a/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc +++ b/extern/ceres/internal/ceres/compressed_col_sparse_matrix_utils.cc @@ -47,8 +47,10 @@ void CompressedColumnScalarMatrixToBlockMatrix( const vector<int>& col_blocks, vector<int>* block_rows, vector<int>* block_cols) { - CHECK_NOTNULL(block_rows)->clear(); - CHECK_NOTNULL(block_cols)->clear(); + CHECK(block_rows != nullptr); + CHECK(block_cols != nullptr); + block_rows->clear(); + block_cols->clear(); const int num_row_blocks = row_blocks.size(); const int num_col_blocks = col_blocks.size(); diff --git a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc index 40977b74c67..1fc0116815c 100644 --- a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc +++ b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.cc @@ -30,6 +30,7 @@ #include "ceres/compressed_row_jacobian_writer.h" +#include <iterator> #include <utility> #include <vector> @@ -70,7 +71,7 @@ void CompressedRowJacobianWriter::PopulateJacobianRowAndColumnBlockVectors( void CompressedRowJacobianWriter::GetOrderedParameterBlocks( const Program* program, int residual_id, - vector<pair<int, int> >* evaluated_jacobian_blocks) { + vector<pair<int, int>>* evaluated_jacobian_blocks) { const ResidualBlock* residual_block = program->residual_blocks()[residual_id]; const int num_parameter_blocks = residual_block->NumParameterBlocks(); @@ -121,6 +122,7 @@ SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { // seems to be the only way to construct it without doing a memory copy. int* rows = jacobian->mutable_rows(); int* cols = jacobian->mutable_cols(); + int row_pos = 0; rows[0] = 0; for (int i = 0; i < residual_blocks.size(); ++i) { @@ -206,7 +208,7 @@ void CompressedRowJacobianWriter::Write(int residual_id, program_->residual_blocks()[residual_id]; const int num_residuals = residual_block->NumResiduals(); - vector<pair<int, int> > evaluated_jacobian_blocks; + vector<pair<int, int>> evaluated_jacobian_blocks; GetOrderedParameterBlocks(program_, residual_id, &evaluated_jacobian_blocks); // Where in the current row does the jacobian for a parameter block begin. diff --git a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h index 1cd01235ccf..9fb414e2519 100644 --- a/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h +++ b/extern/ceres/internal/ceres/compressed_row_jacobian_writer.h @@ -83,7 +83,7 @@ class CompressedRowJacobianWriter { static void GetOrderedParameterBlocks( const Program* program, int residual_id, - std::vector<std::pair<int, int> >* evaluated_jacobian_blocks); + std::vector<std::pair<int, int>>* evaluated_jacobian_blocks); // JacobianWriter interface. diff --git a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc index 91d18bbd604..e56de16bf92 100644 --- a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -35,6 +35,7 @@ #include <vector> #include "ceres/crs_matrix.h" #include "ceres/internal/port.h" +#include "ceres/random.h" #include "ceres/triplet_sparse_matrix.h" #include "glog/logging.h" @@ -54,9 +55,7 @@ namespace { // // If this is the case, this functor will not be a StrictWeakOrdering. struct RowColLessThan { - RowColLessThan(const int* rows, const int* cols) - : rows(rows), cols(cols) { - } + RowColLessThan(const int* rows, const int* cols) : rows(rows), cols(cols) {} bool operator()(const int x, const int y) const { if (rows[x] == rows[y]) { @@ -69,6 +68,91 @@ struct RowColLessThan { const int* cols; }; +void TransposeForCompressedRowSparseStructure(const int num_rows, + const int num_cols, + const int num_nonzeros, + const int* rows, + const int* cols, + const double* values, + int* transpose_rows, + int* transpose_cols, + double* transpose_values) { + // Explicitly zero out transpose_rows. + std::fill(transpose_rows, transpose_rows + num_cols + 1, 0); + + // Count the number of entries in each column of the original matrix + // and assign to transpose_rows[col + 1]. + for (int idx = 0; idx < num_nonzeros; ++idx) { + ++transpose_rows[cols[idx] + 1]; + } + + // Compute the starting position for each row in the transpose by + // computing the cumulative sum of the entries of transpose_rows. + for (int i = 1; i < num_cols + 1; ++i) { + transpose_rows[i] += transpose_rows[i - 1]; + } + + // Populate transpose_cols and (optionally) transpose_values by + // walking the entries of the source matrices. For each entry that + // is added, the value of transpose_row is incremented allowing us + // to keep track of where the next entry for that row should go. + // + // As a result transpose_row is shifted to the left by one entry. + 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; + if (values != NULL && transpose_values != NULL) { + transpose_values[transpose_idx] = values[idx]; + } + } + } + + // This loop undoes the left shift to transpose_rows introduced by + // the previous loop. + for (int i = num_cols - 1; i > 0; --i) { + transpose_rows[i] = transpose_rows[i - 1]; + } + transpose_rows[0] = 0; +} + +void AddRandomBlock(const int num_rows, + const int num_cols, + const int row_block_begin, + const int col_block_begin, + std::vector<int>* rows, + std::vector<int>* cols, + std::vector<double>* values) { + for (int r = 0; r < num_rows; ++r) { + for (int c = 0; c < num_cols; ++c) { + rows->push_back(row_block_begin + r); + cols->push_back(col_block_begin + c); + values->push_back(RandNormal()); + } + } +} + +void AddSymmetricRandomBlock(const int num_rows, + const int row_block_begin, + std::vector<int>* rows, + std::vector<int>* cols, + std::vector<double>* values) { + for (int r = 0; r < num_rows; ++r) { + for (int c = r; c < num_rows; ++c) { + const double v = RandNormal(); + rows->push_back(row_block_begin + r); + cols->push_back(row_block_begin + c); + values->push_back(v); + if (r != c) { + rows->push_back(row_block_begin + c); + cols->push_back(row_block_begin + r); + values->push_back(v); + } + } + } +} + } // namespace // This constructor gives you a semi-initialized CompressedRowSparseMatrix. @@ -77,69 +161,96 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, int max_num_nonzeros) { num_rows_ = num_rows; num_cols_ = num_cols; + storage_type_ = UNSYMMETRIC; 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: " << cols_.size() << ". Allocating " + << (num_rows_ + 1) * sizeof(int) + // NOLINT + cols_.size() * sizeof(int) + // NOLINT + cols_.size() * sizeof(double); // NOLINT +} + +CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix( + const TripletSparseMatrix& input) { + return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, false); +} - 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::FromTripletSparseMatrixTransposed( + const TripletSparseMatrix& input) { + return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, true); } -CompressedRowSparseMatrix::CompressedRowSparseMatrix( - const TripletSparseMatrix& m) { - num_rows_ = m.num_rows(); - num_cols_ = m.num_cols(); +CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix( + const TripletSparseMatrix& input, bool transpose) { + int num_rows = input.num_rows(); + int num_cols = input.num_cols(); + const int* rows = input.rows(); + const int* cols = input.cols(); + const double* values = input.values(); - rows_.resize(num_rows_ + 1, 0); - cols_.resize(m.num_nonzeros(), 0); - values_.resize(m.max_num_nonzeros(), 0.0); + if (transpose) { + std::swap(num_rows, num_cols); + std::swap(rows, cols); + } - // index is the list of indices into the TripletSparseMatrix m. - vector<int> index(m.num_nonzeros(), 0); - for (int i = 0; i < m.num_nonzeros(); ++i) { + // index is the list of indices into the TripletSparseMatrix input. + vector<int> index(input.num_nonzeros(), 0); + for (int i = 0; i < input.num_nonzeros(); ++i) { index[i] = i; } // Sort index such that the entries of m are ordered by row and ties // are broken by column. - sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); + std::sort(index.begin(), index.end(), RowColLessThan(rows, cols)); + + VLOG(1) << "# of rows: " << num_rows << " # of columns: " << num_cols + << " num_nonzeros: " << input.num_nonzeros() << ". Allocating " + << ((num_rows + 1) * sizeof(int) + // NOLINT + input.num_nonzeros() * sizeof(int) + // NOLINT + input.num_nonzeros() * sizeof(double)); // NOLINT + + CompressedRowSparseMatrix* output = + new CompressedRowSparseMatrix(num_rows, num_cols, input.num_nonzeros()); - 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 + if (num_rows == 0) { + // No data to copy. + return output; + } // Copy the contents of the cols and values array in the order given // by index and count the number of entries in each row. - for (int i = 0; i < m.num_nonzeros(); ++i) { + int* output_rows = output->mutable_rows(); + int* output_cols = output->mutable_cols(); + double* output_values = output->mutable_values(); + + output_rows[0] = 0; + for (int i = 0; i < index.size(); ++i) { const int idx = index[i]; - ++rows_[m.rows()[idx] + 1]; - cols_[i] = m.cols()[idx]; - values_[i] = m.values()[idx]; + ++output_rows[rows[idx] + 1]; + output_cols[i] = cols[idx]; + output_values[i] = values[idx]; } // Find the cumulative sum of the row counts. - for (int i = 1; i < num_rows_ + 1; ++i) { - rows_[i] += rows_[i - 1]; + for (int i = 1; i < num_rows + 1; ++i) { + output_rows[i] += output_rows[i - 1]; } - CHECK_EQ(num_nonzeros(), m.num_nonzeros()); + CHECK_EQ(output->num_nonzeros(), input.num_nonzeros()); + return output; } CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, int num_rows) { - CHECK_NOTNULL(diagonal); + CHECK(diagonal != nullptr); num_rows_ = num_rows; num_cols_ = num_rows; + storage_type_ = UNSYMMETRIC; rows_.resize(num_rows + 1); cols_.resize(num_rows); values_.resize(num_rows); @@ -154,47 +265,150 @@ CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, CHECK_EQ(num_nonzeros(), num_rows); } -CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { -} +CompressedRowSparseMatrix::~CompressedRowSparseMatrix() {} void CompressedRowSparseMatrix::SetZero() { std::fill(values_.begin(), values_.end(), 0); } +// TODO(sameeragarwal): Make RightMultiply and LeftMultiply +// block-aware for higher performance. void CompressedRowSparseMatrix::RightMultiply(const double* x, double* y) const { - CHECK_NOTNULL(x); - CHECK_NOTNULL(y); + CHECK(x != nullptr); + CHECK(y != nullptr); + + if (storage_type_ == UNSYMMETRIC) { + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + const int c = cols_[idx]; + const double v = values_[idx]; + y[r] += v * x[c]; + } + } + } else if (storage_type_ == UPPER_TRIANGULAR) { + // Because of their block structure, we will have entries that lie + // above (below) the diagonal for lower (upper) triangular matrices, + // so the loops below need to account for this. + for (int r = 0; r < num_rows_; ++r) { + int idx = rows_[r]; + const int idx_end = rows_[r + 1]; + + // For upper triangular matrices r <= c, so skip entries with r + // > c. + while (idx < idx_end && r > cols_[idx]) { + ++idx; + } - for (int r = 0; r < num_rows_; ++r) { - for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { - y[r] += values_[idx] * x[cols_[idx]]; + for (; idx < idx_end; ++idx) { + const int c = cols_[idx]; + const double v = values_[idx]; + y[r] += v * x[c]; + // Since we are only iterating over the upper triangular part + // of the matrix, add contributions for the strictly lower + // triangular part. + if (r != c) { + y[c] += v * x[r]; + } + } + } + } else if (storage_type_ == LOWER_TRIANGULAR) { + for (int r = 0; r < num_rows_; ++r) { + int idx = rows_[r]; + const int idx_end = rows_[r + 1]; + // For lower triangular matrices, we only iterate till we are r >= + // c. + for (; idx < idx_end && r >= cols_[idx]; ++idx) { + const int c = cols_[idx]; + const double v = values_[idx]; + y[r] += v * x[c]; + // Since we are only iterating over the lower triangular part + // of the matrix, add contributions for the strictly upper + // triangular part. + if (r != c) { + y[c] += v * x[r]; + } + } } + } else { + LOG(FATAL) << "Unknown storage type: " << storage_type_; } } void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { - CHECK_NOTNULL(x); - CHECK_NOTNULL(y); + CHECK(x != nullptr); + CHECK(y != nullptr); - for (int r = 0; r < num_rows_; ++r) { - for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { - y[cols_[idx]] += values_[idx] * x[r]; + if (storage_type_ == UNSYMMETRIC) { + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[cols_[idx]] += values_[idx] * x[r]; + } } + } else { + // Since the matrix is symmetric, LeftMultiply = RightMultiply. + RightMultiply(x, y); } } void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { - CHECK_NOTNULL(x); + CHECK(x != nullptr); std::fill(x, x + num_cols_, 0.0); - for (int idx = 0; idx < rows_[num_rows_]; ++idx) { - x[cols_[idx]] += values_[idx] * values_[idx]; + if (storage_type_ == UNSYMMETRIC) { + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + x[cols_[idx]] += values_[idx] * values_[idx]; + } + } else if (storage_type_ == UPPER_TRIANGULAR) { + // Because of their block structure, we will have entries that lie + // above (below) the diagonal for lower (upper) triangular + // matrices, so the loops below need to account for this. + for (int r = 0; r < num_rows_; ++r) { + int idx = rows_[r]; + const int idx_end = rows_[r + 1]; + + // For upper triangular matrices r <= c, so skip entries with r + // > c. + while (idx < idx_end && r > cols_[idx]) { + ++idx; + } + + for (; idx < idx_end; ++idx) { + const int c = cols_[idx]; + const double v2 = values_[idx] * values_[idx]; + x[c] += v2; + // Since we are only iterating over the upper triangular part + // of the matrix, add contributions for the strictly lower + // triangular part. + if (r != c) { + x[r] += v2; + } + } + } + } else if (storage_type_ == LOWER_TRIANGULAR) { + for (int r = 0; r < num_rows_; ++r) { + int idx = rows_[r]; + const int idx_end = rows_[r + 1]; + // For lower triangular matrices, we only iterate till we are r >= + // c. + for (; idx < idx_end && r >= cols_[idx]; ++idx) { + const int c = cols_[idx]; + const double v2 = values_[idx] * values_[idx]; + x[c] += v2; + // Since we are only iterating over the lower triangular part + // of the matrix, add contributions for the strictly upper + // triangular part. + if (r != c) { + x[r] += v2; + } + } + } + } else { + LOG(FATAL) << "Unknown storage type: " << storage_type_; } } - void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { - CHECK_NOTNULL(scale); + CHECK(scale != nullptr); for (int idx = 0; idx < rows_[num_rows_]; ++idx) { values_[idx] *= scale[cols_[idx]]; @@ -202,7 +416,7 @@ void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { } void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { - CHECK_NOTNULL(dense_matrix); + CHECK(dense_matrix != nullptr); dense_matrix->resize(num_rows_, num_cols_); dense_matrix->setZero(); @@ -216,10 +430,17 @@ void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { CHECK_GE(delta_rows, 0); CHECK_LE(delta_rows, num_rows_); + CHECK_EQ(storage_type_, UNSYMMETRIC); num_rows_ -= delta_rows; rows_.resize(num_rows_ + 1); + // The rest of the code updates the block information. Immediately + // return in case of no block information. + if (row_blocks_.empty()) { + return; + } + // Walk the list of row blocks until we reach the new number of rows // and the drop the rest of the row blocks. int num_row_blocks = 0; @@ -233,9 +454,11 @@ void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { } void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { + CHECK_EQ(storage_type_, UNSYMMETRIC); CHECK_EQ(m.num_cols(), num_cols_); - CHECK(row_blocks_.size() == 0 || m.row_blocks().size() !=0) + CHECK((row_blocks_.empty() && m.row_blocks().empty()) || + (!row_blocks_.empty() && !m.row_blocks().empty())) << "Cannot append a matrix with row blocks to one without and vice versa." << "This matrix has : " << row_blocks_.size() << " row blocks." << "The matrix being appended has: " << m.row_blocks().size() @@ -254,9 +477,8 @@ void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { DCHECK_LT(num_nonzeros(), cols_.size()); if (m.num_nonzeros() > 0) { std::copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]); - std::copy(m.values(), - m.values() + m.num_nonzeros(), - &values_[num_nonzeros()]); + std::copy( + m.values(), m.values() + m.num_nonzeros(), &values_[num_nonzeros()]); } rows_.resize(num_rows_ + m.num_rows() + 1); @@ -270,20 +492,22 @@ void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { } num_rows_ += m.num_rows(); - row_blocks_.insert(row_blocks_.end(), - m.row_blocks().begin(), - m.row_blocks().end()); + + // The rest of the code updates the block information. Immediately + // return in case of no block information. + if (row_blocks_.empty()) { + return; + } + + row_blocks_.insert( + row_blocks_.end(), m.row_blocks().begin(), m.row_blocks().end()); } void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { - CHECK_NOTNULL(file); + CHECK(file != nullptr); 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]); } } } @@ -308,29 +532,8 @@ void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) { values_.resize(num_nonzeros); } -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) { + const double* diagonal, const vector<int>& blocks) { int num_rows = 0; int num_nonzeros = 0; for (int i = 0; i < blocks.size(); ++i) { @@ -373,189 +576,152 @@ 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]; + switch (storage_type_) { + case UNSYMMETRIC: + transpose->set_storage_type(UNSYMMETRIC); + break; + case LOWER_TRIANGULAR: + transpose->set_storage_type(UPPER_TRIANGULAR); + break; + case UPPER_TRIANGULAR: + transpose->set_storage_type(LOWER_TRIANGULAR); + break; + default: + LOG(FATAL) << "Unknown storage type: " << storage_type_; + }; + + TransposeForCompressedRowSparseStructure(num_rows(), + num_cols(), + num_nonzeros(), + rows(), + cols(), + values(), + transpose->mutable_rows(), + transpose->mutable_cols(), + transpose->mutable_values()); + + // The rest of the code updates the block information. Immediately + // return in case of no block information. + if (row_blocks_.empty()) { + return transpose; } - transpose_rows[0] = 0; *(transpose->mutable_row_blocks()) = col_blocks_; *(transpose->mutable_col_blocks()) = row_blocks_; - return transpose; } -namespace { -// A ProductTerm is a term in the outer product of a matrix with -// itself. -struct ProductTerm { - ProductTerm(const int row, const int col, const int index) - : row(row), col(col), index(index) { - } - - bool operator<(const ProductTerm& right) const { - if (row == right.row) { - if (col == right.col) { - return index < right.index; - } - return col < right.col; - } - return row < right.row; - } - - int row; - int col; - int index; -}; - -CompressedRowSparseMatrix* -CompressAndFillProgram(const int num_rows, - const int num_cols, - const vector<ProductTerm>& product, - vector<int>* program) { - CHECK_GT(product.size(), 0); - - // Count the number of unique product term, which in turn is the - // number of non-zeros in the outer product. - int num_nonzeros = 1; - for (int i = 1; i < product.size(); ++i) { - if (product[i].row != product[i - 1].row || - product[i].col != product[i - 1].col) { - ++num_nonzeros; - } - } - - CompressedRowSparseMatrix* matrix = - new CompressedRowSparseMatrix(num_rows, num_cols, num_nonzeros); - - int* crsm_rows = matrix->mutable_rows(); - std::fill(crsm_rows, crsm_rows + num_rows + 1, 0); - int* crsm_cols = matrix->mutable_cols(); - std::fill(crsm_cols, crsm_cols + num_nonzeros, 0); - - CHECK_NOTNULL(program)->clear(); - program->resize(product.size()); - - // Iterate over the sorted product terms. This means each row is - // filled one at a time, and we are able to assign a position in the - // values array to each term. - // - // If terms repeat, i.e., they contribute to the same entry in the - // result matrix), then they do not affect the sparsity structure of - // the result matrix. - int nnz = 0; - crsm_cols[0] = product[0].col; - crsm_rows[product[0].row + 1]++; - (*program)[product[0].index] = nnz; - for (int i = 1; i < product.size(); ++i) { - const ProductTerm& previous = product[i - 1]; - const ProductTerm& current = product[i]; - - // Sparsity structure is updated only if the term is not a repeat. - if (previous.row != current.row || previous.col != current.col) { - crsm_cols[++nnz] = current.col; - crsm_rows[current.row + 1]++; - } - - // All terms get assigned the position in the values array where - // their value is accumulated. - (*program)[current.index] = nnz; - } - - for (int i = 1; i < num_rows + 1; ++i) { - crsm_rows[i] += crsm_rows[i - 1]; - } - - return matrix; -} - -} // namespace - -CompressedRowSparseMatrix* -CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( - const CompressedRowSparseMatrix& m, - vector<int>* program) { - CHECK_NOTNULL(program)->clear(); - CHECK_GT(m.num_nonzeros(), 0) - << "Congratulations, " - << "you found a bug in Ceres. Please report it."; - - vector<ProductTerm> product; - const vector<int>& row_blocks = m.row_blocks(); - int row_block_begin = 0; - // Iterate over row blocks - for (int row_block = 0; row_block < row_blocks.size(); ++row_block) { - const int row_block_end = row_block_begin + row_blocks[row_block]; - // Compute the outer product terms for just one row per row block. - const int r = row_block_begin; - // Compute the lower triangular part of the product. - for (int idx1 = m.rows()[r]; idx1 < m.rows()[r + 1]; ++idx1) { - for (int idx2 = m.rows()[r]; idx2 <= idx1; ++idx2) { - product.push_back(ProductTerm(m.cols()[idx1], - m.cols()[idx2], - product.size())); - } +CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateRandomMatrix( + CompressedRowSparseMatrix::RandomMatrixOptions options) { + CHECK_GT(options.num_row_blocks, 0); + CHECK_GT(options.min_row_block_size, 0); + CHECK_GT(options.max_row_block_size, 0); + CHECK_LE(options.min_row_block_size, options.max_row_block_size); + + if (options.storage_type == UNSYMMETRIC) { + CHECK_GT(options.num_col_blocks, 0); + CHECK_GT(options.min_col_block_size, 0); + CHECK_GT(options.max_col_block_size, 0); + CHECK_LE(options.min_col_block_size, options.max_col_block_size); + } else { + // Symmetric matrices (LOWER_TRIANGULAR or UPPER_TRIANGULAR); + options.num_col_blocks = options.num_row_blocks; + options.min_col_block_size = options.min_row_block_size; + options.max_col_block_size = options.max_row_block_size; + } + + CHECK_GT(options.block_density, 0.0); + CHECK_LE(options.block_density, 1.0); + + vector<int> row_blocks; + vector<int> col_blocks; + + // Generate the row block structure. + for (int i = 0; i < options.num_row_blocks; ++i) { + // Generate a random integer in [min_row_block_size, max_row_block_size] + const int delta_block_size = + Uniform(options.max_row_block_size - options.min_row_block_size); + row_blocks.push_back(options.min_row_block_size + delta_block_size); + } + + if (options.storage_type == UNSYMMETRIC) { + // Generate the col block structure. + for (int i = 0; i < options.num_col_blocks; ++i) { + // Generate a random integer in [min_col_block_size, max_col_block_size] + const int delta_block_size = + Uniform(options.max_col_block_size - options.min_col_block_size); + col_blocks.push_back(options.min_col_block_size + delta_block_size); } - row_block_begin = row_block_end; - } - CHECK_EQ(row_block_begin, m.num_rows()); - sort(product.begin(), product.end()); - return CompressAndFillProgram(m.num_cols(), m.num_cols(), product, program); -} + } else { + // Symmetric matrices (LOWER_TRIANGULAR or UPPER_TRIANGULAR); + col_blocks = row_blocks; + } + + vector<int> tsm_rows; + vector<int> tsm_cols; + vector<double> tsm_values; + + // For ease of construction, we are going to generate the + // CompressedRowSparseMatrix by generating it as a + // TripletSparseMatrix and then converting it to a + // CompressedRowSparseMatrix. + + // It is possible that the random matrix is empty which is likely + // not what the user wants, so do the matrix generation till we have + // at least one non-zero entry. + while (tsm_values.empty()) { + tsm_rows.clear(); + tsm_cols.clear(); + tsm_values.clear(); + + int row_block_begin = 0; + for (int r = 0; r < options.num_row_blocks; ++r) { + int col_block_begin = 0; + for (int c = 0; c < options.num_col_blocks; ++c) { + if (((options.storage_type == UPPER_TRIANGULAR) && (r > c)) || + ((options.storage_type == LOWER_TRIANGULAR) && (r < c))) { + col_block_begin += col_blocks[c]; + continue; + } -void CompressedRowSparseMatrix::ComputeOuterProduct( - const CompressedRowSparseMatrix& m, - const vector<int>& program, - CompressedRowSparseMatrix* result) { - result->SetZero(); - double* values = result->mutable_values(); - const vector<int>& row_blocks = m.row_blocks(); - - int cursor = 0; - int row_block_begin = 0; - const double* m_values = m.values(); - const int* m_rows = m.rows(); - // Iterate over row blocks. - for (int row_block = 0; row_block < row_blocks.size(); ++row_block) { - const int row_block_end = row_block_begin + row_blocks[row_block]; - const int saved_cursor = cursor; - for (int r = row_block_begin; r < row_block_end; ++r) { - // Reuse the program segment for each row in this row block. - cursor = saved_cursor; - const int row_begin = m_rows[r]; - const int row_end = m_rows[r + 1]; - for (int idx1 = row_begin; idx1 < row_end; ++idx1) { - const double v1 = m_values[idx1]; - for (int idx2 = row_begin; idx2 <= idx1; ++idx2, ++cursor) { - values[program[cursor]] += v1 * m_values[idx2]; + // Randomly determine if this block is present or not. + if (RandDouble() <= options.block_density) { + // If the matrix is symmetric, then we take care to generate + // symmetric diagonal blocks. + if (options.storage_type == UNSYMMETRIC || r != c) { + AddRandomBlock(row_blocks[r], + col_blocks[c], + row_block_begin, + col_block_begin, + &tsm_rows, + &tsm_cols, + &tsm_values); + } else { + AddSymmetricRandomBlock(row_blocks[r], + row_block_begin, + &tsm_rows, + &tsm_cols, + &tsm_values); + } } + col_block_begin += col_blocks[c]; } + row_block_begin += row_blocks[r]; } - row_block_begin = row_block_end; } - CHECK_EQ(row_block_begin, m.num_rows()); - CHECK_EQ(cursor, program.size()); + const int num_rows = std::accumulate(row_blocks.begin(), row_blocks.end(), 0); + const int num_cols = std::accumulate(col_blocks.begin(), col_blocks.end(), 0); + const bool kDoNotTranspose = false; + CompressedRowSparseMatrix* matrix = + CompressedRowSparseMatrix::FromTripletSparseMatrix( + TripletSparseMatrix( + num_rows, num_cols, tsm_rows, tsm_cols, tsm_values), + kDoNotTranspose); + (*matrix->mutable_row_blocks()) = row_blocks; + (*matrix->mutable_col_blocks()) = col_blocks; + matrix->set_storage_type(options.storage_type); + return matrix; } } // namespace internal diff --git a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h index 987339d09a1..758b40bbc8a 100644 --- a/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h +++ b/extern/ceres/internal/ceres/compressed_row_sparse_matrix.h @@ -32,7 +32,6 @@ #define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ #include <vector> -#include "ceres/internal/macros.h" #include "ceres/internal/port.h" #include "ceres/sparse_matrix.h" #include "ceres/types.h" @@ -48,13 +47,35 @@ class TripletSparseMatrix; class CompressedRowSparseMatrix : public SparseMatrix { public: - // Build a matrix with the same content as the TripletSparseMatrix - // m. TripletSparseMatrix objects are easier to construct - // incrementally, so we use them to initialize SparseMatrix - // objects. + enum StorageType { + UNSYMMETRIC, + // Matrix is assumed to be symmetric but only the lower triangular + // part of the matrix is stored. + LOWER_TRIANGULAR, + // Matrix is assumed to be symmetric but only the upper triangular + // part of the matrix is stored. + UPPER_TRIANGULAR + }; + + // Create a matrix with the same content as the TripletSparseMatrix + // input. We assume that input does not have any repeated + // entries. // - // We assume that m does not have any repeated entries. - explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); + // The storage type of the matrix is set to UNSYMMETRIC. + // + // Caller owns the result. + static CompressedRowSparseMatrix* FromTripletSparseMatrix( + const TripletSparseMatrix& input); + + // Create a matrix with the same content as the TripletSparseMatrix + // input transposed. We assume that input does not have any repeated + // entries. + // + // The storage type of the matrix is set to UNSYMMETRIC. + // + // Caller owns the result. + static CompressedRowSparseMatrix* FromTripletSparseMatrixTransposed( + const TripletSparseMatrix& input); // Use this constructor only if you know what you are doing. This // creates a "blank" matrix with the appropriate amount of memory @@ -67,30 +88,30 @@ class CompressedRowSparseMatrix : public SparseMatrix { // manually, instead of going via the indirect route of first // constructing a TripletSparseMatrix, which leads to more than // double the peak memory usage. - CompressedRowSparseMatrix(int num_rows, - int num_cols, - int max_num_nonzeros); + // + // The storage type is set to UNSYMMETRIC. + CompressedRowSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); // Build a square sparse diagonal matrix with num_rows rows and // columns. The diagonal m(i,i) = diagonal(i); + // + // The storage type is set to UNSYMMETRIC CompressedRowSparseMatrix(const double* diagonal, int num_rows); - virtual ~CompressedRowSparseMatrix(); - // SparseMatrix interface. - virtual void SetZero(); - virtual void RightMultiply(const double* x, double* y) const; - virtual void LeftMultiply(const double* x, double* y) const; - virtual void SquaredColumnNorm(double* x) const; - virtual void ScaleColumns(const double* scale); - - virtual void ToDenseMatrix(Matrix* dense_matrix) const; - 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_[0]; } - virtual double* mutable_values() { return &values_[0]; } + virtual ~CompressedRowSparseMatrix(); + void SetZero() final; + void RightMultiply(const double* x, double* y) const final; + void LeftMultiply(const double* x, double* y) const final; + void SquaredColumnNorm(double* x) const final; + void ScaleColumns(const double* scale) final; + void ToDenseMatrix(Matrix* dense_matrix) const final; + void ToTextFile(FILE* file) const final; + int num_rows() const final { return num_rows_; } + int num_cols() const final { return num_cols_; } + int num_nonzeros() const final { return rows_[num_rows_]; } + const double* values() const final { return &values_[0]; } + double* mutable_values() final { return &values_[0]; } // Delete the bottom delta_rows. // num_rows -= delta_rows @@ -102,6 +123,15 @@ class CompressedRowSparseMatrix : public SparseMatrix { void ToCRSMatrix(CRSMatrix* matrix) const; + CompressedRowSparseMatrix* Transpose() const; + + // Destructive array resizing method. + void SetMaxNumNonZeros(int num_nonzeros); + + // 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; } + // Low level access methods that expose the structure of the matrix. const int* cols() const { return &cols_[0]; } int* mutable_cols() { return &cols_[0]; } @@ -109,60 +139,79 @@ class CompressedRowSparseMatrix : public SparseMatrix { const int* rows() const { return &rows_[0]; } int* mutable_rows() { return &rows_[0]; } + const StorageType storage_type() const { return storage_type_; } + void set_storage_type(const StorageType storage_type) { + storage_type_ = storage_type; + } + const std::vector<int>& row_blocks() const { return row_blocks_; } std::vector<int>* mutable_row_blocks() { return &row_blocks_; } const std::vector<int>& col_blocks() const { return col_blocks_; } std::vector<int>* mutable_col_blocks() { return &col_blocks_; } - // Destructive array resizing method. - void SetMaxNumNonZeros(int num_nonzeros); - - // 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; - + // Create a block diagonal CompressedRowSparseMatrix with the given + // block structure. The individual blocks are assumed to be laid out + // contiguously in the diagonal array, one block at a time. + // + // Caller owns the result. static CompressedRowSparseMatrix* CreateBlockDiagonalMatrix( - const double* diagonal, - const std::vector<int>& blocks); + const double* diagonal, const std::vector<int>& blocks); - // Compute the sparsity structure of the product m.transpose() * m - // and create a CompressedRowSparseMatrix corresponding to it. + // Options struct to control the generation of random block sparse + // matrices in compressed row sparse format. + // + // The random matrix generation proceeds as follows. // - // Also compute a "program" vector, which for every term in the - // outer product points to the entry in the values array of the - // result matrix where it should be accumulated. + // First the row and column block structure is determined by + // generating random row and column block sizes that lie within the + // given bounds. // - // This program is used by the ComputeOuterProduct function below to - // compute the outer product. + // Then we walk the block structure of the resulting matrix, and with + // probability block_density detemine whether they are structurally + // zero or not. If the answer is no, then we generate entries for the + // block which are distributed normally. + struct RandomMatrixOptions { + // Type of matrix to create. + // + // If storage_type is UPPER_TRIANGULAR (LOWER_TRIANGULAR), then + // create a square symmetric matrix with just the upper triangular + // (lower triangular) part. In this case, num_col_blocks, + // min_col_block_size and max_col_block_size will be ignored and + // assumed to be equal to the corresponding row settings. + StorageType storage_type = UNSYMMETRIC; + + int num_row_blocks = 0; + int min_row_block_size = 0; + int max_row_block_size = 0; + int num_col_blocks = 0; + int min_col_block_size = 0; + int max_col_block_size = 0; + + // 0 < block_density <= 1 is the probability of a block being + // present in the matrix. A given random matrix will not have + // precisely this density. + double block_density = 0.0; + }; + + // Create a random CompressedRowSparseMatrix whose entries are + // normally distributed and whose structure is determined by + // RandomMatrixOptions. // - // Since the entries of the program are the same for rows with the - // same sparsity structure, the program only stores the result for - // one row per row block. The ComputeOuterProduct function reuses - // this information for each row in the row block. - static CompressedRowSparseMatrix* CreateOuterProductMatrixAndProgram( - const CompressedRowSparseMatrix& m, - std::vector<int>* program); - - // Compute the values array for the expression m.transpose() * m, - // where the matrix used to store the result and a program have been - // created using the CreateOuterProductMatrixAndProgram function - // above. - static void ComputeOuterProduct(const CompressedRowSparseMatrix& m, - const std::vector<int>& program, - CompressedRowSparseMatrix* result); + // Caller owns the result. + static CompressedRowSparseMatrix* CreateRandomMatrix( + RandomMatrixOptions options); private: + static CompressedRowSparseMatrix* FromTripletSparseMatrix( + const TripletSparseMatrix& input, bool transpose); + int num_rows_; int num_cols_; std::vector<int> rows_; std::vector<int> cols_; std::vector<double> values_; + StorageType storage_type_; // If the matrix has an underlying block structure, then it can also // carry with it row and column block sizes. This is auxilliary and @@ -171,8 +220,6 @@ class CompressedRowSparseMatrix : public SparseMatrix { // any way. std::vector<int> row_blocks_; std::vector<int> col_blocks_; - - CERES_DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/concurrent_queue.h b/extern/ceres/internal/ceres/concurrent_queue.h new file mode 100644 index 00000000000..52e2903022b --- /dev/null +++ b/extern/ceres/internal/ceres/concurrent_queue.h @@ -0,0 +1,159 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +#ifndef CERES_INTERNAL_CONCURRENT_QUEUE_H_ +#define CERES_INTERNAL_CONCURRENT_QUEUE_H_ + +#include <condition_variable> +#include <mutex> +#include <queue> +#include <thread> + +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// A thread-safe multi-producer, multi-consumer queue for queueing items that +// are typically handled asynchronously by multiple threads. The ConcurrentQueue +// has two states which only affect the Wait call: +// +// (1) Waiters have been enabled (enabled by default or calling +// EnableWaiters). The call to Wait will block until an item is available. +// Push and pop will operate as expected. +// +// (2) StopWaiters has been called. All threads blocked in a Wait() call will +// be woken up and pop any available items from the queue. All future Wait +// requests will either return an element from the queue or return +// immediately if no element is present. Push and pop will operate as +// expected. +// +// A common use case is using the concurrent queue as an interface for +// scheduling tasks for a set of thread workers: +// +// ConcurrentQueue<Task> task_queue; +// +// [Worker threads]: +// Task task; +// while(task_queue.Wait(&task)) { +// ... +// } +// +// [Producers]: +// task_queue.Push(...); +// .. +// task_queue.Push(...); +// ... +// // Signal worker threads to stop blocking on Wait and terminate. +// task_queue.StopWaiters(); +// +template <typename T> +class ConcurrentQueue { + public: + // Defaults the queue to blocking on Wait calls. + ConcurrentQueue() : wait_(true) {} + + // Atomically push an element onto the queue. If a thread was waiting for an + // element, wake it up. + void Push(const T& value) { + std::lock_guard<std::mutex> lock(mutex_); + queue_.push(value); + work_pending_condition_.notify_one(); + } + + // Atomically pop an element from the queue. If an element is present, return + // true. If the queue was empty, return false. + bool Pop(T* value) { + CHECK(value != nullptr); + + std::lock_guard<std::mutex> lock(mutex_); + return PopUnlocked(value); + } + + // Atomically pop an element from the queue. Blocks until one is available or + // StopWaiters is called. Returns true if an element was successfully popped + // from the queue, otherwise returns false. + bool Wait(T* value) { + CHECK(value != nullptr); + + std::unique_lock<std::mutex> lock(mutex_); + work_pending_condition_.wait(lock, + [&]() { return !(wait_ && queue_.empty()); }); + + return PopUnlocked(value); + } + + // Unblock all threads waiting to pop a value from the queue, and they will + // exit Wait() without getting a value. All future Wait requests will return + // immediately if no element is present until EnableWaiters is called. + void StopWaiters() { + std::lock_guard<std::mutex> lock(mutex_); + wait_ = false; + work_pending_condition_.notify_all(); + } + + // Enable threads to block on Wait calls. + void EnableWaiters() { + std::lock_guard<std::mutex> lock(mutex_); + wait_ = true; + } + + private: + // Pops an element from the queue. If an element is present, return + // true. If the queue was empty, return false. Not thread-safe. Must acquire + // the lock before calling. + bool PopUnlocked(T* value) { + if (queue_.empty()) { + return false; + } + + *value = queue_.front(); + queue_.pop(); + + return true; + } + + // The mutex controls read and write access to the queue_ and stop_ + // variables. It is also used to block the calling thread until an element is + // available to pop from the queue. + std::mutex mutex_; + std::condition_variable work_pending_condition_; + + std::queue<T> queue_; + // If true, signals that callers of Wait will block waiting to pop an + // element off the queue. + bool wait_; +}; + + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CONCURRENT_QUEUE_H_ diff --git a/extern/ceres/internal/ceres/conditioned_cost_function.cc b/extern/ceres/internal/ceres/conditioned_cost_function.cc index 08899e3d246..d933ad7c462 100644 --- a/extern/ceres/internal/ceres/conditioned_cost_function.cc +++ b/extern/ceres/internal/ceres/conditioned_cost_function.cc @@ -68,7 +68,7 @@ ConditionedCostFunction::ConditionedCostFunction( ConditionedCostFunction::~ConditionedCostFunction() { if (ownership_ == TAKE_OWNERSHIP) { - STLDeleteElements(&conditioners_); + STLDeleteUniqueContainerPointers(conditioners_.begin(), conditioners_.end()); } else { wrapped_cost_function_.release(); } diff --git a/extern/ceres/internal/ceres/conjugate_gradients_solver.cc b/extern/ceres/internal/ceres/conjugate_gradients_solver.cc index 3702276a2fb..c6f85c15ea0 100644 --- a/extern/ceres/internal/ceres/conjugate_gradients_solver.cc +++ b/extern/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -41,7 +41,6 @@ #include <cmath> #include <cstddef> -#include "ceres/fpclassify.h" #include "ceres/internal/eigen.h" #include "ceres/linear_operator.h" #include "ceres/stringprintf.h" @@ -53,7 +52,7 @@ namespace internal { namespace { bool IsZeroOrInfinity(double x) { - return ((x == 0.0) || (IsInfinite(x))); + return ((x == 0.0) || std::isinf(x)); } } // namespace @@ -68,9 +67,9 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( const double* b, const LinearSolver::PerSolveOptions& per_solve_options, double* x) { - CHECK_NOTNULL(A); - CHECK_NOTNULL(x); - CHECK_NOTNULL(b); + CHECK(A != nullptr); + CHECK(x != nullptr); + CHECK(b != nullptr); CHECK_EQ(A->num_rows(), A->num_cols()); LinearSolver::Summary summary; @@ -148,7 +147,7 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( q.setZero(); A->RightMultiply(p.data(), q.data()); const double pq = p.dot(q); - if ((pq <= 0) || IsInfinite(pq)) { + if ((pq <= 0) || std::isinf(pq)) { summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE; summary.message = StringPrintf( "Matrix is indefinite, no more progress can be made. " @@ -158,7 +157,7 @@ LinearSolver::Summary ConjugateGradientsSolver::Solve( } const double alpha = rho / pq; - if (IsInfinite(alpha)) { + if (std::isinf(alpha)) { summary.termination_type = LINEAR_SOLVER_FAILURE; summary.message = StringPrintf("Numerical failure. alpha = rho / pq = %e, " diff --git a/extern/ceres/internal/ceres/conjugate_gradients_solver.h b/extern/ceres/internal/ceres/conjugate_gradients_solver.h index a1e18334414..d89383e6359 100644 --- a/extern/ceres/internal/ceres/conjugate_gradients_solver.h +++ b/extern/ceres/internal/ceres/conjugate_gradients_solver.h @@ -35,7 +35,6 @@ #define CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ #include "ceres/linear_solver.h" -#include "ceres/internal/macros.h" namespace ceres { namespace internal { @@ -58,14 +57,13 @@ class LinearOperator; class ConjugateGradientsSolver : public LinearSolver { public: explicit ConjugateGradientsSolver(const LinearSolver::Options& options); - virtual Summary Solve(LinearOperator* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x); + Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) final; private: const LinearSolver::Options options_; - CERES_DISALLOW_COPY_AND_ASSIGN(ConjugateGradientsSolver); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/context.cc b/extern/ceres/internal/ceres/context.cc new file mode 100644 index 00000000000..e2232013b4b --- /dev/null +++ b/extern/ceres/internal/ceres/context.cc @@ -0,0 +1,41 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +#include "ceres/context.h" + +#include "ceres/context_impl.h" + +namespace ceres { + +Context* Context::Create() { + return new internal::ContextImpl(); +} + +} // namespace ceres diff --git a/extern/ceres/internal/ceres/context_impl.cc b/extern/ceres/internal/ceres/context_impl.cc new file mode 100644 index 00000000000..622f33a9dc0 --- /dev/null +++ b/extern/ceres/internal/ceres/context_impl.cc @@ -0,0 +1,43 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +#include "ceres/context_impl.h" + +namespace ceres { +namespace internal { + +void ContextImpl::EnsureMinimumThreads(int num_threads) { +#ifdef CERES_USE_CXX_THREADS + thread_pool.Resize(num_threads); +#endif // CERES_USE_CXX_THREADS + +} +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/context_impl.h b/extern/ceres/internal/ceres/context_impl.h new file mode 100644 index 00000000000..5c03ad71bab --- /dev/null +++ b/extern/ceres/internal/ceres/context_impl.h @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +#ifndef CERES_INTERNAL_CONTEXT_IMPL_H_ +#define CERES_INTERNAL_CONTEXT_IMPL_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#include "ceres/context.h" + +#ifdef CERES_USE_CXX_THREADS +#include "ceres/thread_pool.h" +#endif // CERES_USE_CXX_THREADS + +namespace ceres { +namespace internal { + +class ContextImpl : public Context { + public: + ContextImpl() {} + ContextImpl(const ContextImpl&) = delete; + void operator=(const ContextImpl&) = delete; + + virtual ~ContextImpl() {} + + // When compiled with C++ threading support, resize the thread pool to have + // at min(num_thread, num_hardware_threads) where num_hardware_threads is + // defined by the hardware. Otherwise this call is a no-op. + void EnsureMinimumThreads(int num_threads); + +#ifdef CERES_USE_CXX_THREADS + ThreadPool thread_pool; +#endif // CERES_USE_CXX_THREADS +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CONTEXT_IMPL_H_ diff --git a/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc b/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc index c6b42cf1516..c5d56f30bc3 100644 --- a/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc +++ b/extern/ceres/internal/ceres/coordinate_descent_minimizer.cc @@ -30,16 +30,16 @@ #include "ceres/coordinate_descent_minimizer.h" -#ifdef CERES_USE_OPENMP -#include <omp.h> -#endif - +#include <algorithm> #include <iterator> +#include <memory> #include <numeric> #include <vector> + #include "ceres/evaluator.h" #include "ceres/linear_solver.h" #include "ceres/minimizer.h" +#include "ceres/parallel_for.h" #include "ceres/parameter_block.h" #include "ceres/parameter_block_ordering.h" #include "ceres/problem_impl.h" @@ -59,6 +59,11 @@ using std::set; using std::string; using std::vector; +CoordinateDescentMinimizer::CoordinateDescentMinimizer(ContextImpl* context) + : context_(context) { + CHECK(context_ != nullptr); +} + CoordinateDescentMinimizer::~CoordinateDescentMinimizer() { } @@ -74,19 +79,16 @@ bool CoordinateDescentMinimizer::Init( // Serialize the OrderedGroups into a vector of parameter block // offsets for parallel access. map<ParameterBlock*, int> parameter_block_index; - map<int, set<double*> > group_to_elements = ordering.group_to_elements(); - for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); - it != group_to_elements.end(); - ++it) { - for (set<double*>::const_iterator ptr_it = it->second.begin(); - ptr_it != it->second.end(); - ++ptr_it) { - parameter_blocks_.push_back(parameter_map.find(*ptr_it)->second); + map<int, set<double*>> group_to_elements = ordering.group_to_elements(); + for (const auto& g_t_e : group_to_elements) { + const auto& elements = g_t_e.second; + for (double* parameter_block: elements) { + parameter_blocks_.push_back(parameter_map.find(parameter_block)->second); parameter_block_index[parameter_blocks_.back()] = parameter_blocks_.size() - 1; } independent_set_offsets_.push_back( - independent_set_offsets_.back() + it->second.size()); + independent_set_offsets_.back() + elements.size()); } // The ordering does not have to contain all parameter blocks, so @@ -109,8 +111,7 @@ bool CoordinateDescentMinimizer::Init( const int num_parameter_blocks = residual_block->NumParameterBlocks(); for (int j = 0; j < num_parameter_blocks; ++j) { ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; - const map<ParameterBlock*, int>::const_iterator it = - parameter_block_index.find(parameter_block); + const auto it = parameter_block_index.find(parameter_block); if (it != parameter_block_index.end()) { residual_blocks_[it->second].push_back(residual_block); } @@ -120,6 +121,7 @@ bool CoordinateDescentMinimizer::Init( evaluator_options_.linear_solver_type = DENSE_QR; evaluator_options_.num_eliminate_blocks = 0; evaluator_options_.num_threads = 1; + evaluator_options_.context = context_; return true; } @@ -135,11 +137,12 @@ void CoordinateDescentMinimizer::Minimize( parameter_block->SetConstant(); } - scoped_array<LinearSolver*> linear_solvers( + std::unique_ptr<LinearSolver*[]> linear_solvers( new LinearSolver*[options.num_threads]); LinearSolver::Options linear_solver_options; linear_solver_options.type = DENSE_QR; + linear_solver_options.context = context_; for (int i = 0; i < options.num_threads; ++i) { linear_solvers[i] = LinearSolver::Create(linear_solver_options); @@ -148,13 +151,11 @@ void CoordinateDescentMinimizer::Minimize( for (int i = 0; i < independent_set_offsets_.size() - 1; ++i) { const int num_problems = independent_set_offsets_[i + 1] - independent_set_offsets_[i]; - // No point paying the price for an OpemMP call if the set is of - // size zero. + // Avoid parallelization overhead call if the set is empty. if (num_problems == 0) { continue; } -#ifdef CERES_USE_OPENMP const int num_inner_iteration_threads = min(options.num_threads, num_problems); evaluator_options_.num_threads = @@ -162,47 +163,43 @@ void CoordinateDescentMinimizer::Minimize( // The parameter blocks in each independent set can be optimized // in parallel, since they do not co-occur in any residual block. -#pragma omp parallel for num_threads(num_inner_iteration_threads) -#endif - for (int j = independent_set_offsets_[i]; - j < independent_set_offsets_[i + 1]; - ++j) { -#ifdef CERES_USE_OPENMP - int thread_id = omp_get_thread_num(); -#else - int thread_id = 0; -#endif - - ParameterBlock* parameter_block = parameter_blocks_[j]; - const int old_index = parameter_block->index(); - const int old_delta_offset = parameter_block->delta_offset(); - parameter_block->SetVarying(); - parameter_block->set_index(0); - parameter_block->set_delta_offset(0); - - Program inner_program; - inner_program.mutable_parameter_blocks()->push_back(parameter_block); - *inner_program.mutable_residual_blocks() = residual_blocks_[j]; - - // TODO(sameeragarwal): Better error handling. Right now we - // assume that this is not going to lead to problems of any - // sort. Basically we should be checking for numerical failure - // of some sort. - // - // On the other hand, if the optimization is a failure, that in - // some ways is fine, since it won't change the parameters and - // we are fine. - Solver::Summary inner_summary; - Solve(&inner_program, - linear_solvers[thread_id], - parameters + parameter_block->state_offset(), - &inner_summary); - - parameter_block->set_index(old_index); - parameter_block->set_delta_offset(old_delta_offset); - parameter_block->SetState(parameters + parameter_block->state_offset()); - parameter_block->SetConstant(); - } + ParallelFor( + context_, + independent_set_offsets_[i], + independent_set_offsets_[i + 1], + num_inner_iteration_threads, + [&](int thread_id, int j) { + ParameterBlock* parameter_block = parameter_blocks_[j]; + const int old_index = parameter_block->index(); + const int old_delta_offset = parameter_block->delta_offset(); + parameter_block->SetVarying(); + parameter_block->set_index(0); + parameter_block->set_delta_offset(0); + + Program inner_program; + inner_program.mutable_parameter_blocks()->push_back(parameter_block); + *inner_program.mutable_residual_blocks() = residual_blocks_[j]; + + // TODO(sameeragarwal): Better error handling. Right now we + // assume that this is not going to lead to problems of any + // sort. Basically we should be checking for numerical failure + // of some sort. + // + // On the other hand, if the optimization is a failure, that in + // some ways is fine, since it won't change the parameters and + // we are fine. + Solver::Summary inner_summary; + Solve(&inner_program, + linear_solvers[thread_id], + parameters + parameter_block->state_offset(), + &inner_summary); + + parameter_block->set_index(old_index); + parameter_block->set_delta_offset(old_delta_offset); + parameter_block->SetState(parameters + + parameter_block->state_offset()); + parameter_block->SetConstant(); + }); } for (int i = 0; i < parameter_blocks_.size(); ++i) { @@ -227,14 +224,17 @@ void CoordinateDescentMinimizer::Solve(Program* program, Minimizer::Options minimizer_options; minimizer_options.evaluator.reset( - CHECK_NOTNULL(Evaluator::Create(evaluator_options_, program, &error))); + Evaluator::Create(evaluator_options_, program, &error)); + CHECK(minimizer_options.evaluator != nullptr); minimizer_options.jacobian.reset( - CHECK_NOTNULL(minimizer_options.evaluator->CreateJacobian())); + minimizer_options.evaluator->CreateJacobian()); + CHECK(minimizer_options.jacobian != nullptr); TrustRegionStrategy::Options trs_options; trs_options.linear_solver = linear_solver; minimizer_options.trust_region_strategy.reset( - CHECK_NOTNULL(TrustRegionStrategy::Create(trs_options))); + TrustRegionStrategy::Create(trs_options)); + CHECK(minimizer_options.trust_region_strategy != nullptr); minimizer_options.is_silent = true; TrustRegionMinimizer minimizer; @@ -245,17 +245,16 @@ bool CoordinateDescentMinimizer::IsOrderingValid( const Program& program, const ParameterBlockOrdering& ordering, string* message) { - const map<int, set<double*> >& group_to_elements = + const map<int, set<double*>>& group_to_elements = ordering.group_to_elements(); // Verify that each group is an independent set - map<int, set<double*> >::const_iterator it = group_to_elements.begin(); - for (; it != group_to_elements.end(); ++it) { - if (!program.IsParameterBlockSetIndependent(it->second)) { + for (const auto& g_t_e : group_to_elements) { + if (!program.IsParameterBlockSetIndependent(g_t_e.second)) { *message = StringPrintf("The user-provided " "parameter_blocks_for_inner_iterations does not " - "form an independent set. Group Id: %d", it->first); + "form an independent set. Group Id: %d", g_t_e.first); return false; } } @@ -268,7 +267,7 @@ bool CoordinateDescentMinimizer::IsOrderingValid( // points. ParameterBlockOrdering* CoordinateDescentMinimizer::CreateOrdering( const Program& program) { - scoped_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering); + std::unique_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering); ComputeRecursiveIndependentSetOrdering(program, ordering.get()); ordering->Reverse(); return ordering.release(); diff --git a/extern/ceres/internal/ceres/coordinate_descent_minimizer.h b/extern/ceres/internal/ceres/coordinate_descent_minimizer.h index 25ea04ce622..7d17d53eb0f 100644 --- a/extern/ceres/internal/ceres/coordinate_descent_minimizer.h +++ b/extern/ceres/internal/ceres/coordinate_descent_minimizer.h @@ -34,6 +34,7 @@ #include <string> #include <vector> +#include "ceres/context_impl.h" #include "ceres/evaluator.h" #include "ceres/minimizer.h" #include "ceres/problem_impl.h" @@ -57,6 +58,8 @@ class LinearSolver; // program are constant. class CoordinateDescentMinimizer : public Minimizer { public: + explicit CoordinateDescentMinimizer(ContextImpl* context); + bool Init(const Program& program, const ProblemImpl::ParameterMap& parameter_map, const ParameterBlockOrdering& ordering, @@ -64,9 +67,10 @@ class CoordinateDescentMinimizer : public Minimizer { // Minimizer interface. virtual ~CoordinateDescentMinimizer(); - virtual void Minimize(const Minimizer::Options& options, - double* parameters, - Solver::Summary* summary); + + void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) final; // Verify that each group in the ordering forms an independent set. static bool IsOrderingValid(const Program& program, @@ -86,7 +90,7 @@ class CoordinateDescentMinimizer : public Minimizer { Solver::Summary* summary); std::vector<ParameterBlock*> parameter_blocks_; - std::vector<std::vector<ResidualBlock*> > residual_blocks_; + std::vector<std::vector<ResidualBlock*>> residual_blocks_; // The optimization is performed in rounds. In each round all the // parameter blocks that form one independent set are optimized in // parallel. This array, marks the boundaries of the independent @@ -94,6 +98,8 @@ class CoordinateDescentMinimizer : public Minimizer { std::vector<int> independent_set_offsets_; Evaluator::Options evaluator_options_; + + ContextImpl* context_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/corrector.cc b/extern/ceres/internal/ceres/corrector.cc index 720182868c1..4ac0dc3cd86 100644 --- a/extern/ceres/internal/ceres/corrector.cc +++ b/extern/ceres/internal/ceres/corrector.cc @@ -43,7 +43,7 @@ Corrector::Corrector(const double sq_norm, const double rho[3]) { sqrt_rho1_ = sqrt(rho[1]); // If sq_norm = 0.0, the correction becomes trivial, the residual - // and the jacobian are scaled by the squareroot of the derivative + // and the jacobian are scaled by the square root of the derivative // of rho. Handling this case explicitly avoids the divide by zero // error that would occur below. // @@ -59,7 +59,7 @@ Corrector::Corrector(const double sq_norm, const double rho[3]) { // correction which re-wights the gradient of the function by the // square root of the derivative of rho, and the Gauss-Newton // Hessian gets both the scaling and the rank-1 curvature - // correction. Normaly, alpha is upper bounded by one, but with this + // correction. Normally, alpha is upper bounded by one, but with this // change, alpha is bounded above by zero. // // Empirically we have observed that the full Triggs correction and diff --git a/extern/ceres/internal/ceres/corrector.h b/extern/ceres/internal/ceres/corrector.h index 315f012ab1d..a5b03dda803 100644 --- a/extern/ceres/internal/ceres/corrector.h +++ b/extern/ceres/internal/ceres/corrector.h @@ -43,7 +43,7 @@ namespace internal { // radial robust loss. // // The key idea here is to look at the expressions for the robustified -// gauss newton approximation and then take its squareroot to get the +// gauss newton approximation and then take its square root to get the // corresponding corrections to the residual and jacobian. For the // full expressions see Eq. 10 and 11 in BANS by Triggs et al. class Corrector { diff --git a/extern/ceres/internal/ceres/covariance.cc b/extern/ceres/internal/ceres/covariance.cc index cb280a36847..8256078409a 100644 --- a/extern/ceres/internal/ceres/covariance.cc +++ b/extern/ceres/internal/ceres/covariance.cc @@ -50,15 +50,15 @@ Covariance::~Covariance() { } bool Covariance::Compute( - const vector<pair<const double*, const double*> >& covariance_blocks, + const vector<pair<const double*, const double*>>& covariance_blocks, Problem* problem) { - return impl_->Compute(covariance_blocks, problem->problem_impl_.get()); + return impl_->Compute(covariance_blocks, problem->impl_.get()); } bool Covariance::Compute( const vector<const double*>& parameter_blocks, Problem* problem) { - return impl_->Compute(parameter_blocks, problem->problem_impl_.get()); + return impl_->Compute(parameter_blocks, problem->impl_.get()); } bool Covariance::GetCovarianceBlock(const double* parameter_block1, @@ -82,7 +82,7 @@ bool Covariance::GetCovarianceBlockInTangentSpace( bool Covariance::GetCovarianceMatrix( const vector<const double*>& parameter_blocks, - double* covariance_matrix) { + double* covariance_matrix) const { return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks, true, // ambient covariance_matrix); @@ -90,7 +90,7 @@ bool Covariance::GetCovarianceMatrix( bool Covariance::GetCovarianceMatrixInTangentSpace( const std::vector<const double *>& parameter_blocks, - double *covariance_matrix) { + double *covariance_matrix) const { return impl_->GetCovarianceMatrixInTangentOrAmbientSpace(parameter_blocks, false, // tangent covariance_matrix); diff --git a/extern/ceres/internal/ceres/covariance_impl.cc b/extern/ceres/internal/ceres/covariance_impl.cc index d698f88fa9b..6c26412d854 100644 --- a/extern/ceres/internal/ceres/covariance_impl.cc +++ b/extern/ceres/internal/ceres/covariance_impl.cc @@ -30,14 +30,12 @@ #include "ceres/covariance_impl.h" -#ifdef CERES_USE_OPENMP -#include <omp.h> -#endif - #include <algorithm> #include <cstdlib> +#include <memory> #include <numeric> #include <sstream> +#include <unordered_set> #include <utility> #include <vector> @@ -45,13 +43,14 @@ #include "Eigen/SparseQR" #include "Eigen/SVD" -#include "ceres/collections_port.h" #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/parallel_for.h" +#include "ceres/parallel_utils.h" #include "ceres/parameter_block.h" #include "ceres/problem_impl.h" #include "ceres/residual_block.h" @@ -69,21 +68,22 @@ using std::sort; using std::swap; using std::vector; -typedef vector<pair<const double*, const double*> > CovarianceBlocks; +typedef vector<pair<const double*, const double*>> CovarianceBlocks; CovarianceImpl::CovarianceImpl(const Covariance::Options& options) : options_(options), is_computed_(false), is_valid_(false) { -#ifndef CERES_USE_OPENMP +#ifdef CERES_NO_THREADS if (options_.num_threads > 1) { LOG(WARNING) - << "OpenMP support is not compiled into this binary; " + << "No threading support is compiled into this binary; " << "only options.num_threads = 1 is supported. Switching " << "to single threaded mode."; options_.num_threads = 1; } #endif + evaluate_options_.num_threads = options_.num_threads; evaluate_options_.apply_loss_function = options_.apply_loss_function; } @@ -97,7 +97,7 @@ template <typename T> void CheckForDuplicates(vector<T> blocks) { std::adjacent_find(blocks.begin(), blocks.end()); if (it != blocks.end()) { // In case there are duplicates, we search for their location. - map<T, vector<int> > blocks_map; + map<T, vector<int>> blocks_map; for (int i = 0; i < blocks.size(); ++i) { blocks_map[blocks[i]].push_back(i); } @@ -122,7 +122,7 @@ template <typename T> void CheckForDuplicates(vector<T> blocks) { bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks, ProblemImpl* problem) { - CheckForDuplicates<pair<const double*, const double*> >(covariance_blocks); + CheckForDuplicates<pair<const double*, const double*>>(covariance_blocks); problem_ = problem; parameter_block_to_row_index_.clear(); covariance_matrix_.reset(NULL); @@ -333,55 +333,47 @@ bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace( // Assemble the blocks in the covariance matrix. MatrixRef covariance(covariance_matrix, covariance_size, covariance_size); const int num_threads = options_.num_threads; - scoped_array<double> workspace( + std::unique_ptr<double[]> workspace( new double[num_threads * max_covariance_block_size * max_covariance_block_size]); bool success = true; -// The collapse() directive is only supported in OpenMP 3.0 and higher. OpenMP -// 3.0 was released in May 2008 (hence the version number). -#if _OPENMP >= 200805 -# pragma omp parallel for num_threads(num_threads) schedule(dynamic) collapse(2) -#else -# pragma omp parallel for num_threads(num_threads) schedule(dynamic) -#endif - for (int i = 0; i < num_parameters; ++i) { - for (int j = 0; j < num_parameters; ++j) { - // The second loop can't start from j = i for compatibility with OpenMP - // collapse command. The conditional serves as a workaround - if (j >= i) { + // Technically the following code is a double nested loop where + // i = 1:n, j = i:n. + int iteration_count = (num_parameters * (num_parameters + 1)) / 2; + problem_->context()->EnsureMinimumThreads(num_threads); + ParallelFor( + problem_->context(), + 0, + iteration_count, + num_threads, + [&](int thread_id, int k) { + int i, j; + LinearIndexToUpperTriangularIndex(k, num_parameters, &i, &j); + int covariance_row_idx = cum_parameter_size[i]; int covariance_col_idx = cum_parameter_size[j]; int size_i = parameter_sizes[i]; int size_j = parameter_sizes[j]; -#ifdef CERES_USE_OPENMP - int thread_id = omp_get_thread_num(); -#else - int thread_id = 0; -#endif double* covariance_block = - workspace.get() + - thread_id * max_covariance_block_size * max_covariance_block_size; + workspace.get() + thread_id * max_covariance_block_size * + max_covariance_block_size; if (!GetCovarianceBlockInTangentOrAmbientSpace( - parameters[i], parameters[j], lift_covariance_to_ambient_space, - covariance_block)) { + parameters[i], parameters[j], + lift_covariance_to_ambient_space, covariance_block)) { success = false; } - covariance.block(covariance_row_idx, covariance_col_idx, - size_i, size_j) = - MatrixRef(covariance_block, size_i, size_j); + covariance.block(covariance_row_idx, covariance_col_idx, size_i, + size_j) = MatrixRef(covariance_block, size_i, size_j); if (i != j) { covariance.block(covariance_col_idx, covariance_row_idx, size_j, size_i) = MatrixRef(covariance_block, size_i, size_j).transpose(); - } - } - } - } + }); return success; } @@ -397,7 +389,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity( vector<double*> all_parameter_blocks; problem->GetParameterBlocks(&all_parameter_blocks); const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map(); - HashSet<ParameterBlock*> parameter_blocks_in_use; + std::unordered_set<ParameterBlock*> parameter_blocks_in_use; vector<ResidualBlock*> residual_blocks; problem->GetResidualBlocks(&residual_blocks); @@ -496,13 +488,10 @@ bool CovarianceImpl::ComputeCovarianceSparsity( // 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; + for (const auto& entry : parameter_block_to_row_index_) { + const double* row_block = entry.first; const int row_block_size = problem->ParameterBlockLocalSize(row_block); - int row_begin = it->second; + int row_begin = entry.second; // Iterate over the covariance blocks contained in this row block // and count the number of columns in this row block. @@ -538,24 +527,37 @@ bool CovarianceImpl::ComputeCovarianceSparsity( } bool CovarianceImpl::ComputeCovarianceValues() { - switch (options_.algorithm_type) { - case DENSE_SVD: - return ComputeCovarianceValuesUsingDenseSVD(); - case SUITE_SPARSE_QR: -#ifndef CERES_NO_SUITESPARSE + if (options_.algorithm_type == DENSE_SVD) { + return ComputeCovarianceValuesUsingDenseSVD(); + } + + if (options_.algorithm_type == SPARSE_QR) { + if (options_.sparse_linear_algebra_library_type == EIGEN_SPARSE) { + return ComputeCovarianceValuesUsingEigenSparseQR(); + } + + if (options_.sparse_linear_algebra_library_type == SUITE_SPARSE) { +#if !defined(CERES_NO_SUITESPARSE) return ComputeCovarianceValuesUsingSuiteSparseQR(); #else - LOG(ERROR) << "SuiteSparse is required to use the " - << "SUITE_SPARSE_QR algorithm."; + LOG(ERROR) << "SuiteSparse is required to use the SPARSE_QR algorithm " + << "with " + << "Covariance::Options::sparse_linear_algebra_library_type " + << "= SUITE_SPARSE."; return false; #endif - case EIGEN_SPARSE_QR: - return ComputeCovarianceValuesUsingEigenSparseQR(); - default: - LOG(ERROR) << "Unsupported covariance estimation algorithm type: " - << CovarianceAlgorithmTypeToString(options_.algorithm_type); - return false; + } + + LOG(ERROR) << "Unsupported " + << "Covariance::Options::sparse_linear_algebra_library_type " + << "= " + << SparseLinearAlgebraLibraryTypeToString( + options_.sparse_linear_algebra_library_type); + return false; } + + LOG(ERROR) << "Unsupported Covariance::Options::algorithm_type = " + << CovarianceAlgorithmTypeToString(options_.algorithm_type); return false; } @@ -649,8 +651,12 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { &permutation, &cc); event_logger.AddEvent("Numeric Factorization"); - CHECK_NOTNULL(permutation); - CHECK_NOTNULL(R); + if (R == nullptr) { + LOG(ERROR) << "Something is wrong. SuiteSparseQR returned R = nullptr."; + free(permutation); + cholmod_l_finish(&cc); + return false; + } if (rank < cholmod_jacobian.ncol) { LOG(ERROR) << "Jacobian matrix is rank deficient. " @@ -663,8 +669,14 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { } vector<int> inverse_permutation(num_cols); - for (SuiteSparse_long i = 0; i < num_cols; ++i) { - inverse_permutation[permutation[i]] = i; + if (permutation) { + for (SuiteSparse_long i = 0; i < num_cols; ++i) { + inverse_permutation[permutation[i]] = i; + } + } else { + for (SuiteSparse_long i = 0; i < num_cols; ++i) { + inverse_permutation[i] = i; + } } const int* rows = covariance_matrix_->rows(); @@ -681,35 +693,29 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() { // 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]]; - } - } + std::unique_ptr<double[]> workspace(new double[num_threads * num_cols]); + + problem_->context()->EnsureMinimumThreads(num_threads); + ParallelFor( + problem_->context(), + 0, + num_cols, + num_threads, + [&](int thread_id, int r) { + const int row_begin = rows[r]; + const int row_end = rows[r + 1]; + if (row_end != row_begin) { + 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]]; + } + } + }); free(permutation); cholmod_l_free_sparse(&R, &cc); @@ -746,8 +752,8 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() { } event_logger.AddEvent("ConvertToDenseMatrix"); - Eigen::JacobiSVD<Matrix> svd(dense_jacobian, - Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::BDCSVD<Matrix> svd(dense_jacobian, + Eigen::ComputeThinU | Eigen::ComputeThinV); event_logger.AddEvent("SingularValueDecomposition"); @@ -838,7 +844,7 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() { jacobian.rows.data(), jacobian.cols.data(), jacobian.values.data()); event_logger.AddEvent("ConvertToSparseMatrix"); - Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int> > + Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int>> qr_solver(sparse_jacobian); event_logger.AddEvent("QRDecomposition"); @@ -873,38 +879,35 @@ bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() { // are equal. const int num_cols = jacobian.num_cols; const int num_threads = options_.num_threads; - scoped_array<double> workspace(new double[num_threads * num_cols]); - -#pragma omp parallel for num_threads(num_threads) schedule(dynamic) - for (int r = 0; r < num_cols; ++r) { - const int row_begin = rows[r]; - const int row_end = rows[r + 1]; - if (row_end == row_begin) { - continue; - } - -# ifdef CERES_USE_OPENMP - int thread_id = omp_get_thread_num(); -# else - int thread_id = 0; -# endif - - double* solution = workspace.get() + thread_id * num_cols; - SolveRTRWithSparseRHS<int>( - num_cols, - qr_solver.matrixR().innerIndexPtr(), - qr_solver.matrixR().outerIndexPtr(), - &qr_solver.matrixR().data().value(0), - inverse_permutation.indices().coeff(r), - solution); - - // Assign the values of the computed covariance using the - // inverse permutation used in the QR factorization. - for (int idx = row_begin; idx < row_end; ++idx) { - const int c = cols[idx]; - values[idx] = solution[inverse_permutation.indices().coeff(c)]; - } - } + std::unique_ptr<double[]> workspace(new double[num_threads * num_cols]); + + problem_->context()->EnsureMinimumThreads(num_threads); + ParallelFor( + problem_->context(), + 0, + num_cols, + num_threads, + [&](int thread_id, int r) { + const int row_begin = rows[r]; + const int row_end = rows[r + 1]; + if (row_end != row_begin) { + double* solution = workspace.get() + thread_id * num_cols; + SolveRTRWithSparseRHS<int>( + num_cols, + qr_solver.matrixR().innerIndexPtr(), + qr_solver.matrixR().outerIndexPtr(), + &qr_solver.matrixR().data().value(0), + inverse_permutation.indices().coeff(r), + solution); + + // Assign the values of the computed covariance using the + // inverse permutation used in the QR factorization. + for (int idx = row_begin; idx < row_end; ++idx) { + const int c = cols[idx]; + values[idx] = solution[inverse_permutation.indices().coeff(c)]; + } + } + }); event_logger.AddEvent("Inverse"); diff --git a/extern/ceres/internal/ceres/covariance_impl.h b/extern/ceres/internal/ceres/covariance_impl.h index a3f0761f57c..065e43c60fc 100644 --- a/extern/ceres/internal/ceres/covariance_impl.h +++ b/extern/ceres/internal/ceres/covariance_impl.h @@ -32,11 +32,11 @@ #define CERES_INTERNAL_COVARIANCE_IMPL_H_ #include <map> +#include <memory> #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" @@ -52,7 +52,7 @@ class CovarianceImpl { bool Compute( const std::vector<std::pair<const double*, - const double*> >& covariance_blocks, + const double*>>& covariance_blocks, ProblemImpl* problem); bool Compute( @@ -72,7 +72,7 @@ class CovarianceImpl { bool ComputeCovarianceSparsity( const std::vector<std::pair<const double*, - const double*> >& covariance_blocks, + const double*>>& covariance_blocks, ProblemImpl* problem); bool ComputeCovarianceValues(); @@ -92,7 +92,7 @@ class CovarianceImpl { bool is_valid_; std::map<const double*, int> parameter_block_to_row_index_; std::set<const double*> constant_parameter_blocks_; - scoped_ptr<CompressedRowSparseMatrix> covariance_matrix_; + std::unique_ptr<CompressedRowSparseMatrix> covariance_matrix_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/cxsparse.cc b/extern/ceres/internal/ceres/cxsparse.cc new file mode 100644 index 00000000000..5a028773206 --- /dev/null +++ b/extern/ceres/internal/ceres/cxsparse.cc @@ -0,0 +1,284 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: strandmark@google.com (Petter Strandmark) + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_NO_CXSPARSE + +#include "ceres/cxsparse.h" + +#include <string> +#include <vector> + +#include "ceres/compressed_col_sparse_matrix_utils.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::vector; + +CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) {} + +CXSparse::~CXSparse() { + if (scratch_size_ > 0) { + cs_di_free(scratch_); + } +} + +csn* CXSparse::Cholesky(cs_di* A, cs_dis* symbolic_factor) { + return cs_di_chol(A, symbolic_factor); +} + +void CXSparse::Solve(cs_dis* symbolic_factor, csn* numeric_factor, double* b) { + // Make sure we have enough scratch space available. + const int num_cols = numeric_factor->L->n; + if (scratch_size_ < num_cols) { + if (scratch_size_ > 0) { + cs_di_free(scratch_); + } + scratch_ = + reinterpret_cast<CS_ENTRY*>(cs_di_malloc(num_cols, sizeof(CS_ENTRY))); + scratch_size_ = num_cols; + } + + // When the Cholesky factor succeeded, these methods are + // guaranteed to succeeded as well. In the comments below, "x" + // refers to the scratch space. + // + // Set x = P * b. + CHECK(cs_di_ipvec(symbolic_factor->pinv, b, scratch_, num_cols)); + // Set x = L \ x. + CHECK(cs_di_lsolve(numeric_factor->L, scratch_)); + // Set x = L' \ x. + CHECK(cs_di_ltsolve(numeric_factor->L, scratch_)); + // Set b = P' * x. + CHECK(cs_di_pvec(symbolic_factor->pinv, scratch_, b, num_cols)); +} + +bool CXSparse::SolveCholesky(cs_di* lhs, double* rhs_and_solution) { + return cs_cholsol(1, lhs, rhs_and_solution); +} + +cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) { + // order = 1 for Cholesky factor. + 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); + std::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_factor = + reinterpret_cast<cs_dis*>(cs_calloc(1, sizeof(cs_dis))); + symbolic_factor->pinv = cs_pinv(&scalar_ordering[0], A->n); + cs* permuted_A = cs_symperm(A, symbolic_factor->pinv, 0); + + symbolic_factor->parent = cs_etree(permuted_A, 0); + int* postordering = cs_post(symbolic_factor->parent, A->n); + int* column_counts = + cs_counts(permuted_A, symbolic_factor->parent, postordering, 0); + cs_free(postordering); + cs_spfree(permuted_A); + + symbolic_factor->cp = (int*)cs_malloc(A->n + 1, sizeof(int)); + symbolic_factor->lnz = cs_cumsum(symbolic_factor->cp, column_counts, A->n); + symbolic_factor->unz = symbolic_factor->lnz; + + cs_free(column_counts); + + if (symbolic_factor->lnz < 0) { + cs_sfree(symbolic_factor); + symbolic_factor = NULL; + } + + return symbolic_factor; +} + +cs_di CXSparse::CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A) { + cs_di At; + At.m = A->num_cols(); + At.n = A->num_rows(); + At.nz = -1; + At.nzmax = A->num_nonzeros(); + At.p = A->mutable_rows(); + At.i = A->mutable_cols(); + At.x = A->mutable_values(); + return At; +} + +cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) { + cs_di_sparse tsm_wrapper; + tsm_wrapper.nzmax = tsm->num_nonzeros(); + tsm_wrapper.nz = tsm->num_nonzeros(); + tsm_wrapper.m = tsm->num_rows(); + tsm_wrapper.n = tsm->num_cols(); + tsm_wrapper.p = tsm->mutable_cols(); + tsm_wrapper.i = tsm->mutable_rows(); + tsm_wrapper.x = tsm->mutable_values(); + + return cs_compress(&tsm_wrapper); +} + +void CXSparse::ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering) { + int* cs_ordering = cs_amd(1, A); + std::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); } + +void CXSparse::Free(cs_dis* symbolic_factor) { cs_di_sfree(symbolic_factor); } + +void CXSparse::Free(csn* numeric_factor) { cs_di_nfree(numeric_factor); } + +std::unique_ptr<SparseCholesky> CXSparseCholesky::Create( + const OrderingType ordering_type) { + return std::unique_ptr<SparseCholesky>(new CXSparseCholesky(ordering_type)); +} + +CompressedRowSparseMatrix::StorageType CXSparseCholesky::StorageType() const { + return CompressedRowSparseMatrix::LOWER_TRIANGULAR; +} + +CXSparseCholesky::CXSparseCholesky(const OrderingType ordering_type) + : ordering_type_(ordering_type), + symbolic_factor_(NULL), + numeric_factor_(NULL) {} + +CXSparseCholesky::~CXSparseCholesky() { + FreeSymbolicFactorization(); + FreeNumericFactorization(); +} + +LinearSolverTerminationType CXSparseCholesky::Factorize( + CompressedRowSparseMatrix* lhs, std::string* message) { + CHECK_EQ(lhs->storage_type(), StorageType()); + if (lhs == NULL) { + *message = "Failure: Input lhs is NULL."; + return LINEAR_SOLVER_FATAL_ERROR; + } + + cs_di cs_lhs = cs_.CreateSparseMatrixTransposeView(lhs); + + if (symbolic_factor_ == NULL) { + if (ordering_type_ == NATURAL) { + symbolic_factor_ = cs_.AnalyzeCholeskyWithNaturalOrdering(&cs_lhs); + } else { + if (!lhs->col_blocks().empty() && !(lhs->row_blocks().empty())) { + symbolic_factor_ = cs_.BlockAnalyzeCholesky( + &cs_lhs, lhs->col_blocks(), lhs->row_blocks()); + } else { + symbolic_factor_ = cs_.AnalyzeCholesky(&cs_lhs); + } + } + + if (symbolic_factor_ == NULL) { + *message = "CXSparse Failure : Symbolic factorization failed."; + return LINEAR_SOLVER_FATAL_ERROR; + } + } + + FreeNumericFactorization(); + numeric_factor_ = cs_.Cholesky(&cs_lhs, symbolic_factor_); + if (numeric_factor_ == NULL) { + *message = "CXSparse Failure : Numeric factorization failed."; + return LINEAR_SOLVER_FAILURE; + } + + return LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType CXSparseCholesky::Solve(const double* rhs, + double* solution, + std::string* message) { + CHECK(numeric_factor_ != NULL) + << "Solve called without a call to Factorize first."; + const int num_cols = numeric_factor_->L->n; + memcpy(solution, rhs, num_cols * sizeof(*solution)); + cs_.Solve(symbolic_factor_, numeric_factor_, solution); + return LINEAR_SOLVER_SUCCESS; +} + +void CXSparseCholesky::FreeSymbolicFactorization() { + if (symbolic_factor_ != NULL) { + cs_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } +} + +void CXSparseCholesky::FreeNumericFactorization() { + if (numeric_factor_ != NULL) { + cs_.Free(numeric_factor_); + numeric_factor_ = NULL; + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_CXSPARSE diff --git a/extern/ceres/internal/ceres/cxsparse.h b/extern/ceres/internal/ceres/cxsparse.h index 26dd1927a78..dc4740ceaee 100644 --- a/extern/ceres/internal/ceres/cxsparse.h +++ b/extern/ceres/internal/ceres/cxsparse.h @@ -36,7 +36,12 @@ #ifndef CERES_NO_CXSPARSE +#include <memory> +#include <string> #include <vector> + +#include "ceres/linear_solver.h" +#include "ceres/sparse_cholesky.h" #include "cs.h" namespace ceres { @@ -47,21 +52,27 @@ class TripletSparseMatrix; // This object provides access to solving linear systems using Cholesky // factorization with a known symbolic factorization. This features does not -// explicity exist in CXSparse. The methods in the class are nonstatic because +// explicitly exist in CXSparse. The methods in the class are nonstatic because // the class manages internal scratch space. class CXSparse { public: CXSparse(); ~CXSparse(); - // Solves a symmetric linear system A * x = b using Cholesky factorization. - // A - The system matrix. - // symbolic_factorization - The symbolic factorization of A. This is obtained - // from AnalyzeCholesky. - // b - The right hand size of the linear equation. This - // array will also recieve the solution. - // Returns false if Cholesky factorization of A fails. - bool SolveCholesky(cs_di* A, cs_dis* symbolic_factorization, double* b); + // Solve the system lhs * solution = rhs in place by using an + // approximate minimum degree fill reducing ordering. + bool SolveCholesky(cs_di* lhs, double* rhs_and_solution); + + // Solves a linear system given its symbolic and numeric factorization. + void Solve(cs_dis* symbolic_factor, + csn* numeric_factor, + double* rhs_and_solution); + + // Compute the numeric Cholesky factorization of A, given its + // symbolic factorization. + // + // Caller owns the result. + csn* Cholesky(cs_di* A, cs_dis* symbolic_factor); // Creates a sparse matrix from a compressed-column form. No memory is // allocated or copied; the structure A is filled out with info from the @@ -117,6 +128,7 @@ class CXSparse { void Free(cs_di* sparse_matrix); void Free(cs_dis* symbolic_factorization); + void Free(csn* numeric_factorization); private: // Cached scratch space @@ -124,10 +136,37 @@ class CXSparse { int scratch_size_; }; +// An implementation of SparseCholesky interface using the CXSparse +// library. +class CXSparseCholesky : public SparseCholesky { + public: + // Factory + static std::unique_ptr<SparseCholesky> Create(OrderingType ordering_type); + + // SparseCholesky interface. + virtual ~CXSparseCholesky(); + CompressedRowSparseMatrix::StorageType StorageType() const final; + LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, + std::string* message) final; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) final; + + private: + CXSparseCholesky(const OrderingType ordering_type); + void FreeSymbolicFactorization(); + void FreeNumericFactorization(); + + const OrderingType ordering_type_; + CXSparse cs_; + cs_dis* symbolic_factor_; + csn* numeric_factor_; +}; + } // namespace internal } // namespace ceres -#else // CERES_NO_CXSPARSE +#else // CERES_NO_CXSPARSE typedef void cs_dis; diff --git a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc index b13cf3fc9f6..fe7d931a3fd 100644 --- a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc +++ b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.cc @@ -36,7 +36,6 @@ #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" diff --git a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h index 11287ebf675..976718e8615 100644 --- a/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h +++ b/extern/ceres/internal/ceres/dense_normal_cholesky_solver.h @@ -35,7 +35,6 @@ #define CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ #include "ceres/linear_solver.h" -#include "ceres/internal/macros.h" namespace ceres { namespace internal { @@ -79,11 +78,11 @@ class DenseNormalCholeskySolver: public DenseSparseMatrixSolver { explicit DenseNormalCholeskySolver(const LinearSolver::Options& options); private: - virtual LinearSolver::Summary SolveImpl( + LinearSolver::Summary SolveImpl( DenseSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, - double* x); + double* x) final; LinearSolver::Summary SolveUsingLAPACK( DenseSparseMatrix* A, @@ -98,7 +97,6 @@ class DenseNormalCholeskySolver: public DenseSparseMatrixSolver { double* x); const LinearSolver::Options options_; - CERES_DISALLOW_COPY_AND_ASSIGN(DenseNormalCholeskySolver); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/dense_qr_solver.cc b/extern/ceres/internal/ceres/dense_qr_solver.cc index e85fdfc0c68..161e9c67a00 100644 --- a/extern/ceres/internal/ceres/dense_qr_solver.cc +++ b/extern/ceres/internal/ceres/dense_qr_solver.cc @@ -30,12 +30,10 @@ #include "ceres/dense_qr_solver.h" - #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" diff --git a/extern/ceres/internal/ceres/dense_qr_solver.h b/extern/ceres/internal/ceres/dense_qr_solver.h index 1a6e0898c56..9ea959db68d 100644 --- a/extern/ceres/internal/ceres/dense_qr_solver.h +++ b/extern/ceres/internal/ceres/dense_qr_solver.h @@ -34,7 +34,6 @@ #include "ceres/linear_solver.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/macros.h" namespace ceres { namespace internal { @@ -84,11 +83,11 @@ class DenseQRSolver: public DenseSparseMatrixSolver { explicit DenseQRSolver(const LinearSolver::Options& options); private: - virtual LinearSolver::Summary SolveImpl( + LinearSolver::Summary SolveImpl( DenseSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, - double* x); + double* x) final; LinearSolver::Summary SolveUsingEigen( DenseSparseMatrix* A, @@ -106,7 +105,6 @@ class DenseQRSolver: public DenseSparseMatrixSolver { ColMajorMatrix lhs_; Vector rhs_; Vector work_; - CERES_DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/dense_sparse_matrix.cc b/extern/ceres/internal/ceres/dense_sparse_matrix.cc index 19db867d4aa..72e08360dd0 100644 --- a/extern/ceres/internal/ceres/dense_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/dense_sparse_matrix.cc @@ -166,7 +166,7 @@ ColMajorMatrixRef DenseSparseMatrix::mutable_matrix() { void DenseSparseMatrix::ToTextFile(FILE* file) const { - CHECK_NOTNULL(file); + CHECK(file != nullptr); const int active_rows = (has_diagonal_reserved_ && !has_diagonal_appended_) ? (m_.rows() - m_.cols()) diff --git a/extern/ceres/internal/ceres/dense_sparse_matrix.h b/extern/ceres/internal/ceres/dense_sparse_matrix.h index b011bfddee7..6d3d504ea36 100644 --- a/extern/ceres/internal/ceres/dense_sparse_matrix.h +++ b/extern/ceres/internal/ceres/dense_sparse_matrix.h @@ -33,10 +33,8 @@ #ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ #define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ -#include "ceres/sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/macros.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/sparse_matrix.h" #include "ceres/types.h" namespace ceres { @@ -57,18 +55,18 @@ class DenseSparseMatrix : public SparseMatrix { virtual ~DenseSparseMatrix() {} // SparseMatrix interface. - virtual void SetZero(); - virtual void RightMultiply(const double* x, double* y) const; - virtual void LeftMultiply(const double* x, double* y) const; - virtual void SquaredColumnNorm(double* x) const; - virtual void ScaleColumns(const double* scale); - virtual void ToDenseMatrix(Matrix* dense_matrix) const; - virtual void ToTextFile(FILE* file) const; - virtual int num_rows() const; - virtual int num_cols() const; - virtual int num_nonzeros() const; - virtual const double* values() const { return m_.data(); } - virtual double* mutable_values() { return m_.data(); } + void SetZero() final; + void RightMultiply(const double* x, double* y) const final; + void LeftMultiply(const double* x, double* y) const final; + void SquaredColumnNorm(double* x) const final; + void ScaleColumns(const double* scale) final; + void ToDenseMatrix(Matrix* dense_matrix) const final; + void ToTextFile(FILE* file) const final; + int num_rows() const final; + int num_cols() const final; + int num_nonzeros() const final; + const double* values() const final { return m_.data(); } + double* mutable_values() final { return m_.data(); } ConstColMajorMatrixRef matrix() const; ColMajorMatrixRef mutable_matrix(); diff --git a/extern/ceres/internal/ceres/dogleg_strategy.cc b/extern/ceres/internal/ceres/dogleg_strategy.cc index 839e1816338..ecc6b882338 100644 --- a/extern/ceres/internal/ceres/dogleg_strategy.cc +++ b/extern/ceres/internal/ceres/dogleg_strategy.cc @@ -30,7 +30,9 @@ #include "ceres/dogleg_strategy.h" +#include <algorithm> #include <cmath> + #include "Eigen/Dense" #include "ceres/array_utils.h" #include "ceres/internal/eigen.h" @@ -64,7 +66,7 @@ DoglegStrategy::DoglegStrategy(const TrustRegionStrategy::Options& options) dogleg_step_norm_(0.0), reuse_(false), dogleg_type_(options.dogleg_type) { - CHECK_NOTNULL(linear_solver_); + CHECK(linear_solver_ != nullptr); CHECK_GT(min_diagonal_, 0.0); CHECK_LE(min_diagonal_, max_diagonal_); CHECK_GT(max_radius_, 0.0); @@ -79,9 +81,9 @@ TrustRegionStrategy::Summary DoglegStrategy::ComputeStep( SparseMatrix* jacobian, const double* residuals, double* step) { - CHECK_NOTNULL(jacobian); - CHECK_NOTNULL(residuals); - CHECK_NOTNULL(step); + CHECK(jacobian != nullptr); + CHECK(residuals != nullptr); + CHECK(step != nullptr); const int n = jacobian->num_cols(); if (reuse_) { @@ -469,7 +471,7 @@ double DoglegStrategy::EvaluateSubspaceModel(const Vector2d& x) const { // In the failure case, another step should be taken, such as the traditional // dogleg step. bool DoglegStrategy::FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const { - CHECK_NOTNULL(minimum); + CHECK(minimum != nullptr); // Return (0, 0) in all error cases. minimum->setZero(); diff --git a/extern/ceres/internal/ceres/dogleg_strategy.h b/extern/ceres/internal/ceres/dogleg_strategy.h index 046b9d824c9..1150940efd3 100644 --- a/extern/ceres/internal/ceres/dogleg_strategy.h +++ b/extern/ceres/internal/ceres/dogleg_strategy.h @@ -58,15 +58,14 @@ class DoglegStrategy : public TrustRegionStrategy { virtual ~DoglegStrategy() {} // TrustRegionStrategy interface - virtual Summary ComputeStep(const PerSolveOptions& per_solve_options, + Summary ComputeStep(const PerSolveOptions& per_solve_options, SparseMatrix* jacobian, const double* residuals, - double* step); - virtual void StepAccepted(double step_quality); - virtual void StepRejected(double step_quality); - virtual void StepIsInvalid(); - - virtual double Radius() const; + double* step) final; + void StepAccepted(double step_quality) final; + void StepRejected(double step_quality) final; + void StepIsInvalid(); + double Radius() const final; // These functions are predominantly for testing. Vector gradient() const { return gradient_; } @@ -103,7 +102,7 @@ class DoglegStrategy : public TrustRegionStrategy { // mu is used to scale the diagonal matrix used to make the // Gauss-Newton solve full rank. In each solve, the strategy starts - // out with mu = min_mu, and tries values upto max_mu. If the user + // out with mu = min_mu, and tries values up to max_mu. If the user // reports an invalid step, the value of mu_ is increased so that // the next solve starts with a stronger regularization. // diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc index fd5d89e350a..1749449043e 100644 --- a/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_jacobian_writer.cc @@ -28,9 +28,10 @@ // // Author: richie.stebbing@gmail.com (Richard Stebbing) -#include "ceres/compressed_row_jacobian_writer.h" #include "ceres/dynamic_compressed_row_jacobian_writer.h" + #include "ceres/casts.h" +#include "ceres/compressed_row_jacobian_writer.h" #include "ceres/dynamic_compressed_row_sparse_matrix.h" #include "ceres/parameter_block.h" #include "ceres/program.h" @@ -48,43 +49,28 @@ DynamicCompressedRowJacobianWriter::CreateEvaluatePreparers(int num_threads) { } SparseMatrix* DynamicCompressedRowJacobianWriter::CreateJacobian() const { - // Initialize `jacobian` with zero number of `max_num_nonzeros`. - const int num_residuals = program_->NumResiduals(); - const int num_effective_parameters = program_->NumEffectiveParameters(); - DynamicCompressedRowSparseMatrix* jacobian = - new DynamicCompressedRowSparseMatrix(num_residuals, - num_effective_parameters, - 0); - - vector<int>* row_blocks = jacobian->mutable_row_blocks(); - for (int i = 0; i < jacobian->num_rows(); ++i) { - row_blocks->push_back(1); - } - - vector<int>* col_blocks = jacobian->mutable_col_blocks(); - for (int i = 0; i < jacobian->num_cols(); ++i) { - col_blocks->push_back(1); - } - + new DynamicCompressedRowSparseMatrix(program_->NumResiduals(), + program_->NumEffectiveParameters(), + 0 /* max_num_nonzeros */); return jacobian; } void DynamicCompressedRowJacobianWriter::Write(int residual_id, int residual_offset, - double **jacobians, + double** jacobians, SparseMatrix* base_jacobian) { DynamicCompressedRowSparseMatrix* jacobian = - down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); + down_cast<DynamicCompressedRowSparseMatrix*>(base_jacobian); // Get the `residual_block` of interest. const ResidualBlock* residual_block = program_->residual_blocks()[residual_id]; const int num_residuals = residual_block->NumResiduals(); - vector<pair<int, int> > evaluated_jacobian_blocks; + vector<pair<int, int>> evaluated_jacobian_blocks; CompressedRowJacobianWriter::GetOrderedParameterBlocks( - program_, residual_id, &evaluated_jacobian_blocks); + program_, residual_id, &evaluated_jacobian_blocks); // `residual_offset` is the residual row in the global jacobian. // Empty the jacobian rows. @@ -97,16 +83,17 @@ void DynamicCompressedRowJacobianWriter::Write(int residual_id, const int parameter_block_jacobian_index = evaluated_jacobian_blocks[i].second; const int parameter_block_size = parameter_block->LocalSize(); + const double* parameter_jacobian = + jacobians[parameter_block_jacobian_index]; // For each parameter block only insert its non-zero entries. for (int r = 0; r < num_residuals; ++r) { - for (int c = 0; c < parameter_block_size; ++c) { - const double& v = jacobians[parameter_block_jacobian_index][ - r * parameter_block_size + c]; + for (int c = 0; c < parameter_block_size; ++c, ++parameter_jacobian) { + const double v = *parameter_jacobian; // Only insert non-zero entries. if (v != 0.0) { jacobian->InsertEntry( - residual_offset + r, parameter_block->delta_offset() + c, v); + residual_offset + r, parameter_block->delta_offset() + c, v); } } } diff --git a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h index cab860bddbd..ad41da7b15a 100644 --- a/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h +++ b/extern/ceres/internal/ceres/dynamic_compressed_row_sparse_matrix.h @@ -91,8 +91,8 @@ class DynamicCompressedRowSparseMatrix : public CompressedRowSparseMatrix { void Finalize(int num_additional_elements); private: - std::vector<std::vector<int> > dynamic_cols_; - std::vector<std::vector<double> > dynamic_values_; + std::vector<std::vector<int>> dynamic_cols_; + std::vector<std::vector<double>> dynamic_values_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc new file mode 100644 index 00000000000..25d5417bca8 --- /dev/null +++ b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc @@ -0,0 +1,286 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/dynamic_sparse_normal_cholesky_solver.h" + +#include <algorithm> +#include <cstring> +#include <ctime> +#include <memory> +#include <sstream> + +#include "Eigen/SparseCore" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/cxsparse.h" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" + +#ifdef CERES_USE_EIGEN_SPARSE +#include "Eigen/SparseCholesky" +#endif + +namespace ceres { +namespace internal { + +DynamicSparseNormalCholeskySolver::DynamicSparseNormalCholeskySolver( + const LinearSolver::Options& options) + : options_(options) {} + +LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + const int num_cols = A->num_cols(); + VectorRef(x, num_cols).setZero(); + A->LeftMultiply(b, x); + + if (per_solve_options.D != nullptr) { + // Temporarily append a diagonal block to the A matrix, but undo + // it before returning the matrix to the user. + std::unique_ptr<CompressedRowSparseMatrix> regularizer; + if (!A->col_blocks().empty()) { + regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + per_solve_options.D, A->col_blocks())); + } else { + regularizer.reset( + new CompressedRowSparseMatrix(per_solve_options.D, num_cols)); + } + A->AppendRows(*regularizer); + } + + LinearSolver::Summary summary; + switch (options_.sparse_linear_algebra_library_type) { + case SUITE_SPARSE: + summary = SolveImplUsingSuiteSparse(A, x); + break; + case CX_SPARSE: + summary = SolveImplUsingCXSparse(A, x); + break; + case EIGEN_SPARSE: + summary = SolveImplUsingEigen(A, x); + break; + default: + LOG(FATAL) << "Unsupported sparse linear algebra library for " + << "dynamic sparsity: " + << SparseLinearAlgebraLibraryTypeToString( + options_.sparse_linear_algebra_library_type); + } + + if (per_solve_options.D != nullptr) { + A->DeleteRows(num_cols); + } + + return summary; +} + +LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImplUsingEigen( + CompressedRowSparseMatrix* A, double* rhs_and_solution) { +#ifndef CERES_USE_EIGEN_SPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE " + "because Ceres was not built with support for " + "Eigen's SimplicialLDLT decomposition. " + "This requires enabling building with -DEIGENSPARSE=ON."; + return summary; + +#else + + EventLogger event_logger("DynamicSparseNormalCholeskySolver::Eigen::Solve"); + + Eigen::MappedSparseMatrix<double, Eigen::RowMajor> a(A->num_rows(), + A->num_cols(), + A->num_nonzeros(), + A->mutable_rows(), + A->mutable_cols(), + A->mutable_values()); + + Eigen::SparseMatrix<double> lhs = a.transpose() * a; + Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver; + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + solver.analyzePattern(lhs); + if (VLOG_IS_ON(2)) { + std::stringstream ss; + solver.dumpMemory(ss); + VLOG(2) << "Symbolic Analysis\n" << ss.str(); + } + + event_logger.AddEvent("Analyze"); + if (solver.info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = "Eigen failure. Unable to find symbolic factorization."; + return summary; + } + + solver.factorize(lhs); + event_logger.AddEvent("Factorize"); + if (solver.info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to find numeric factorization."; + return summary; + } + + const Vector rhs = VectorRef(rhs_and_solution, lhs.cols()); + VectorRef(rhs_and_solution, lhs.cols()) = solver.solve(rhs); + event_logger.AddEvent("Solve"); + if (solver.info() != Eigen::Success) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Eigen failure. Unable to do triangular solve."; + return summary; + } + + return summary; +#endif // CERES_USE_EIGEN_SPARSE +} + +LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImplUsingCXSparse( + CompressedRowSparseMatrix* A, double* rhs_and_solution) { +#ifdef CERES_NO_CXSPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE " + "because Ceres was not built with support for CXSparse. " + "This requires enabling building with -DCXSPARSE=ON."; + + return summary; + +#else + EventLogger event_logger( + "DynamicSparseNormalCholeskySolver::CXSparse::Solve"); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.message = "Success."; + + CXSparse cxsparse; + + // Wrap the augmented Jacobian in a compressed sparse column matrix. + cs_di a_transpose = cxsparse.CreateSparseMatrixTransposeView(A); + + // Compute the normal equations. J'J delta = J'f and solve them + // using a sparse Cholesky factorization. Notice that when compared + // to SuiteSparse we have to explicitly compute the transpose of Jt, + // and then the normal equations before they can be + // factorized. CHOLMOD/SuiteSparse on the other hand can just work + // off of Jt to compute the Cholesky factorization of the normal + // equations. + cs_di* a = cxsparse.TransposeMatrix(&a_transpose); + cs_di* lhs = cxsparse.MatrixMatrixMultiply(&a_transpose, a); + cxsparse.Free(a); + event_logger.AddEvent("NormalEquations"); + + if (!cxsparse.SolveCholesky(lhs, rhs_and_solution)) { + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "CXSparse::SolveCholesky failed"; + } + event_logger.AddEvent("Solve"); + + cxsparse.Free(lhs); + event_logger.AddEvent("TearDown"); + return summary; +#endif +} + +LinearSolver::Summary +DynamicSparseNormalCholeskySolver::SolveImplUsingSuiteSparse( + CompressedRowSparseMatrix* A, double* rhs_and_solution) { +#ifdef CERES_NO_SUITESPARSE + + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + summary.message = + "SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE " + "because Ceres was not built with support for SuiteSparse. " + "This requires enabling building with -DSUITESPARSE=ON."; + return summary; + +#else + + EventLogger event_logger( + "DynamicSparseNormalCholeskySolver::SuiteSparse::Solve"); + LinearSolver::Summary summary; + summary.termination_type = LINEAR_SOLVER_SUCCESS; + summary.num_iterations = 1; + summary.message = "Success."; + + SuiteSparse ss; + const int num_cols = A->num_cols(); + cholmod_sparse lhs = ss.CreateSparseMatrixTransposeView(A); + event_logger.AddEvent("Setup"); + cholmod_factor* factor = ss.AnalyzeCholesky(&lhs, &summary.message); + event_logger.AddEvent("Analysis"); + + if (factor == nullptr) { + summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; + return summary; + } + + summary.termination_type = ss.Cholesky(&lhs, factor, &summary.message); + if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { + cholmod_dense cholmod_rhs = + ss.CreateDenseVectorView(rhs_and_solution, num_cols); + cholmod_dense* solution = ss.Solve(factor, &cholmod_rhs, &summary.message); + event_logger.AddEvent("Solve"); + if (solution != nullptr) { + memcpy( + rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution)); + ss.Free(solution); + } else { + summary.termination_type = LINEAR_SOLVER_FAILURE; + } + } + + ss.Free(factor); + event_logger.AddEvent("Teardown"); + return summary; + +#endif +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.h b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.h new file mode 100644 index 00000000000..4e31c7a8492 --- /dev/null +++ b/extern/ceres/internal/ceres/dynamic_sparse_normal_cholesky_solver.h @@ -0,0 +1,86 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// A solver for sparse linear least squares problem based on solving +// the normal equations via a sparse cholesky factorization. + +#ifndef CERES_INTERNAL_DYNAMIC_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ +#define CERES_INTERNAL_DYNAMIC_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; + +// A variant of SparseNormalCholeskySolver in the case where matrix +// sparsity is not constant across calls to Solve. This means that +// there is no benefit to symbolically factorizing the matrix and +// caching this factorization. +// +// TODO(alex): Add support for Accelerate sparse solvers: +// https://github.com/ceres-solver/ceres-solver/issues/397 +class DynamicSparseNormalCholeskySolver + : public CompressedRowSparseMatrixSolver { + public: + explicit DynamicSparseNormalCholeskySolver( + const LinearSolver::Options& options); + virtual ~DynamicSparseNormalCholeskySolver() {} + + private: + LinearSolver::Summary SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x) final; + + LinearSolver::Summary SolveImplUsingSuiteSparse( + CompressedRowSparseMatrix* A, + double* rhs_and_solution); + + LinearSolver::Summary SolveImplUsingCXSparse( + CompressedRowSparseMatrix* A, + double* rhs_and_solution); + + LinearSolver::Summary SolveImplUsingEigen( + CompressedRowSparseMatrix* A, + double* rhs_and_solution); + + const LinearSolver::Options options_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DYNAMIC_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/ceres/internal/ceres/eigensparse.cc b/extern/ceres/internal/ceres/eigensparse.cc new file mode 100644 index 00000000000..22ed2c43b5d --- /dev/null +++ b/extern/ceres/internal/ceres/eigensparse.cc @@ -0,0 +1,190 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/eigensparse.h" + +#ifdef CERES_USE_EIGEN_SPARSE + +#include <sstream> + +#include "Eigen/SparseCholesky" +#include "Eigen/SparseCore" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +// TODO(sameeragarwal): Use enable_if to clean up the implementations +// for when Scalar == double. +template <typename Solver> +class EigenSparseCholeskyTemplate : public SparseCholesky { + public: + EigenSparseCholeskyTemplate() : analyzed_(false) {} + virtual ~EigenSparseCholeskyTemplate() {} + CompressedRowSparseMatrix::StorageType StorageType() const final { + return CompressedRowSparseMatrix::LOWER_TRIANGULAR; + } + + LinearSolverTerminationType Factorize( + const Eigen::SparseMatrix<typename Solver::Scalar>& lhs, + std::string* message) { + if (!analyzed_) { + solver_.analyzePattern(lhs); + + if (VLOG_IS_ON(2)) { + std::stringstream ss; + solver_.dumpMemory(ss); + VLOG(2) << "Symbolic Analysis\n" << ss.str(); + } + + if (solver_.info() != Eigen::Success) { + *message = "Eigen failure. Unable to find symbolic factorization."; + return LINEAR_SOLVER_FATAL_ERROR; + } + + analyzed_ = true; + } + + solver_.factorize(lhs); + if (solver_.info() != Eigen::Success) { + *message = "Eigen failure. Unable to find numeric factorization."; + return LINEAR_SOLVER_FAILURE; + } + return LINEAR_SOLVER_SUCCESS; + } + + LinearSolverTerminationType Solve(const double* rhs_ptr, + double* solution_ptr, + std::string* message) { + CHECK(analyzed_) << "Solve called without a call to Factorize first."; + + scalar_rhs_ = ConstVectorRef(rhs_ptr, solver_.cols()) + .template cast<typename Solver::Scalar>(); + + // The two casts are needed if the Scalar in this class is not + // double. For code simplicity we are going to assume that Eigen + // is smart enough to figure out that casting a double Vector to a + // double Vector is a straight copy. If this turns into a + // performance bottleneck (unlikely), we can revisit this. + scalar_solution_ = solver_.solve(scalar_rhs_); + VectorRef(solution_ptr, solver_.cols()) = + scalar_solution_.template cast<double>(); + + if (solver_.info() != Eigen::Success) { + *message = "Eigen failure. Unable to do triangular solve."; + return LINEAR_SOLVER_FAILURE; + } + return LINEAR_SOLVER_SUCCESS; + } + + LinearSolverTerminationType Factorize(CompressedRowSparseMatrix* lhs, + std::string* message) final { + CHECK_EQ(lhs->storage_type(), StorageType()); + + typename Solver::Scalar* values_ptr = NULL; + if (std::is_same<typename Solver::Scalar, double>::value) { + values_ptr = + reinterpret_cast<typename Solver::Scalar*>(lhs->mutable_values()); + } else { + // In the case where the scalar used in this class is not + // double. In that case, make a copy of the values array in the + // CompressedRowSparseMatrix and cast it to Scalar along the way. + values_ = ConstVectorRef(lhs->values(), lhs->num_nonzeros()) + .cast<typename Solver::Scalar>(); + values_ptr = values_.data(); + } + + Eigen::MappedSparseMatrix<typename Solver::Scalar, Eigen::ColMajor> + eigen_lhs(lhs->num_rows(), + lhs->num_rows(), + lhs->num_nonzeros(), + lhs->mutable_rows(), + lhs->mutable_cols(), + values_ptr); + return Factorize(eigen_lhs, message); + } + + private: + Eigen::Matrix<typename Solver::Scalar, Eigen::Dynamic, 1> values_, + scalar_rhs_, scalar_solution_; + bool analyzed_; + Solver solver_; +}; + +std::unique_ptr<SparseCholesky> EigenSparseCholesky::Create( + const OrderingType ordering_type) { + std::unique_ptr<SparseCholesky> sparse_cholesky; + + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, + Eigen::Upper, + Eigen::AMDOrdering<int>> + WithAMDOrdering; + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, + Eigen::Upper, + Eigen::NaturalOrdering<int>> + WithNaturalOrdering; + if (ordering_type == AMD) { + sparse_cholesky.reset(new EigenSparseCholeskyTemplate<WithAMDOrdering>()); + } else { + sparse_cholesky.reset( + new EigenSparseCholeskyTemplate<WithNaturalOrdering>()); + } + return sparse_cholesky; +} + +EigenSparseCholesky::~EigenSparseCholesky() {} + +std::unique_ptr<SparseCholesky> FloatEigenSparseCholesky::Create( + const OrderingType ordering_type) { + std::unique_ptr<SparseCholesky> sparse_cholesky; + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>, + Eigen::Upper, + Eigen::AMDOrdering<int>> + WithAMDOrdering; + typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>, + Eigen::Upper, + Eigen::NaturalOrdering<int>> + WithNaturalOrdering; + if (ordering_type == AMD) { + sparse_cholesky.reset(new EigenSparseCholeskyTemplate<WithAMDOrdering>()); + } else { + sparse_cholesky.reset( + new EigenSparseCholeskyTemplate<WithNaturalOrdering>()); + } + return sparse_cholesky; +} + +FloatEigenSparseCholesky::~FloatEigenSparseCholesky() {} + +} // namespace internal +} // namespace ceres + +#endif // CERES_USE_EIGEN_SPARSE diff --git a/extern/ceres/internal/ceres/eigensparse.h b/extern/ceres/internal/ceres/eigensparse.h new file mode 100644 index 00000000000..2e6c6f01abb --- /dev/null +++ b/extern/ceres/internal/ceres/eigensparse.h @@ -0,0 +1,90 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// A simple C++ interface to the Eigen's Sparse Cholesky routines. + +#ifndef CERES_INTERNAL_EIGENSPARSE_H_ +#define CERES_INTERNAL_EIGENSPARSE_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifdef CERES_USE_EIGEN_SPARSE + +#include <memory> +#include <string> + +#include "Eigen/SparseCore" +#include "ceres/linear_solver.h" +#include "ceres/sparse_cholesky.h" + +namespace ceres { +namespace internal { + +class EigenSparseCholesky : public SparseCholesky { + public: + // Factory + static std::unique_ptr<SparseCholesky> Create( + const OrderingType ordering_type); + + // SparseCholesky interface. + virtual ~EigenSparseCholesky(); + virtual LinearSolverTerminationType Factorize( + CompressedRowSparseMatrix* lhs, std::string* message) = 0; + virtual CompressedRowSparseMatrix::StorageType StorageType() const = 0; + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) = 0; +}; + +// Even though the input is double precision linear system, this class +// solves it by computing a single precision Cholesky factorization. +class FloatEigenSparseCholesky : public SparseCholesky { + public: + // Factory + static std::unique_ptr<SparseCholesky> Create( + const OrderingType ordering_type); + + // SparseCholesky interface. + virtual ~FloatEigenSparseCholesky(); + virtual LinearSolverTerminationType Factorize( + CompressedRowSparseMatrix* lhs, std::string* message) = 0; + virtual CompressedRowSparseMatrix::StorageType StorageType() const = 0; + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_USE_EIGEN_SPARSE + +#endif // CERES_INTERNAL_EIGENSPARSE_H_ diff --git a/extern/ceres/internal/ceres/evaluator.cc b/extern/ceres/internal/ceres/evaluator.cc index baba9afa11b..8387983553d 100644 --- a/extern/ceres/internal/ceres/evaluator.cc +++ b/extern/ceres/internal/ceres/evaluator.cc @@ -51,6 +51,8 @@ Evaluator::~Evaluator() {} Evaluator* Evaluator::Create(const Evaluator::Options& options, Program* program, std::string* error) { + CHECK(options.context != NULL); + switch (options.linear_solver_type) { case DENSE_QR: case DENSE_NORMAL_CHOLESKY: @@ -71,9 +73,9 @@ Evaluator* Evaluator::Create(const Evaluator::Options& options, DynamicCompressedRowJacobianFinalizer>( options, program); } else { - return new ProgramEvaluator<ScratchEvaluatePreparer, - CompressedRowJacobianWriter>(options, - program); + return new ProgramEvaluator<BlockEvaluatePreparer, + BlockJacobianWriter>(options, + program); } default: diff --git a/extern/ceres/internal/ceres/evaluator.h b/extern/ceres/internal/ceres/evaluator.h index fea307919d0..b820958ed77 100644 --- a/extern/ceres/internal/ceres/evaluator.h +++ b/extern/ceres/internal/ceres/evaluator.h @@ -36,6 +36,7 @@ #include <string> #include <vector> +#include "ceres/context_impl.h" #include "ceres/execution_summary.h" #include "ceres/internal/port.h" #include "ceres/types.h" @@ -43,6 +44,7 @@ namespace ceres { struct CRSMatrix; +class EvaluationCallback; namespace internal { @@ -58,47 +60,18 @@ class Evaluator { virtual ~Evaluator(); struct Options { - Options() - : num_threads(1), - num_eliminate_blocks(-1), - linear_solver_type(DENSE_QR), - dynamic_sparsity(false) {} - - int num_threads; - int num_eliminate_blocks; - LinearSolverType linear_solver_type; - bool dynamic_sparsity; + int num_threads = 1; + int num_eliminate_blocks = -1; + LinearSolverType linear_solver_type = DENSE_QR; + bool dynamic_sparsity = false; + ContextImpl* context = nullptr; + EvaluationCallback* evaluation_callback = nullptr; }; static Evaluator* Create(const Options& options, Program* program, std::string* error); - // This is used for computing the cost, residual and Jacobian for - // returning to the user. For actually solving the optimization - // problem, the optimization algorithm uses the ProgramEvaluator - // objects directly. - // - // The residual, gradients and jacobian pointers can be NULL, in - // which case they will not be evaluated. cost cannot be NULL. - // - // The parallelism of the evaluator is controlled by num_threads; it - // should be at least 1. - // - // Note: That this function does not take a parameter vector as - // input. The parameter blocks are evaluated on the values contained - // in the arrays pointed to by their user_state pointers. - // - // Also worth noting is that this function mutates program by - // calling Program::SetParameterOffsetsAndIndex() on it so that an - // evaluator object can be constructed. - static bool Evaluate(Program* program, - int num_threads, - double* cost, - std::vector<double>* residuals, - std::vector<double>* gradient, - CRSMatrix* jacobian); - // Build and return a sparse matrix for storing and working with the Jacobian // of the objective function. The jacobian has dimensions // NumEffectiveParameters() by NumParameters(), and is typically extremely @@ -117,16 +90,14 @@ class Evaluator { // Schur complement based methods. virtual SparseMatrix* CreateJacobian() const = 0; - // Options struct to control Evaluator::Evaluate; struct EvaluateOptions { - EvaluateOptions() - : apply_loss_function(true) { - } - // If false, the loss function correction is not applied to the // residual blocks. - bool apply_loss_function; + bool apply_loss_function = true; + + // If false, this evaluation point is the same as the last one. + bool new_evaluation_point = true; }; // Evaluate the cost function for the given state. Returns the cost, @@ -190,12 +161,8 @@ class Evaluator { // that the base class implementation does not have to worry about // life time issues. Further, these calls are not expected to be // frequent or performance sensitive. - virtual std::map<std::string, int> CallStatistics() const { - return std::map<std::string, int>(); - } - - virtual std::map<std::string, double> TimeStatistics() const { - return std::map<std::string, double>(); + virtual std::map<std::string, CallStatistics> Statistics() const { + return std::map<std::string, CallStatistics>(); } }; diff --git a/extern/ceres/internal/ceres/execution_summary.h b/extern/ceres/internal/ceres/execution_summary.h index aa9929d8974..17fd882af03 100644 --- a/extern/ceres/internal/ceres/execution_summary.h +++ b/extern/ceres/internal/ceres/execution_summary.h @@ -32,47 +32,45 @@ #define CERES_INTERNAL_EXECUTION_SUMMARY_H_ #include <map> +#include <mutex> #include <string> #include "ceres/internal/port.h" #include "ceres/wall_time.h" -#include "ceres/mutex.h" namespace ceres { namespace internal { -// Struct used by various objects to report statistics and other -// information about their execution. e.g., ExecutionSummary::times -// can be used for reporting times associated with various activities. +struct CallStatistics { + CallStatistics() : time(0.), calls(0) {} + double time; + int calls; +}; + +// Struct used by various objects to report statistics about their +// execution. class ExecutionSummary { public: void IncrementTimeBy(const std::string& name, const double value) { - CeresMutexLock l(×_mutex_); - times_[name] += value; + std::lock_guard<std::mutex> l(mutex_); + CallStatistics& call_stats = statistics_[name]; + call_stats.time += value; + ++call_stats.calls; } - void IncrementCall(const std::string& name) { - CeresMutexLock l(&calls_mutex_); - calls_[name] += 1; + const std::map<std::string, CallStatistics>& statistics() const { + return statistics_; } - const std::map<std::string, double>& times() const { return times_; } - const std::map<std::string, int>& calls() const { return calls_; } - private: - Mutex times_mutex_; - std::map<std::string, double> times_; - - Mutex calls_mutex_; - std::map<std::string, int> calls_; + std::mutex mutex_; + std::map<std::string, CallStatistics> statistics_; }; class ScopedExecutionTimer { public: ScopedExecutionTimer(const std::string& name, ExecutionSummary* summary) - : start_time_(WallTimeInSeconds()), - name_(name), - summary_(summary) {} + : start_time_(WallTimeInSeconds()), name_(name), summary_(summary) {} ~ScopedExecutionTimer() { summary_->IncrementTimeBy(name_, WallTimeInSeconds() - start_time_); diff --git a/extern/ceres/internal/ceres/float_cxsparse.cc b/extern/ceres/internal/ceres/float_cxsparse.cc new file mode 100644 index 00000000000..6c688303444 --- /dev/null +++ b/extern/ceres/internal/ceres/float_cxsparse.cc @@ -0,0 +1,47 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/float_cxsparse.h" + +#if !defined(CERES_NO_CXSPARSE) + +namespace ceres { +namespace internal { + +std::unique_ptr<SparseCholesky> FloatCXSparseCholesky::Create( + OrderingType ordering_type) { + LOG(FATAL) << "FloatCXSparseCholesky is not available."; + return std::unique_ptr<SparseCholesky>(); +} + +} // namespace internal +} // namespace ceres + +#endif // !defined(CERES_NO_CXSPARSE) diff --git a/extern/ceres/internal/ceres/float_cxsparse.h b/extern/ceres/internal/ceres/float_cxsparse.h new file mode 100644 index 00000000000..57fc5e4c010 --- /dev/null +++ b/extern/ceres/internal/ceres/float_cxsparse.h @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_FLOAT_CXSPARSE_H_ +#define CERES_INTERNAL_FLOAT_CXSPARSE_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#if !defined(CERES_NO_CXSPARSE) + +#include <memory> +#include "ceres/sparse_cholesky.h" + +namespace ceres { +namespace internal { + +// Fake implementation of a single precision Sparse Cholesky using +// CXSparse. +class FloatCXSparseCholesky : public SparseCholesky { + public: + static std::unique_ptr<SparseCholesky> Create( + OrderingType ordering_type); +}; + +} // namespace internal +} // namespace ceres + +#endif // !defined(CERES_NO_CXSPARSE) + +#endif // CERES_INTERNAL_FLOAT_CXSPARSE_H_ diff --git a/extern/ceres/internal/ceres/float_suitesparse.cc b/extern/ceres/internal/ceres/float_suitesparse.cc new file mode 100644 index 00000000000..03604572b5c --- /dev/null +++ b/extern/ceres/internal/ceres/float_suitesparse.cc @@ -0,0 +1,47 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/float_suitesparse.h" + +#if !defined(CERES_NO_SUITESPARSE) + +namespace ceres { +namespace internal { + +std::unique_ptr<SparseCholesky> FloatSuiteSparseCholesky::Create( + OrderingType ordering_type) { + LOG(FATAL) << "FloatSuiteSparseCholesky is not available."; + return std::unique_ptr<SparseCholesky>(); +} + +} // namespace internal +} // namespace ceres + +#endif // !defined(CERES_NO_SUITESPARSE) diff --git a/extern/ceres/internal/ceres/float_suitesparse.h b/extern/ceres/internal/ceres/float_suitesparse.h new file mode 100644 index 00000000000..ac4d4090922 --- /dev/null +++ b/extern/ceres/internal/ceres/float_suitesparse.h @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_FLOAT_SUITESPARSE_H_ +#define CERES_INTERNAL_FLOAT_SUITESPARSE_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#include <memory> +#include "ceres/sparse_cholesky.h" + +#if !defined(CERES_NO_SUITESPARSE) + +namespace ceres { +namespace internal { + +// Fake implementation of a single precision Sparse Cholesky using +// SuiteSparse. +class FloatSuiteSparseCholesky : public SparseCholesky { + public: + static std::unique_ptr<SparseCholesky> Create( + OrderingType ordering_type); +}; + +} // namespace internal +} // namespace ceres + +#endif // !defined(CERES_NO_SUITESPARSE) + +#endif // CERES_INTERNAL_FLOAT_SUITESPARSE_H_ diff --git a/extern/ceres/internal/ceres/function_sample.cc b/extern/ceres/internal/ceres/function_sample.cc new file mode 100644 index 00000000000..2fd3dbdf7c5 --- /dev/null +++ b/extern/ceres/internal/ceres/function_sample.cc @@ -0,0 +1,73 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/function_sample.h" +#include "ceres/stringprintf.h" + +namespace ceres { +namespace internal { + +FunctionSample::FunctionSample() + : x(0.0), + vector_x_is_valid(false), + value(0.0), + value_is_valid(false), + vector_gradient_is_valid(false), + gradient(0.0), + gradient_is_valid(false) {} + +FunctionSample::FunctionSample(const double x, const double value) + : x(x), + vector_x_is_valid(false), + value(value), + value_is_valid(true), + vector_gradient_is_valid(false), + gradient(0.0), + gradient_is_valid(false) {} + +FunctionSample::FunctionSample(const double x, + const double value, + const double gradient) + : x(x), + vector_x_is_valid(false), + value(value), + value_is_valid(true), + vector_gradient_is_valid(false), + gradient(gradient), + gradient_is_valid(true) {} + +std::string FunctionSample::ToDebugString() const { + return StringPrintf("[x: %.8e, value: %.8e, gradient: %.8e, " + "value_is_valid: %d, gradient_is_valid: %d]", + x, value, gradient, value_is_valid, gradient_is_valid); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/function_sample.h b/extern/ceres/internal/ceres/function_sample.h new file mode 100644 index 00000000000..df79aef944f --- /dev/null +++ b/extern/ceres/internal/ceres/function_sample.h @@ -0,0 +1,94 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_FUNCTION_SAMPLE_H_ +#define CERES_INTERNAL_FUNCTION_SAMPLE_H_ + +#include <string> +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +// FunctionSample is used by the line search routines to store and +// communicate the value and (optionally) the gradient of the function +// being minimized. +// +// Since line search as the name implies happens along a certain +// line/direction. FunctionSample contains the information in two +// ways. Information in the ambient space and information along the +// direction of search. +struct FunctionSample { + FunctionSample(); + FunctionSample(double x, double value); + FunctionSample(double x, double value, double gradient); + + std::string ToDebugString() const; + + // x is the location of the sample along the search direction. + double x; + + // Let p be a point and d be the search direction then + // + // vector_x = p + x * d; + Vector vector_x; + // True if vector_x has been assigned a valid value. + bool vector_x_is_valid; + + // value = f(vector_x) + double value; + // True of the evaluation was successful and value is a finite + // number. + bool value_is_valid; + + // vector_gradient = Df(vector_position); + // + // D is the derivative operator. + Vector vector_gradient; + // True if the vector gradient was evaluated and the evaluation was + // successful (the value is a finite number). + bool vector_gradient_is_valid; + + // gradient = d.transpose() * vector_gradient + // + // where d is the search direction. + double gradient; + // True if the evaluation of the gradient was sucessful and the + // value is a finite number. + bool gradient_is_valid; +}; + + + + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_FUNCTION_SAMPLE_H_ diff --git a/extern/ceres/internal/ceres/generate_template_specializations.py b/extern/ceres/internal/ceres/generate_template_specializations.py new file mode 100644 index 00000000000..5e91f8d2b6a --- /dev/null +++ b/extern/ceres/internal/ceres/generate_template_specializations.py @@ -0,0 +1,246 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# 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) +# +# Script for explicitly generating template specialization of the +# SchurEliminator class. It is a rather large class +# and the number of explicit instantiations is also large. Explicitly +# generating these instantiations in separate .cc files breaks the +# compilation into separate compilation unit rather than one large cc +# file which takes 2+GB of RAM to compile. +# +# This script creates three sets of files. +# +# 1. schur_eliminator_x_x_x.cc and partitioned_matrix_view_x_x_x.cc +# where, the x indicates the template parameters and +# +# 2. schur_eliminator.cc & partitioned_matrix_view.cc +# +# that contains a factory function for instantiating these classes +# based on runtime parameters. +# +# 3. schur_templates.cc +# +# that contains a function which can be queried to determine what +# template specializations are available. +# +# The following list of tuples, specializations indicates the set of +# specializations that is generated. +SPECIALIZATIONS = [(2, 2, 2), + (2, 2, 3), + (2, 2, 4), + (2, 2, "Eigen::Dynamic"), + (2, 3, 3), + (2, 3, 4), + (2, 3, 6), + (2, 3, 9), + (2, 3, "Eigen::Dynamic"), + (2, 4, 3), + (2, 4, 4), + (2, 4, 6), + (2, 4, 8), + (2, 4, 9), + (2, 4, "Eigen::Dynamic"), + (2, "Eigen::Dynamic", "Eigen::Dynamic"), + (3, 3, 3), + (4, 4, 2), + (4, 4, 3), + (4, 4, 4), + (4, 4, "Eigen::Dynamic")] + +import schur_eliminator_template +import partitioned_matrix_view_template +import os +import glob + +def SuffixForSize(size): + if size == "Eigen::Dynamic": + return "d" + return str(size) + +def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size): + return "_".join([prefix] + map(SuffixForSize, (row_block_size, + e_block_size, + f_block_size))) + +def GenerateFactoryConditional(row_block_size, e_block_size, f_block_size): + conditionals = [] + if (row_block_size != "Eigen::Dynamic"): + conditionals.append("(options.row_block_size == %s)" % row_block_size) + if (e_block_size != "Eigen::Dynamic"): + conditionals.append("(options.e_block_size == %s)" % e_block_size) + if (f_block_size != "Eigen::Dynamic"): + conditionals.append("(options.f_block_size == %s)" % f_block_size) + if (len(conditionals) == 0): + return "%s" + + if (len(conditionals) == 1): + return " if " + conditionals[0] + "{\n %s\n }\n" + + return " if (" + " &&\n ".join(conditionals) + ") {\n %s\n }\n" + +def Specialize(name, data): + """ + Generate specialization code and the conditionals to instantiate it. + """ + + # Specialization files + for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: + output = SpecializationFilename("generated/" + name, + row_block_size, + e_block_size, + f_block_size) + ".cc" + + with open(output, "w") as f: + f.write(data["HEADER"]) + f.write(data["SPECIALIZATION_FILE"] % + (row_block_size, e_block_size, f_block_size)) + + # Generate the _d_d_d specialization. + output = SpecializationFilename("generated/" + name, + "Eigen::Dynamic", + "Eigen::Dynamic", + "Eigen::Dynamic") + ".cc" + with open(output, "w") as f: + f.write(data["HEADER"]) + f.write(data["DYNAMIC_FILE"] % + ("Eigen::Dynamic", "Eigen::Dynamic", "Eigen::Dynamic")) + + # Factory + with open(name + ".cc", "w") as f: + f.write(data["HEADER"]) + f.write(data["FACTORY_FILE_HEADER"]) + for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: + factory_conditional = GenerateFactoryConditional( + row_block_size, e_block_size, f_block_size) + factory = data["FACTORY"] % (row_block_size, e_block_size, f_block_size) + f.write(factory_conditional % factory); + f.write(data["FACTORY_FOOTER"]) + +QUERY_HEADER = """// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// What template specializations are available. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. +""" + +QUERY_FILE_HEADER = """ +#include "ceres/internal/eigen.h" +#include "ceres/schur_templates.h" + +namespace ceres { +namespace internal { + +void GetBestSchurTemplateSpecialization(int* row_block_size, + int* e_block_size, + int* f_block_size) { + LinearSolver::Options options; + options.row_block_size = *row_block_size; + options.e_block_size = *e_block_size; + options.f_block_size = *f_block_size; + *row_block_size = Eigen::Dynamic; + *e_block_size = Eigen::Dynamic; + *f_block_size = Eigen::Dynamic; +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION +""" + +QUERY_FOOTER = """ +#endif + return; +} + +} // namespace internal +} // namespace ceres +""" + +QUERY_ACTION = """ *row_block_size = %s; + *e_block_size = %s; + *f_block_size = %s; + return;""" + +def GenerateQueryFile(): + """ + Generate file that allows querying for available template specializations. + """ + + with open("schur_templates.cc", "w") as f: + f.write(QUERY_HEADER) + f.write(QUERY_FILE_HEADER) + for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: + factory_conditional = GenerateFactoryConditional( + row_block_size, e_block_size, f_block_size) + action = QUERY_ACTION % (row_block_size, e_block_size, f_block_size) + f.write(factory_conditional % action) + f.write(QUERY_FOOTER) + + +if __name__ == "__main__": + for f in glob.glob("generated/*"): + os.remove(f) + + Specialize("schur_eliminator", + schur_eliminator_template.__dict__) + Specialize("partitioned_matrix_view", + partitioned_matrix_view_template.__dict__) + GenerateQueryFile() diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc index 500115b9897..86ad17b4f71 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_2.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc index 1384cb619e3..33018d573a4 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc index 030035ec97b..a429a546e3d 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc index c9501b50170..f6f03ea6dcc 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_2_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc index c2639bff69e..0b73e1a2aa8 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc index 693e43959c1..bc4a86194eb 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc index 7b9368ffefd..fe8f7dd37af 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_6.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc index e72c5f6937a..ac493fcd0c0 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_9.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc index c1f410eb64c..e29efaf4832 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_3_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc index 7292c333d5d..e61e0a31314 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc index 891d65a8646..2e1170da01f 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_6.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_6.cc new file mode 100644 index 00000000000..4a5590d9751 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_6.cc @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<2, 4, 6>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc index 395f6bd4c13..83015f1ecc5 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_8.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc index 88952b10e34..25671f913dd 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_9.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc index 7733e1993eb..d259802bd5a 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_4_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc index 117a0cdb8c1..c9567595acd 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_2_d_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_3_3_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_3_3_3.cc new file mode 100644 index 00000000000..d3b20be70e7 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_3_3_3.cc @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Template specialization of PartitionedMatrixView. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/partitioned_matrix_view_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class PartitionedMatrixView<3, 3, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc index a620bb70dba..f08049c9653 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_2.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc index 2978630832c..9342612022f 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc index bcd03b02e3a..8b273fa0da0 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc index 6b541ecf0d9..e8b45e49eca 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_4_4_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc index 85111e722c4..3545b869d5f 100644 --- a/extern/ceres/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc +++ b/extern/ceres/internal/ceres/generated/partitioned_matrix_view_d_d_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. #include "ceres/partitioned_matrix_view_impl.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc index ac07a3f229e..79fcf437981 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc index 0ec09553f9e..edd7fb649b4 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc index 74a42cc4a16..692267dba46 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc index 5ce757fda5d..33d9c6de270 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc index 2e7ae28b4ea..4a5e2fe30c0 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc index 443207070cf..7ee63d069aa 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc index ac2f358b383..108760ef1f8 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_6.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc index 930ab440fa5..4fea2fa4417 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc index 486c53d36f4..0d13c99e7ca 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc index 6f247a7b832..3827c653a63 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc index c44cd045263..47bdfab1f22 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_6.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_6.cc new file mode 100644 index 00000000000..3777be22707 --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_6.cc @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 6>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc index c9a0d5fc729..862c76a2a9c 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_8.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc index b0455b0bca0..5b5b7ccd415 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_9.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc index 3234380f23c..ce2d450b073 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc index 311f8556932..9b02bd9db5a 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_2_d_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_3_3_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_3_3_3.cc new file mode 100644 index 00000000000..1cbeadf518f --- /dev/null +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_3_3_3.cc @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<3, 3, 3>; + +} // namespace internal +} // namespace ceres + +#endif // CERES_RESTRICT_SCHUR_SPECIALIZATION diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc index bc40bd55296..10f709d7577 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc index cca88c802b0..bcbcc745519 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc index 33c94a907b9..44ecc87deba 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc index 1a1866f93a8..69c856304f0 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" diff --git a/extern/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc b/extern/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc index 6b18ef8c863..348708bb335 100644 --- a/extern/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc +++ b/extern/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. #include "ceres/schur_eliminator_impl.h" diff --git a/extern/ceres/internal/ceres/gradient_checker.cc b/extern/ceres/internal/ceres/gradient_checker.cc index c16c141db09..ef56666970d 100644 --- a/extern/ceres/internal/ceres/gradient_checker.cc +++ b/extern/ceres/internal/ceres/gradient_checker.cc @@ -34,6 +34,7 @@ #include <algorithm> #include <cmath> +#include <cstdint> #include <numeric> #include <string> #include <vector> @@ -61,11 +62,11 @@ bool EvaluateCostFunction( Vector* residuals, std::vector<Matrix>* jacobians, std::vector<Matrix>* local_jacobians) { - CHECK_NOTNULL(residuals); - CHECK_NOTNULL(jacobians); - CHECK_NOTNULL(local_jacobians); + CHECK(residuals != nullptr); + CHECK(jacobians != nullptr); + CHECK(local_jacobians != nullptr); - const vector<int32>& block_sizes = function->parameter_block_sizes(); + const vector<int32_t>& block_sizes = function->parameter_block_sizes(); const int num_parameter_blocks = block_sizes.size(); // Allocate Jacobian matrices in local space. @@ -110,7 +111,7 @@ bool EvaluateCostFunction( Matrix global_J_local(global_size, local_size); local_parameterizations.at(i)->ComputeJacobian( parameters[i], global_J_local.data()); - local_jacobians->at(i) = jacobians->at(i) * global_J_local; + local_jacobians->at(i).noalias() = jacobians->at(i) * global_J_local; } } return true; @@ -122,20 +123,20 @@ GradientChecker::GradientChecker( const vector<const LocalParameterization*>* local_parameterizations, const NumericDiffOptions& options) : function_(function) { - CHECK_NOTNULL(function); + CHECK(function != nullptr); if (local_parameterizations != NULL) { local_parameterizations_ = *local_parameterizations; } else { local_parameterizations_.resize(function->parameter_block_sizes().size(), NULL); } - DynamicNumericDiffCostFunction<CostFunction, CENTRAL>* + DynamicNumericDiffCostFunction<CostFunction, RIDDERS>* finite_diff_cost_function = - new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>( + new DynamicNumericDiffCostFunction<CostFunction, RIDDERS>( function, DO_NOT_TAKE_OWNERSHIP, options); finite_diff_cost_function_.reset(finite_diff_cost_function); - const vector<int32>& parameter_block_sizes = + const vector<int32_t>& parameter_block_sizes = function->parameter_block_sizes(); const int num_parameter_blocks = parameter_block_sizes.size(); for (int i = 0; i < num_parameter_blocks; ++i) { diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc index f2c73367891..13d6c58a6b7 100644 --- a/extern/ceres/internal/ceres/gradient_checking_cost_function.cc +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -33,13 +33,13 @@ #include <algorithm> #include <cmath> +#include <cstdint> #include <numeric> #include <string> #include <vector> #include "ceres/gradient_checker.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/parameter_block.h" #include "ceres/problem.h" #include "ceres/problem_impl.h" @@ -74,8 +74,8 @@ class GradientCheckingCostFunction : public CostFunction { relative_precision_(relative_precision), extra_info_(extra_info), callback_(callback) { - CHECK_NOTNULL(callback_); - const vector<int32>& parameter_block_sizes = + CHECK(callback_ != nullptr); + const vector<int32_t>& parameter_block_sizes = function->parameter_block_sizes(); *mutable_parameter_block_sizes() = parameter_block_sizes; set_num_residuals(function->num_residuals()); @@ -83,9 +83,9 @@ class GradientCheckingCostFunction : public CostFunction { virtual ~GradientCheckingCostFunction() { } - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const final { if (!jacobians) { // Nothing to check in this case; just forward. return function_->Evaluate(parameters, residuals, NULL); @@ -107,7 +107,7 @@ class GradientCheckingCostFunction : public CostFunction { MatrixRef(residuals, num_residuals, 1) = results.residuals; // Copy the original jacobian blocks into the jacobians array. - const vector<int32>& block_sizes = function_->parameter_block_sizes(); + const vector<int32_t>& block_sizes = function_->parameter_block_sizes(); for (int k = 0; k < block_sizes.size(); k++) { if (jacobians[k] != NULL) { MatrixRef(jacobians[k], @@ -148,10 +148,9 @@ CallbackReturnType GradientCheckingIterationCallback::operator()( } void GradientCheckingIterationCallback::SetGradientErrorDetected( std::string& error_log) { - mutex_.Lock(); + std::lock_guard<std::mutex> l(mutex_); gradient_error_detected_ = true; error_log_ += "\n" + error_log; - mutex_.Unlock(); } CostFunction* CreateGradientCheckingCostFunction( @@ -176,7 +175,7 @@ ProblemImpl* CreateGradientCheckingProblemImpl( double relative_step_size, double relative_precision, GradientCheckingIterationCallback* callback) { - CHECK_NOTNULL(callback); + CHECK(callback != nullptr); // We create new CostFunctions by wrapping the original CostFunction // in a gradient checking CostFunction. So its okay for the // ProblemImpl to take ownership of it and destroy it. The @@ -189,6 +188,7 @@ ProblemImpl* CreateGradientCheckingProblemImpl( DO_NOT_TAKE_OWNERSHIP; gradient_checking_problem_options.local_parameterization_ownership = DO_NOT_TAKE_OWNERSHIP; + gradient_checking_problem_options.context = problem_impl->context(); NumericDiffOptions numeric_diff_options; numeric_diff_options.relative_step_size = relative_step_size; @@ -212,6 +212,17 @@ ProblemImpl* CreateGradientCheckingProblemImpl( gradient_checking_problem_impl->SetParameterBlockConstant( parameter_block->mutable_user_state()); } + + for (int i = 0; i < parameter_block->Size(); ++i) { + gradient_checking_problem_impl->SetParameterUpperBound( + parameter_block->mutable_user_state(), + i, + parameter_block->UpperBound(i)); + gradient_checking_problem_impl->SetParameterLowerBound( + parameter_block->mutable_user_state(), + i, + parameter_block->LowerBound(i)); + } } // For every ResidualBlock in problem_impl, create a new @@ -255,7 +266,8 @@ ProblemImpl* CreateGradientCheckingProblemImpl( gradient_checking_problem_impl->AddResidualBlock( gradient_checking_cost_function, const_cast<LossFunction*>(residual_block->loss_function()), - parameter_blocks); + parameter_blocks.data(), + static_cast<int>(parameter_blocks.size())); } // Normally, when a problem is given to the solver, we guarantee diff --git a/extern/ceres/internal/ceres/gradient_checking_cost_function.h b/extern/ceres/internal/ceres/gradient_checking_cost_function.h index 497f8e2a594..e9a34f7eb9f 100644 --- a/extern/ceres/internal/ceres/gradient_checking_cost_function.h +++ b/extern/ceres/internal/ceres/gradient_checking_cost_function.h @@ -32,12 +32,12 @@ #ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ #define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ +#include <mutex> #include <string> #include "ceres/cost_function.h" #include "ceres/iteration_callback.h" #include "ceres/local_parameterization.h" -#include "ceres/mutex.h" namespace ceres { namespace internal { @@ -52,7 +52,7 @@ class GradientCheckingIterationCallback : public IterationCallback { // Will return SOLVER_CONTINUE until a gradient error has been detected, // then return SOLVER_ABORT. - virtual CallbackReturnType operator()(const IterationSummary& summary); + CallbackReturnType operator()(const IterationSummary& summary) final; // Notify this that a gradient error has occurred (thread safe). void SetGradientErrorDetected(std::string& error_log); @@ -63,8 +63,7 @@ class GradientCheckingIterationCallback : public IterationCallback { private: bool gradient_error_detected_; std::string error_log_; - // Mutex protecting member variables. - ceres::internal::Mutex mutex_; + std::mutex mutex_; }; // Creates a CostFunction that checks the Jacobians that cost_function computes diff --git a/extern/ceres/internal/ceres/gradient_problem_evaluator.h b/extern/ceres/internal/ceres/gradient_problem_evaluator.h index 2c562544768..c5ad1d71607 100644 --- a/extern/ceres/internal/ceres/gradient_problem_evaluator.h +++ b/extern/ceres/internal/ceres/gradient_problem_evaluator.h @@ -48,43 +48,46 @@ class GradientProblemEvaluator : public Evaluator { explicit GradientProblemEvaluator(const GradientProblem& problem) : problem_(problem) {} virtual ~GradientProblemEvaluator() {} - virtual SparseMatrix* CreateJacobian() const { return NULL; } - virtual bool Evaluate(const EvaluateOptions& evaluate_options, - const double* state, - double* cost, - double* residuals, - double* gradient, - SparseMatrix* jacobian) { + SparseMatrix* CreateJacobian() const final { return nullptr; } + bool Evaluate(const EvaluateOptions& evaluate_options, + const double* state, + double* cost, + double* residuals, + double* gradient, + SparseMatrix* jacobian) final { CHECK(jacobian == NULL); ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); + // The reason we use Residual and Jacobian here even when we are + // only computing the cost and gradient has to do with the fact + // that the line search minimizer code is used by both the + // GradientProblemSolver and the main CeresSolver coder where the + // Evaluator evaluates the Jacobian, and these magic strings need + // to be consistent across the code base for the time accounting + // to work. ScopedExecutionTimer call_type_timer( - gradient == NULL ? "Evaluator::Cost" : "Evaluator::Gradient", + gradient == NULL ? "Evaluator::Residual" : "Evaluator::Jacobian", &execution_summary_); return problem_.Evaluate(state, cost, gradient); } - virtual bool Plus(const double* state, - const double* delta, - double* state_plus_delta) const { + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const final { return problem_.Plus(state, delta, state_plus_delta); } - virtual int NumParameters() const { + int NumParameters() const final { return problem_.NumParameters(); } - virtual int NumEffectiveParameters() const { + int NumEffectiveParameters() const final { return problem_.NumLocalParameters(); } - virtual int NumResiduals() const { return 1; } + int NumResiduals() const final { return 1; } - virtual std::map<std::string, int> CallStatistics() const { - return execution_summary_.calls(); - } - - virtual std::map<std::string, double> TimeStatistics() const { - return execution_summary_.times(); + std::map<std::string, internal::CallStatistics> Statistics() const final { + return execution_summary_.statistics(); } private: diff --git a/extern/ceres/internal/ceres/gradient_problem_solver.cc b/extern/ceres/internal/ceres/gradient_problem_solver.cc index 8709f8f3fbd..1639e30666c 100644 --- a/extern/ceres/internal/ceres/gradient_problem_solver.cc +++ b/extern/ceres/internal/ceres/gradient_problem_solver.cc @@ -30,6 +30,7 @@ #include "ceres/gradient_problem_solver.h" +#include <memory> #include "ceres/callbacks.h" #include "ceres/gradient_problem.h" #include "ceres/gradient_problem_evaluator.h" @@ -72,6 +73,7 @@ Solver::Options GradientProblemSolverOptionsToSolverOptions( COPY_OPTION(max_line_search_step_expansion); COPY_OPTION(max_num_iterations); COPY_OPTION(max_solver_time_in_seconds); + COPY_OPTION(parameter_tolerance); COPY_OPTION(function_tolerance); COPY_OPTION(gradient_tolerance); COPY_OPTION(logging_type); @@ -97,16 +99,18 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, const GradientProblem& problem, double* parameters_ptr, GradientProblemSolver::Summary* summary) { - using internal::scoped_ptr; - using internal::WallTimeInSeconds; - using internal::Minimizer; + using internal::CallStatistics; using internal::GradientProblemEvaluator; + using internal::GradientProblemSolverStateUpdatingCallback; using internal::LoggingCallback; + using internal::Minimizer; using internal::SetSummaryFinalCost; + using internal::WallTimeInSeconds; double start_time = WallTimeInSeconds(); - *CHECK_NOTNULL(summary) = Summary(); + CHECK(summary != nullptr); + *summary = Summary(); summary->num_parameters = problem.NumParameters(); summary->num_local_parameters = problem.NumLocalParameters(); summary->line_search_direction_type = options.line_search_direction_type; // NOLINT @@ -121,6 +125,10 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, return; } + VectorRef parameters(parameters_ptr, problem.NumParameters()); + Vector solution(problem.NumParameters()); + solution = parameters; + // TODO(sameeragarwal): This is a bit convoluted, we should be able // to convert to minimizer options directly, but this will do for // now. @@ -128,7 +136,7 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, Minimizer::Options(GradientProblemSolverOptionsToSolverOptions(options)); minimizer_options.evaluator.reset(new GradientProblemEvaluator(problem)); - scoped_ptr<IterationCallback> logging_callback; + std::unique_ptr<IterationCallback> logging_callback; if (options.logging_type != SILENT) { logging_callback.reset( new LoggingCallback(LINE_SEARCH, options.minimizer_progress_to_stdout)); @@ -136,10 +144,16 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, logging_callback.get()); } - scoped_ptr<Minimizer> minimizer(Minimizer::Create(LINE_SEARCH)); - Vector solution(problem.NumParameters()); - VectorRef parameters(parameters_ptr, problem.NumParameters()); - solution = parameters; + std::unique_ptr<IterationCallback> state_updating_callback; + if (options.update_state_every_iteration) { + state_updating_callback.reset( + new GradientProblemSolverStateUpdatingCallback( + problem.NumParameters(), solution.data(), parameters_ptr)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + state_updating_callback.get()); + } + + std::unique_ptr<Minimizer> minimizer(Minimizer::Create(LINE_SEARCH)); Solver::Summary solver_summary; solver_summary.fixed_cost = 0.0; @@ -162,34 +176,23 @@ void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options, SetSummaryFinalCost(summary); } - const std::map<string, double>& evaluator_time_statistics = - minimizer_options.evaluator->TimeStatistics(); - summary->cost_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); - summary->gradient_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); + const std::map<string, CallStatistics>& evaluator_statistics = + minimizer_options.evaluator->Statistics(); + { + const CallStatistics& call_stats = FindWithDefault( + evaluator_statistics, "Evaluator::Residual", CallStatistics()); + summary->cost_evaluation_time_in_seconds = call_stats.time; + summary->num_cost_evaluations = call_stats.calls; + } - summary->total_time_in_seconds = WallTimeInSeconds() - start_time; -} + { + const CallStatistics& call_stats = FindWithDefault( + evaluator_statistics, "Evaluator::Jacobian", CallStatistics()); + summary->gradient_evaluation_time_in_seconds = call_stats.time; + summary->num_gradient_evaluations = call_stats.calls; + } -// Invalid values for most fields, to ensure that we are not -// accidentally reporting default values. -GradientProblemSolver::Summary::Summary() - : termination_type(FAILURE), - message("ceres::GradientProblemSolve was not called."), - initial_cost(-1.0), - final_cost(-1.0), - total_time_in_seconds(-1.0), - cost_evaluation_time_in_seconds(-1.0), - gradient_evaluation_time_in_seconds(-1.0), - line_search_polynomial_minimization_time_in_seconds(-1.0), - num_parameters(-1), - num_local_parameters(-1), - line_search_direction_type(LBFGS), - line_search_type(ARMIJO), - line_search_interpolation_type(BISECTION), - nonlinear_conjugate_gradient_type(FLETCHER_REEVES), - max_lbfgs_rank(-1) { + summary->total_time_in_seconds = WallTimeInSeconds() - start_time; } bool GradientProblemSolver::Summary::IsSolutionUsable() const { @@ -256,15 +259,15 @@ string GradientProblemSolver::Summary::FullReport() const { static_cast<int>(iterations.size())); StringAppendF(&report, "\nTime (in seconds):\n"); - - StringAppendF(&report, "\n Cost evaluation %23.4f\n", - cost_evaluation_time_in_seconds); - StringAppendF(&report, " Gradient evaluation %23.4f\n", - gradient_evaluation_time_in_seconds); - StringAppendF(&report, " Polynomial minimization %17.4f\n", + StringAppendF(&report, "\n Cost evaluation %23.6f (%d)\n", + cost_evaluation_time_in_seconds, + num_cost_evaluations); + StringAppendF(&report, " Gradient & cost evaluation %16.6f (%d)\n", + gradient_evaluation_time_in_seconds, + num_gradient_evaluations); + StringAppendF(&report, " Polynomial minimization %17.6f\n", line_search_polynomial_minimization_time_in_seconds); - - StringAppendF(&report, "Total %25.4f\n\n", + StringAppendF(&report, "Total %25.6f\n\n", total_time_in_seconds); StringAppendF(&report, "Termination: %25s (%s)\n", diff --git a/extern/ceres/internal/ceres/graph.h b/extern/ceres/internal/ceres/graph.h index b96b67265cb..4e1fd81c1ea 100644 --- a/extern/ceres/internal/ceres/graph.h +++ b/extern/ceres/internal/ceres/graph.h @@ -32,11 +32,11 @@ #define CERES_INTERNAL_GRAPH_H_ #include <limits> +#include <unordered_set> +#include <unordered_map> #include <utility> -#include "ceres/integral_types.h" #include "ceres/map_util.h" -#include "ceres/collections_port.h" -#include "ceres/internal/macros.h" +#include "ceres/pair_hash.h" #include "ceres/types.h" #include "glog/logging.h" @@ -53,7 +53,7 @@ class Graph { // Add a vertex. void AddVertex(const Vertex& vertex) { if (vertices_.insert(vertex).second) { - edges_[vertex] = HashSet<Vertex>(); + edges_[vertex] = std::unordered_set<Vertex>(); } } @@ -63,10 +63,9 @@ class Graph { } vertices_.erase(vertex); - const HashSet<Vertex>& sinks = edges_[vertex]; - for (typename HashSet<Vertex>::const_iterator it = sinks.begin(); - it != sinks.end(); ++it) { - edges_[*it].erase(vertex); + const std::unordered_set<Vertex>& sinks = edges_[vertex]; + for (const Vertex& s : sinks) { + edges_[s].erase(vertex); } edges_.erase(vertex); @@ -90,19 +89,17 @@ class Graph { // Calling Neighbors on a vertex not in the graph will result in // undefined behaviour. - const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + const std::unordered_set<Vertex>& Neighbors(const Vertex& vertex) const { return FindOrDie(edges_, vertex); } - const HashSet<Vertex>& vertices() const { + const std::unordered_set<Vertex>& vertices() const { return vertices_; } private: - HashSet<Vertex> vertices_; - HashMap<Vertex, HashSet<Vertex> > edges_; - - CERES_DISALLOW_COPY_AND_ASSIGN(Graph); + std::unordered_set<Vertex> vertices_; + std::unordered_map<Vertex, std::unordered_set<Vertex>> edges_; }; // A weighted undirected graph templated over the vertex ids. Vertex @@ -117,7 +114,7 @@ class WeightedGraph { void AddVertex(const Vertex& vertex, double weight) { if (vertices_.find(vertex) == vertices_.end()) { vertices_.insert(vertex); - edges_[vertex] = HashSet<Vertex>(); + edges_[vertex] = std::unordered_set<Vertex>(); } vertex_weights_[vertex] = weight; } @@ -135,15 +132,14 @@ class WeightedGraph { vertices_.erase(vertex); vertex_weights_.erase(vertex); - const HashSet<Vertex>& sinks = edges_[vertex]; - for (typename HashSet<Vertex>::const_iterator it = sinks.begin(); - it != sinks.end(); ++it) { - if (vertex < *it) { - edge_weights_.erase(std::make_pair(vertex, *it)); + const std::unordered_set<Vertex>& sinks = edges_[vertex]; + for (const Vertex& s : sinks) { + if (vertex < s) { + edge_weights_.erase(std::make_pair(vertex, s)); } else { - edge_weights_.erase(std::make_pair(*it, vertex)); + edge_weights_.erase(std::make_pair(s, vertex)); } - edges_[*it].erase(vertex); + edges_[s].erase(vertex); } edges_.erase(vertex); @@ -198,11 +194,11 @@ class WeightedGraph { // Calling Neighbors on a vertex not in the graph will result in // undefined behaviour. - const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + const std::unordered_set<Vertex>& Neighbors(const Vertex& vertex) const { return FindOrDie(edges_, vertex); } - const HashSet<Vertex>& vertices() const { + const std::unordered_set<Vertex>& vertices() const { return vertices_; } @@ -211,12 +207,11 @@ class WeightedGraph { } private: - HashSet<Vertex> vertices_; - HashMap<Vertex, double> vertex_weights_; - HashMap<Vertex, HashSet<Vertex> > edges_; - HashMap<std::pair<Vertex, Vertex>, double> edge_weights_; - - CERES_DISALLOW_COPY_AND_ASSIGN(WeightedGraph); + std::unordered_set<Vertex> vertices_; + std::unordered_map<Vertex, double> vertex_weights_; + std::unordered_map<Vertex, std::unordered_set<Vertex>> edges_; + std::unordered_map<std::pair<Vertex, Vertex>, double, pair_hash> + edge_weights_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/graph_algorithms.h b/extern/ceres/internal/ceres/graph_algorithms.h index d1d3f52cd22..b0629313159 100644 --- a/extern/ceres/internal/ceres/graph_algorithms.h +++ b/extern/ceres/internal/ceres/graph_algorithms.h @@ -34,9 +34,10 @@ #define CERES_INTERNAL_GRAPH_ALGORITHMS_H_ #include <algorithm> +#include <unordered_map> +#include <unordered_set> #include <vector> #include <utility> -#include "ceres/collections_port.h" #include "ceres/graph.h" #include "ceres/wall_time.h" #include "glog/logging.h" @@ -96,10 +97,10 @@ class VertexDegreeLessThan { template <typename Vertex> int IndependentSetOrdering(const Graph<Vertex>& graph, std::vector<Vertex>* ordering) { - const HashSet<Vertex>& vertices = graph.vertices(); + const std::unordered_set<Vertex>& vertices = graph.vertices(); const int num_vertices = vertices.size(); - CHECK_NOTNULL(ordering); + CHECK(ordering != nullptr); ordering->clear(); ordering->reserve(num_vertices); @@ -109,34 +110,29 @@ int IndependentSetOrdering(const Graph<Vertex>& graph, const char kBlack = 2; // Mark all vertices white. - HashMap<Vertex, char> vertex_color; + std::unordered_map<Vertex, char> vertex_color; std::vector<Vertex> vertex_queue; - for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); - it != vertices.end(); - ++it) { - vertex_color[*it] = kWhite; - vertex_queue.push_back(*it); + for (const Vertex& vertex : vertices) { + vertex_color[vertex] = kWhite; + vertex_queue.push_back(vertex); } - - std::sort(vertex_queue.begin(), vertex_queue.end(), + std::sort(vertex_queue.begin(), + vertex_queue.end(), 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]; + for (const Vertex& vertex : vertex_queue) { 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; + const std::unordered_set<Vertex>& neighbors = graph.Neighbors(vertex); + for (const Vertex& neighbor : neighbors) { + vertex_color[neighbor] = kGrey; } } @@ -145,10 +141,7 @@ int IndependentSetOrdering(const Graph<Vertex>& graph, // 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 std::vector<Vertex>::const_iterator it = vertex_queue.begin(); - it != vertex_queue.end(); - ++it) { - const Vertex vertex = *it; + for (const Vertex& vertex : vertex_queue) { DCHECK(vertex_color[vertex] != kWhite); if (vertex_color[vertex] != kBlack) { ordering->push_back(vertex); @@ -172,8 +165,8 @@ int IndependentSetOrdering(const Graph<Vertex>& graph, template <typename Vertex> int StableIndependentSetOrdering(const Graph<Vertex>& graph, std::vector<Vertex>* ordering) { - CHECK_NOTNULL(ordering); - const HashSet<Vertex>& vertices = graph.vertices(); + CHECK(ordering != nullptr); + const std::unordered_set<Vertex>& vertices = graph.vertices(); const int num_vertices = vertices.size(); CHECK_EQ(vertices.size(), ordering->size()); @@ -188,11 +181,9 @@ int StableIndependentSetOrdering(const Graph<Vertex>& graph, 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; + std::unordered_map<Vertex, char> vertex_color; + for (const Vertex& vertex : vertices) { + vertex_color[vertex] = kWhite; } ordering->clear(); @@ -207,11 +198,9 @@ int StableIndependentSetOrdering(const Graph<Vertex>& graph, 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; + const std::unordered_set<Vertex>& neighbors = graph.Neighbors(vertex); + for (const Vertex& neighbor : neighbors) { + vertex_color[neighbor] = kGrey; } } @@ -220,10 +209,7 @@ int StableIndependentSetOrdering(const Graph<Vertex>& graph, // 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 std::vector<Vertex>::const_iterator it = vertex_queue.begin(); - it != vertex_queue.end(); - ++it) { - const Vertex vertex = *it; + for (const Vertex& vertex : vertex_queue) { DCHECK(vertex_color[vertex] != kWhite); if (vertex_color[vertex] != kBlack) { ordering->push_back(vertex); @@ -242,8 +228,8 @@ int StableIndependentSetOrdering(const Graph<Vertex>& graph, // is what gives this data structure its efficiency. template <typename Vertex> Vertex FindConnectedComponent(const Vertex& vertex, - HashMap<Vertex, Vertex>* union_find) { - typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex); + std::unordered_map<Vertex, Vertex>* union_find) { + auto it = union_find->find(vertex); DCHECK(it != union_find->end()); if (it->second != vertex) { it->second = FindConnectedComponent(it->second, union_find); @@ -274,30 +260,24 @@ template <typename Vertex> WeightedGraph<Vertex>* Degree2MaximumSpanningForest(const WeightedGraph<Vertex>& graph) { // Array of edges sorted in decreasing order of their weights. - std::vector<std::pair<double, std::pair<Vertex, Vertex> > > weighted_edges; + std::vector<std::pair<double, std::pair<Vertex, Vertex>>> weighted_edges; WeightedGraph<Vertex>* forest = new WeightedGraph<Vertex>(); // Disjoint-set to keep track of the connected components in the // maximum spanning tree. - HashMap<Vertex, Vertex> disjoint_set; + std::unordered_map<Vertex, Vertex> disjoint_set; // Sort of the edges in the graph in decreasing order of their // weight. Also add the vertices of the graph to the Maximum // Spanning Tree graph and set each vertex to be its own connected // component in the disjoint_set structure. - const HashSet<Vertex>& vertices = graph.vertices(); - for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); - it != vertices.end(); - ++it) { - const Vertex vertex1 = *it; + const std::unordered_set<Vertex>& vertices = graph.vertices(); + for (const Vertex& vertex1 : vertices) { forest->AddVertex(vertex1, graph.VertexWeight(vertex1)); disjoint_set[vertex1] = vertex1; - const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1); - for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin(); - it2 != neighbors.end(); - ++it2) { - const Vertex vertex2 = *it2; + const std::unordered_set<Vertex>& neighbors = graph.Neighbors(vertex1); + for (const Vertex& vertex2 : neighbors) { if (vertex1 >= vertex2) { continue; } diff --git a/extern/ceres/internal/ceres/implicit_schur_complement.cc b/extern/ceres/internal/ceres/implicit_schur_complement.cc index d05f03817b7..bf680d1d952 100644 --- a/extern/ceres/internal/ceres/implicit_schur_complement.cc +++ b/extern/ceres/internal/ceres/implicit_schur_complement.cc @@ -34,7 +34,6 @@ #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "glog/logging.h" diff --git a/extern/ceres/internal/ceres/implicit_schur_complement.h b/extern/ceres/internal/ceres/implicit_schur_complement.h index 5d822ebaeef..f4ddf724910 100644 --- a/extern/ceres/internal/ceres/implicit_schur_complement.h +++ b/extern/ceres/internal/ceres/implicit_schur_complement.h @@ -34,11 +34,11 @@ #ifndef CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ #define CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ +#include <memory> #include "ceres/linear_operator.h" #include "ceres/linear_solver.h" #include "ceres/partitioned_matrix_view.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" namespace ceres { @@ -113,11 +113,11 @@ class ImplicitSchurComplement : public LinearOperator { 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; + void RightMultiply(const double* x, double* y) const final; // The Schur complement is a symmetric positive definite matrix, // thus the left and right multiply operators are the same. - virtual void LeftMultiply(const double* x, double* y) const { + void LeftMultiply(const double* x, double* y) const final { RightMultiply(x, y); } @@ -127,8 +127,8 @@ class ImplicitSchurComplement : public LinearOperator { // complement. void BackSubstitute(const double* x, double* y); - virtual int num_rows() const { return A_->num_cols_f(); } - virtual int num_cols() const { return A_->num_cols_f(); } + int num_rows() const final { return A_->num_cols_f(); } + int num_cols() const final { return A_->num_cols_f(); } const Vector& rhs() const { return rhs_; } const BlockSparseMatrix* block_diagonal_EtE_inverse() const { @@ -145,12 +145,12 @@ class ImplicitSchurComplement : public LinearOperator { const LinearSolver::Options& options_; - scoped_ptr<PartitionedMatrixViewBase> A_; + std::unique_ptr<PartitionedMatrixViewBase> A_; const double* D_; const double* b_; - scoped_ptr<BlockSparseMatrix> block_diagonal_EtE_inverse_; - scoped_ptr<BlockSparseMatrix> block_diagonal_FtF_inverse_; + std::unique_ptr<BlockSparseMatrix> block_diagonal_EtE_inverse_; + std::unique_ptr<BlockSparseMatrix> block_diagonal_FtF_inverse_; Vector rhs_; diff --git a/extern/ceres/internal/ceres/inner_product_computer.cc b/extern/ceres/internal/ceres/inner_product_computer.cc new file mode 100644 index 00000000000..2bf88365d99 --- /dev/null +++ b/extern/ceres/internal/ceres/inner_product_computer.cc @@ -0,0 +1,330 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/inner_product_computer.h" + +#include <algorithm> +#include "ceres/small_blas.h" + +namespace ceres { +namespace internal { + + +// Create the CompressedRowSparseMatrix matrix that will contain the +// inner product. +// +// storage_type controls whether the result matrix contains the upper +// or the lower triangular part of the product. +// +// num_nonzeros is the number of non-zeros in the result matrix. +CompressedRowSparseMatrix* InnerProductComputer::CreateResultMatrix( + const CompressedRowSparseMatrix::StorageType storage_type, + const int num_nonzeros) { + CompressedRowSparseMatrix* matrix = + new CompressedRowSparseMatrix(m_.num_cols(), m_.num_cols(), num_nonzeros); + matrix->set_storage_type(storage_type); + + const CompressedRowBlockStructure* bs = m_.block_structure(); + const std::vector<Block>& blocks = bs->cols; + matrix->mutable_row_blocks()->resize(blocks.size()); + matrix->mutable_col_blocks()->resize(blocks.size()); + for (int i = 0; i < blocks.size(); ++i) { + (*(matrix->mutable_row_blocks()))[i] = blocks[i].size; + (*(matrix->mutable_col_blocks()))[i] = blocks[i].size; + } + + return matrix; +} + +// Given the set of product terms in the inner product, return the +// total number of non-zeros in the result and for each row block of +// the result matrix, compute the number of non-zeros in any one row +// of the row block. +int InnerProductComputer::ComputeNonzeros( + const std::vector<InnerProductComputer::ProductTerm>& product_terms, + std::vector<int>* row_nnz) { + const CompressedRowBlockStructure* bs = m_.block_structure(); + const std::vector<Block>& blocks = bs->cols; + + row_nnz->resize(blocks.size()); + std::fill(row_nnz->begin(), row_nnz->end(), 0); + + // First product term. + (*row_nnz)[product_terms[0].row] = blocks[product_terms[0].col].size; + int num_nonzeros = + blocks[product_terms[0].row].size * blocks[product_terms[0].col].size; + + // Remaining product terms. + for (int i = 1; i < product_terms.size(); ++i) { + const ProductTerm& previous = product_terms[i - 1]; + const ProductTerm& current = product_terms[i]; + + // Each (row, col) block counts only once. + // This check depends on product sorted on (row, col). + if (current.row != previous.row || current.col != previous.col) { + (*row_nnz)[current.row] += blocks[current.col].size; + num_nonzeros += blocks[current.row].size * blocks[current.col].size; + } + } + + return num_nonzeros; +} + +InnerProductComputer::InnerProductComputer(const BlockSparseMatrix& m, + const int start_row_block, + const int end_row_block) + : m_(m), start_row_block_(start_row_block), end_row_block_(end_row_block) {} + +// Compute the sparsity structure of the product m.transpose() * m +// and create a CompressedRowSparseMatrix corresponding to it. +// +// Also compute the "program" vector, which for every term in the +// block outer product provides the information for the entry in the +// values array of the result matrix where it should be accumulated. +// +// Since the entries of the program are the same for rows with the +// same sparsity structure, the program only stores the result for one +// row per row block. The Compute function reuses this information for +// each row in the row block. +// +// product_storage_type controls the form of the output matrix. It +// can be LOWER_TRIANGULAR or UPPER_TRIANGULAR. +InnerProductComputer* InnerProductComputer::Create( + const BlockSparseMatrix& m, + CompressedRowSparseMatrix::StorageType product_storage_type) { + return InnerProductComputer::Create( + m, 0, m.block_structure()->rows.size(), product_storage_type); +} + +InnerProductComputer* InnerProductComputer::Create( + const BlockSparseMatrix& m, + const int start_row_block, + const int end_row_block, + CompressedRowSparseMatrix::StorageType product_storage_type) { + CHECK(product_storage_type == CompressedRowSparseMatrix::LOWER_TRIANGULAR || + product_storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR); + CHECK_GT(m.num_nonzeros(), 0) + << "Congratulations, you found a bug in Ceres. Please report it."; + InnerProductComputer* inner_product_computer = + new InnerProductComputer(m, start_row_block, end_row_block); + inner_product_computer->Init(product_storage_type); + return inner_product_computer; +} + +void InnerProductComputer::Init( + const CompressedRowSparseMatrix::StorageType product_storage_type) { + std::vector<InnerProductComputer::ProductTerm> product_terms; + const CompressedRowBlockStructure* bs = m_.block_structure(); + + // Give input matrix m in Block Sparse format + // (row_block, col_block) + // represent each block multiplication + // (row_block, col_block1)' X (row_block, col_block2) + // by its product term: + // (col_block1, col_block2, index) + for (int row_block = start_row_block_; row_block < end_row_block_; + ++row_block) { + const CompressedRow& row = bs->rows[row_block]; + for (int c1 = 0; c1 < row.cells.size(); ++c1) { + const Cell& cell1 = row.cells[c1]; + int c2_begin, c2_end; + if (product_storage_type == CompressedRowSparseMatrix::LOWER_TRIANGULAR) { + c2_begin = 0; + c2_end = c1 + 1; + } else { + c2_begin = c1; + c2_end = row.cells.size(); + } + + for (int c2 = c2_begin; c2 < c2_end; ++c2) { + const Cell& cell2 = row.cells[c2]; + product_terms.push_back(InnerProductComputer::ProductTerm( + cell1.block_id, cell2.block_id, product_terms.size())); + } + } + } + + std::sort(product_terms.begin(), product_terms.end()); + ComputeOffsetsAndCreateResultMatrix(product_storage_type, product_terms); +} + +void InnerProductComputer::ComputeOffsetsAndCreateResultMatrix( + const CompressedRowSparseMatrix::StorageType product_storage_type, + const std::vector<InnerProductComputer::ProductTerm>& product_terms) { + const std::vector<Block>& col_blocks = m_.block_structure()->cols; + + std::vector<int> row_block_nnz; + const int num_nonzeros = ComputeNonzeros(product_terms, &row_block_nnz); + + result_.reset(CreateResultMatrix(product_storage_type, num_nonzeros)); + + // Populate the row non-zero counts in the result matrix. + int* crsm_rows = result_->mutable_rows(); + crsm_rows[0] = 0; + for (int i = 0; i < col_blocks.size(); ++i) { + for (int j = 0; j < col_blocks[i].size; ++j, ++crsm_rows) { + *(crsm_rows + 1) = *crsm_rows + row_block_nnz[i]; + } + } + + // The following macro FILL_CRSM_COL_BLOCK is key to understanding + // how this class works. + // + // It does two things. + // + // Sets the value for the current term in the result_offsets_ array + // and populates the cols array of the result matrix. + // + // row_block and col_block as the names imply, refer to the row and + // column blocks of the current term. + // + // row_nnz is the number of nonzeros in the result_matrix at the + // beginning of the first row of row_block. + // + // col_nnz is the number of nonzeros in the first row of the row + // block that occur before the current column block, i.e. this is + // sum of the sizes of all the column blocks in this row block that + // came before this column block. + // + // Given these two numbers and the total number of nonzeros in this + // row (nnz_in_row), we can now populate the cols array as follows: + // + // nnz + j * nnz_in_row is the beginning of the j^th row. + // + // nnz + j * nnz_in_row + col_nnz is the beginning of the column + // block in the j^th row. + // + // nnz + j * nnz_in_row + col_nnz + k is then the j^th row and the + // k^th column of the product block, whose value is + // + // col_blocks[col_block].position + k, which is the column number of + // the k^th column of the current column block. +#define FILL_CRSM_COL_BLOCK \ + const int row_block = current->row; \ + const int col_block = current->col; \ + const int nnz_in_row = row_block_nnz[row_block]; \ + int* crsm_cols = result_->mutable_cols(); \ + result_offsets_[current->index] = nnz + col_nnz; \ + for (int j = 0; j < col_blocks[row_block].size; ++j) { \ + for (int k = 0; k < col_blocks[col_block].size; ++k) { \ + crsm_cols[nnz + j * nnz_in_row + col_nnz + k] = \ + col_blocks[col_block].position + k; \ + } \ + } + + result_offsets_.resize(product_terms.size()); + int col_nnz = 0; + int nnz = 0; + + // Process the first term. + const InnerProductComputer::ProductTerm* current = &product_terms[0]; + FILL_CRSM_COL_BLOCK; + + // Process the rest of the terms. + for (int i = 1; i < product_terms.size(); ++i) { + current = &product_terms[i]; + const InnerProductComputer::ProductTerm* previous = &product_terms[i - 1]; + + // If the current term is the same as the previous term, then it + // stores its product at the same location as the previous term. + if (previous->row == current->row && previous->col == current->col) { + result_offsets_[current->index] = result_offsets_[previous->index]; + continue; + } + + if (previous->row == current->row) { + // if the current and previous terms are in the same row block, + // then they differ in the column block, in which case advance + // col_nnz by the column size of the prevous term. + col_nnz += col_blocks[previous->col].size; + } else { + // If we have moved to a new row-block , then col_nnz is zero, + // and nnz is set to the beginning of the row block. + col_nnz = 0; + nnz += row_block_nnz[previous->row] * col_blocks[previous->row].size; + } + + FILL_CRSM_COL_BLOCK; + } +} + +// Use the results_offsets_ array to numerically compute the product +// m' * m and store it in result_. +// +// TODO(sameeragarwal): Multithreading support. +void InnerProductComputer::Compute() { + const double* m_values = m_.values(); + const CompressedRowBlockStructure* bs = m_.block_structure(); + + const CompressedRowSparseMatrix::StorageType storage_type = + result_->storage_type(); + result_->SetZero(); + double* values = result_->mutable_values(); + const int* rows = result_->rows(); + int cursor = 0; + + // Iterate row blocks. + for (int r = start_row_block_; r < end_row_block_; ++r) { + const CompressedRow& m_row = bs->rows[r]; + for (int c1 = 0; c1 < m_row.cells.size(); ++c1) { + const Cell& cell1 = m_row.cells[c1]; + const int c1_size = bs->cols[cell1.block_id].size; + const int row_nnz = rows[bs->cols[cell1.block_id].position + 1] - + rows[bs->cols[cell1.block_id].position]; + + int c2_begin, c2_end; + if (storage_type == CompressedRowSparseMatrix::LOWER_TRIANGULAR) { + c2_begin = 0; + c2_end = c1 + 1; + } else { + c2_begin = c1; + c2_end = m_row.cells.size(); + } + + for (int c2 = c2_begin; c2 < c2_end; ++c2, ++cursor) { + const Cell& cell2 = m_row.cells[c2]; + const int c2_size = bs->cols[cell2.block_id].size; + MatrixTransposeMatrixMultiply<Eigen::Dynamic, Eigen::Dynamic, + Eigen::Dynamic, Eigen::Dynamic, 1>( + m_values + cell1.position, + m_row.block.size, c1_size, + m_values + cell2.position, + m_row.block.size, c2_size, + values + result_offsets_[cursor], + 0, 0, c1_size, row_nnz); + } + } + } + + CHECK_EQ(cursor, result_offsets_.size()); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/inner_product_computer.h b/extern/ceres/internal/ceres/inner_product_computer.h new file mode 100644 index 00000000000..73073f8ad06 --- /dev/null +++ b/extern/ceres/internal/ceres/inner_product_computer.h @@ -0,0 +1,157 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_INNER_PRODUCT_COMPUTER_H_ +#define CERES_INTERNAL_INNER_PRODUCT_COMPUTER_H_ + +#include <memory> +#include <vector> + +#include "ceres/block_sparse_matrix.h" +#include "ceres/compressed_row_sparse_matrix.h" + +namespace ceres { +namespace internal { + +// This class is used to repeatedly compute the inner product +// +// result = m' * m +// +// where the sparsity structure of m remains constant across calls. +// +// Upon creation, the class computes and caches information needed to +// compute v, and then uses it to efficiently compute the product +// every time InnerProductComputer::Compute is called. +// +// See sparse_normal_cholesky_solver.cc for example usage. +// +// Note that the result matrix is a block upper or lower-triangular +// matrix, i.e., it will contain entries in the upper or lower +// triangular part of the matrix corresponding to the block that occur +// along its diagonal. +// +// This is not a problem as sparse linear algebra libraries can ignore +// these entries with ease and the space used is minimal/linear in the +// size of the matrices. +class InnerProductComputer { + public: + // Factory + // + // m is the input matrix + // + // Since m' * m is a symmetric matrix, we only compute half of the + // matrix and the value of storage_type which must be + // UPPER_TRIANGULAR or LOWER_TRIANGULAR determines which half is + // computed. + // + // The user must ensure that the matrix m is valid for the life time + // of this object. + static InnerProductComputer* Create( + const BlockSparseMatrix& m, + CompressedRowSparseMatrix::StorageType storage_type); + + // This factory method allows the user control over range of row + // blocks of m that should be used to compute the inner product. + // + // a = m(start_row_block : end_row_block, :); + // result = a' * a; + static InnerProductComputer* Create( + const BlockSparseMatrix& m, + int start_row_block, + int end_row_block, + CompressedRowSparseMatrix::StorageType storage_type); + + // Update result_ to be numerically equal to m' * m. + void Compute(); + + // Accessors for the result containing the inner product. + // + // Compute must be called before accessing this result for + // the first time. + const CompressedRowSparseMatrix& result() const { return *result_; } + CompressedRowSparseMatrix* mutable_result() const { return result_.get(); } + + private: + // A ProductTerm is a term in the block inner product of a matrix + // with itself. + struct ProductTerm { + ProductTerm(const int row, const int col, const int index) + : row(row), col(col), index(index) {} + + bool operator<(const ProductTerm& right) const { + if (row == right.row) { + if (col == right.col) { + return index < right.index; + } + return col < right.col; + } + return row < right.row; + } + + int row; + int col; + int index; + }; + + InnerProductComputer(const BlockSparseMatrix& m, + int start_row_block, + int end_row_block); + + void Init(CompressedRowSparseMatrix::StorageType storage_type); + + CompressedRowSparseMatrix* CreateResultMatrix( + const CompressedRowSparseMatrix::StorageType storage_type, + int num_nonzeros); + + int ComputeNonzeros(const std::vector<ProductTerm>& product_terms, + std::vector<int>* row_block_nnz); + + void ComputeOffsetsAndCreateResultMatrix( + const CompressedRowSparseMatrix::StorageType storage_type, + const std::vector<ProductTerm>& product_terms); + + const BlockSparseMatrix& m_; + const int start_row_block_; + const int end_row_block_; + std::unique_ptr<CompressedRowSparseMatrix> result_; + + // For each term in the inner product, result_offsets_ contains the + // location in the values array of the result_ matrix where it + // should be stored. + // + // This is the principal look up table that allows this class to + // compute the inner product fast. + std::vector<int> result_offsets_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INNER_PRODUCT_COMPUTER_H_ diff --git a/extern/ceres/internal/ceres/invert_psd_matrix.h b/extern/ceres/internal/ceres/invert_psd_matrix.h new file mode 100644 index 00000000000..21d301a77ee --- /dev/null +++ b/extern/ceres/internal/ceres/invert_psd_matrix.h @@ -0,0 +1,79 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_INVERT_PSD_MATRIX_H_ +#define CERES_INTERNAL_INVERT_PSD_MATRIX_H_ + +#include "ceres/internal/eigen.h" +#include "glog/logging.h" +#include "Eigen/Dense" + +namespace ceres { +namespace internal { + +// Helper routine to compute the inverse or pseudo-inverse of a +// symmetric positive semi-definite matrix. +// +// assume_full_rank controls whether a Cholesky factorization or an +// Singular Value Decomposition is used to compute the inverse and the +// pseudo-inverse respectively. +// +// The template parameter kSize can either be Eigen::Dynamic or a +// positive integer equal to the number of rows of m. +template <int kSize> +typename EigenTypes<kSize, kSize>::Matrix InvertPSDMatrix( + const bool assume_full_rank, + const typename EigenTypes<kSize, kSize>::Matrix& m) { + using MType = typename EigenTypes<kSize, kSize>::Matrix; + const int size = m.rows(); + + // If the matrix can be assumed to be full rank, then if it is small + // (< 5) and fixed size, use Eigen's optimized inverse() + // implementation. + // + // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title3 + if (assume_full_rank) { + if (kSize > 0 && kSize < 5) { + return m.inverse(); + } + return m.template selfadjointView<Eigen::Upper>().llt().solve( + MType::Identity(size, size)); + } + + // For a thin SVD the number of columns of the matrix need to be dynamic. + using SVDMType = typename EigenTypes<kSize, Eigen::Dynamic>::Matrix; + Eigen::JacobiSVD<SVDMType> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV); + return svd.solve(MType::Identity(size, size)); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INVERT_PSD_MATRIX_H_ diff --git a/extern/ceres/internal/ceres/iterative_refiner.cc b/extern/ceres/internal/ceres/iterative_refiner.cc new file mode 100644 index 00000000000..fb0e45bdcdd --- /dev/null +++ b/extern/ceres/internal/ceres/iterative_refiner.cc @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <string> +#include "ceres/iterative_refiner.h" + +#include "Eigen/Core" +#include "ceres/sparse_cholesky.h" +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +IterativeRefiner::IterativeRefiner(const int max_num_iterations) + : max_num_iterations_(max_num_iterations) {} + +IterativeRefiner::~IterativeRefiner() {} + +void IterativeRefiner::Allocate(int num_cols) { + residual_.resize(num_cols); + correction_.resize(num_cols); + lhs_x_solution_.resize(num_cols); +} + +void IterativeRefiner::Refine(const SparseMatrix& lhs, + const double* rhs_ptr, + SparseCholesky* sparse_cholesky, + double* solution_ptr) { + const int num_cols = lhs.num_cols(); + Allocate(num_cols); + ConstVectorRef rhs(rhs_ptr, num_cols); + VectorRef solution(solution_ptr, num_cols); + for (int i = 0; i < max_num_iterations_; ++i) { + // residual = rhs - lhs * solution + lhs_x_solution_.setZero(); + lhs.RightMultiply(solution_ptr, lhs_x_solution_.data()); + residual_ = rhs - lhs_x_solution_; + // solution += lhs^-1 residual + std::string ignored_message; + sparse_cholesky->Solve( + residual_.data(), correction_.data(), &ignored_message); + solution += correction_; + } +}; + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/iterative_refiner.h b/extern/ceres/internal/ceres/iterative_refiner.h new file mode 100644 index 00000000000..f969935ae46 --- /dev/null +++ b/extern/ceres/internal/ceres/iterative_refiner.h @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_ITERATIVE_REFINER_H_ +#define CERES_INTERNAL_ITERATIVE_REFINER_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class SparseCholesky; +class SparseMatrix; + +// Iterative refinement +// (https://en.wikipedia.org/wiki/Iterative_refinement) is the process +// of improving the solution to a linear system, by using the +// following iteration. +// +// r_i = b - Ax_i +// Ad_i = r_i +// x_{i+1} = x_i + d_i +// +// IterativeRefiner implements this process for Symmetric Positive +// Definite linear systems. +// +// The above iterative loop is run until max_num_iterations is reached. +class IterativeRefiner { + public: + // max_num_iterations is the number of refinement iterations to + // perform. + IterativeRefiner(int max_num_iterations); + + // Needed for mocking. + virtual ~IterativeRefiner(); + + // Given an initial estimate of the solution of lhs * x = rhs, use + // max_num_iterations rounds of iterative refinement to improve it. + // + // sparse_cholesky is assumed to contain an already computed + // factorization (or approximation thereof) of lhs. + // + // solution is expected to contain a approximation to the solution + // to lhs * x = rhs. It can be zero. + // + // This method is virtual to facilitate mocking. + virtual void Refine(const SparseMatrix& lhs, + const double* rhs, + SparseCholesky* sparse_cholesky, + double* solution); + + private: + void Allocate(int num_cols); + + int max_num_iterations_; + Vector residual_; + Vector correction_; + Vector lhs_x_solution_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_ITERATIVE_REFINER_H_ diff --git a/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc index 9d4e30d69d2..6076c38c71d 100644 --- a/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc +++ b/extern/ceres/internal/ceres/iterative_schur_complement_solver.cc @@ -41,7 +41,6 @@ #include "ceres/detect_structure.h" #include "ceres/implicit_schur_complement.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/linear_solver.h" #include "ceres/preconditioner.h" #include "ceres/schur_jacobi_preconditioner.h" @@ -59,8 +58,7 @@ IterativeSchurComplementSolver::IterativeSchurComplementSolver( : options_(options) { } -IterativeSchurComplementSolver::~IterativeSchurComplementSolver() { -} +IterativeSchurComplementSolver::~IterativeSchurComplementSolver() {} LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( BlockSparseMatrix* A, @@ -69,7 +67,7 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( double* x) { EventLogger event_logger("IterativeSchurComplementSolver::Solve"); - CHECK_NOTNULL(A->block_structure()); + CHECK(A->block_structure() != nullptr); const int num_eliminate_blocks = options_.elimination_groups[0]; // Initialize a ImplicitSchurComplement object. if (schur_complement_ == NULL) { @@ -86,29 +84,61 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( A->block_structure()->cols.size() - num_eliminate_blocks; 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 = LINEAR_SOLVER_SUCCESS; + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_SUCCESS; schur_complement_->BackSubstitute(NULL, x); - return cg_summary; + return summary; } // Initialize the solution to the Schur complement system to zero. reduced_linear_system_solution_.resize(schur_complement_->num_rows()); reduced_linear_system_solution_.setZero(); - // Instantiate a conjugate gradient solver that runs on the Schur - // complement matrix with the block diagonal of the matrix F'F as - // the preconditioner. LinearSolver::Options cg_options; cg_options.min_num_iterations = options_.min_num_iterations; cg_options.max_num_iterations = options_.max_num_iterations; ConjugateGradientsSolver cg_solver(cg_options); - LinearSolver::PerSolveOptions cg_per_solve_options; + LinearSolver::PerSolveOptions cg_per_solve_options; cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance; cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; + CreatePreconditioner(A); + if (preconditioner_.get() != NULL) { + if (!preconditioner_->Update(*A, per_solve_options.D)) { + LinearSolver::Summary summary; + summary.num_iterations = 0; + summary.termination_type = LINEAR_SOLVER_FAILURE; + summary.message = "Preconditioner update failed."; + return summary; + } + + cg_per_solve_options.preconditioner = preconditioner_.get(); + } + + event_logger.AddEvent("Setup"); + LinearSolver::Summary summary = + cg_solver.Solve(schur_complement_.get(), + schur_complement_->rhs().data(), + cg_per_solve_options, + reduced_linear_system_solution_.data()); + if (summary.termination_type != LINEAR_SOLVER_FAILURE && + summary.termination_type != LINEAR_SOLVER_FATAL_ERROR) { + schur_complement_->BackSubstitute(reduced_linear_system_solution_.data(), + x); + } + event_logger.AddEvent("Solve"); + return summary; +} + +void IterativeSchurComplementSolver::CreatePreconditioner( + BlockSparseMatrix* A) { + if (options_.preconditioner_type == IDENTITY || + preconditioner_.get() != NULL) { + return; + } + Preconditioner::Options preconditioner_options; preconditioner_options.type = options_.preconditioner_type; preconditioner_options.visibility_clustering_type = @@ -120,63 +150,27 @@ LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( preconditioner_options.e_block_size = options_.e_block_size; preconditioner_options.f_block_size = options_.f_block_size; preconditioner_options.elimination_groups = options_.elimination_groups; + CHECK(options_.context != NULL); + preconditioner_options.context = options_.context; switch (options_.preconditioner_type) { - case IDENTITY: - break; case JACOBI: - preconditioner_.reset( - new SparseMatrixPreconditionerWrapper( - schur_complement_->block_diagonal_FtF_inverse())); + preconditioner_.reset(new SparseMatrixPreconditionerWrapper( + schur_complement_->block_diagonal_FtF_inverse())); break; case SCHUR_JACOBI: - if (preconditioner_.get() == NULL) { - preconditioner_.reset( - new SchurJacobiPreconditioner(*A->block_structure(), - preconditioner_options)); - } + preconditioner_.reset(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)); - } + preconditioner_.reset(new VisibilityBasedPreconditioner( + *A->block_structure(), preconditioner_options)); break; default: LOG(FATAL) << "Unknown Preconditioner Type"; } - - bool preconditioner_update_was_successful = true; - if (preconditioner_.get() != NULL) { - preconditioner_update_was_successful = - preconditioner_->Update(*A, per_solve_options.D); - cg_per_solve_options.preconditioner = preconditioner_.get(); - } - event_logger.AddEvent("Setup"); - - LinearSolver::Summary cg_summary; - cg_summary.num_iterations = 0; - cg_summary.termination_type = LINEAR_SOLVER_FAILURE; - - // TODO(sameeragarwal): Refactor preconditioners to return a more - // sane message. - cg_summary.message = "Preconditioner update failed."; - if (preconditioner_update_was_successful) { - cg_summary = cg_solver.Solve(schur_complement_.get(), - schur_complement_->rhs().data(), - cg_per_solve_options, - reduced_linear_system_solution_.data()); - if (cg_summary.termination_type != LINEAR_SOLVER_FAILURE && - cg_summary.termination_type != LINEAR_SOLVER_FATAL_ERROR) { - schur_complement_->BackSubstitute( - reduced_linear_system_solution_.data(), x); - } - } - event_logger.AddEvent("Solve"); - return cg_summary; -} +}; } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/ceres/internal/ceres/iterative_schur_complement_solver.h index e90d310de07..9aed94fcb1a 100644 --- a/extern/ceres/internal/ceres/iterative_schur_complement_solver.h +++ b/extern/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -31,9 +31,9 @@ #ifndef CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ #define CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ +#include <memory> #include "ceres/linear_solver.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" namespace ceres { @@ -70,20 +70,24 @@ class Preconditioner; class IterativeSchurComplementSolver : public BlockSparseMatrixSolver { public: explicit IterativeSchurComplementSolver(const LinearSolver::Options& options); + IterativeSchurComplementSolver(const IterativeSchurComplementSolver&) = delete; + void operator=(const IterativeSchurComplementSolver&) = delete; + virtual ~IterativeSchurComplementSolver(); private: - virtual LinearSolver::Summary SolveImpl( + LinearSolver::Summary SolveImpl( BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& options, - double* x); + double* x) final; + + void CreatePreconditioner(BlockSparseMatrix* A); LinearSolver::Options options_; - scoped_ptr<internal::ImplicitSchurComplement> schur_complement_; - scoped_ptr<Preconditioner> preconditioner_; + std::unique_ptr<internal::ImplicitSchurComplement> schur_complement_; + std::unique_ptr<Preconditioner> preconditioner_; Vector reduced_linear_system_solution_; - CERES_DISALLOW_COPY_AND_ASSIGN(IterativeSchurComplementSolver); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/lapack.cc b/extern/ceres/internal/ceres/lapack.cc index 6fc23f4e658..37efbcd27d1 100644 --- a/extern/ceres/internal/ceres/lapack.cc +++ b/extern/ceres/internal/ceres/lapack.cc @@ -34,6 +34,7 @@ #include "ceres/linear_solver.h" #include "glog/logging.h" +#ifndef CERES_NO_LAPACK // C interface to the LAPACK Cholesky factorization and triangular solve. extern "C" void dpotrf_(char* uplo, int* n, @@ -61,7 +62,7 @@ extern "C" void dgels_(char* uplo, double* work, int* lwork, int* info); - +#endif namespace ceres { namespace internal { diff --git a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc index e9833805ef5..9eec631e6dd 100644 --- a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc +++ b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.cc @@ -30,7 +30,9 @@ #include "ceres/levenberg_marquardt_strategy.h" +#include <algorithm> #include <cmath> + #include "Eigen/Core" #include "ceres/array_utils.h" #include "ceres/internal/eigen.h" @@ -53,7 +55,7 @@ LevenbergMarquardtStrategy::LevenbergMarquardtStrategy( max_diagonal_(options.max_lm_diagonal), decrease_factor_(2.0), reuse_diagonal_(false) { - CHECK_NOTNULL(linear_solver_); + CHECK(linear_solver_ != nullptr); CHECK_GT(min_diagonal_, 0.0); CHECK_LE(min_diagonal_, max_diagonal_); CHECK_GT(max_radius_, 0.0); @@ -67,9 +69,9 @@ TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( SparseMatrix* jacobian, const double* residuals, double* step) { - CHECK_NOTNULL(jacobian); - CHECK_NOTNULL(residuals); - CHECK_NOTNULL(step); + CHECK(jacobian != nullptr); + CHECK(residuals != nullptr); + CHECK(step != nullptr); const int num_parameters = jacobian->num_cols(); if (!reuse_diagonal_) { @@ -98,7 +100,7 @@ TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( // Invalidate the output array lm_step, so that we can detect if // the linear solver generated numerical garbage. This is known // to happen for the DENSE_QR and then DENSE_SCHUR solver when - // the Jacobin is severly rank deficient and mu is too small. + // the Jacobin is severely rank deficient and mu is too small. InvalidateArray(num_parameters, step); // Instead of solving Jx = -r, solve Jy = r. diff --git a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h index c87a016c8f4..8fb37f32959 100644 --- a/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h +++ b/extern/ceres/internal/ceres/levenberg_marquardt_strategy.h @@ -49,14 +49,14 @@ class LevenbergMarquardtStrategy : public TrustRegionStrategy { virtual ~LevenbergMarquardtStrategy(); // TrustRegionStrategy interface - virtual TrustRegionStrategy::Summary ComputeStep( + TrustRegionStrategy::Summary ComputeStep( const TrustRegionStrategy::PerSolveOptions& per_solve_options, SparseMatrix* jacobian, const double* residuals, - double* step); - virtual void StepAccepted(double step_quality); - virtual void StepRejected(double step_quality); - virtual void StepIsInvalid() { + double* step) final; + void StepAccepted(double step_quality) final; + void StepRejected(double step_quality) final; + void StepIsInvalid() final { // Treat the current step as a rejected step with no increase in // solution quality. Since rejected steps lead to decrease in the // size of the trust region, the next time ComputeStep is called, @@ -64,7 +64,7 @@ class LevenbergMarquardtStrategy : public TrustRegionStrategy { StepRejected(0.0); } - virtual double Radius() const; + double Radius() const final; private: LinearSolver* linear_solver_; diff --git a/extern/ceres/internal/ceres/line_search.cc b/extern/ceres/internal/ceres/line_search.cc index 9cdcb7b77e5..352c64f5057 100644 --- a/extern/ceres/internal/ceres/line_search.cc +++ b/extern/ceres/internal/ceres/line_search.cc @@ -30,17 +30,19 @@ #include "ceres/line_search.h" +#include <algorithm> +#include <cmath> #include <iomanip> #include <iostream> // NOLINT -#include "glog/logging.h" #include "ceres/evaluator.h" +#include "ceres/function_sample.h" #include "ceres/internal/eigen.h" -#include "ceres/fpclassify.h" #include "ceres/map_util.h" #include "ceres/polynomial.h" #include "ceres/stringprintf.h" #include "ceres/wall_time.h" +#include "glog/logging.h" namespace ceres { namespace internal { @@ -53,30 +55,8 @@ using std::vector; namespace { // Precision used for floating point values in error message output. const int kErrorMessageNumericPrecision = 8; - -FunctionSample ValueSample(const double x, const double value) { - FunctionSample sample; - sample.x = x; - sample.value = value; - sample.value_is_valid = true; - return sample; -} - -FunctionSample ValueAndGradientSample(const double x, - const double value, - const double gradient) { - FunctionSample sample; - sample.x = x; - sample.value = value; - sample.gradient = gradient; - sample.value_is_valid = true; - sample.gradient_is_valid = true; - return sample; -} - } // namespace - ostream& operator<<(ostream &os, const FunctionSample& sample); // Convenience stream operator for pushing FunctionSamples into log messages. @@ -112,9 +92,7 @@ LineSearchFunction::LineSearchFunction(Evaluator* evaluator) : evaluator_(evaluator), position_(evaluator->NumParameters()), direction_(evaluator->NumEffectiveParameters()), - evaluation_point_(evaluator->NumParameters()), scaled_direction_(evaluator->NumEffectiveParameters()), - gradient_(evaluator->NumEffectiveParameters()), initial_evaluator_residual_time_in_seconds(0.0), initial_evaluator_jacobian_time_in_seconds(0.0) {} @@ -124,27 +102,48 @@ void LineSearchFunction::Init(const Vector& position, direction_ = direction; } -bool LineSearchFunction::Evaluate(double x, double* f, double* g) { - scaled_direction_ = x * direction_; +void LineSearchFunction::Evaluate(const double x, + const bool evaluate_gradient, + FunctionSample* output) { + output->x = x; + output->vector_x_is_valid = false; + output->value_is_valid = false; + output->gradient_is_valid = false; + output->vector_gradient_is_valid = false; + + scaled_direction_ = output->x * direction_; + output->vector_x.resize(position_.rows(), 1); if (!evaluator_->Plus(position_.data(), scaled_direction_.data(), - evaluation_point_.data())) { - return false; + output->vector_x.data())) { + return; } + output->vector_x_is_valid = true; - if (g == NULL) { - return (evaluator_->Evaluate(evaluation_point_.data(), - f, NULL, NULL, NULL) && - IsFinite(*f)); + double* gradient = NULL; + if (evaluate_gradient) { + output->vector_gradient.resize(direction_.rows(), 1); + gradient = output->vector_gradient.data(); } + const bool eval_status = evaluator_->Evaluate( + output->vector_x.data(), &(output->value), NULL, gradient, NULL); - if (!evaluator_->Evaluate(evaluation_point_.data(), - f, NULL, gradient_.data(), NULL)) { - return false; + if (!eval_status || !std::isfinite(output->value)) { + return; + } + + output->value_is_valid = true; + if (!evaluate_gradient) { + return; + } + + output->gradient = direction_.dot(output->vector_gradient); + if (!std::isfinite(output->gradient)) { + return; } - *g = direction_.dot(gradient_); - return IsFinite(*f) && IsFinite(*g); + output->gradient_is_valid = true; + output->vector_gradient_is_valid = true; } double LineSearchFunction::DirectionInfinityNorm() const { @@ -152,21 +151,28 @@ double LineSearchFunction::DirectionInfinityNorm() const { } void LineSearchFunction::ResetTimeStatistics() { - const map<string, double> evaluator_time_statistics = - evaluator_->TimeStatistics(); + const map<string, CallStatistics> evaluator_statistics = + evaluator_->Statistics(); + initial_evaluator_residual_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); + FindWithDefault( + evaluator_statistics, "Evaluator::Residual", CallStatistics()) + .time; initial_evaluator_jacobian_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); + FindWithDefault( + evaluator_statistics, "Evaluator::Jacobian", CallStatistics()) + .time; } void LineSearchFunction::TimeStatistics( double* cost_evaluation_time_in_seconds, double* gradient_evaluation_time_in_seconds) const { - const map<string, double> evaluator_time_statistics = - evaluator_->TimeStatistics(); + const map<string, CallStatistics> evaluator_time_statistics = + evaluator_->Statistics(); *cost_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0) - + FindWithDefault( + evaluator_time_statistics, "Evaluator::Residual", CallStatistics()) + .time - initial_evaluator_residual_time_in_seconds; // Strictly speaking this will slightly underestimate the time spent // evaluating the gradient of the line search univariate cost function as it @@ -175,7 +181,9 @@ void LineSearchFunction::TimeStatistics( // allows direct subtraction of the timing information from the totals for // the evaluator returned in the solver summary. *gradient_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0) - + FindWithDefault( + evaluator_time_statistics, "Evaluator::Jacobian", CallStatistics()) + .time - initial_evaluator_jacobian_time_in_seconds; } @@ -184,12 +192,12 @@ void LineSearch::Search(double step_size_estimate, double initial_gradient, Summary* summary) const { const double start_time = WallTimeInSeconds(); - *CHECK_NOTNULL(summary) = LineSearch::Summary(); + CHECK(summary != nullptr); + *summary = LineSearch::Summary(); summary->cost_evaluation_time_in_seconds = 0.0; summary->gradient_evaluation_time_in_seconds = 0.0; summary->polynomial_minimization_time_in_seconds = 0.0; - options().function->ResetTimeStatistics(); this->DoSearch(step_size_estimate, initial_cost, initial_gradient, summary); options().function-> @@ -243,12 +251,12 @@ double LineSearch::InterpolatingPolynomialMinimizingStepSize( 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)); + samples.push_back(FunctionSample(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)); + samples.push_back(FunctionSample(previous.x, previous.value)); } } else if (interpolation_type == CUBIC) { // Two point interpolation using the function values and the gradients. @@ -286,34 +294,26 @@ void ArmijoLineSearch::DoSearch(const double step_size_estimate, // 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 initial_position(0.0, initial_cost, initial_gradient); + initial_position.vector_x = function->position(); + initial_position.vector_x_is_valid = true; - 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; + const double descent_direction_max_norm = function->DirectionInfinityNorm(); + FunctionSample previous; + FunctionSample current; // As the Armijo line search algorithm always uses the initial point, for // which both the function value and derivative are known, when fitting a // minimizing polynomial, we can fit up to a quadratic without requiring the // gradient at the current query point. - const bool interpolation_uses_gradient_at_current_sample = - options().interpolation_type == CUBIC; - const double descent_direction_max_norm = function->DirectionInfinityNorm(); + const bool kEvaluateGradient = options().interpolation_type == CUBIC; ++summary->num_function_evaluations; - if (interpolation_uses_gradient_at_current_sample) { + if (kEvaluateGradient) { ++summary->num_gradient_evaluations; } - current.value_is_valid = - function->Evaluate(current.x, - ¤t.value, - interpolation_uses_gradient_at_current_sample - ? ¤t.gradient : NULL); - current.gradient_is_valid = - interpolation_uses_gradient_at_current_sample && current.value_is_valid; + + function->Evaluate(step_size_estimate, kEvaluateGradient, ¤t); while (!current.value_is_valid || current.value > (initial_cost + options().sufficient_decrease @@ -354,22 +354,16 @@ void ArmijoLineSearch::DoSearch(const double step_size_estimate, } previous = current; - current.x = step_size; ++summary->num_function_evaluations; - if (interpolation_uses_gradient_at_current_sample) { + if (kEvaluateGradient) { ++summary->num_gradient_evaluations; } - current.value_is_valid = - function->Evaluate(current.x, - ¤t.value, - interpolation_uses_gradient_at_current_sample - ? ¤t.gradient : NULL); - current.gradient_is_valid = - interpolation_uses_gradient_at_current_sample && current.value_is_valid; + + function->Evaluate(step_size, kEvaluateGradient, ¤t); } - summary->optimal_step_size = current.x; + summary->optimal_point = current; summary->success = true; } @@ -391,9 +385,9 @@ void WolfeLineSearch::DoSearch(const double step_size_estimate, // 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 initial_position(0.0, initial_cost, initial_gradient); + initial_position.vector_x = options().function->position(); + initial_position.vector_x_is_valid = true; 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 @@ -435,7 +429,7 @@ void WolfeLineSearch::DoSearch(const double step_size_estimate, // produce a valid point when ArmijoLineSearch would succeed, we return the // point with the lowest cost found thus far which satsifies the Armijo // condition (but not the Wolfe conditions). - summary->optimal_step_size = bracket_low.x; + summary->optimal_point = bracket_low; summary->success = true; return; } @@ -481,11 +475,13 @@ void WolfeLineSearch::DoSearch(const double step_size_estimate, // 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 = solution.x; + if (!solution.value_is_valid || solution.value > bracket_low.value) { + summary->optimal_point = bracket_low; + } else { + summary->optimal_point = solution; + } + summary->success = true; } @@ -515,8 +511,7 @@ bool WolfeLineSearch::BracketingPhase( LineSearchFunction* function = options().function; FunctionSample previous = initial_position; - FunctionSample current = ValueAndGradientSample(step_size_estimate, 0.0, 0.0); - current.value_is_valid = false; + FunctionSample current; const double descent_direction_max_norm = function->DirectionInfinityNorm(); @@ -535,12 +530,8 @@ bool WolfeLineSearch::BracketingPhase( // issues). ++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; - + const bool kEvaluateGradient = true; + function->Evaluate(step_size_estimate, kEvaluateGradient, ¤t); while (true) { ++summary->num_iterations; @@ -637,7 +628,18 @@ bool WolfeLineSearch::BracketingPhase( // being a boundary of a bracket. // If f(current) is valid, (but meets no criteria) expand the search by - // increasing the step size. + // increasing the step size. If f(current) is invalid, contract the step + // size. + // + // In Nocedal & Wright [1] (p60), the step-size can only increase in the + // bracketing phase: step_size_{k+1} \in [step_size_k, step_size_k * factor]. + // However this does not account for the function returning invalid values + // which we support, in which case we need to contract the step size whilst + // ensuring that we do not invert the bracket, i.e, we require that: + // step_size_{k-1} <= step_size_{k+1} < step_size_k. + const double min_step_size = + current.value_is_valid + ? current.x : previous.x; const double max_step_size = current.value_is_valid ? (current.x * options().max_step_expansion) : current.x; @@ -656,7 +658,7 @@ bool WolfeLineSearch::BracketingPhase( previous, unused_previous, current, - previous.x, + min_step_size, max_step_size); summary->polynomial_minimization_time_in_seconds += (WallTimeInSeconds() - polynomial_minimization_start_time); @@ -669,16 +671,14 @@ bool WolfeLineSearch::BracketingPhase( return false; } + // Only advance the lower boundary (in x) of the bracket if f(current) + // is valid such that we can support contracting the step size when + // f(current) is invalid without risking inverting the bracket in x, i.e. + // prevent previous.x > current.x. previous = current.value_is_valid ? current : previous; - current.x = step_size; - ++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; + function->Evaluate(step_size, kEvaluateGradient, ¤t); } // Ensure that even if a valid bracket was found, we will only mark a zoom @@ -799,7 +799,7 @@ bool WolfeLineSearch::ZoomPhase(const FunctionSample& initial_position, const FunctionSample unused_previous; DCHECK(!unused_previous.value_is_valid); const double polynomial_minimization_start_time = WallTimeInSeconds(); - solution->x = + const double step_size = this->InterpolatingPolynomialMinimizingStepSize( options().interpolation_type, lower_bound_step, @@ -823,12 +823,9 @@ bool WolfeLineSearch::ZoomPhase(const FunctionSample& initial_position, // to numerical issues). ++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) { + const bool kEvaluateGradient = true; + function->Evaluate(step_size, kEvaluateGradient, solution); + if (!solution->value_is_valid || !solution->gradient_is_valid) { summary->error = StringPrintf("Line search failed: Wolfe Zoom phase found " "step_size: %.5e, for which function is invalid, " diff --git a/extern/ceres/internal/ceres/line_search.h b/extern/ceres/internal/ceres/line_search.h index 6a21cbeac11..d59fd777367 100644 --- a/extern/ceres/internal/ceres/line_search.h +++ b/extern/ceres/internal/ceres/line_search.h @@ -35,6 +35,7 @@ #include <string> #include <vector> +#include "ceres/function_sample.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" #include "ceres/types.h" @@ -43,7 +44,6 @@ namespace ceres { namespace internal { class Evaluator; -struct FunctionSample; class LineSearchFunction; // Line search is another name for a one dimensional optimization @@ -51,7 +51,7 @@ class LineSearchFunction; // dimensional optimization problems that arise as subproblems of // general multidimensional optimization problems. // -// While finding the exact minimum of a one dimensionl function is +// While finding the exact minimum of a one dimensional function is // hard, instances of LineSearch find a point that satisfies a // sufficient decrease condition. Depending on the particular // condition used, we get a variety of different line search @@ -61,21 +61,9 @@ class LineSearch { struct Summary; struct Options { - Options() - : interpolation_type(CUBIC), - sufficient_decrease(1e-4), - 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), - is_silent(false), - function(NULL) {} - // Degree of the polynomial used to approximate the objective // function. - LineSearchInterpolationType interpolation_type; + LineSearchInterpolationType interpolation_type = CUBIC; // Armijo and Wolfe line search parameters. @@ -88,7 +76,7 @@ class LineSearch { // s.t. // // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size - double sufficient_decrease; + double sufficient_decrease = 1e-4; // In each iteration of the Armijo / Wolfe line search, // @@ -98,7 +86,7 @@ class LineSearch { // // 0 < max_step_contraction < min_step_contraction < 1 // - double max_step_contraction; + double max_step_contraction = 1e-3; // In each iteration of the Armijo / Wolfe line search, // @@ -107,16 +95,16 @@ class LineSearch { // // 0 < max_step_contraction < min_step_contraction < 1 // - double min_step_contraction; + double min_step_contraction = 0.9; // If during the line search, the step_size falls below this // value, it is truncated to zero. - double min_step_size; + double min_step_size = 1e-9; // 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; + int max_num_iterations = 20; // Wolfe-specific line search parameters. @@ -131,7 +119,7 @@ class LineSearch { // // 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; + double sufficient_curvature_decrease = 0.9; // During the bracketing phase of the Wolfe search, the step size is // increased until either a point satisfying the Wolfe conditions is @@ -142,43 +130,32 @@ class LineSearch { // new_step_size <= max_step_expansion * step_size. // // By definition for expansion, max_step_expansion > 1.0. - double max_step_expansion; + double max_step_expansion = 10; - bool is_silent; + bool is_silent = false; // The one dimensional function that the line search algorithm // minimizes. - LineSearchFunction* function; + LineSearchFunction* function = nullptr; }; // Result of the line search. struct Summary { - Summary() - : success(false), - optimal_step_size(0.0), - num_function_evaluations(0), - num_gradient_evaluations(0), - num_iterations(0), - cost_evaluation_time_in_seconds(-1.0), - gradient_evaluation_time_in_seconds(-1.0), - polynomial_minimization_time_in_seconds(-1.0), - total_time_in_seconds(-1.0) {} - - bool success; - double optimal_step_size; - int num_function_evaluations; - int num_gradient_evaluations; - int num_iterations; + bool success = false; + FunctionSample optimal_point; + int num_function_evaluations = 0; + int num_gradient_evaluations = 0; + int num_iterations = 0; // Cumulative time spent evaluating the value of the cost function across // all iterations. - double cost_evaluation_time_in_seconds; + double cost_evaluation_time_in_seconds = 0.0; // Cumulative time spent evaluating the gradient of the cost function across // all iterations. - double gradient_evaluation_time_in_seconds; + double gradient_evaluation_time_in_seconds = 0.0; // Cumulative time spent minimizing the interpolating polynomial to compute // the next candidate step size across all iterations. - double polynomial_minimization_time_in_seconds; - double total_time_in_seconds; + double polynomial_minimization_time_in_seconds = 0.0; + double total_time_in_seconds = 0.0; std::string error; }; @@ -234,6 +211,7 @@ class LineSearchFunction { public: explicit LineSearchFunction(Evaluator* evaluator); void Init(const Vector& position, const Vector& direction); + // Evaluate the line search objective // // f(x) = p(position + x * direction) @@ -241,27 +219,29 @@ class LineSearchFunction { // Where, p is the objective function of the general optimization // problem. // - // g is the gradient f'(x) at x. + // evaluate_gradient controls whether the gradient will be evaluated + // or not. // - // f must not be null. The gradient is computed only if g is not null. - bool Evaluate(double x, double* f, double* g); + // On return output->*_is_valid indicate indicate whether the + // corresponding fields have numerically valid values or not. + void Evaluate(double x, bool evaluate_gradient, FunctionSample* output); + double DirectionInfinityNorm() const; + // Resets to now, the start point for the results from TimeStatistics(). void ResetTimeStatistics(); void TimeStatistics(double* cost_evaluation_time_in_seconds, double* gradient_evaluation_time_in_seconds) const; + const Vector& position() const { return position_; } + const Vector& direction() const { return direction_; } private: Evaluator* evaluator_; Vector position_; Vector direction_; - // evaluation_point = Evaluator::Plus(position_, x * direction_); - Vector evaluation_point_; - // scaled_direction = x * direction_; Vector scaled_direction_; - Vector gradient_; // We may not exclusively own the evaluator (e.g. in the Trust Region // minimizer), hence we need to save the initial evaluation durations for the @@ -282,10 +262,10 @@ class ArmijoLineSearch : public LineSearch { virtual ~ArmijoLineSearch() {} private: - virtual void DoSearch(double step_size_estimate, - double initial_cost, - double initial_gradient, - Summary* summary) const; + void DoSearch(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary) const final; }; // Bracketing / Zoom Strong Wolfe condition line search. This implementation @@ -315,10 +295,10 @@ class WolfeLineSearch : public LineSearch { Summary* summary) const; private: - virtual void DoSearch(double step_size_estimate, - double initial_cost, - double initial_gradient, - Summary* summary) const; + void DoSearch(double step_size_estimate, + double initial_cost, + double initial_gradient, + Summary* summary) const final; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/line_search_minimizer.cc b/extern/ceres/internal/ceres/line_search_minimizer.cc index fdde1ca9c86..931f56c960c 100644 --- a/extern/ceres/internal/ceres/line_search_minimizer.cc +++ b/extern/ceres/internal/ceres/line_search_minimizer.cc @@ -43,6 +43,7 @@ #include <algorithm> #include <cstdlib> #include <cmath> +#include <memory> #include <string> #include <vector> @@ -51,7 +52,6 @@ #include "ceres/evaluator.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/line_search.h" #include "ceres/line_search_direction.h" #include "ceres/stringprintf.h" @@ -63,27 +63,14 @@ namespace ceres { namespace internal { namespace { -// TODO(sameeragarwal): I think there is a small bug here, in that if -// the evaluation fails, then the state can contain garbage. Look at -// this more carefully. -bool Evaluate(Evaluator* evaluator, - const Vector& x, - LineSearchMinimizer::State* state, - std::string* message) { - if (!evaluator->Evaluate(x.data(), - &(state->cost), - NULL, - state->gradient.data(), - NULL)) { - *message = "Gradient evaluation failed."; - return false; - } - +bool EvaluateGradientNorms(Evaluator* evaluator, + const Vector& x, + LineSearchMinimizer::State* state, + std::string* message) { Vector negative_gradient = -state->gradient; Vector projected_gradient_step(x.size()); - if (!evaluator->Plus(x.data(), - negative_gradient.data(), - projected_gradient_step.data())) { + if (!evaluator->Plus( + x.data(), negative_gradient.data(), projected_gradient_step.data())) { *message = "projected_gradient_step = Plus(x, -gradient) failed."; return false; } @@ -103,7 +90,8 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, double start_time = WallTimeInSeconds(); double iteration_start_time = start_time; - Evaluator* evaluator = CHECK_NOTNULL(options.evaluator.get()); + CHECK(options.evaluator != nullptr); + Evaluator* evaluator = options.evaluator.get(); const int num_parameters = evaluator->NumParameters(); const int num_effective_parameters = evaluator->NumEffectiveParameters(); @@ -116,9 +104,6 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, State current_state(num_parameters, num_effective_parameters); State previous_state(num_parameters, num_effective_parameters); - Vector delta(num_effective_parameters); - Vector x_plus_delta(num_parameters); - IterationSummary iteration_summary; iteration_summary.iteration = 0; iteration_summary.step_is_valid = false; @@ -130,8 +115,19 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, iteration_summary.linear_solver_iterations = 0; iteration_summary.step_solver_time_in_seconds = 0; - // Do initial cost and Jacobian evaluation. - if (!Evaluate(evaluator, x, ¤t_state, &summary->message)) { + // Do initial cost and gradient evaluation. + if (!evaluator->Evaluate(x.data(), + &(current_state.cost), + nullptr, + current_state.gradient.data(), + nullptr)) { + summary->termination_type = FAILURE; + summary->message = "Initial cost and jacobian evaluation failed."; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } + + if (!EvaluateGradientNorms(evaluator, x, ¤t_state, &summary->message)) { summary->termination_type = FAILURE; summary->message = "Initial cost and jacobian evaluation failed. " "More details: " + summary->message; @@ -142,9 +138,8 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, summary->initial_cost = current_state.cost + summary->fixed_cost; iteration_summary.cost = current_state.cost + summary->fixed_cost; - iteration_summary.gradient_max_norm = current_state.gradient_max_norm; iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm); - + iteration_summary.gradient_max_norm = current_state.gradient_max_norm; if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) { summary->message = StringPrintf("Gradient tolerance reached. " "Gradient max norm: %e <= %e", @@ -170,7 +165,7 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, 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( + std::unique_ptr<LineSearchDirection> line_search_direction( LineSearchDirection::Create(line_search_direction_options)); LineSearchFunction line_search_function(evaluator); @@ -194,11 +189,11 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, line_search_options.is_silent = options.is_silent; line_search_options.function = &line_search_function; - scoped_ptr<LineSearch> + std::unique_ptr<LineSearch> line_search(LineSearch::Create(options.line_search_type, line_search_options, &summary->message)); - if (line_search.get() == NULL) { + if (line_search.get() == nullptr) { summary->termination_type = FAILURE; LOG_IF(ERROR, is_not_silent) << "Terminating: " << summary->message; return; @@ -326,28 +321,37 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, break; } - current_state.step_size = line_search_summary.optimal_step_size; - delta = current_state.step_size * current_state.search_direction; - + const FunctionSample& optimal_point = line_search_summary.optimal_point; + CHECK(optimal_point.vector_x_is_valid) + << "Congratulations, you found a bug in Ceres. Please report it."; + current_state.step_size = optimal_point.x; previous_state = current_state; iteration_summary.step_solver_time_in_seconds = WallTimeInSeconds() - iteration_start_time; - const double x_norm = x.norm(); - - if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) { - summary->termination_type = FAILURE; - summary->message = - "x_plus_delta = Plus(x, delta) failed. This should not happen " - "as the step was valid when it was selected by the line search."; - LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; - break; + if (optimal_point.vector_gradient_is_valid) { + current_state.cost = optimal_point.value; + current_state.gradient = optimal_point.vector_gradient; + } else { + Evaluator::EvaluateOptions evaluate_options; + evaluate_options.new_evaluation_point = false; + if (!evaluator->Evaluate(evaluate_options, + optimal_point.vector_x.data(), + &(current_state.cost), + nullptr, + current_state.gradient.data(), + nullptr)) { + summary->termination_type = FAILURE; + summary->message = "Cost and jacobian evaluation failed."; + LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message; + return; + } } - if (!Evaluate(evaluator, - x_plus_delta, - ¤t_state, - &summary->message)) { + if (!EvaluateGradientNorms(evaluator, + optimal_point.vector_x, + ¤t_state, + &summary->message)) { summary->termination_type = FAILURE; summary->message = "Step failed to evaluate. This should not happen as the step was " @@ -358,8 +362,9 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, } // Compute the norm of the step in the ambient space. - iteration_summary.step_norm = (x_plus_delta - x).norm(); - x = x_plus_delta; + iteration_summary.step_norm = (optimal_point.vector_x - x).norm(); + const double x_norm = x.norm(); + x = optimal_point.vector_x; iteration_summary.gradient_max_norm = current_state.gradient_max_norm; iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm); @@ -380,6 +385,7 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, iteration_summary.cumulative_time_in_seconds = WallTimeInSeconds() - start_time + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); // Iterations inside the line search algorithm are considered // 'steps' in the broader context, to distinguish these inner @@ -423,20 +429,18 @@ void LineSearchMinimizer::Minimize(const Minimizer::Options& options, } const double absolute_function_tolerance = - options.function_tolerance * previous_state.cost; - if (fabs(iteration_summary.cost_change) <= absolute_function_tolerance) { - summary->message = - StringPrintf("Function tolerance reached. " - "|cost_change|/cost: %e <= %e", - fabs(iteration_summary.cost_change) / - previous_state.cost, - options.function_tolerance); + options.function_tolerance * std::abs(previous_state.cost); + if (std::abs(iteration_summary.cost_change) <= + absolute_function_tolerance) { + summary->message = StringPrintf( + "Function tolerance reached. " + "|cost_change|/cost: %e <= %e", + std::abs(iteration_summary.cost_change) / previous_state.cost, + options.function_tolerance); summary->termination_type = CONVERGENCE; VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message; break; } - - summary->iterations.push_back(iteration_summary); } } diff --git a/extern/ceres/internal/ceres/line_search_minimizer.h b/extern/ceres/internal/ceres/line_search_minimizer.h index 54b7202e0c3..191128a933b 100644 --- a/extern/ceres/internal/ceres/line_search_minimizer.h +++ b/extern/ceres/internal/ceres/line_search_minimizer.h @@ -66,9 +66,9 @@ class LineSearchMinimizer : public Minimizer { }; ~LineSearchMinimizer() {} - virtual void Minimize(const Minimizer::Options& options, - double* parameters, - Solver::Summary* summary); + void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) final; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/line_search_preprocessor.cc b/extern/ceres/internal/ceres/line_search_preprocessor.cc index 831f5e8d079..5a21809e509 100644 --- a/extern/ceres/internal/ceres/line_search_preprocessor.cc +++ b/extern/ceres/internal/ceres/line_search_preprocessor.cc @@ -32,6 +32,8 @@ #include <numeric> #include <string> +#include "ceres/casts.h" +#include "ceres/context_impl.h" #include "ceres/evaluator.h" #include "ceres/minimizer.h" #include "ceres/problem_impl.h" @@ -57,6 +59,9 @@ bool SetupEvaluator(PreprocessedProblem* pp) { pp->evaluator_options.linear_solver_type = CGNR; pp->evaluator_options.num_eliminate_blocks = 0; pp->evaluator_options.num_threads = pp->options.num_threads; + pp->evaluator_options.context = pp->problem->context(); + pp->evaluator_options.evaluation_callback = + pp->reduced_program->mutable_evaluation_callback(); pp->evaluator.reset(Evaluator::Create(pp->evaluator_options, pp->reduced_program.get(), &pp->error)); @@ -71,7 +76,7 @@ LineSearchPreprocessor::~LineSearchPreprocessor() { bool LineSearchPreprocessor::Preprocess(const Solver::Options& options, ProblemImpl* problem, PreprocessedProblem* pp) { - CHECK_NOTNULL(pp); + CHECK(pp != nullptr); pp->options = options; ChangeNumThreadsIfNeeded(&pp->options); diff --git a/extern/ceres/internal/ceres/line_search_preprocessor.h b/extern/ceres/internal/ceres/line_search_preprocessor.h index 132d83a0a9a..12ccb53e011 100644 --- a/extern/ceres/internal/ceres/line_search_preprocessor.h +++ b/extern/ceres/internal/ceres/line_search_preprocessor.h @@ -39,9 +39,9 @@ namespace internal { class LineSearchPreprocessor : public Preprocessor { public: virtual ~LineSearchPreprocessor(); - virtual bool Preprocess(const Solver::Options& options, - ProblemImpl* problem, - PreprocessedProblem* preprocessed_problem); + bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* preprocessed_problem) final; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/linear_least_squares_problems.cc b/extern/ceres/internal/ceres/linear_least_squares_problems.cc index 0a69375f7b5..7c523d399e1 100644 --- a/extern/ceres/internal/ceres/linear_least_squares_problems.cc +++ b/extern/ceres/internal/ceres/linear_least_squares_problems.cc @@ -31,13 +31,14 @@ #include "ceres/linear_least_squares_problems.h" #include <cstdio> +#include <memory> #include <string> #include <vector> + #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/casts.h" #include "ceres/file.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/stringprintf.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" @@ -297,7 +298,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { problem->num_eliminate_blocks = 2; CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; - scoped_array<double> values(new double[num_rows * num_cols]); + std::unique_ptr<double[]> values(new double[num_rows * num_cols]); for (int c = 0; c < num_cols; ++c) { bs->cols.push_back(Block()); @@ -431,7 +432,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { problem->num_eliminate_blocks = 2; CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; - scoped_array<double> values(new double[num_rows * num_cols]); + std::unique_ptr<double[]> values(new double[num_rows * num_cols]); for (int c = 0; c < num_cols; ++c) { bs->cols.push_back(Block()); @@ -538,7 +539,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem4() { problem->num_eliminate_blocks = 1; CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; - scoped_array<double> values(new double[num_rows * num_cols]); + std::unique_ptr<double[]> values(new double[num_rows * num_cols]); // Column block structure bs->cols.push_back(Block()); @@ -613,7 +614,7 @@ bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, const double* b, const double* x, int num_eliminate_blocks) { - CHECK_NOTNULL(A); + CHECK(A != nullptr); Matrix AA; A->ToDenseMatrix(&AA); LOG(INFO) << "A^T: \n" << AA.transpose(); @@ -636,10 +637,10 @@ bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, void WriteArrayToFileOrDie(const string& filename, const double* x, const int size) { - CHECK_NOTNULL(x); + CHECK(x != nullptr); VLOG(2) << "Writing array to: " << filename; FILE* fptr = fopen(filename.c_str(), "w"); - CHECK_NOTNULL(fptr); + CHECK(fptr != nullptr); for (int i = 0; i < size; ++i) { fprintf(fptr, "%17f\n", x[i]); } @@ -652,7 +653,7 @@ bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base, const double* b, const double* x, int num_eliminate_blocks) { - CHECK_NOTNULL(A); + CHECK(A != nullptr); LOG(INFO) << "writing to: " << filename_base << "*"; string matlab_script; @@ -666,7 +667,7 @@ bool DumpLinearLeastSquaresProblemToTextFile(const string& filename_base, { string filename = filename_base + "_A.txt"; FILE* fptr = fopen(filename.c_str(), "w"); - CHECK_NOTNULL(fptr); + CHECK(fptr != nullptr); A->ToTextFile(fptr); fclose(fptr); StringAppendF(&matlab_script, diff --git a/extern/ceres/internal/ceres/linear_least_squares_problems.h b/extern/ceres/internal/ceres/linear_least_squares_problems.h index 384efb59a2b..5dfcd34e109 100644 --- a/extern/ceres/internal/ceres/linear_least_squares_problems.h +++ b/extern/ceres/internal/ceres/linear_least_squares_problems.h @@ -31,11 +31,11 @@ #ifndef CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ #define CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ +#include <memory> #include <string> #include <vector> #include "ceres/sparse_matrix.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" namespace ceres { namespace internal { @@ -44,21 +44,20 @@ namespace internal { // ground truth solutions. To be used by various LinearSolver tests. struct LinearLeastSquaresProblem { LinearLeastSquaresProblem() - : A(NULL), b(NULL), D(NULL), num_eliminate_blocks(0), - x(NULL), x_D(NULL) { + : num_eliminate_blocks(0) { } - scoped_ptr<SparseMatrix> A; - scoped_array<double> b; - scoped_array<double> D; + std::unique_ptr<SparseMatrix> A; + std::unique_ptr<double[]> b; + std::unique_ptr<double[]> D; // If using the schur eliminator then how many of the variable // blocks are e_type blocks. int num_eliminate_blocks; // Solution to min_x |Ax - b|^2 - scoped_array<double> x; + std::unique_ptr<double[]> x; // Solution to min_x |Ax - b|^2 + |Dx|^2 - scoped_array<double> x_D; + std::unique_ptr<double[]> x_D; }; // Factories for linear least squares problem. diff --git a/extern/ceres/internal/ceres/linear_solver.cc b/extern/ceres/internal/ceres/linear_solver.cc index 38e4625f747..107af6afcc8 100644 --- a/extern/ceres/internal/ceres/linear_solver.cc +++ b/extern/ceres/internal/ceres/linear_solver.cc @@ -35,6 +35,7 @@ #include "ceres/dense_qr_solver.h" #include "ceres/iterative_schur_complement_solver.h" #include "ceres/schur_complement_solver.h" +#include "ceres/dynamic_sparse_normal_cholesky_solver.h" #include "ceres/sparse_normal_cholesky_solver.h" #include "ceres/types.h" #include "glog/logging.h" @@ -70,23 +71,25 @@ LinearSolverType LinearSolver::LinearSolverForZeroEBlocks( } LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { + CHECK(options.context != NULL); + switch (options.type) { case CGNR: return new CgnrSolver(options); case SPARSE_NORMAL_CHOLESKY: -#if defined(CERES_NO_SUITESPARSE) && \ - defined(CERES_NO_CXSPARSE) && \ - !defined(CERES_USE_EIGEN_SPARSE) +#if defined(CERES_NO_SPARSE) return NULL; #else + if (options.dynamic_sparsity) { + return new DynamicSparseNormalCholeskySolver(options); + } + return new SparseNormalCholeskySolver(options); #endif case SPARSE_SCHUR: -#if defined(CERES_NO_SUITESPARSE) && \ - defined(CERES_NO_CXSPARSE) && \ - !defined(CERES_USE_EIGEN_SPARSE) +#if defined(CERES_NO_SPARSE) return NULL; #else return new SparseSchurComplementSolver(options); diff --git a/extern/ceres/internal/ceres/linear_solver.h b/extern/ceres/internal/ceres/linear_solver.h index fb9332ca6e3..cb624b372dd 100644 --- a/extern/ceres/internal/ceres/linear_solver.h +++ b/extern/ceres/internal/ceres/linear_solver.h @@ -41,6 +41,7 @@ #include "ceres/block_sparse_matrix.h" #include "ceres/casts.h" #include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/context_impl.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/execution_summary.h" #include "ceres/triplet_sparse_matrix.h" @@ -69,6 +70,16 @@ enum LinearSolverTerminationType { LINEAR_SOLVER_FATAL_ERROR }; +// This enum controls the fill-reducing ordering a sparse linear +// algebra library should use before computing a sparse factorization +// (usually Cholesky). +enum OrderingType { + NATURAL, // Do not re-order the matrix. This is useful when the + // matrix has been ordered using a fill-reducing ordering + // already. + AMD // Use the Approximate Minimum Degree algorithm to re-order + // the matrix. +}; class LinearOperator; @@ -91,42 +102,25 @@ class LinearOperator; class LinearSolver { public: struct Options { - Options() - : type(SPARSE_NORMAL_CHOLESKY), - preconditioner_type(JACOBI), - visibility_clustering_type(CANONICAL_VIEWS), - dense_linear_algebra_library_type(EIGEN), - sparse_linear_algebra_library_type(SUITE_SPARSE), - use_postordering(false), - dynamic_sparsity(false), - use_explicit_schur_complement(false), - min_num_iterations(1), - max_num_iterations(1), - num_threads(1), - residual_reset_period(10), - row_block_size(Eigen::Dynamic), - e_block_size(Eigen::Dynamic), - f_block_size(Eigen::Dynamic) { - } - - LinearSolverType type; - PreconditionerType preconditioner_type; - VisibilityClusteringType visibility_clustering_type; - DenseLinearAlgebraLibraryType dense_linear_algebra_library_type; - SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; + LinearSolverType type = SPARSE_NORMAL_CHOLESKY; + PreconditionerType preconditioner_type = JACOBI; + VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS; + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = + SUITE_SPARSE; // See solver.h for information about these flags. - bool use_postordering; - bool dynamic_sparsity; - bool use_explicit_schur_complement; + bool use_postordering = false; + bool dynamic_sparsity = false; + bool use_explicit_schur_complement = false; // Number of internal iterations that the solver uses. This // parameter only makes sense for iterative solvers like CG. - int min_num_iterations; - int max_num_iterations; + int min_num_iterations = 1; + int max_num_iterations = 1; // If possible, how many threads can the solver use. - int num_threads; + int num_threads = 1; // Hints about the order in which the parameter blocks should be // eliminated by the linear solver. @@ -151,7 +145,7 @@ class LinearSolver { // inaccurate over time. Thus for non-zero values of this // parameter, the solver can be told to recalculate the value of // the residual using a |b - Ax| evaluation. - int residual_reset_period; + int residual_reset_period = 10; // If the block sizes in a BlockSparseMatrix are fixed, then in // some cases the Schur complement based solvers can detect and @@ -162,20 +156,18 @@ class LinearSolver { // // Please see schur_complement_solver.h and schur_eliminator.h for // more details. - int row_block_size; - int e_block_size; - int f_block_size; + int row_block_size = Eigen::Dynamic; + int e_block_size = Eigen::Dynamic; + int f_block_size = Eigen::Dynamic; + + bool use_mixed_precision_solves = false; + int max_num_refinement_iterations = 0; + int subset_preconditioner_start_row_block = -1; + ContextImpl* context = nullptr; }; // Options for the Solve method. struct PerSolveOptions { - PerSolveOptions() - : D(NULL), - preconditioner(NULL), - r_tolerance(0.0), - q_tolerance(0.0) { - } - // This option only makes sense for unsymmetric linear solvers // that can solve rectangular linear systems. // @@ -199,7 +191,7 @@ class LinearSolver { // does not have full column rank, the results returned by the // solver cannot be relied on. D, if it is not null is an array of // size n. b is an array of size m and x is an array of size n. - double * D; + double* D = nullptr; // This option only makes sense for iterative solvers. // @@ -221,7 +213,7 @@ class LinearSolver { // // A null preconditioner is equivalent to an identity matrix being // used a preconditioner. - LinearOperator* preconditioner; + LinearOperator* preconditioner = nullptr; // The following tolerance related options only makes sense for @@ -233,7 +225,7 @@ class LinearSolver { // // This is the most commonly used termination criterion for // iterative solvers. - double r_tolerance; + double r_tolerance = 0.0; // For PSD matrices A, let // @@ -257,22 +249,16 @@ class LinearSolver { // Journal of Computational and Applied Mathematics, // 124(1-2), 45-59, 2000. // - double q_tolerance; + double q_tolerance = 0.0; }; // Summary of a call to the Solve method. We should move away from // the true/false method for determining solver success. We should // let the summary object do the talking. struct Summary { - Summary() - : residual_norm(0.0), - num_iterations(-1), - termination_type(LINEAR_SOLVER_FAILURE) { - } - - double residual_norm; - int num_iterations; - LinearSolverTerminationType termination_type; + double residual_norm = -1.0; + int num_iterations = -1; + LinearSolverTerminationType termination_type = LINEAR_SOLVER_FAILURE; std::string message; }; @@ -292,16 +278,12 @@ class LinearSolver { const PerSolveOptions& per_solve_options, double* x) = 0; - // The following two methods return copies instead of references so - // that the base class implementation does not have to worry about - // life time issues. Further, these calls are not expected to be - // frequent or performance sensitive. - virtual std::map<std::string, int> CallStatistics() const { - return std::map<std::string, int>(); - } - - virtual std::map<std::string, double> TimeStatistics() const { - return std::map<std::string, double>(); + // This method returns copies instead of references so that the base + // class implementation does not have to worry about life time + // issues. Further, this calls are not expected to be frequent or + // performance sensitive. + virtual std::map<std::string, CallStatistics> Statistics() const { + return std::map<std::string, CallStatistics>(); } // Factory @@ -325,18 +307,14 @@ class TypedLinearSolver : public LinearSolver { const LinearSolver::PerSolveOptions& per_solve_options, double* x) { ScopedExecutionTimer total_time("LinearSolver::Solve", &execution_summary_); - CHECK_NOTNULL(A); - CHECK_NOTNULL(b); - CHECK_NOTNULL(x); + CHECK(A != nullptr); + CHECK(b != nullptr); + CHECK(x != nullptr); return SolveImpl(down_cast<MatrixType*>(A), b, per_solve_options, x); } - virtual std::map<std::string, int> CallStatistics() const { - return execution_summary_.calls(); - } - - virtual std::map<std::string, double> TimeStatistics() const { - return execution_summary_.times(); + virtual std::map<std::string, CallStatistics> Statistics() const { + return execution_summary_.statistics(); } private: diff --git a/extern/ceres/internal/ceres/local_parameterization.cc b/extern/ceres/internal/ceres/local_parameterization.cc index a6bf1f6ddcc..97fdbed3eda 100644 --- a/extern/ceres/internal/ceres/local_parameterization.cc +++ b/extern/ceres/internal/ceres/local_parameterization.cc @@ -31,10 +31,11 @@ #include "ceres/local_parameterization.h" #include <algorithm> + #include "Eigen/Geometry" -#include "ceres/householder_vector.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" +#include "ceres/internal/householder_vector.h" #include "ceres/rotation.h" #include "glog/logging.h" @@ -42,13 +43,16 @@ namespace ceres { using std::vector; -LocalParameterization::~LocalParameterization() { -} +LocalParameterization::~LocalParameterization() {} bool LocalParameterization::MultiplyByJacobian(const double* x, const int num_rows, const double* global_matrix, double* local_matrix) const { + if (LocalSize() == 0) { + return true; + } + Matrix jacobian(GlobalSize(), LocalSize()); if (!ComputeJacobian(x, jacobian.data())) { return false; @@ -74,7 +78,7 @@ bool IdentityParameterization::Plus(const double* x, bool IdentityParameterization::ComputeJacobian(const double* x, double* jacobian) const { - MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_); + MatrixRef(jacobian, size_, size_).setIdentity(); return true; } @@ -82,9 +86,8 @@ bool IdentityParameterization::MultiplyByJacobian(const double* x, const int num_cols, const double* global_matrix, double* local_matrix) const { - std::copy(global_matrix, - global_matrix + num_cols * GlobalSize(), - local_matrix); + std::copy( + global_matrix, global_matrix + num_cols * GlobalSize(), local_matrix); return true; } @@ -93,8 +96,8 @@ SubsetParameterization::SubsetParameterization( : local_size_(size - constant_parameters.size()), constancy_mask_(size, 0) { vector<int> constant = constant_parameters; std::sort(constant.begin(), constant.end()); - CHECK_GE(constant.front(), 0) - << "Indices indicating constant parameter must be greater than zero."; + CHECK_GE(constant.front(), 0) << "Indices indicating constant parameter must " + "be greater than equal to zero."; CHECK_LT(constant.back(), size) << "Indices indicating constant parameter must be less than the size " << "of the parameter block."; @@ -108,7 +111,8 @@ SubsetParameterization::SubsetParameterization( bool SubsetParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const { - for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + const int global_size = GlobalSize(); + for (int i = 0, j = 0; i < global_size; ++i) { if (constancy_mask_[i]) { x_plus_delta[i] = x[i]; } else { @@ -124,9 +128,10 @@ bool SubsetParameterization::ComputeJacobian(const double* x, return true; } - MatrixRef m(jacobian, constancy_mask_.size(), local_size_); + const int global_size = GlobalSize(); + MatrixRef m(jacobian, global_size, local_size_); m.setZero(); - for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + for (int i = 0, j = 0; i < global_size; ++i) { if (!constancy_mask_[i]) { m(i, j++) = 1.0; } @@ -135,18 +140,19 @@ bool SubsetParameterization::ComputeJacobian(const double* x, } bool SubsetParameterization::MultiplyByJacobian(const double* x, - const int num_rows, - const double* global_matrix, - double* local_matrix) const { + const int num_cols, + const double* global_matrix, + double* local_matrix) const { if (local_size_ == 0) { return true; } - for (int row = 0; row < num_rows; ++row) { - for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) { - if (!constancy_mask_[col]) { - local_matrix[row * LocalSize() + j++] = - global_matrix[row * GlobalSize() + col]; + const int global_size = GlobalSize(); + for (int col = 0; col < num_cols; ++col) { + for (int i = 0, j = 0; i < global_size; ++i) { + if (!constancy_mask_[i]) { + local_matrix[col * local_size_ + j++] = + global_matrix[col * global_size + i]; } } } @@ -176,10 +182,12 @@ bool QuaternionParameterization::Plus(const double* x, bool QuaternionParameterization::ComputeJacobian(const double* x, double* jacobian) const { - jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT - jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT - jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT - jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT + // clang-format off + jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; + jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; + jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; + jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; + // clang-format on return true; } @@ -209,10 +217,12 @@ bool EigenQuaternionParameterization::Plus(const double* x_ptr, bool EigenQuaternionParameterization::ComputeJacobian(const double* x, double* jacobian) const { - jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT - jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT - jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT - jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT + // clang-format off + jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; + jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; + jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; + jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; + // clang-format on return true; } @@ -241,21 +251,26 @@ bool HomogeneousVectorParameterization::Plus(const double* x_ptr, // (2nd Edition) for a detailed description. Note there is a typo on Page // 625, line 4 so check the book errata. const double norm_delta_div_2 = 0.5 * norm_delta; - const double sin_delta_by_delta = sin(norm_delta_div_2) / - norm_delta_div_2; + const double sin_delta_by_delta = + std::sin(norm_delta_div_2) / norm_delta_div_2; Vector y(size_); y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta; - y(size_ - 1) = cos(norm_delta_div_2); + y(size_ - 1) = std::cos(norm_delta_div_2); Vector v(size_); double beta; - internal::ComputeHouseholderVector<double>(x, &v, &beta); + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector<ConstVectorRef, double, Eigen::Dynamic>( + x, &v, &beta); // Apply the delta update to remain on the unit sphere. See section A6.9.3 // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed // description. - x_plus_delta = x.norm() * (y - v * (beta * (v.transpose() * y))); + x_plus_delta = x.norm() * (y - v * (beta * (v.transpose() * y))); return true; } @@ -267,7 +282,12 @@ bool HomogeneousVectorParameterization::ComputeJacobian( Vector v(size_); double beta; - internal::ComputeHouseholderVector<double>(x, &v, &beta); + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector<ConstVectorRef, double, Eigen::Dynamic>( + x, &v, &beta); // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the // Householder matrix (H = I - beta * v * v'). @@ -280,65 +300,14 @@ bool HomogeneousVectorParameterization::ComputeJacobian( return true; } -ProductParameterization::ProductParameterization( - LocalParameterization* local_param1, - LocalParameterization* local_param2) { - local_params_.push_back(local_param1); - local_params_.push_back(local_param2); - Init(); -} - -ProductParameterization::ProductParameterization( - LocalParameterization* local_param1, - LocalParameterization* local_param2, - LocalParameterization* local_param3) { - local_params_.push_back(local_param1); - local_params_.push_back(local_param2); - local_params_.push_back(local_param3); - Init(); -} - -ProductParameterization::ProductParameterization( - LocalParameterization* local_param1, - LocalParameterization* local_param2, - LocalParameterization* local_param3, - LocalParameterization* local_param4) { - local_params_.push_back(local_param1); - local_params_.push_back(local_param2); - local_params_.push_back(local_param3); - local_params_.push_back(local_param4); - Init(); -} - -ProductParameterization::~ProductParameterization() { - for (int i = 0; i < local_params_.size(); ++i) { - delete local_params_[i]; - } -} - -void ProductParameterization::Init() { - global_size_ = 0; - local_size_ = 0; - buffer_size_ = 0; - for (int i = 0; i < local_params_.size(); ++i) { - const LocalParameterization* param = local_params_[i]; - buffer_size_ = std::max(buffer_size_, - param->LocalSize() * param->GlobalSize()); - global_size_ += param->GlobalSize(); - local_size_ += param->LocalSize(); - } -} - bool ProductParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const { int x_cursor = 0; int delta_cursor = 0; - for (int i = 0; i < local_params_.size(); ++i) { - const LocalParameterization* param = local_params_[i]; - if (!param->Plus(x + x_cursor, - delta + delta_cursor, - x_plus_delta + x_cursor)) { + for (const auto& param : local_params_) { + if (!param->Plus( + x + x_cursor, delta + delta_cursor, x_plus_delta + x_cursor)) { return false; } delta_cursor += param->LocalSize(); @@ -356,16 +325,15 @@ bool ProductParameterization::ComputeJacobian(const double* x, int x_cursor = 0; int delta_cursor = 0; - for (int i = 0; i < local_params_.size(); ++i) { - const LocalParameterization* param = local_params_[i]; + for (const auto& param : local_params_) { const int local_size = param->LocalSize(); const int global_size = param->GlobalSize(); - if (!param->ComputeJacobian(x + x_cursor, buffer.get())) { + if (!param->ComputeJacobian(x + x_cursor, buffer.data())) { return false; } - jacobian.block(x_cursor, delta_cursor, global_size, local_size) - = MatrixRef(buffer.get(), global_size, local_size); + jacobian.block(x_cursor, delta_cursor, global_size, local_size) = + MatrixRef(buffer.data(), global_size, local_size); delta_cursor += local_size; x_cursor += global_size; diff --git a/extern/ceres/internal/ceres/loss_function.cc b/extern/ceres/internal/ceres/loss_function.cc index eb5026784dd..2c21a7377ca 100644 --- a/extern/ceres/internal/ceres/loss_function.cc +++ b/extern/ceres/internal/ceres/loss_function.cc @@ -32,6 +32,7 @@ #include "ceres/loss_function.h" +#include <algorithm> #include <cmath> #include <cstddef> #include <limits> @@ -101,7 +102,9 @@ void TolerantLoss::Evaluate(double s, double rho[3]) const { // large, it will overflow. Since numerically 1 + e^x == e^x when the // x is greater than about ln(2^53) for doubles, beyond this threshold // we substitute x for ln(1 + e^x) as a numerically equivalent approximation. - static const double kLog2Pow53 = 36.7; // ln(MathLimits<double>::kEpsilon). + + // ln(MathLimits<double>::kEpsilon). + static constexpr double kLog2Pow53 = 36.7; if (x > kLog2Pow53) { rho[0] = s - a_ - c_; rho[1] = 1.0; @@ -119,12 +122,12 @@ void TukeyLoss::Evaluate(double s, double* rho) const { // Inlier region. const double value = 1.0 - s / a_squared_; const double value_sq = value * value; - rho[0] = a_squared_ / 6.0 * (1.0 - value_sq * value); - rho[1] = 0.5 * value_sq; - rho[2] = -1.0 / a_squared_ * value; + rho[0] = a_squared_ / 3.0 * (1.0 - value_sq * value); + rho[1] = value_sq; + rho[2] = -2.0 / a_squared_ * value; } else { // Outlier region. - rho[0] = a_squared_ / 6.0; + rho[0] = a_squared_ / 3.0; rho[1] = 0.0; rho[2] = 0.0; } @@ -132,10 +135,12 @@ void TukeyLoss::Evaluate(double s, double* rho) const { ComposedLoss::ComposedLoss(const LossFunction* f, Ownership ownership_f, const LossFunction* g, Ownership ownership_g) - : f_(CHECK_NOTNULL(f)), - g_(CHECK_NOTNULL(g)), + : f_(f), + g_(g), ownership_f_(ownership_f), ownership_g_(ownership_g) { + CHECK(f_ != nullptr); + CHECK(g_ != nullptr); } ComposedLoss::~ComposedLoss() { diff --git a/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc b/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc index 1c6c9925f1c..f3953c46006 100644 --- a/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc +++ b/extern/ceres/internal/ceres/low_rank_inverse_hessian.cc @@ -175,12 +175,10 @@ void LowRankInverseHessian::RightMultiply(const double* x_ptr, << "approximation."; } - for (list<int>::const_iterator it = indices_.begin(); - it != indices_.end(); - ++it) { - const double beta = delta_gradient_history_.col(*it).dot(search_direction) / - delta_x_dot_delta_gradient_(*it); - search_direction += delta_x_history_.col(*it) * (alpha(*it) - beta); + for (const int i : indices_) { + const double beta = delta_gradient_history_.col(i).dot(search_direction) / + delta_x_dot_delta_gradient_(i); + search_direction += delta_x_history_.col(i) * (alpha(i) - beta); } } diff --git a/extern/ceres/internal/ceres/low_rank_inverse_hessian.h b/extern/ceres/internal/ceres/low_rank_inverse_hessian.h index 2c768c2ca53..0028a988923 100644 --- a/extern/ceres/internal/ceres/low_rank_inverse_hessian.h +++ b/extern/ceres/internal/ceres/low_rank_inverse_hessian.h @@ -54,7 +54,7 @@ namespace internal { // enhanced with scaling rule by Byrd, Nocedal and Schanbel. // // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with Limited -// Storage". Mathematics of Computation 35 (151): 773–782. +// Storage". Mathematics of Computation 35 (151): 773-782. // // Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994). // "Representations of Quasi-Newton Matrices and their use in @@ -84,12 +84,12 @@ class LowRankInverseHessian : public LinearOperator { bool Update(const Vector& delta_x, const Vector& delta_gradient); // LinearOperator interface - virtual void RightMultiply(const double* x, double* y) const; - virtual void LeftMultiply(const double* x, double* y) const { + void RightMultiply(const double* x, double* y) const final; + void LeftMultiply(const double* x, double* y) const final { RightMultiply(x, y); } - virtual int num_rows() const { return num_parameters_; } - virtual int num_cols() const { return num_parameters_; } + int num_rows() const final { return num_parameters_; } + int num_cols() const final { return num_parameters_; } private: const int num_parameters_; diff --git a/extern/ceres/internal/ceres/minimizer.h b/extern/ceres/internal/ceres/minimizer.h index b59372806e7..afdd60d2944 100644 --- a/extern/ceres/internal/ceres/minimizer.h +++ b/extern/ceres/internal/ceres/minimizer.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_MINIMIZER_H_ #define CERES_INTERNAL_MINIMIZER_H_ +#include <memory> #include <string> #include <vector> #include "ceres/internal/port.h" @@ -167,19 +168,19 @@ class Minimizer { // Object responsible for evaluating the cost, residuals and // Jacobian matrix. - shared_ptr<Evaluator> evaluator; + std::shared_ptr<Evaluator> evaluator; // Object responsible for actually computing the trust region // step, and sizing the trust region radius. - shared_ptr<TrustRegionStrategy> trust_region_strategy; + std::shared_ptr<TrustRegionStrategy> trust_region_strategy; // Object holding the Jacobian matrix. It is assumed that the // sparsity structure of the matrix has already been initialized // and will remain constant for the life time of the // optimization. - shared_ptr<SparseMatrix> jacobian; + std::shared_ptr<SparseMatrix> jacobian; - shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; + std::shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; }; static Minimizer* Create(MinimizerType minimizer_type); diff --git a/extern/ceres/internal/ceres/mutex.h b/extern/ceres/internal/ceres/mutex.h deleted file mode 100644 index 2ce97772755..00000000000 --- a/extern/ceres/internal/ceres/mutex.h +++ /dev/null @@ -1,329 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://ceres-solver.org/ -// -// 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: Craig Silverstein. -// -// A simple mutex wrapper, supporting locks and read-write locks. -// You should assume the locks are *not* re-entrant. -// -// This class is meant to be internal-only and should be wrapped by an -// internal namespace. Before you use this module, please give the -// name of your internal namespace for this module. Or, if you want -// to expose it, you'll want to move it to the Google namespace. We -// cannot put this class in global namespace because there can be some -// problems when we have multiple versions of Mutex in each shared object. -// -// NOTE: by default, we have #ifdef'ed out the TryLock() method. -// This is for two reasons: -// 1) TryLock() under Windows is a bit annoying (it requires a -// #define to be defined very early). -// 2) TryLock() is broken for NO_THREADS mode, at least in NDEBUG -// mode. -// If you need TryLock(), and either these two caveats are not a -// problem for you, or you're willing to work around them, then -// feel free to #define GMUTEX_TRYLOCK, or to remove the #ifdefs -// in the code below. -// -// CYGWIN NOTE: Cygwin support for rwlock seems to be buggy: -// http://www.cygwin.com/ml/cygwin/2008-12/msg00017.html -// Because of that, we might as well use windows locks for -// cygwin. They seem to be more reliable than the cygwin pthreads layer. -// -// TRICKY IMPLEMENTATION NOTE: -// This class is designed to be safe to use during -// dynamic-initialization -- that is, by global constructors that are -// run before main() starts. The issue in this case is that -// dynamic-initialization happens in an unpredictable order, and it -// could be that someone else's dynamic initializer could call a -// function that tries to acquire this mutex -- but that all happens -// before this mutex's constructor has run. (This can happen even if -// the mutex and the function that uses the mutex are in the same .cc -// file.) Basically, because Mutex does non-trivial work in its -// constructor, it's not, in the naive implementation, safe to use -// before dynamic initialization has run on it. -// -// The solution used here is to pair the actual mutex primitive with a -// bool that is set to true when the mutex is dynamically initialized. -// (Before that it's false.) Then we modify all mutex routines to -// look at the bool, and not try to lock/unlock until the bool makes -// it to true (which happens after the Mutex constructor has run.) -// -// This works because before main() starts -- particularly, during -// dynamic initialization -- there are no threads, so a) it's ok that -// the mutex operations are a no-op, since we don't need locking then -// anyway; and b) we can be quite confident our bool won't change -// state between a call to Lock() and a call to Unlock() (that would -// require a global constructor in one translation unit to call Lock() -// and another global constructor in another translation unit to call -// Unlock() later, which is pretty perverse). -// -// That said, it's tricky, and can conceivably fail; it's safest to -// avoid trying to acquire a mutex in a global constructor, if you -// can. One way it can fail is that a really smart compiler might -// initialize the bool to true at static-initialization time (too -// early) rather than at dynamic-initialization time. To discourage -// that, we set is_safe_ to true in code (not the constructor -// colon-initializer) and set it to true via a function that always -// evaluates to true, but that the compiler can't know always -// evaluates to true. This should be good enough. - -#ifndef CERES_INTERNAL_MUTEX_H_ -#define CERES_INTERNAL_MUTEX_H_ - -#include "ceres/internal/port.h" - -#if defined(CERES_NO_THREADS) - typedef int MutexType; // to keep a lock-count -#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) -# define CERES_WIN32_LEAN_AND_MEAN // We only need minimal includes -# ifdef CERES_GMUTEX_TRYLOCK - // We need Windows NT or later for TryEnterCriticalSection(). If you - // don't need that functionality, you can remove these _WIN32_WINNT - // lines, and change TryLock() to assert(0) or something. -# ifndef _WIN32_WINNT -# define _WIN32_WINNT 0x0400 -# endif -# endif -// Unfortunately, windows.h defines a bunch of macros with common -// names. Two in particular need avoiding: ERROR and min/max. -// To avoid macro definition of ERROR. -# define NOGDI -// To avoid macro definition of min/max. -# ifndef NOMINMAX -# define NOMINMAX -# endif -# include <windows.h> - typedef CRITICAL_SECTION MutexType; -#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) - // Needed for pthread_rwlock_*. If it causes problems, you could take it - // out, but then you'd have to unset CERES_HAVE_RWLOCK (at least on linux -- - // it *does* cause problems for FreeBSD, or MacOSX, but isn't needed for - // locking there.) -# if defined(__linux__) && !defined(_XOPEN_SOURCE) -# define _XOPEN_SOURCE 500 // may be needed to get the rwlock calls -# endif -# include <pthread.h> - typedef pthread_rwlock_t MutexType; -#elif defined(CERES_HAVE_PTHREAD) -# include <pthread.h> - typedef pthread_mutex_t MutexType; -#else -# error Need to implement mutex.h for your architecture, or #define NO_THREADS -#endif - -// We need to include these header files after defining _XOPEN_SOURCE -// as they may define the _XOPEN_SOURCE macro. -#include <assert.h> -#include <stdlib.h> // for abort() - -namespace ceres { -namespace internal { - -class Mutex { - public: - // Create a Mutex that is not held by anybody. This constructor is - // typically used for Mutexes allocated on the heap or the stack. - // See below for a recommendation for constructing global Mutex - // objects. - inline Mutex(); - - // Destructor - inline ~Mutex(); - - inline void Lock(); // Block if needed until free then acquire exclusively - inline void Unlock(); // Release a lock acquired via Lock() -#ifdef CERES_GMUTEX_TRYLOCK - inline bool TryLock(); // If free, Lock() and return true, else return false -#endif - // Note that on systems that don't support read-write locks, these may - // be implemented as synonyms to Lock() and Unlock(). So you can use - // these for efficiency, but don't use them anyplace where being able - // to do shared reads is necessary to avoid deadlock. - inline void ReaderLock(); // Block until free or shared then acquire a share - inline void ReaderUnlock(); // Release a read share of this Mutex - inline void WriterLock() { Lock(); } // Acquire an exclusive lock - inline void WriterUnlock() { Unlock(); } // Release a lock from WriterLock() - - // TODO(hamaji): Do nothing, implement correctly. - inline void AssertHeld() {} - - private: - MutexType mutex_; - // We want to make sure that the compiler sets is_safe_ to true only - // when we tell it to, and never makes assumptions is_safe_ is - // always true. volatile is the most reliable way to do that. - volatile bool is_safe_; - - inline void SetIsSafe() { is_safe_ = true; } - - // Catch the error of writing Mutex when intending MutexLock. - Mutex(Mutex* /*ignored*/) {} - // Disallow "evil" constructors - Mutex(const Mutex&); - void operator=(const Mutex&); -}; - -// Now the implementation of Mutex for various systems -#if defined(CERES_NO_THREADS) - -// When we don't have threads, we can be either reading or writing, -// but not both. We can have lots of readers at once (in no-threads -// mode, that's most likely to happen in recursive function calls), -// but only one writer. We represent this by having mutex_ be -1 when -// writing and a number > 0 when reading (and 0 when no lock is held). -// -// In debug mode, we assert these invariants, while in non-debug mode -// we do nothing, for efficiency. That's why everything is in an -// assert. - -Mutex::Mutex() : mutex_(0) { } -Mutex::~Mutex() { assert(mutex_ == 0); } -void Mutex::Lock() { assert(--mutex_ == -1); } -void Mutex::Unlock() { assert(mutex_++ == -1); } -#ifdef CERES_GMUTEX_TRYLOCK -bool Mutex::TryLock() { if (mutex_) return false; Lock(); return true; } -#endif -void Mutex::ReaderLock() { assert(++mutex_ > 0); } -void Mutex::ReaderUnlock() { assert(mutex_-- > 0); } - -#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) - -Mutex::Mutex() { InitializeCriticalSection(&mutex_); SetIsSafe(); } -Mutex::~Mutex() { DeleteCriticalSection(&mutex_); } -void Mutex::Lock() { if (is_safe_) EnterCriticalSection(&mutex_); } -void Mutex::Unlock() { if (is_safe_) LeaveCriticalSection(&mutex_); } -#ifdef GMUTEX_TRYLOCK -bool Mutex::TryLock() { return is_safe_ ? - TryEnterCriticalSection(&mutex_) != 0 : true; } -#endif -void Mutex::ReaderLock() { Lock(); } // we don't have read-write locks -void Mutex::ReaderUnlock() { Unlock(); } - -#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) - -#define CERES_SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ - if (is_safe_ && fncall(&mutex_) != 0) abort(); \ -} while (0) - -Mutex::Mutex() { - SetIsSafe(); - if (is_safe_ && pthread_rwlock_init(&mutex_, NULL) != 0) abort(); -} -Mutex::~Mutex() { CERES_SAFE_PTHREAD(pthread_rwlock_destroy); } -void Mutex::Lock() { CERES_SAFE_PTHREAD(pthread_rwlock_wrlock); } -void Mutex::Unlock() { CERES_SAFE_PTHREAD(pthread_rwlock_unlock); } -#ifdef CERES_GMUTEX_TRYLOCK -bool Mutex::TryLock() { return is_safe_ ? - pthread_rwlock_trywrlock(&mutex_) == 0 : - true; } -#endif -void Mutex::ReaderLock() { CERES_SAFE_PTHREAD(pthread_rwlock_rdlock); } -void Mutex::ReaderUnlock() { CERES_SAFE_PTHREAD(pthread_rwlock_unlock); } -#undef CERES_SAFE_PTHREAD - -#elif defined(CERES_HAVE_PTHREAD) - -#define CERES_SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ - if (is_safe_ && fncall(&mutex_) != 0) abort(); \ -} while (0) - -Mutex::Mutex() { - SetIsSafe(); - if (is_safe_ && pthread_mutex_init(&mutex_, NULL) != 0) abort(); -} -Mutex::~Mutex() { CERES_SAFE_PTHREAD(pthread_mutex_destroy); } -void Mutex::Lock() { CERES_SAFE_PTHREAD(pthread_mutex_lock); } -void Mutex::Unlock() { CERES_SAFE_PTHREAD(pthread_mutex_unlock); } -#ifdef CERES_GMUTEX_TRYLOCK -bool Mutex::TryLock() { return is_safe_ ? - pthread_mutex_trylock(&mutex_) == 0 : true; } -#endif -void Mutex::ReaderLock() { Lock(); } -void Mutex::ReaderUnlock() { Unlock(); } -#undef CERES_SAFE_PTHREAD - -#endif - -// -------------------------------------------------------------------------- -// Some helper classes - -// Note: The weird "Ceres" prefix for the class is a workaround for having two -// similar mutex.h files included in the same translation unit. This is a -// problem because macros do not respect C++ namespaces, and as a result, this -// does not work well (e.g. inside Chrome). The offending macros are -// "MutexLock(x) COMPILE_ASSERT(false)". To work around this, "Ceres" is -// prefixed to the class names; this permits defining the classes. - -// CeresMutexLock(mu) acquires mu when constructed and releases it -// when destroyed. -class CeresMutexLock { - public: - explicit CeresMutexLock(Mutex *mu) : mu_(mu) { mu_->Lock(); } - ~CeresMutexLock() { mu_->Unlock(); } - private: - Mutex * const mu_; - // Disallow "evil" constructors - CeresMutexLock(const CeresMutexLock&); - void operator=(const CeresMutexLock&); -}; - -// CeresReaderMutexLock and CeresWriterMutexLock do the same, for rwlocks -class CeresReaderMutexLock { - public: - explicit CeresReaderMutexLock(Mutex *mu) : mu_(mu) { mu_->ReaderLock(); } - ~CeresReaderMutexLock() { mu_->ReaderUnlock(); } - private: - Mutex * const mu_; - // Disallow "evil" constructors - CeresReaderMutexLock(const CeresReaderMutexLock&); - void operator=(const CeresReaderMutexLock&); -}; - -class CeresWriterMutexLock { - public: - explicit CeresWriterMutexLock(Mutex *mu) : mu_(mu) { mu_->WriterLock(); } - ~CeresWriterMutexLock() { mu_->WriterUnlock(); } - private: - Mutex * const mu_; - // Disallow "evil" constructors - CeresWriterMutexLock(const CeresWriterMutexLock&); - void operator=(const CeresWriterMutexLock&); -}; - -// Catch bug where variable name is omitted, e.g. MutexLock (&mu); -#define CeresMutexLock(x) \ - COMPILE_ASSERT(0, ceres_mutex_lock_decl_missing_var_name) -#define CeresReaderMutexLock(x) \ - COMPILE_ASSERT(0, ceres_rmutex_lock_decl_missing_var_name) -#define CeresWriterMutexLock(x) \ - COMPILE_ASSERT(0, ceres_wmutex_lock_decl_missing_var_name) - -} // namespace internal -} // namespace ceres - -#endif // CERES_INTERNAL_MUTEX_H_ diff --git a/extern/ceres/internal/ceres/normal_prior.cc b/extern/ceres/internal/ceres/normal_prior.cc index b3666cd702f..a3d5d8ed772 100644 --- a/extern/ceres/internal/ceres/normal_prior.cc +++ b/extern/ceres/internal/ceres/normal_prior.cc @@ -33,7 +33,6 @@ #include <cstddef> #include <vector> #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" #include "glog/logging.h" diff --git a/extern/ceres/internal/ceres/pair_hash.h b/extern/ceres/internal/ceres/pair_hash.h new file mode 100644 index 00000000000..80453bae7db --- /dev/null +++ b/extern/ceres/internal/ceres/pair_hash.h @@ -0,0 +1,112 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// A hasher for std::pair<T, T>. + +#ifndef CERES_INTERNAL_PAIR_HASH_H_ +#define CERES_INTERNAL_PAIR_HASH_H_ + +#include "ceres/internal/port.h" +#include <cstdint> +#include <utility> + +namespace ceres { +namespace internal { + +#if defined(_WIN32) && !defined(__MINGW64__) && !defined(__MINGW32__) +#define GG_LONGLONG(x) x##I64 +#define GG_ULONGLONG(x) x##UI64 +#else +#define GG_LONGLONG(x) x##LL +#define GG_ULONGLONG(x) x##ULL +#endif + +// The hash function is due to Bob Jenkins (see +// http://burtleburtle.net/bob/hash/index.html). Each mix takes 36 instructions, +// in 18 cycles if you're lucky. On x86 architectures, this requires 45 +// instructions in 27 cycles, if you're lucky. +// +// 32bit version +inline void hash_mix(uint32_t& a, uint32_t& b, uint32_t& c) { + a -= b; a -= c; a ^= (c>>13); + b -= c; b -= a; b ^= (a<<8); + c -= a; c -= b; c ^= (b>>13); + a -= b; a -= c; a ^= (c>>12); + b -= c; b -= a; b ^= (a<<16); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>3); + b -= c; b -= a; b ^= (a<<10); + c -= a; c -= b; c ^= (b>>15); +} + +// 64bit version +inline void hash_mix(uint64_t& a, uint64_t& b, uint64_t& c) { + a -= b; a -= c; a ^= (c>>43); + b -= c; b -= a; b ^= (a<<9); + c -= a; c -= b; c ^= (b>>8); + a -= b; a -= c; a ^= (c>>38); + b -= c; b -= a; b ^= (a<<23); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>35); + b -= c; b -= a; b ^= (a<<49); + c -= a; c -= b; c ^= (b>>11); +} + +inline uint32_t Hash32NumWithSeed(uint32_t num, uint32_t c) { + // The golden ratio; an arbitrary value. + uint32_t b = 0x9e3779b9UL; + hash_mix(num, b, c); + return c; +} + +inline uint64_t Hash64NumWithSeed(uint64_t num, uint64_t c) { + // More of the golden ratio. + uint64_t b = GG_ULONGLONG(0xe08c1d668b756f82); + hash_mix(num, b, c); + return c; +} + +// Hasher for STL pairs. Requires hashers for both members to be defined. +struct pair_hash { + public: + template <typename T> + std::size_t operator()(const std::pair<T, T>& p) const { + const std::size_t h1 = std::hash<T>()(p.first); + const std::size_t h2 = std::hash<T>()(p.second); + // The decision below is at compile time + return (sizeof(h1) <= sizeof(uint32_t)) ? Hash32NumWithSeed(h1, h2) + : Hash64NumWithSeed(h1, h2); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PAIR_HASH_H_ diff --git a/extern/ceres/internal/ceres/parallel_for.h b/extern/ceres/internal/ceres/parallel_for.h new file mode 100644 index 00000000000..2da2320c137 --- /dev/null +++ b/extern/ceres/internal/ceres/parallel_for.h @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +#ifndef CERES_INTERNAL_PARALLEL_FOR_ +#define CERES_INTERNAL_PARALLEL_FOR_ + +#include <functional> + +#include "ceres/context_impl.h" + +namespace ceres { +namespace internal { + +// Returns the maximum number of threads supported by the threading backend +// Ceres was compiled with. +int MaxNumThreadsAvailable(); + +// Execute the function for every element in the range [start, end) with at most +// num_threads. It will execute all the work on the calling thread if +// num_threads is 1. +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int)>& function); + +// Execute the function for every element in the range [start, end) with at most +// num_threads. It will execute all the work on the calling thread if +// num_threads is 1. Each invocation of function() will be passed a thread_id +// in [0, num_threads) that is guaranteed to be distinct from the value passed +// to any concurrent execution of function(). +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int thread_id, int i)>& function); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARALLEL_FOR_H_ diff --git a/extern/ceres/internal/ceres/parallel_for_cxx.cc b/extern/ceres/internal/ceres/parallel_for_cxx.cc new file mode 100644 index 00000000000..8e358f5900b --- /dev/null +++ b/extern/ceres/internal/ceres/parallel_for_cxx.cc @@ -0,0 +1,247 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifdef CERES_USE_CXX_THREADS + +#include "ceres/parallel_for.h" + +#include <cmath> +#include <condition_variable> +#include <memory> +#include <mutex> + +#include "ceres/concurrent_queue.h" +#include "ceres/scoped_thread_token.h" +#include "ceres/thread_token_provider.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { +// This class creates a thread safe barrier which will block until a +// pre-specified number of threads call Finished. This allows us to block the +// main thread until all the parallel threads are finished processing all the +// work. +class BlockUntilFinished { + public: + explicit BlockUntilFinished(int num_total) + : num_finished_(0), num_total_(num_total) {} + + // Increment the number of jobs that have finished and signal the blocking + // thread if all jobs have finished. + void Finished() { + std::lock_guard<std::mutex> lock(mutex_); + ++num_finished_; + CHECK_LE(num_finished_, num_total_); + if (num_finished_ == num_total_) { + condition_.notify_one(); + } + } + + // Block until all threads have signaled they are finished. + void Block() { + std::unique_lock<std::mutex> lock(mutex_); + condition_.wait(lock, [&]() { return num_finished_ == num_total_; }); + } + + private: + std::mutex mutex_; + std::condition_variable condition_; + // The current number of jobs finished. + int num_finished_; + // The total number of jobs. + int num_total_; +}; + +// Shared state between the parallel tasks. Each thread will use this +// information to get the next block of work to be performed. +struct SharedState { + SharedState(int start, int end, int num_work_items) + : start(start), + end(end), + num_work_items(num_work_items), + i(0), + thread_token_provider(num_work_items), + block_until_finished(num_work_items) {} + + // The start and end index of the for loop. + const int start; + const int end; + // The number of blocks that need to be processed. + const int num_work_items; + + // The next block of work to be assigned to a worker. The parallel for loop + // range is split into num_work_items blocks of work, i.e. a single block of + // work is: + // for (int j = start + i; j < end; j += num_work_items) { ... }. + int i; + std::mutex mutex_i; + + // Provides a unique thread ID among all active threads working on the same + // group of tasks. Thread-safe. + ThreadTokenProvider thread_token_provider; + + // Used to signal when all the work has been completed. Thread safe. + BlockUntilFinished block_until_finished; +}; + +} // namespace + +int MaxNumThreadsAvailable() { + return ThreadPool::MaxNumThreadsAvailable(); +} + +// See ParallelFor (below) for more details. +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int)>& function) { + CHECK_GT(num_threads, 0); + CHECK(context != NULL); + if (end <= start) { + return; + } + + // Fast path for when it is single threaded. + if (num_threads == 1) { + for (int i = start; i < end; ++i) { + function(i); + } + return; + } + + ParallelFor(context, start, end, num_threads, + [&function](int /*thread_id*/, int i) { function(i); }); +} + +// This implementation uses a fixed size max worker pool with a shared task +// queue. The problem of executing the function for the interval of [start, end) +// is broken up into at most num_threads blocks and added to the thread pool. To +// avoid deadlocks, the calling thread is allowed to steal work from the worker +// pool. This is implemented via a shared state between the tasks. In order for +// the calling thread or thread pool to get a block of work, it will query the +// shared state for the next block of work to be done. If there is nothing left, +// it will return. We will exit the ParallelFor call when all of the work has +// been done, not when all of the tasks have been popped off the task queue. +// +// A unique thread ID among all active tasks will be acquired once for each +// block of work. This avoids the significant performance penalty for acquiring +// it on every iteration of the for loop. The thread ID is guaranteed to be in +// [0, num_threads). +// +// A performance analysis has shown this implementation is onpar with OpenMP and +// TBB. +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int thread_id, int i)>& function) { + CHECK_GT(num_threads, 0); + CHECK(context != NULL); + if (end <= start) { + return; + } + + // Fast path for when it is single threaded. + if (num_threads == 1) { + // Even though we only have one thread, use the thread token provider to + // guarantee the exact same behavior when running with multiple threads. + ThreadTokenProvider thread_token_provider(num_threads); + const ScopedThreadToken scoped_thread_token(&thread_token_provider); + const int thread_id = scoped_thread_token.token(); + for (int i = start; i < end; ++i) { + function(thread_id, i); + } + return; + } + + // We use a std::shared_ptr because the main thread can finish all + // the work before the tasks have been popped off the queue. So the + // shared state needs to exist for the duration of all the tasks. + const int num_work_items = std::min((end - start), num_threads); + std::shared_ptr<SharedState> shared_state( + new SharedState(start, end, num_work_items)); + + // A function which tries to perform a chunk of work. This returns false if + // there is no work to be done. + auto task_function = [shared_state, &function]() { + int i = 0; + { + // Get the next available chunk of work to be performed. If there is no + // work, return false. + std::lock_guard<std::mutex> lock(shared_state->mutex_i); + if (shared_state->i >= shared_state->num_work_items) { + return false; + } + i = shared_state->i; + ++shared_state->i; + } + + const ScopedThreadToken scoped_thread_token( + &shared_state->thread_token_provider); + const int thread_id = scoped_thread_token.token(); + + // Perform each task. + for (int j = shared_state->start + i; + j < shared_state->end; + j += shared_state->num_work_items) { + function(thread_id, j); + } + shared_state->block_until_finished.Finished(); + return true; + }; + + // Add all the tasks to the thread pool. + for (int i = 0; i < num_work_items; ++i) { + // Note we are taking the task_function as value so the shared_state + // shared pointer is copied and the ref count is increased. This is to + // prevent it from being deleted when the main thread finishes all the + // work and exits before the threads finish. + context->thread_pool.AddTask([task_function]() { task_function(); }); + } + + // Try to do any available work on the main thread. This may steal work from + // the thread pool, but when there is no work left the thread pool tasks + // will be no-ops. + while (task_function()) { + } + + // Wait until all tasks have finished. + shared_state->block_until_finished.Block(); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_USE_CXX_THREADS diff --git a/extern/ceres/internal/ceres/parallel_for_nothreads.cc b/extern/ceres/internal/ceres/parallel_for_nothreads.cc new file mode 100644 index 00000000000..e8f450a714d --- /dev/null +++ b/extern/ceres/internal/ceres/parallel_for_nothreads.cc @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: alexs.mac@gmail.com (Alex Stewart) + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifdef CERES_NO_THREADS + +#include "ceres/parallel_for.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +int MaxNumThreadsAvailable() { return 1; } + +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int)>& function) { + CHECK_GT(num_threads, 0); + CHECK(context != NULL); + if (end <= start) { + return; + } + for (int i = start; i < end; ++i) { + function(i); + } +} + +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int thread_id, int i)>& function) { + CHECK_GT(num_threads, 0); + CHECK(context != NULL); + if (end <= start) { + return; + } + const int thread_id = 0; + for (int i = start; i < end; ++i) { + function(thread_id, i); + } +} + +} +} + +#endif // CERES_NO_THREADS diff --git a/extern/ceres/internal/ceres/householder_vector.h b/extern/ceres/internal/ceres/parallel_for_openmp.cc index f54feea054d..8afe3b11f8d 100644 --- a/extern/ceres/internal/ceres/householder_vector.h +++ b/extern/ceres/internal/ceres/parallel_for_openmp.cc @@ -1,6 +1,6 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. -// http://code.google.com/p/ceres-solver/ +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -28,58 +28,61 @@ // // Author: vitus@google.com (Michael Vitus) -#ifndef CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ -#define CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" -#include "Eigen/Core" +#if defined(CERES_USE_OPENMP) + +#include "ceres/parallel_for.h" + +#include "ceres/scoped_thread_token.h" +#include "ceres/thread_token_provider.h" #include "glog/logging.h" +#include "omp.h" namespace ceres { namespace internal { -// Algorithm 5.1.1 from 'Matrix Computations' by Golub et al. (Johns Hopkins -// Studies in Mathematical Sciences) but using the nth element of the input -// vector as pivot instead of first. This computes the vector v with v(n) = 1 -// and beta such that H = I - beta * v * v^T is orthogonal and -// H * x = ||x||_2 * e_n. -template <typename Scalar> -void ComputeHouseholderVector(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& x, - Eigen::Matrix<Scalar, Eigen::Dynamic, 1>* v, - Scalar* beta) { - CHECK_NOTNULL(beta); - CHECK_NOTNULL(v); - CHECK_GT(x.rows(), 1); - CHECK_EQ(x.rows(), v->rows()); - - Scalar sigma = x.head(x.rows() - 1).squaredNorm(); - *v = x; - (*v)(v->rows() - 1) = Scalar(1.0); - - *beta = Scalar(0.0); - const Scalar& x_pivot = x(x.rows() - 1); +int MaxNumThreadsAvailable() { + return omp_get_max_threads(); +} - if (sigma <= Scalar(std::numeric_limits<double>::epsilon())) { - if (x_pivot < Scalar(0.0)) { - *beta = Scalar(2.0); - } +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int)>& function) { + CHECK_GT(num_threads, 0); + CHECK(context != NULL); + if (end <= start) { return; } - const Scalar mu = sqrt(x_pivot * x_pivot + sigma); - Scalar v_pivot = Scalar(1.0); - - if (x_pivot <= Scalar(0.0)) { - v_pivot = x_pivot - mu; - } else { - v_pivot = -sigma / (x_pivot + mu); +#ifdef CERES_USE_OPENMP +#pragma omp parallel for num_threads(num_threads) \ + schedule(dynamic) if (num_threads > 1) +#endif // CERES_USE_OPENMP + for (int i = start; i < end; ++i) { + function(i); } +} - *beta = Scalar(2.0) * v_pivot * v_pivot / (sigma + v_pivot * v_pivot); +void ParallelFor(ContextImpl* context, + int start, + int end, + int num_threads, + const std::function<void(int thread_id, int i)>& function) { + CHECK(context != NULL); - v->head(v->rows() - 1) /= v_pivot; + ThreadTokenProvider thread_token_provider(num_threads); + ParallelFor(context, start, end, num_threads, [&](int i) { + const ScopedThreadToken scoped_thread_token(&thread_token_provider); + const int thread_id = scoped_thread_token.token(); + function(thread_id, i); + }); } } // namespace internal } // namespace ceres -#endif // CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ +#endif // defined(CERES_USE_OPENMP) diff --git a/extern/ceres/internal/ceres/parallel_utils.cc b/extern/ceres/internal/ceres/parallel_utils.cc new file mode 100644 index 00000000000..e1cb5f979ec --- /dev/null +++ b/extern/ceres/internal/ceres/parallel_utils.cc @@ -0,0 +1,90 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: wjr@google.com (William Rucklidge) + +#include "ceres/parallel_utils.h" + +namespace ceres { +namespace internal { + +void LinearIndexToUpperTriangularIndex(int k, int n, int* i, int* j) { + // This works by unfolding a rectangle into a triangle. + // Say n is even. 4 is a nice even number. The 10 i,j pairs that we + // want to produce are: + // 0,0 0,1 0,2 0,3 + // 1,1 1,2 1,3 + // 2,2 2,3 + // 3,3 + // This triangle can be folded into a 5x2 rectangle: + // 3,3 0,0 0,1 0,2 0,3 + // 2,2 2,3 1,1 1,2 1,3 + + // If N is odd, say 5, then the 15 i,j pairs are: + // 0,0 0,1 0,2 0,3 0,4 + // 1,1 1,2 1,3 1,4 + // 2,2 2,3 2,3 + // 3,3 3,4 + // 4,4 + // which folds to a 5x3 rectangle: + // 0,0 0,1 0,2 0,3 0,4 + // 4,4 1,1 1,2 1,3 1,4 + // 3,3 3,4 2,2 2,3 2,4 + + // All this function does is map the linear iteration position to a + // location in the rectangle and work out the appropriate (i, j) for that + // location. + if (n & 1) { + // Odd n. The tip of the triangle is on row 1. + int w = n; // Width of the rectangle to unfold + int i0 = k / w; + int j0 = k % w; + if (j0 >= i0) { + *i = i0; + *j = j0; + } else { + *i = n - i0; + *j = *i + j0; + } + } else { + // Even n. The tip of the triangle is on row 0, making it one wider. + int w = n + 1; + int i0 = k / w; + int j0 = k % w; + if (j0 > i0) { + *i = i0; + *j = j0 - 1; + } else { + *i = n - 1 - i0; + *j = *i + j0; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/parallel_utils.h b/extern/ceres/internal/ceres/parallel_utils.h new file mode 100644 index 00000000000..1291428228a --- /dev/null +++ b/extern/ceres/internal/ceres/parallel_utils.h @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: wjr@google.com (William Rucklidge) + +#ifndef CERES_INTERNAL_PARALLEL_UTILS_H_ +#define CERES_INTERNAL_PARALLEL_UTILS_H_ + +namespace ceres { +namespace internal { + +// Converts a linear iteration order into a triangular iteration order. +// Suppose you have nested loops that look like +// for (int i = 0; i < n; i++) { +// for (int j = i; j < n; j++) { +// ... use i and j +// } +// } +// Naively using ParallelFor to parallelise those loops might look like +// ParallelFor(..., 0, n * n, num_threads, +// [](int thread_id, int k) { +// int i = k / n, j = k % n; +// if (j < i) return; +// ... +// }); +// but these empty work items can lead to very unbalanced threading. Instead, +// do this: +// int actual_work_items = (n * (n + 1)) / 2; +// ParallelFor(..., 0, actual_work_items, num_threads, +// [](int thread_id, int k) { +// int i, j; +// UnfoldIteration(k, n, &i, &j); +// ... +// }); +// which in each iteration will produce i and j satisfying +// 0 <= i <= j < n +void LinearIndexToUpperTriangularIndex(int k, int n, int* i, int* j); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARALLEL_UTILS_H_ diff --git a/extern/ceres/internal/ceres/parameter_block.h b/extern/ceres/internal/ceres/parameter_block.h index 8e21553c668..88943dfbcff 100644 --- a/extern/ceres/internal/ceres/parameter_block.h +++ b/extern/ceres/internal/ceres/parameter_block.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -32,15 +32,16 @@ #define CERES_INTERNAL_PARAMETER_BLOCK_H_ #include <algorithm> +#include <cstdint> #include <cstdlib> #include <limits> +#include <memory> #include <string> +#include <unordered_set> + #include "ceres/array_utils.h" -#include "ceres/collections_port.h" -#include "ceres/integral_types.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/local_parameterization.h" #include "ceres/stringprintf.h" #include "glog/logging.h" @@ -62,29 +63,28 @@ class ResidualBlock; // responsible for the proper disposal of the local parameterization. class ParameterBlock { public: - // TODO(keir): Decide what data structure is best here. Should this be a set? - // Probably not, because sets are memory inefficient. However, if it's a - // vector, you can get into pathological linear performance when removing a - // residual block from a problem where all the residual blocks depend on one - // parameter; for example, shared focal length in a bundle adjustment - // problem. It might be worth making a custom structure that is just an array - // when it is small, but transitions to a hash set when it has more elements. - // - // For now, use a hash set. - typedef HashSet<ResidualBlock*> ResidualBlockSet; + typedef std::unordered_set<ResidualBlock*> ResidualBlockSet; // Create a parameter block with the user state, size, and index specified. // The size is the size of the parameter block and the index is the position // of the parameter block inside a Program (if any). - ParameterBlock(double* user_state, int size, int index) { - Init(user_state, size, index, NULL); - } + ParameterBlock(double* user_state, int size, int index) + : user_state_(user_state), + size_(size), + state_(user_state), + index_(index) {} ParameterBlock(double* user_state, int size, int index, - LocalParameterization* local_parameterization) { - Init(user_state, size, index, local_parameterization); + LocalParameterization* local_parameterization) + : user_state_(user_state), + size_(size), + state_(user_state), + index_(index) { + if (local_parameterization != nullptr) { + SetParameterization(local_parameterization); + } } // The size of the parameter block. @@ -92,12 +92,10 @@ class ParameterBlock { // Manipulate the parameter state. bool SetState(const double* x) { - CHECK(x != NULL) - << "Tried to set the state of constant parameter " - << "with user location " << user_state_; - CHECK(!is_constant_) - << "Tried to set the state of constant parameter " - << "with user location " << user_state_; + CHECK(x != nullptr) << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + CHECK(!IsConstant()) << "Tried to set the state of constant parameter " + << "with user location " << user_state_; state_ = x; return UpdateLocalParameterizationJacobian(); @@ -106,9 +104,9 @@ class ParameterBlock { // Copy the current parameter state out to x. This is "GetState()" rather than // simply "state()" since it is actively copying the data into the passed // pointer. - void GetState(double *x) const { + void GetState(double* x) const { if (x != state_) { - memcpy(x, state_, sizeof(*state_) * size_); + std::copy(state_, state_ + size_, x); } } @@ -116,7 +114,7 @@ class ParameterBlock { const double* state() const { return state_; } const double* user_state() const { return user_state_; } double* mutable_user_state() { return user_state_; } - LocalParameterization* local_parameterization() const { + const LocalParameterization* local_parameterization() const { return local_parameterization_; } LocalParameterization* mutable_local_parameterization() { @@ -124,9 +122,22 @@ class ParameterBlock { } // Set this parameter block to vary or not. - void SetConstant() { is_constant_ = true; } - void SetVarying() { is_constant_ = false; } - bool IsConstant() const { return is_constant_; } + void SetConstant() { is_set_constant_ = true; } + void SetVarying() { is_set_constant_ = false; } + bool IsConstant() const { return (is_set_constant_ || LocalSize() == 0); } + + double UpperBound(int index) const { + return (upper_bounds_ ? upper_bounds_[index] + : std::numeric_limits<double>::max()); + } + + double LowerBound(int index) const { + return (lower_bounds_ ? lower_bounds_[index] + : -std::numeric_limits<double>::max()); + } + + bool IsUpperBounded() const { return (upper_bounds_ == nullptr); } + bool IsLowerBounded() const { return (lower_bounds_ == nullptr); } // This parameter block's index in an array. int index() const { return index_; } @@ -142,7 +153,7 @@ class ParameterBlock { // Methods relating to the parameter block's parameterization. - // The local to global jacobian. Returns NULL if there is no local + // The local to global jacobian. Returns nullptr if there is no local // parameterization for this parameter block. The returned matrix is row-major // and has Size() rows and LocalSize() columns. const double* LocalParameterizationJacobian() const { @@ -150,27 +161,24 @@ class ParameterBlock { } int LocalSize() const { - return (local_parameterization_ == NULL) - ? size_ - : local_parameterization_->LocalSize(); + return (local_parameterization_ == nullptr) + ? size_ + : local_parameterization_->LocalSize(); } - // Set the parameterization. The parameterization can be set exactly once; - // multiple calls to set the parameterization to different values will crash. - // It is an error to pass NULL for the parameterization. The parameter block - // does not take ownership of the parameterization. + // Set the parameterization. The parameter block does not take + // ownership of the parameterization. void SetParameterization(LocalParameterization* new_parameterization) { - CHECK(new_parameterization != NULL) << "NULL parameterization invalid."; // Nothing to do if the new parameterization is the same as the // old parameterization. if (new_parameterization == local_parameterization_) { return; } - CHECK(local_parameterization_ == NULL) - << "Can't re-set the local parameterization; it leads to " - << "ambiguous ownership. Current local parameterization is: " - << local_parameterization_; + if (new_parameterization == nullptr) { + local_parameterization_ = nullptr; + return; + } CHECK(new_parameterization->GlobalSize() == size_) << "Invalid parameterization for parameter block. The parameter block " @@ -178,9 +186,9 @@ class ParameterBlock { << "size of " << new_parameterization->GlobalSize() << ". Did you " << "accidentally use the wrong parameter block or parameterization?"; - CHECK_GT(new_parameterization->LocalSize(), 0) - << "Invalid parameterization. Parameterizations must have a positive " - << "dimensional tangent space."; + CHECK_GE(new_parameterization->LocalSize(), 0) + << "Invalid parameterization. Parameterizations must have a " + << "non-negative dimensional tangent space."; local_parameterization_ = new_parameterization; local_parameterization_jacobian_.reset( @@ -194,7 +202,11 @@ class ParameterBlock { void SetUpperBound(int index, double upper_bound) { CHECK_LT(index, size_); - if (upper_bounds_.get() == NULL) { + if (upper_bound >= std::numeric_limits<double>::max() && !upper_bounds_) { + return; + } + + if (!upper_bounds_) { upper_bounds_.reset(new double[size_]); std::fill(upper_bounds_.get(), upper_bounds_.get() + size_, @@ -207,7 +219,11 @@ class ParameterBlock { void SetLowerBound(int index, double lower_bound) { CHECK_LT(index, size_); - if (lower_bounds_.get() == NULL) { + if (lower_bound <= -std::numeric_limits<double>::max() && !lower_bounds_) { + return; + } + + if (!lower_bounds_) { lower_bounds_.reset(new double[size_]); std::fill(lower_bounds_.get(), lower_bounds_.get() + size_, @@ -220,24 +236,24 @@ class ParameterBlock { // Generalization of the addition operation. This is the same as // LocalParameterization::Plus() followed by projection onto the // hyper cube implied by the bounds constraints. - bool Plus(const double *x, const double* delta, double* x_plus_delta) { - if (local_parameterization_ != NULL) { + bool Plus(const double* x, const double* delta, double* x_plus_delta) { + if (local_parameterization_ != nullptr) { if (!local_parameterization_->Plus(x, delta, x_plus_delta)) { return false; } } else { - VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) + - ConstVectorRef(delta, size_); + VectorRef(x_plus_delta, size_) = + ConstVectorRef(x, size_) + ConstVectorRef(delta, size_); } // Project onto the box constraints. - if (lower_bounds_.get() != NULL) { + if (lower_bounds_.get() != nullptr) { for (int i = 0; i < size_; ++i) { x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]); } } - if (upper_bounds_.get() != NULL) { + if (upper_bounds_.get() != nullptr) { for (int i = 0; i < size_; ++i) { x_plus_delta[i] = std::min(x_plus_delta[i], upper_bounds_[i]); } @@ -247,35 +263,36 @@ class ParameterBlock { } std::string ToString() const { - return StringPrintf("{ this=%p, user_state=%p, state=%p, size=%d, " - "constant=%d, index=%d, state_offset=%d, " - "delta_offset=%d }", - this, - user_state_, - state_, - size_, - is_constant_, - index_, - state_offset_, - delta_offset_); + return StringPrintf( + "{ this=%p, user_state=%p, state=%p, size=%d, " + "constant=%d, index=%d, state_offset=%d, " + "delta_offset=%d }", + this, + user_state_, + state_, + size_, + is_set_constant_, + index_, + state_offset_, + delta_offset_); } void EnableResidualBlockDependencies() { - CHECK(residual_blocks_.get() == NULL) + CHECK(residual_blocks_.get() == nullptr) << "Ceres bug: There is already a residual block collection " << "for parameter block: " << ToString(); residual_blocks_.reset(new ResidualBlockSet); } void AddResidualBlock(ResidualBlock* residual_block) { - CHECK(residual_blocks_.get() != NULL) + CHECK(residual_blocks_.get() != nullptr) << "Ceres bug: The residual block collection is null for parameter " << "block: " << ToString(); residual_blocks_->insert(residual_block); } void RemoveResidualBlock(ResidualBlock* residual_block) { - CHECK(residual_blocks_.get() != NULL) + CHECK(residual_blocks_.get() != nullptr) << "Ceres bug: The residual block collection is null for parameter " << "block: " << ToString(); CHECK(residual_blocks_->find(residual_block) != residual_blocks_->end()) @@ -285,12 +302,10 @@ class ParameterBlock { // This is only intended for iterating; perhaps this should only expose // .begin() and .end(). - ResidualBlockSet* mutable_residual_blocks() { - return residual_blocks_.get(); - } + ResidualBlockSet* mutable_residual_blocks() { return residual_blocks_.get(); } double LowerBoundForParameter(int index) const { - if (lower_bounds_.get() == NULL) { + if (lower_bounds_.get() == nullptr) { return -std::numeric_limits<double>::max(); } else { return lower_bounds_[index]; @@ -298,7 +313,7 @@ class ParameterBlock { } double UpperBoundForParameter(int index) const { - if (upper_bounds_.get() == NULL) { + if (upper_bounds_.get() == nullptr) { return std::numeric_limits<double>::max(); } else { return upper_bounds_[index]; @@ -306,27 +321,8 @@ class ParameterBlock { } private: - void Init(double* user_state, - int size, - int index, - LocalParameterization* local_parameterization) { - user_state_ = user_state; - size_ = size; - index_ = index; - is_constant_ = false; - state_ = user_state_; - - local_parameterization_ = NULL; - if (local_parameterization != NULL) { - SetParameterization(local_parameterization); - } - - state_offset_ = -1; - delta_offset_ = -1; - } - bool UpdateLocalParameterizationJacobian() { - if (local_parameterization_ == NULL) { + if (local_parameterization_ == nullptr) { return true; } @@ -335,13 +331,12 @@ class ParameterBlock { // at that time. const int jacobian_size = Size() * LocalSize(); - InvalidateArray(jacobian_size, - local_parameterization_jacobian_.get()); + InvalidateArray(jacobian_size, local_parameterization_jacobian_.get()); if (!local_parameterization_->ComputeJacobian( - state_, - local_parameterization_jacobian_.get())) { + state_, local_parameterization_jacobian_.get())) { LOG(WARNING) << "Local parameterization Jacobian computation failed" - "for x: " << ConstVectorRef(state_, Size()).transpose(); + "for x: " + << ConstVectorRef(state_, Size()).transpose(); return false; } @@ -358,30 +353,30 @@ class ParameterBlock { return true; } - double* user_state_; - int size_; - bool is_constant_; - LocalParameterization* local_parameterization_; + double* user_state_ = nullptr; + int size_ = -1; + bool is_set_constant_ = false; + LocalParameterization* local_parameterization_ = nullptr; // The "state" of the parameter. These fields are only needed while the // solver is running. While at first glance using mutable is a bad idea, this // ends up simplifying the internals of Ceres enough to justify the potential // pitfalls of using "mutable." - mutable const double* state_; - mutable scoped_array<double> local_parameterization_jacobian_; + mutable const double* state_ = nullptr; + mutable std::unique_ptr<double[]> local_parameterization_jacobian_; // The index of the parameter. This is used by various other parts of Ceres to // permit switching from a ParameterBlock* to an index in another array. - int32 index_; + int index_ = -1; // The offset of this parameter block inside a larger state vector. - int32 state_offset_; + int state_offset_ = -1; // The offset of this parameter block inside a larger delta vector. - int32 delta_offset_; + int delta_offset_ = -1; // If non-null, contains the residual blocks this parameter block is in. - scoped_ptr<ResidualBlockSet> residual_blocks_; + std::unique_ptr<ResidualBlockSet> residual_blocks_; // Upper and lower bounds for the parameter block. SetUpperBound // and SetLowerBound lazily initialize the upper_bounds_ and @@ -394,8 +389,8 @@ class ParameterBlock { // std::numeric_limits<double>::max() and // -std::numeric_limits<double>::max() respectively which correspond // to the parameter block being unconstrained. - scoped_array<double> upper_bounds_; - scoped_array<double> lower_bounds_; + std::unique_ptr<double[]> upper_bounds_; + std::unique_ptr<double[]> lower_bounds_; // Necessary so ProblemImpl can clean up the parameterizations. friend class ProblemImpl; diff --git a/extern/ceres/internal/ceres/parameter_block_ordering.cc b/extern/ceres/internal/ceres/parameter_block_ordering.cc index efba339977c..ef521c0e11b 100644 --- a/extern/ceres/internal/ceres/parameter_block_ordering.cc +++ b/extern/ceres/internal/ceres/parameter_block_ordering.cc @@ -30,9 +30,11 @@ #include "ceres/parameter_block_ordering.h" +#include <memory> +#include <unordered_set> + #include "ceres/graph.h" #include "ceres/graph_algorithms.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/map_util.h" #include "ceres/parameter_block.h" #include "ceres/program.h" @@ -49,13 +51,14 @@ using std::vector; int ComputeStableSchurOrdering(const Program& program, vector<ParameterBlock*>* ordering) { - CHECK_NOTNULL(ordering)->clear(); + CHECK(ordering != nullptr); + ordering->clear(); EventLogger event_logger("ComputeStableSchurOrdering"); - scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + std::unique_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); event_logger.AddEvent("CreateHessianGraph"); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - const HashSet<ParameterBlock*>& vertices = graph->vertices(); + const std::unordered_set<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]); @@ -80,9 +83,10 @@ int ComputeStableSchurOrdering(const Program& program, int ComputeSchurOrdering(const Program& program, vector<ParameterBlock*>* ordering) { - CHECK_NOTNULL(ordering)->clear(); + CHECK(ordering != nullptr); + ordering->clear(); - scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + std::unique_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); int independent_set_size = IndependentSetOrdering(*graph, ordering); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); @@ -99,9 +103,10 @@ int ComputeSchurOrdering(const Program& program, void ComputeRecursiveIndependentSetOrdering(const Program& program, ParameterBlockOrdering* ordering) { - CHECK_NOTNULL(ordering)->Clear(); + CHECK(ordering != nullptr); + ordering->Clear(); const vector<ParameterBlock*> parameter_blocks = program.parameter_blocks(); - scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + std::unique_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); int num_covered = 0; int round = 0; @@ -120,7 +125,8 @@ void ComputeRecursiveIndependentSetOrdering(const Program& program, } Graph<ParameterBlock*>* CreateHessianGraph(const Program& program) { - Graph<ParameterBlock*>* graph = CHECK_NOTNULL(new Graph<ParameterBlock*>); + Graph<ParameterBlock*>* graph = new Graph<ParameterBlock*>; + CHECK(graph != nullptr); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); for (int i = 0; i < parameter_blocks.size(); ++i) { ParameterBlock* parameter_block = parameter_blocks[i]; @@ -155,17 +161,16 @@ Graph<ParameterBlock*>* CreateHessianGraph(const Program& program) { void OrderingToGroupSizes(const ParameterBlockOrdering* ordering, vector<int>* group_sizes) { - CHECK_NOTNULL(group_sizes)->clear(); + CHECK(group_sizes != nullptr); + group_sizes->clear(); if (ordering == NULL) { return; } - const map<int, set<double*> >& group_to_elements = + const map<int, set<double*>>& group_to_elements = ordering->group_to_elements(); - for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); - it != group_to_elements.end(); - ++it) { - group_sizes->push_back(it->second.size()); + for (const auto& g_t_e : group_to_elements) { + group_sizes->push_back(g_t_e.second.size()); } } diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view.cc b/extern/ceres/internal/ceres/partitioned_matrix_view.cc index 8054964e039..d7a998d68a3 100644 --- a/extern/ceres/internal/ceres/partitioned_matrix_view.cc +++ b/extern/ceres/internal/ceres/partitioned_matrix_view.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. #include "ceres/linear_solver.h" #include "ceres/partitioned_matrix_view.h" @@ -51,126 +50,105 @@ PartitionedMatrixViewBase* PartitionedMatrixViewBase::Create(const LinearSolver::Options& options, const BlockSparseMatrix& matrix) { #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == 2)) { - return new PartitionedMatrixView<2, 2, 2>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == 3)) { - return new PartitionedMatrixView<2, 2, 3>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == 4)) { - return new PartitionedMatrixView<2, 2, 4>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == Eigen::Dynamic)) { - return new PartitionedMatrixView<2, 2, Eigen::Dynamic>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 3)) { - return new PartitionedMatrixView<2, 3, 3>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 4)) { - return new PartitionedMatrixView<2, 3, 4>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 6)) { - return new PartitionedMatrixView<2, 3, 6>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 9)) { - return new PartitionedMatrixView<2, 3, 9>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == Eigen::Dynamic)) { - return new PartitionedMatrixView<2, 3, Eigen::Dynamic>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 3)) { - return new PartitionedMatrixView<2, 4, 3>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 4)) { - return new PartitionedMatrixView<2, 4, 4>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 8)) { - return new PartitionedMatrixView<2, 4, 8>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 9)) { - return new PartitionedMatrixView<2, 4, 9>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == Eigen::Dynamic)) { - return new PartitionedMatrixView<2, 4, Eigen::Dynamic>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 2) && - (options.e_block_size == Eigen::Dynamic) && - (options.f_block_size == Eigen::Dynamic)) { - return new PartitionedMatrixView<2, Eigen::Dynamic, Eigen::Dynamic>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == 2)) { - return new PartitionedMatrixView<4, 4, 2>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == 3)) { - return new PartitionedMatrixView<4, 4, 3>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == 4)) { - return new PartitionedMatrixView<4, 4, 4>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == Eigen::Dynamic)) { - return new PartitionedMatrixView<4, 4, Eigen::Dynamic>( - matrix, options.elimination_groups[0]); - } - if ((options.row_block_size == Eigen::Dynamic) && - (options.e_block_size == Eigen::Dynamic) && - (options.f_block_size == Eigen::Dynamic)) { - return new PartitionedMatrixView<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>( - matrix, options.elimination_groups[0]); - } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + return new PartitionedMatrixView<2, 2, 2>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<2, 2, 3>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<2, 2, 4>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2)) { + return new PartitionedMatrixView<2, 2, Eigen::Dynamic>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<2, 3, 3>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<2, 3, 4>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 6)) { + return new PartitionedMatrixView<2, 3, 6>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + return new PartitionedMatrixView<2, 3, 9>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3)) { + return new PartitionedMatrixView<2, 3, Eigen::Dynamic>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<2, 4, 3>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<2, 4, 4>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 6)) { + return new PartitionedMatrixView<2, 4, 6>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 8)) { + return new PartitionedMatrixView<2, 4, 8>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 9)) { + return new PartitionedMatrixView<2, 4, 9>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4)) { + return new PartitionedMatrixView<2, 4, Eigen::Dynamic>(matrix, options.elimination_groups[0]); + } + if (options.row_block_size == 2){ + return new PartitionedMatrixView<2, Eigen::Dynamic, Eigen::Dynamic>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 3) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<3, 3, 3>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + return new PartitionedMatrixView<4, 4, 2>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new PartitionedMatrixView<4, 4, 3>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new PartitionedMatrixView<4, 4, 4>(matrix, options.elimination_groups[0]); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4)) { + return new PartitionedMatrixView<4, 4, Eigen::Dynamic>(matrix, options.elimination_groups[0]); + } #endif VLOG(1) << "Template specializations not found for <" diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view.h b/extern/ceres/internal/ceres/partitioned_matrix_view.h index 6e75060a47e..3853ea10dd6 100644 --- a/extern/ceres/internal/ceres/partitioned_matrix_view.h +++ b/extern/ceres/internal/ceres/partitioned_matrix_view.h @@ -119,20 +119,20 @@ class PartitionedMatrixView : public PartitionedMatrixViewBase { PartitionedMatrixView(const BlockSparseMatrix& matrix, int num_col_blocks_e); virtual ~PartitionedMatrixView(); - virtual void LeftMultiplyE(const double* x, double* y) const; - virtual void LeftMultiplyF(const double* x, double* y) const; - virtual void RightMultiplyE(const double* x, double* y) const; - virtual void RightMultiplyF(const double* x, double* y) const; - virtual BlockSparseMatrix* CreateBlockDiagonalEtE() const; - virtual BlockSparseMatrix* CreateBlockDiagonalFtF() const; - virtual void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const; - virtual void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const; - virtual int num_col_blocks_e() const { return num_col_blocks_e_; } - virtual int num_col_blocks_f() const { return num_col_blocks_f_; } - virtual int num_cols_e() const { return num_cols_e_; } - virtual int num_cols_f() const { return num_cols_f_; } - virtual int num_rows() const { return matrix_.num_rows(); } - virtual int num_cols() const { return matrix_.num_cols(); } + void LeftMultiplyE(const double* x, double* y) const final; + void LeftMultiplyF(const double* x, double* y) const final; + void RightMultiplyE(const double* x, double* y) const final; + void RightMultiplyF(const double* x, double* y) const final; + BlockSparseMatrix* CreateBlockDiagonalEtE() const final; + BlockSparseMatrix* CreateBlockDiagonalFtF() const final; + void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const final; + void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const final; + int num_col_blocks_e() const final { return num_col_blocks_e_; } + int num_col_blocks_f() const final { return num_col_blocks_f_; } + int num_cols_e() const final { return num_cols_e_; } + int num_cols_f() const final { return num_cols_f_; } + int num_rows() const final { return matrix_.num_rows(); } + int num_cols() const final { return matrix_.num_cols(); } private: BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, diff --git a/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h b/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h index 86fb278fa27..f3f548c7a80 100644 --- a/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h +++ b/extern/ceres/internal/ceres/partitioned_matrix_view_impl.h @@ -50,7 +50,7 @@ PartitionedMatrixView( : matrix_(matrix), num_col_blocks_e_(num_col_blocks_e) { const CompressedRowBlockStructure* bs = matrix_.block_structure(); - CHECK_NOTNULL(bs); + CHECK(bs != nullptr); num_col_blocks_f_ = bs->cols.size() - num_col_blocks_e_; diff --git a/extern/ceres/internal/ceres/generate_partitioned_matrix_view_specializations.py b/extern/ceres/internal/ceres/partitioned_matrix_view_template.py index c4ac3cf2332..7894523cdea 100644 --- a/extern/ceres/internal/ceres/generate_partitioned_matrix_view_specializations.py +++ b/extern/ceres/internal/ceres/partitioned_matrix_view_template.py @@ -46,29 +46,8 @@ # The list of tuples, specializations indicates the set of # specializations that is generated. -# Set of template specializations to generate -SPECIALIZATIONS = [(2, 2, 2), - (2, 2, 3), - (2, 2, 4), - (2, 2, "Eigen::Dynamic"), - (2, 3, 3), - (2, 3, 4), - (2, 3, 6), - (2, 3, 9), - (2, 3, "Eigen::Dynamic"), - (2, 4, 3), - (2, 4, 4), - (2, 4, 8), - (2, 4, 9), - (2, 4, "Eigen::Dynamic"), - (2, "Eigen::Dynamic", "Eigen::Dynamic"), - (4, 4, 2), - (4, 4, 3), - (4, 4, 4), - (4, 4, "Eigen::Dynamic"), - ("Eigen::Dynamic", "Eigen::Dynamic", "Eigen::Dynamic")] HEADER = """// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -106,8 +85,7 @@ HEADER = """// Ceres Solver - A fast non-linear least squares minimizer // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_partitioned_matrix_view_specializations.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. """ DYNAMIC_FILE = """ @@ -157,14 +135,7 @@ PartitionedMatrixViewBase::Create(const LinearSolver::Options& options, const BlockSparseMatrix& matrix) { #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION """ - -FACTORY_CONDITIONAL = """ if ((options.row_block_size == %s) && - (options.e_block_size == %s) && - (options.f_block_size == %s)) { - return new PartitionedMatrixView<%s, %s, %s>( - matrix, options.elimination_groups[0]); - } -""" +FACTORY = """ return new PartitionedMatrixView<%s, %s, %s>(matrix, options.elimination_groups[0]);""" FACTORY_FOOTER = """ #endif @@ -179,54 +150,3 @@ FACTORY_FOOTER = """ } // namespace internal } // namespace ceres """ - - -def SuffixForSize(size): - if size == "Eigen::Dynamic": - return "d" - return str(size) - - -def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size): - return "_".join([prefix] + map(SuffixForSize, (row_block_size, - e_block_size, - f_block_size))) - - -def Specialize(): - """ - Generate specialization code and the conditionals to instantiate it. - """ - f = open("partitioned_matrix_view.cc", "w") - f.write(HEADER) - f.write(FACTORY_FILE_HEADER) - - for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: - output = SpecializationFilename("generated/partitioned_matrix_view", - row_block_size, - e_block_size, - f_block_size) + ".cc" - fptr = open(output, "w") - fptr.write(HEADER) - - template = SPECIALIZATION_FILE - if (row_block_size == "Eigen::Dynamic" and - e_block_size == "Eigen::Dynamic" and - f_block_size == "Eigen::Dynamic"): - template = DYNAMIC_FILE - - fptr.write(template % (row_block_size, e_block_size, f_block_size)) - fptr.close() - - f.write(FACTORY_CONDITIONAL % (row_block_size, - e_block_size, - f_block_size, - row_block_size, - e_block_size, - f_block_size)) - f.write(FACTORY_FOOTER) - f.close() - - -if __name__ == "__main__": - Specialize() diff --git a/extern/ceres/internal/ceres/polynomial.cc b/extern/ceres/internal/ceres/polynomial.cc index 13bf8edeee6..20812f4de81 100644 --- a/extern/ceres/internal/ceres/polynomial.cc +++ b/extern/ceres/internal/ceres/polynomial.cc @@ -36,14 +36,13 @@ #include <vector> #include "Eigen/Dense" +#include "ceres/function_sample.h" #include "ceres/internal/port.h" -#include "ceres/stringprintf.h" #include "glog/logging.h" namespace ceres { namespace internal { -using std::string; using std::vector; namespace { @@ -53,7 +52,7 @@ namespace { // In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304, // Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404 void BalanceCompanionMatrix(Matrix* companion_matrix_ptr) { - CHECK_NOTNULL(companion_matrix_ptr); + CHECK(companion_matrix_ptr != nullptr); Matrix& companion_matrix = *companion_matrix_ptr; Matrix companion_matrix_offdiagonal = companion_matrix; companion_matrix_offdiagonal.diagonal().setZero(); @@ -105,7 +104,7 @@ void BalanceCompanionMatrix(Matrix* companion_matrix_ptr) { void BuildCompanionMatrix(const Vector& polynomial, Matrix* companion_matrix_ptr) { - CHECK_NOTNULL(companion_matrix_ptr); + CHECK(companion_matrix_ptr != nullptr); Matrix& companion_matrix = *companion_matrix_ptr; const int degree = polynomial.size() - 1; @@ -327,12 +326,6 @@ void MinimizePolynomial(const Vector& polynomial, } } -string FunctionSample::ToDebugString() const { - return StringPrintf("[x: %.8e, value: %.8e, gradient: %.8e, " - "value_is_valid: %d, gradient_is_valid: %d]", - x, value, gradient, value_is_valid, gradient_is_valid); -} - Vector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) { const int num_samples = samples.size(); int num_constraints = 0; @@ -370,7 +363,10 @@ Vector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) { } } - return lhs.fullPivLu().solve(rhs); + // TODO(sameeragarwal): This is a hack. + // https://github.com/ceres-solver/ceres-solver/issues/248 + Eigen::FullPivLU<Matrix> lu(lhs); + return lu.setThreshold(0.0).solve(rhs); } void MinimizeInterpolatingPolynomial(const vector<FunctionSample>& samples, diff --git a/extern/ceres/internal/ceres/polynomial.h b/extern/ceres/internal/ceres/polynomial.h index 09a64c577f5..3e09bae3d0f 100644 --- a/extern/ceres/internal/ceres/polynomial.h +++ b/extern/ceres/internal/ceres/polynomial.h @@ -32,7 +32,6 @@ #ifndef CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ #define CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ -#include <string> #include <vector> #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" @@ -40,6 +39,8 @@ namespace ceres { namespace internal { +struct FunctionSample; + // All polynomials are assumed to be the form // // sum_{i=0}^N polynomial(i) x^{N-i}. @@ -84,27 +85,6 @@ void MinimizePolynomial(const Vector& polynomial, double* optimal_x, double* optimal_value); -// Structure for storing sample values of a function. -// -// Clients can use this struct to communicate the value of the -// function and or its gradient at a given point x. -struct FunctionSample { - FunctionSample() - : x(0.0), - value(0.0), - value_is_valid(false), - gradient(0.0), - gradient_is_valid(false) { - } - std::string ToDebugString() const; - - double x; - double value; // value = f(x) - bool value_is_valid; - double gradient; // gradient = f'(x) - bool gradient_is_valid; -}; - // Given a set of function value and/or gradient samples, find a // polynomial whose value and gradients are exactly equal to the ones // in samples. diff --git a/extern/ceres/internal/ceres/preconditioner.cc b/extern/ceres/internal/ceres/preconditioner.cc index 82621dae50c..f98374e0cf8 100644 --- a/extern/ceres/internal/ceres/preconditioner.cc +++ b/extern/ceres/internal/ceres/preconditioner.cc @@ -49,7 +49,8 @@ PreconditionerType Preconditioner::PreconditionerForZeroEBlocks( SparseMatrixPreconditionerWrapper::SparseMatrixPreconditionerWrapper( const SparseMatrix* matrix) - : matrix_(CHECK_NOTNULL(matrix)) { + : matrix_(matrix) { + CHECK(matrix != nullptr); } SparseMatrixPreconditionerWrapper::~SparseMatrixPreconditionerWrapper() { diff --git a/extern/ceres/internal/ceres/preconditioner.h b/extern/ceres/internal/ceres/preconditioner.h index a248eae060d..3e46ed83db2 100644 --- a/extern/ceres/internal/ceres/preconditioner.h +++ b/extern/ceres/internal/ceres/preconditioner.h @@ -34,6 +34,7 @@ #include <vector> #include "ceres/casts.h" #include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/context_impl.h" #include "ceres/linear_operator.h" #include "ceres/sparse_matrix.h" #include "ceres/types.h" @@ -47,22 +48,27 @@ class SparseMatrix; class Preconditioner : public LinearOperator { public: struct Options { - Options() - : type(JACOBI), - visibility_clustering_type(CANONICAL_VIEWS), - sparse_linear_algebra_library_type(SUITE_SPARSE), - num_threads(1), - row_block_size(Eigen::Dynamic), - e_block_size(Eigen::Dynamic), - f_block_size(Eigen::Dynamic) { - } - - PreconditionerType type; - VisibilityClusteringType visibility_clustering_type; - SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; + PreconditionerType type = JACOBI; + VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS; + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = SUITE_SPARSE; + + // When using the subset preconditioner, all row blocks starting + // from this row block are used to construct the preconditioner. + // + // i.e., the Jacobian matrix A is horizontally partitioned as + // + // A = [P] + // [Q] + // + // where P has subset_preconditioner_start_row_block row blocks, + // and the preconditioner is the inverse of the matrix Q'Q. + int subset_preconditioner_start_row_block = -1; + + // See solver.h for information about these flags. + bool use_postordering = false; // If possible, how many threads the preconditioner can use. - int num_threads; + int num_threads = 1; // Hints about the order in which the parameter blocks should be // eliminated by the linear solver. @@ -91,9 +97,11 @@ class Preconditioner : public LinearOperator { // // Please see schur_complement_solver.h and schur_eliminator.h for // more details. - int row_block_size; - int e_block_size; - int f_block_size; + int row_block_size = Eigen::Dynamic; + int e_block_size = Eigen::Dynamic; + int f_block_size = Eigen::Dynamic; + + ContextImpl* context = nullptr; }; // If the optimization problem is such that there are no remaining @@ -123,13 +131,13 @@ class Preconditioner : public LinearOperator { // LeftMultiply and num_cols are just calls to RightMultiply and // num_rows respectively. Update() must be called before // RightMultiply can be called. - virtual void RightMultiply(const double* x, double* y) const = 0; - virtual void LeftMultiply(const double* x, double* y) const { + void RightMultiply(const double* x, double* y) const override = 0; + void LeftMultiply(const double* x, double* y) const override { return RightMultiply(x, y); } - virtual int num_rows() const = 0; - virtual int num_cols() const { + int num_rows() const override = 0; + int num_cols() const override { return num_rows(); } }; @@ -141,7 +149,7 @@ template <typename MatrixType> class TypedPreconditioner : public Preconditioner { public: virtual ~TypedPreconditioner() {} - virtual bool Update(const LinearOperator& A, const double* D) { + bool Update(const LinearOperator& A, const double* D) final { return UpdateImpl(*down_cast<const MatrixType*>(&A), D); } @@ -149,7 +157,7 @@ class TypedPreconditioner : public Preconditioner { virtual bool UpdateImpl(const MatrixType& A, const double* D) = 0; }; -// Preconditioners that depend on acccess to the low level structure +// Preconditioners that depend on access to the low level structure // of a SparseMatrix. typedef TypedPreconditioner<SparseMatrix> SparseMatrixPreconditioner; // NOLINT typedef TypedPreconditioner<BlockSparseMatrix> BlockSparseMatrixPreconditioner; // NOLINT diff --git a/extern/ceres/internal/ceres/preprocessor.cc b/extern/ceres/internal/ceres/preprocessor.cc index 4aba6a39ce8..02219147d75 100644 --- a/extern/ceres/internal/ceres/preprocessor.cc +++ b/extern/ceres/internal/ceres/preprocessor.cc @@ -31,6 +31,7 @@ #include "ceres/callbacks.h" #include "ceres/gradient_checking_cost_function.h" #include "ceres/line_search_preprocessor.h" +#include "ceres/parallel_for.h" #include "ceres/preprocessor.h" #include "ceres/problem_impl.h" #include "ceres/solver.h" @@ -56,25 +57,15 @@ Preprocessor::~Preprocessor() { } void ChangeNumThreadsIfNeeded(Solver::Options* options) { -#ifndef CERES_USE_OPENMP - if (options->num_threads > 1) { + const int num_threads_available = MaxNumThreadsAvailable(); + if (options->num_threads > num_threads_available) { LOG(WARNING) - << "OpenMP support is not compiled into this binary; " - << "only options.num_threads = 1 is supported. Switching " - << "to single threaded mode."; - options->num_threads = 1; + << "Specified options.num_threads: " << options->num_threads + << " exceeds maximum available from the threading model Ceres " + << "was compiled with: " << num_threads_available + << ". Bounding to maximum number available."; + options->num_threads = num_threads_available; } - - // Only the Trust Region solver currently uses a linear solver. - if (options->minimizer_type == TRUST_REGION && - options->num_linear_solver_threads > 1) { - LOG(WARNING) - << "OpenMP support is not compiled into this binary; " - << "only options.num_linear_solver_threads=1 is supported. Switching " - << "to single threaded mode."; - options->num_linear_solver_threads = 1; - } -#endif // CERES_USE_OPENMP } void SetupCommonMinimizerOptions(PreprocessedProblem* pp) { diff --git a/extern/ceres/internal/ceres/preprocessor.h b/extern/ceres/internal/ceres/preprocessor.h index ff53d6f0d3f..99bd6c0c5dd 100644 --- a/extern/ceres/internal/ceres/preprocessor.h +++ b/extern/ceres/internal/ceres/preprocessor.h @@ -31,6 +31,7 @@ #ifndef CERES_INTERNAL_PREPROCESSOR_H_ #define CERES_INTERNAL_PREPROCESSOR_H_ +#include <memory> #include <string> #include <vector> @@ -38,7 +39,6 @@ #include "ceres/evaluator.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/iteration_callback.h" #include "ceres/linear_solver.h" #include "ceres/minimizer.h" @@ -91,14 +91,14 @@ struct PreprocessedProblem { Minimizer::Options minimizer_options; ProblemImpl* problem; - scoped_ptr<ProblemImpl> gradient_checking_problem; - scoped_ptr<Program> reduced_program; - scoped_ptr<LinearSolver> linear_solver; - scoped_ptr<IterationCallback> logging_callback; - scoped_ptr<IterationCallback> state_updating_callback; + std::unique_ptr<ProblemImpl> gradient_checking_problem; + std::unique_ptr<Program> reduced_program; + std::unique_ptr<LinearSolver> linear_solver; + std::unique_ptr<IterationCallback> logging_callback; + std::unique_ptr<IterationCallback> state_updating_callback; - shared_ptr<Evaluator> evaluator; - shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; + std::shared_ptr<Evaluator> evaluator; + std::shared_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; std::vector<double*> removed_parameter_blocks; Vector reduced_parameters; @@ -107,8 +107,9 @@ struct PreprocessedProblem { // Common functions used by various preprocessors. -// If OpenMP support is not available and user has requested more than -// one thread, then set the *_num_threads options as needed to 1. +// If the user has specified a num_threads > the maximum number of threads +// available from the compiled threading model, bound the number of threads +// to the maximum. void ChangeNumThreadsIfNeeded(Solver::Options* options); // Extract the effective parameter vector from the preprocessed diff --git a/extern/ceres/internal/ceres/problem.cc b/extern/ceres/internal/ceres/problem.cc index 730ce642036..767fe977296 100644 --- a/extern/ceres/internal/ceres/problem.cc +++ b/extern/ceres/internal/ceres/problem.cc @@ -32,6 +32,7 @@ #include "ceres/problem.h" #include <vector> + #include "ceres/crs_matrix.h" #include "ceres/problem_impl.h" @@ -39,166 +40,90 @@ namespace ceres { using std::vector; -Problem::Problem() : problem_impl_(new internal::ProblemImpl) {} +Problem::Problem() : impl_(new internal::ProblemImpl) {} Problem::Problem(const Problem::Options& options) - : problem_impl_(new internal::ProblemImpl(options)) {} + : impl_(new internal::ProblemImpl(options)) {} +// Not inline defaulted in declaration due to use of std::unique_ptr. +Problem::Problem(Problem&&) = default; +Problem& Problem::operator=(Problem&&) = default; Problem::~Problem() {} ResidualBlockId Problem::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, const vector<double*>& parameter_blocks) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - parameter_blocks); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1, x2); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1, x2, x3); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1, x2, x3, x4); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1, x2, x3, x4, x5); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1, x2, x3, x4, x5, x6); + return impl_->AddResidualBlock(cost_function, + loss_function, + parameter_blocks.data(), + static_cast<int>(parameter_blocks.size())); } -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6, double* x7) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1, x2, x3, x4, x5, x6, x7); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8) { - return problem_impl_->AddResidualBlock(cost_function, - loss_function, - x0, x1, x2, x3, x4, x5, x6, x7, x8); -} - -ResidualBlockId Problem::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8, double* x9) { - return problem_impl_->AddResidualBlock( - cost_function, - loss_function, - x0, x1, x2, x3, x4, x5, x6, x7, x8, x9); +ResidualBlockId Problem::AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* const* const parameter_blocks, + int num_parameter_blocks) { + return impl_->AddResidualBlock( + cost_function, loss_function, parameter_blocks, num_parameter_blocks); } void Problem::AddParameterBlock(double* values, int size) { - problem_impl_->AddParameterBlock(values, size); + impl_->AddParameterBlock(values, size); } void Problem::AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization) { - problem_impl_->AddParameterBlock(values, size, local_parameterization); + impl_->AddParameterBlock(values, size, local_parameterization); } void Problem::RemoveResidualBlock(ResidualBlockId residual_block) { - problem_impl_->RemoveResidualBlock(residual_block); + impl_->RemoveResidualBlock(residual_block); } -void Problem::RemoveParameterBlock(double* values) { - problem_impl_->RemoveParameterBlock(values); +void Problem::RemoveParameterBlock(const double* values) { + impl_->RemoveParameterBlock(values); } -void Problem::SetParameterBlockConstant(double* values) { - problem_impl_->SetParameterBlockConstant(values); +void Problem::SetParameterBlockConstant(const double* values) { + impl_->SetParameterBlockConstant(values); } void Problem::SetParameterBlockVariable(double* values) { - problem_impl_->SetParameterBlockVariable(values); + impl_->SetParameterBlockVariable(values); } -bool Problem::IsParameterBlockConstant(double* values) const { - return problem_impl_->IsParameterBlockConstant(values); +bool Problem::IsParameterBlockConstant(const double* values) const { + return impl_->IsParameterBlockConstant(values); } void Problem::SetParameterization( - double* values, - LocalParameterization* local_parameterization) { - problem_impl_->SetParameterization(values, local_parameterization); + double* values, LocalParameterization* local_parameterization) { + impl_->SetParameterization(values, local_parameterization); } const LocalParameterization* Problem::GetParameterization( - double* values) const { - return problem_impl_->GetParameterization(values); + const double* values) const { + return impl_->GetParameterization(values); } void Problem::SetParameterLowerBound(double* values, int index, double lower_bound) { - problem_impl_->SetParameterLowerBound(values, index, lower_bound); + impl_->SetParameterLowerBound(values, index, lower_bound); } void Problem::SetParameterUpperBound(double* values, int index, double upper_bound) { - problem_impl_->SetParameterUpperBound(values, index, upper_bound); + impl_->SetParameterUpperBound(values, index, upper_bound); +} + +double Problem::GetParameterUpperBound(const double* values, int index) const { + return impl_->GetParameterUpperBound(values, index); +} + +double Problem::GetParameterLowerBound(const double* values, int index) const { + return impl_->GetParameterLowerBound(values, index); } bool Problem::Evaluate(const EvaluateOptions& evaluate_options, @@ -206,72 +131,66 @@ bool Problem::Evaluate(const EvaluateOptions& evaluate_options, vector<double>* residuals, vector<double>* gradient, CRSMatrix* jacobian) { - return problem_impl_->Evaluate(evaluate_options, - cost, - residuals, - gradient, - jacobian); + return impl_->Evaluate(evaluate_options, cost, residuals, gradient, jacobian); } -int Problem::NumParameterBlocks() const { - return problem_impl_->NumParameterBlocks(); +bool Problem::EvaluateResidualBlock(ResidualBlockId residual_block_id, + bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians) const { + return impl_->EvaluateResidualBlock( + residual_block_id, apply_loss_function, cost, residuals, jacobians); } -int Problem::NumParameters() const { - return problem_impl_->NumParameters(); -} +int Problem::NumParameterBlocks() const { return impl_->NumParameterBlocks(); } -int Problem::NumResidualBlocks() const { - return problem_impl_->NumResidualBlocks(); -} +int Problem::NumParameters() const { return impl_->NumParameters(); } -int Problem::NumResiduals() const { - return problem_impl_->NumResiduals(); -} +int Problem::NumResidualBlocks() const { return impl_->NumResidualBlocks(); } + +int Problem::NumResiduals() const { return impl_->NumResiduals(); } int Problem::ParameterBlockSize(const double* parameter_block) const { - return problem_impl_->ParameterBlockSize(parameter_block); + return impl_->ParameterBlockSize(parameter_block); } int Problem::ParameterBlockLocalSize(const double* parameter_block) const { - return problem_impl_->ParameterBlockLocalSize(parameter_block); + return impl_->ParameterBlockLocalSize(parameter_block); } bool Problem::HasParameterBlock(const double* values) const { - return problem_impl_->HasParameterBlock(values); + return impl_->HasParameterBlock(values); } void Problem::GetParameterBlocks(vector<double*>* parameter_blocks) const { - problem_impl_->GetParameterBlocks(parameter_blocks); + impl_->GetParameterBlocks(parameter_blocks); } void Problem::GetResidualBlocks( vector<ResidualBlockId>* residual_blocks) const { - problem_impl_->GetResidualBlocks(residual_blocks); + impl_->GetResidualBlocks(residual_blocks); } void Problem::GetParameterBlocksForResidualBlock( const ResidualBlockId residual_block, vector<double*>* parameter_blocks) const { - problem_impl_->GetParameterBlocksForResidualBlock(residual_block, - parameter_blocks); + impl_->GetParameterBlocksForResidualBlock(residual_block, parameter_blocks); } const CostFunction* Problem::GetCostFunctionForResidualBlock( const ResidualBlockId residual_block) const { - return problem_impl_->GetCostFunctionForResidualBlock(residual_block); + return impl_->GetCostFunctionForResidualBlock(residual_block); } const LossFunction* Problem::GetLossFunctionForResidualBlock( const ResidualBlockId residual_block) const { - return problem_impl_->GetLossFunctionForResidualBlock(residual_block); + return impl_->GetLossFunctionForResidualBlock(residual_block); } void Problem::GetResidualBlocksForParameterBlock( - const double* values, - vector<ResidualBlockId>* residual_blocks) const { - problem_impl_->GetResidualBlocksForParameterBlock(values, - residual_blocks); + const double* values, vector<ResidualBlockId>* residual_blocks) const { + impl_->GetResidualBlocksForParameterBlock(values, residual_blocks); } } // namespace ceres diff --git a/extern/ceres/internal/ceres/problem_impl.cc b/extern/ceres/internal/ceres/problem_impl.cc index 4abea8b33ee..6cc4d336c6a 100644 --- a/extern/ceres/internal/ceres/problem_impl.cc +++ b/extern/ceres/internal/ceres/problem_impl.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,21 +33,31 @@ #include <algorithm> #include <cstddef> +#include <cstdint> #include <iterator> +#include <memory> #include <set> #include <string> #include <utility> #include <vector> + #include "ceres/casts.h" +#include "ceres/compressed_row_jacobian_writer.h" #include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/context_impl.h" #include "ceres/cost_function.h" #include "ceres/crs_matrix.h" +#include "ceres/evaluation_callback.h" #include "ceres/evaluator.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/port.h" #include "ceres/loss_function.h" #include "ceres/map_util.h" #include "ceres/parameter_block.h" #include "ceres/program.h" +#include "ceres/program_evaluator.h" #include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" #include "ceres/stl_util.h" #include "ceres/stringprintf.h" #include "glog/logging.h" @@ -58,36 +68,66 @@ namespace internal { using std::map; using std::string; using std::vector; -typedef std::map<double*, internal::ParameterBlock*> ParameterMap; namespace { // Returns true if two regions of memory, a and b, with sizes size_a and size_b // respectively, overlap. -bool RegionsAlias(const double* a, int size_a, - const double* b, int size_b) { - return (a < b) ? b < (a + size_a) - : a < (b + size_b); +bool RegionsAlias(const double* a, int size_a, const double* b, int size_b) { + return (a < b) ? b < (a + size_a) : a < (b + size_b); } void CheckForNoAliasing(double* existing_block, int existing_block_size, double* new_block, int new_block_size) { - CHECK(!RegionsAlias(existing_block, existing_block_size, - new_block, new_block_size)) + CHECK(!RegionsAlias( + existing_block, existing_block_size, new_block, new_block_size)) << "Aliasing detected between existing parameter block at memory " - << "location " << existing_block - << " and has size " << existing_block_size << " with new parameter " + << "location " << existing_block << " and has size " + << existing_block_size << " with new parameter " << "block that has memory address " << new_block << " and would have " << "size " << new_block_size << "."; } +template <typename KeyType> +void DecrementValueOrDeleteKey(const KeyType key, + std::map<KeyType, int>* container) { + auto it = container->find(key); + if (it->second == 1) { + delete key; + container->erase(it); + } else { + --it->second; + } +} + +template <typename ForwardIterator> +void STLDeleteContainerPairFirstPointers(ForwardIterator begin, + ForwardIterator end) { + while (begin != end) { + delete begin->first; + ++begin; + } +} + +void InitializeContext(Context* context, + ContextImpl** context_impl, + bool* context_impl_owned) { + if (context == nullptr) { + *context_impl_owned = true; + *context_impl = new ContextImpl; + } else { + *context_impl_owned = false; + *context_impl = down_cast<ContextImpl*>(context); + } +} + } // namespace ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, int size) { - CHECK(values != NULL) << "Null pointer passed to AddParameterBlock " - << "for a parameter with size " << size; + CHECK(values != nullptr) << "Null pointer passed to AddParameterBlock " + << "for a parameter with size " << size; // Ignore the request if there is a block for the given pointer already. ParameterMap::iterator it = parameter_block_map_.find(values); @@ -97,8 +137,7 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, CHECK(size == existing_size) << "Tried adding a parameter block with the same double pointer, " << values << ", twice, but with different block sizes. Original " - << "size was " << existing_size << " but new size is " - << size; + << "size was " << existing_size << " but new size is " << size; } return it->second; } @@ -113,18 +152,13 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, if (lb != parameter_block_map_.begin()) { ParameterMap::iterator previous = lb; --previous; - CheckForNoAliasing(previous->first, - previous->second->Size(), - values, - size); + CheckForNoAliasing( + previous->first, previous->second->Size(), values, size); } // If lb is not off the end, check lb for aliasing. if (lb != parameter_block_map_.end()) { - CheckForNoAliasing(lb->first, - lb->second->Size(), - values, - size); + CheckForNoAliasing(lb->first, lb->second->Size(), values, size); } } } @@ -145,7 +179,7 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, } void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) { - CHECK_NOTNULL(residual_block); + CHECK(residual_block != nullptr); // Perform no check on the validity of residual_block, that is handled in // the public method: RemoveResidualBlock(). @@ -154,8 +188,8 @@ void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) { const int num_parameter_blocks_for_residual = residual_block->NumParameterBlocks(); for (int i = 0; i < num_parameter_blocks_for_residual; ++i) { - residual_block->parameter_blocks()[i] - ->RemoveResidualBlock(residual_block); + residual_block->parameter_blocks()[i]->RemoveResidualBlock( + residual_block); } ResidualBlockSet::iterator it = residual_block_set_.find(residual_block); @@ -173,16 +207,19 @@ void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { // The const casts here are legit, since ResidualBlock holds these // pointers as const pointers but we have ownership of them and // have the right to destroy them when the destructor is called. - if (options_.cost_function_ownership == TAKE_OWNERSHIP && - residual_block->cost_function() != NULL) { - cost_functions_to_delete_.push_back( - const_cast<CostFunction*>(residual_block->cost_function())); + CostFunction* cost_function = + const_cast<CostFunction*>(residual_block->cost_function()); + if (options_.cost_function_ownership == TAKE_OWNERSHIP) { + DecrementValueOrDeleteKey(cost_function, &cost_function_ref_count_); } + + LossFunction* loss_function = + const_cast<LossFunction*>(residual_block->loss_function()); if (options_.loss_function_ownership == TAKE_OWNERSHIP && - residual_block->loss_function() != NULL) { - loss_functions_to_delete_.push_back( - const_cast<LossFunction*>(residual_block->loss_function())); + loss_function != nullptr) { + DecrementValueOrDeleteKey(loss_function, &loss_function_ref_count_); } + delete residual_block; } @@ -193,7 +230,7 @@ void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { // without doing a full scan. void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { if (options_.local_parameterization_ownership == TAKE_OWNERSHIP && - parameter_block->local_parameterization() != NULL) { + parameter_block->local_parameterization() != nullptr) { local_parameterizations_to_delete_.push_back( parameter_block->mutable_local_parameterization()); } @@ -201,18 +238,29 @@ void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { delete parameter_block; } -ProblemImpl::ProblemImpl() : program_(new internal::Program) {} +ProblemImpl::ProblemImpl() + : options_(Problem::Options()), program_(new internal::Program) { + InitializeContext(options_.context, &context_impl_, &context_impl_owned_); +} + ProblemImpl::ProblemImpl(const Problem::Options& options) - : options_(options), - program_(new internal::Program) {} + : options_(options), program_(new internal::Program) { + program_->evaluation_callback_ = options.evaluation_callback; + InitializeContext(options_.context, &context_impl_, &context_impl_owned_); +} ProblemImpl::~ProblemImpl() { - // Collect the unique cost/loss functions and delete the residuals. - const int num_residual_blocks = program_->residual_blocks_.size(); - cost_functions_to_delete_.reserve(num_residual_blocks); - loss_functions_to_delete_.reserve(num_residual_blocks); - for (int i = 0; i < program_->residual_blocks_.size(); ++i) { - DeleteBlock(program_->residual_blocks_[i]); + STLDeleteContainerPointers(program_->residual_blocks_.begin(), + program_->residual_blocks_.end()); + + if (options_.cost_function_ownership == TAKE_OWNERSHIP) { + STLDeleteContainerPairFirstPointers(cost_function_ref_count_.begin(), + cost_function_ref_count_.end()); + } + + if (options_.loss_function_ownership == TAKE_OWNERSHIP) { + STLDeleteContainerPairFirstPointers(loss_function_ref_count_.begin(), + loss_function_ref_count_.end()); } // Collect the unique parameterizations and delete the parameters. @@ -220,57 +268,57 @@ ProblemImpl::~ProblemImpl() { DeleteBlock(program_->parameter_blocks_[i]); } - // Delete the owned cost/loss functions and parameterizations. + // Delete the owned parameterizations. STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), local_parameterizations_to_delete_.end()); - STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(), - cost_functions_to_delete_.end()); - STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(), - loss_functions_to_delete_.end()); + + if (context_impl_owned_) { + delete context_impl_; + } } -ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlockId ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, - const vector<double*>& parameter_blocks) { - CHECK_NOTNULL(cost_function); - CHECK_EQ(parameter_blocks.size(), - cost_function->parameter_block_sizes().size()); + double* const* const parameter_blocks, + int num_parameter_blocks) { + CHECK(cost_function != nullptr); + CHECK_EQ(num_parameter_blocks, cost_function->parameter_block_sizes().size()); // Check the sizes match. - const vector<int32>& parameter_block_sizes = + const vector<int32_t>& parameter_block_sizes = cost_function->parameter_block_sizes(); if (!options_.disable_all_safety_checks) { - CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) + CHECK_EQ(parameter_block_sizes.size(), num_parameter_blocks) << "Number of blocks input is different than the number of blocks " << "that the cost function expects."; // Check for duplicate parameter blocks. - vector<double*> sorted_parameter_blocks(parameter_blocks); + vector<double*> sorted_parameter_blocks( + parameter_blocks, parameter_blocks + num_parameter_blocks); sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); const bool has_duplicate_items = (std::adjacent_find(sorted_parameter_blocks.begin(), - sorted_parameter_blocks.end()) - != sorted_parameter_blocks.end()); + sorted_parameter_blocks.end()) != + sorted_parameter_blocks.end()); if (has_duplicate_items) { string blocks; - for (int i = 0; i < parameter_blocks.size(); ++i) { + for (int i = 0; i < num_parameter_blocks; ++i) { blocks += StringPrintf(" %p ", parameter_blocks[i]); } LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " - << "are not allowed. Parameter block pointers: [" - << blocks << "]"; + << "are not allowed. Parameter block pointers: [" << blocks + << "]"; } } // Add parameter blocks and convert the double*'s to parameter blocks. - vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); - for (int i = 0; i < parameter_blocks.size(); ++i) { - parameter_block_ptrs[i] = - InternalAddParameterBlock(parameter_blocks[i], - parameter_block_sizes[i]); + vector<ParameterBlock*> parameter_block_ptrs(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + parameter_block_ptrs[i] = InternalAddParameterBlock( + parameter_blocks[i], parameter_block_sizes[i]); } if (!options_.disable_all_safety_checks) { @@ -279,8 +327,8 @@ ResidualBlock* ProblemImpl::AddResidualBlock( for (int i = 0; i < parameter_block_ptrs.size(); ++i) { CHECK_EQ(cost_function->parameter_block_sizes()[i], parameter_block_ptrs[i]->Size()) - << "The cost function expects parameter block " << i - << " of size " << cost_function->parameter_block_sizes()[i] + << "The cost function expects parameter block " << i << " of size " + << cost_function->parameter_block_sizes()[i] << " but was given a block of size " << parameter_block_ptrs[i]->Size(); } @@ -294,7 +342,7 @@ ResidualBlock* ProblemImpl::AddResidualBlock( // Add dependencies on the residual to the parameter blocks. if (options_.enable_fast_removal) { - for (int i = 0; i < parameter_blocks.size(); ++i) { + for (int i = 0; i < num_parameter_blocks; ++i) { parameter_block_ptrs[i]->AddResidualBlock(new_residual_block); } } @@ -305,148 +353,19 @@ ResidualBlock* ProblemImpl::AddResidualBlock( residual_block_set_.insert(new_residual_block); } - return new_residual_block; -} - -// Unfortunately, macros don't help much to reduce this code, and var args don't -// work because of the ambiguous case that there is no loss function. -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} - -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} - -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} - -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - residual_parameters.push_back(x3); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} + if (options_.cost_function_ownership == TAKE_OWNERSHIP) { + // Increment the reference count, creating an entry in the table if + // needed. Note: C++ maps guarantee that new entries have default + // constructed values; this implies integers are zero initialized. + ++cost_function_ref_count_[cost_function]; + } -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - residual_parameters.push_back(x3); - residual_parameters.push_back(x4); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} + if (options_.loss_function_ownership == TAKE_OWNERSHIP && + loss_function != nullptr) { + ++loss_function_ref_count_[loss_function]; + } -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - residual_parameters.push_back(x3); - residual_parameters.push_back(x4); - residual_parameters.push_back(x5); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} - -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - residual_parameters.push_back(x3); - residual_parameters.push_back(x4); - residual_parameters.push_back(x5); - residual_parameters.push_back(x6); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} - -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6, double* x7) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - residual_parameters.push_back(x3); - residual_parameters.push_back(x4); - residual_parameters.push_back(x5); - residual_parameters.push_back(x6); - residual_parameters.push_back(x7); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} - -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - residual_parameters.push_back(x3); - residual_parameters.push_back(x4); - residual_parameters.push_back(x5); - residual_parameters.push_back(x6); - residual_parameters.push_back(x7); - residual_parameters.push_back(x8); - return AddResidualBlock(cost_function, loss_function, residual_parameters); -} - -ResidualBlock* ProblemImpl::AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8, double* x9) { - vector<double*> residual_parameters; - residual_parameters.push_back(x0); - residual_parameters.push_back(x1); - residual_parameters.push_back(x2); - residual_parameters.push_back(x3); - residual_parameters.push_back(x4); - residual_parameters.push_back(x5); - residual_parameters.push_back(x6); - residual_parameters.push_back(x7); - residual_parameters.push_back(x8); - residual_parameters.push_back(x9); - return AddResidualBlock(cost_function, loss_function, residual_parameters); + return new_residual_block; } void ProblemImpl::AddParameterBlock(double* values, int size) { @@ -454,12 +373,9 @@ void ProblemImpl::AddParameterBlock(double* values, int size) { } void ProblemImpl::AddParameterBlock( - double* values, - int size, - LocalParameterization* local_parameterization) { - ParameterBlock* parameter_block = - InternalAddParameterBlock(values, size); - if (local_parameterization != NULL) { + double* values, int size, LocalParameterization* local_parameterization) { + ParameterBlock* parameter_block = InternalAddParameterBlock(values, size); + if (local_parameterization != nullptr) { parameter_block->SetParameterization(local_parameterization); } } @@ -468,13 +384,12 @@ void ProblemImpl::AddParameterBlock( // This is done in constant time by moving an element from the end of the // vector over the element to remove, then popping the last element. It // destroys the ordering in the interest of speed. -template<typename Block> +template <typename Block> void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, Block* block_to_remove) { CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) << "You found a Ceres bug! \n" - << "Block requested: " - << block_to_remove->ToString() << "\n" + << "Block requested: " << block_to_remove->ToString() << "\n" << "Block present: " << (*mutable_blocks)[block_to_remove->index()]->ToString(); @@ -493,24 +408,23 @@ void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, } void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { - CHECK_NOTNULL(residual_block); + CHECK(residual_block != nullptr); // Verify that residual_block identifies a residual in the current problem. - const string residual_not_found_message = - StringPrintf("Residual block to remove: %p not found. This usually means " - "one of three things have happened:\n" - " 1) residual_block is uninitialised and points to a random " - "area in memory.\n" - " 2) residual_block represented a residual that was added to" - " the problem, but referred to a parameter block which has " - "since been removed, which removes all residuals which " - "depend on that parameter block, and was thus removed.\n" - " 3) residual_block referred to a residual that has already " - "been removed from the problem (by the user).", - residual_block); + const string residual_not_found_message = StringPrintf( + "Residual block to remove: %p not found. This usually means " + "one of three things have happened:\n" + " 1) residual_block is uninitialised and points to a random " + "area in memory.\n" + " 2) residual_block represented a residual that was added to" + " the problem, but referred to a parameter block which has " + "since been removed, which removes all residuals which " + "depend on that parameter block, and was thus removed.\n" + " 3) residual_block referred to a residual that has already " + "been removed from the problem (by the user).", + residual_block); if (options_.enable_fast_removal) { - CHECK(residual_block_set_.find(residual_block) != - residual_block_set_.end()) + CHECK(residual_block_set_.find(residual_block) != residual_block_set_.end()) << residual_not_found_message; } else { // Perform a full search over all current residuals. @@ -523,10 +437,10 @@ void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { InternalRemoveResidualBlock(residual_block); } -void ProblemImpl::RemoveParameterBlock(double* values) { - ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - if (parameter_block == NULL) { +void ProblemImpl::RemoveParameterBlock(const double* values) { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "it can be removed."; @@ -561,10 +475,10 @@ void ProblemImpl::RemoveParameterBlock(double* values) { DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block); } -void ProblemImpl::SetParameterBlockConstant(double* values) { - ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - if (parameter_block == NULL) { +void ProblemImpl::SetParameterBlockConstant(const double* values) { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "it can be set constant."; @@ -573,20 +487,19 @@ void ProblemImpl::SetParameterBlockConstant(double* values) { parameter_block->SetConstant(); } -bool ProblemImpl::IsParameterBlockConstant(double* values) const { - const ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - CHECK(parameter_block != NULL) - << "Parameter block not found: " << values << ". You must add the " - << "parameter block to the problem before it can be queried."; - +bool ProblemImpl::IsParameterBlockConstant(const double* values) const { + const ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + CHECK(parameter_block != nullptr) + << "Parameter block not found: " << values << ". You must add the " + << "parameter block to the problem before it can be queried."; return parameter_block->IsConstant(); } void ProblemImpl::SetParameterBlockVariable(double* values) { ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - if (parameter_block == NULL) { + FindWithDefault(parameter_block_map_, values, nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "it can be set varying."; @@ -596,24 +509,32 @@ void ProblemImpl::SetParameterBlockVariable(double* values) { } void ProblemImpl::SetParameterization( - double* values, - LocalParameterization* local_parameterization) { + double* values, LocalParameterization* local_parameterization) { ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - if (parameter_block == NULL) { + FindWithDefault(parameter_block_map_, values, nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "you can set its local parameterization."; } + // If the parameter block already has a local parameterization and + // we are to take ownership of local parameterizations, then add it + // to local_parameterizations_to_delete_ for eventual deletion. + if (parameter_block->local_parameterization_ && + options_.local_parameterization_ownership == TAKE_OWNERSHIP) { + local_parameterizations_to_delete_.push_back( + parameter_block->local_parameterization_); + } + parameter_block->SetParameterization(local_parameterization); } const LocalParameterization* ProblemImpl::GetParameterization( - double* values) const { - ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - if (parameter_block == NULL) { + const double* values) const { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "you can get its local parameterization."; @@ -626,8 +547,8 @@ void ProblemImpl::SetParameterLowerBound(double* values, int index, double lower_bound) { ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - if (parameter_block == NULL) { + FindWithDefault(parameter_block_map_, values, nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "you can set a lower bound on one of its components."; @@ -640,8 +561,8 @@ void ProblemImpl::SetParameterUpperBound(double* values, int index, double upper_bound) { ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, NULL); - if (parameter_block == NULL) { + FindWithDefault(parameter_block_map_, values, nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "you can set an upper bound on one of its components."; @@ -649,15 +570,37 @@ void ProblemImpl::SetParameterUpperBound(double* values, parameter_block->SetUpperBound(index, upper_bound); } +double ProblemImpl::GetParameterLowerBound(const double* values, + int index) const { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can get the lower bound on one of its components."; + } + return parameter_block->LowerBound(index); +} + +double ProblemImpl::GetParameterUpperBound(const double* values, + int index) const { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can set an upper bound on one of its components."; + } + return parameter_block->UpperBound(index); +} + bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, double* cost, vector<double>* residuals, vector<double>* gradient, CRSMatrix* jacobian) { - if (cost == NULL && - residuals == NULL && - gradient == NULL && - jacobian == NULL) { + if (cost == nullptr && residuals == nullptr && gradient == nullptr && + jacobian == nullptr) { LOG(INFO) << "Nothing to do."; return true; } @@ -667,7 +610,8 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, Program program; *program.mutable_residual_blocks() = ((evaluate_options.residual_blocks.size() > 0) - ? evaluate_options.residual_blocks : program_->residual_blocks()); + ? evaluate_options.residual_blocks + : program_->residual_blocks()); const vector<double*>& parameter_block_ptrs = evaluate_options.parameter_blocks; @@ -688,10 +632,9 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, // 1. Convert double* into ParameterBlock* parameter_blocks.resize(parameter_block_ptrs.size()); for (int i = 0; i < parameter_block_ptrs.size(); ++i) { - parameter_blocks[i] = FindWithDefault(parameter_block_map_, - parameter_block_ptrs[i], - NULL); - if (parameter_blocks[i] == NULL) { + parameter_blocks[i] = FindWithDefault( + parameter_block_map_, parameter_block_ptrs[i], nullptr); + if (parameter_blocks[i] == nullptr) { LOG(FATAL) << "No known parameter block for " << "Problem::Evaluate::Options.parameter_blocks[" << i << "]" << " = " << parameter_block_ptrs[i]; @@ -742,45 +685,36 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, // the Evaluator decides the storage for the Jacobian based on the // type of linear solver being used. evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; -#ifndef CERES_USE_OPENMP +#ifdef CERES_NO_THREADS LOG_IF(WARNING, evaluate_options.num_threads > 1) - << "OpenMP support is not compiled into this binary; " + << "No threading support is compiled into this binary; " << "only evaluate_options.num_threads = 1 is supported. Switching " << "to single threaded mode."; evaluator_options.num_threads = 1; #else evaluator_options.num_threads = evaluate_options.num_threads; -#endif // CERES_USE_OPENMP - - string error; - scoped_ptr<Evaluator> evaluator( - Evaluator::Create(evaluator_options, &program, &error)); - if (evaluator.get() == NULL) { - LOG(ERROR) << "Unable to create an Evaluator object. " - << "Error: " << error - << "This is a Ceres bug; please contact the developers!"; - - // Make the parameter blocks that were temporarily marked - // constant, variable again. - for (int i = 0; i < variable_parameter_blocks.size(); ++i) { - variable_parameter_blocks[i]->SetVarying(); - } - - program_->SetParameterBlockStatePtrsToUserStatePtrs(); - program_->SetParameterOffsetsAndIndex(); - return false; - } - - if (residuals !=NULL) { +#endif // CERES_NO_THREADS + + // The main thread also does work so we only need to launch num_threads - 1. + context_impl_->EnsureMinimumThreads(evaluator_options.num_threads - 1); + evaluator_options.context = context_impl_; + evaluator_options.evaluation_callback = + program_->mutable_evaluation_callback(); + std::unique_ptr<Evaluator> evaluator( + new ProgramEvaluator<ScratchEvaluatePreparer, + CompressedRowJacobianWriter>(evaluator_options, + &program)); + + if (residuals != nullptr) { residuals->resize(evaluator->NumResiduals()); } - if (gradient != NULL) { + if (gradient != nullptr) { gradient->resize(evaluator->NumEffectiveParameters()); } - scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian; - if (jacobian != NULL) { + std::unique_ptr<CompressedRowSparseMatrix> tmp_jacobian; + if (jacobian != nullptr) { tmp_jacobian.reset( down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); } @@ -804,12 +738,13 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, Evaluator::EvaluateOptions evaluator_evaluate_options; evaluator_evaluate_options.apply_loss_function = evaluate_options.apply_loss_function; - bool status = evaluator->Evaluate(evaluator_evaluate_options, - parameters.data(), - &tmp_cost, - residuals != NULL ? &(*residuals)[0] : NULL, - gradient != NULL ? &(*gradient)[0] : NULL, - tmp_jacobian.get()); + bool status = + evaluator->Evaluate(evaluator_evaluate_options, + parameters.data(), + &tmp_cost, + residuals != nullptr ? &(*residuals)[0] : nullptr, + gradient != nullptr ? &(*gradient)[0] : nullptr, + tmp_jacobian.get()); // Make the parameter blocks that were temporarily marked constant, // variable again. @@ -818,10 +753,10 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, } if (status) { - if (cost != NULL) { + if (cost != nullptr) { *cost = tmp_cost; } - if (jacobian != NULL) { + if (jacobian != nullptr) { tmp_jacobian->ToCRSMatrix(jacobian); } } @@ -831,26 +766,53 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, return status; } +bool ProblemImpl::EvaluateResidualBlock(ResidualBlock* residual_block, + bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians) const { + ParameterBlock* const* parameter_blocks = residual_block->parameter_blocks(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int i = 0; i < num_parameter_blocks; ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (parameter_block->IsConstant()) { + if (jacobians != nullptr && jacobians[i] != nullptr) { + LOG(ERROR) << "Jacobian requested for parameter block : " << i + << ". But the parameter block is marked constant."; + return false; + } + } else { + CHECK(parameter_block->SetState(parameter_block->user_state())) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + } + + double dummy_cost = 0.0; + FixedArray<double> scratch(residual_block->NumScratchDoublesForEvaluate()); + return residual_block->Evaluate(apply_loss_function, + cost ? cost : &dummy_cost, + residuals, + jacobians, + scratch.data()); +} + int ProblemImpl::NumParameterBlocks() const { return program_->NumParameterBlocks(); } -int ProblemImpl::NumParameters() const { - return program_->NumParameters(); -} +int ProblemImpl::NumParameters() const { return program_->NumParameters(); } int ProblemImpl::NumResidualBlocks() const { return program_->NumResidualBlocks(); } -int ProblemImpl::NumResiduals() const { - return program_->NumResiduals(); -} +int ProblemImpl::NumResiduals() const { return program_->NumResiduals(); } int ProblemImpl::ParameterBlockSize(const double* values) const { - ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL); - if (parameter_block == NULL) { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "you can get its size."; @@ -860,9 +822,9 @@ int ProblemImpl::ParameterBlockSize(const double* values) const { } int ProblemImpl::ParameterBlockLocalSize(const double* values) const { - ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL); - if (parameter_block == NULL) { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "you can get its local size."; @@ -877,18 +839,17 @@ bool ProblemImpl::HasParameterBlock(const double* parameter_block) const { } void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { - CHECK_NOTNULL(parameter_blocks); + CHECK(parameter_blocks != nullptr); parameter_blocks->resize(0); - for (ParameterMap::const_iterator it = parameter_block_map_.begin(); - it != parameter_block_map_.end(); - ++it) { - parameter_blocks->push_back(it->first); + parameter_blocks->reserve(parameter_block_map_.size()); + for (const auto& entry : parameter_block_map_) { + parameter_blocks->push_back(entry.first); } } void ProblemImpl::GetResidualBlocks( vector<ResidualBlockId>* residual_blocks) const { - CHECK_NOTNULL(residual_blocks); + CHECK(residual_blocks != nullptr); *residual_blocks = program().residual_blocks(); } @@ -896,7 +857,8 @@ void ProblemImpl::GetParameterBlocksForResidualBlock( const ResidualBlockId residual_block, vector<double*>* parameter_blocks) const { int num_parameter_blocks = residual_block->NumParameterBlocks(); - CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks); + CHECK(parameter_blocks != nullptr); + parameter_blocks->resize(num_parameter_blocks); for (int i = 0; i < num_parameter_blocks; ++i) { (*parameter_blocks)[i] = residual_block->parameter_blocks()[i]->mutable_user_state(); @@ -914,11 +876,10 @@ const LossFunction* ProblemImpl::GetLossFunctionForResidualBlock( } void ProblemImpl::GetResidualBlocksForParameterBlock( - const double* values, - vector<ResidualBlockId>* residual_blocks) const { - ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL); - if (parameter_block == NULL) { + const double* values, vector<ResidualBlockId>* residual_blocks) const { + ParameterBlock* parameter_block = FindWithDefault( + parameter_block_map_, const_cast<double*>(values), nullptr); + if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " << "you can get the residual blocks that depend on it."; @@ -927,8 +888,8 @@ void ProblemImpl::GetResidualBlocksForParameterBlock( if (options_.enable_fast_removal) { // In this case the residual blocks that depend on the parameter block are // stored in the parameter block already, so just copy them out. - CHECK_NOTNULL(residual_blocks)->resize( - parameter_block->mutable_residual_blocks()->size()); + CHECK(residual_blocks != nullptr); + residual_blocks->resize(parameter_block->mutable_residual_blocks()->size()); std::copy(parameter_block->mutable_residual_blocks()->begin(), parameter_block->mutable_residual_blocks()->end(), residual_blocks->begin()); @@ -936,11 +897,11 @@ void ProblemImpl::GetResidualBlocksForParameterBlock( } // Find residual blocks that depend on the parameter block. - CHECK_NOTNULL(residual_blocks)->clear(); + CHECK(residual_blocks != nullptr); + residual_blocks->clear(); const int num_residual_blocks = NumResidualBlocks(); for (int i = 0; i < num_residual_blocks; ++i) { - ResidualBlock* residual_block = - (*(program_->mutable_residual_blocks()))[i]; + ResidualBlock* residual_block = (*(program_->mutable_residual_blocks()))[i]; const int num_parameter_blocks = residual_block->NumParameterBlocks(); for (int j = 0; j < num_parameter_blocks; ++j) { if (residual_block->parameter_blocks()[j] == parameter_block) { diff --git a/extern/ceres/internal/ceres/problem_impl.h b/extern/ceres/internal/ceres/problem_impl.h index a4689c362f6..8bbe7238d27 100644 --- a/extern/ceres/internal/ceres/problem_impl.h +++ b/extern/ceres/internal/ceres/problem_impl.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -39,19 +39,21 @@ #ifndef CERES_PUBLIC_PROBLEM_IMPL_H_ #define CERES_PUBLIC_PROBLEM_IMPL_H_ +#include <array> #include <map> +#include <memory> +#include <unordered_set> #include <vector> -#include "ceres/internal/macros.h" +#include "ceres/context_impl.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" -#include "ceres/collections_port.h" #include "ceres/problem.h" #include "ceres/types.h" namespace ceres { class CostFunction; +class EvaluationCallback; class LossFunction; class LocalParameterization; struct CRSMatrix; @@ -64,78 +66,55 @@ class ResidualBlock; class ProblemImpl { public: typedef std::map<double*, ParameterBlock*> ParameterMap; - typedef HashSet<ResidualBlock*> ResidualBlockSet; + typedef std::unordered_set<ResidualBlock*> ResidualBlockSet; + typedef std::map<CostFunction*, int> CostFunctionRefCount; + typedef std::map<LossFunction*, int> LossFunctionRefCount; ProblemImpl(); explicit ProblemImpl(const Problem::Options& options); + ProblemImpl(const ProblemImpl&) = delete; + void operator=(const ProblemImpl&) = delete; ~ProblemImpl(); // See the public problem.h file for description of these methods. - ResidualBlockId AddResidualBlock( - CostFunction* cost_function, - LossFunction* loss_function, - const std::vector<double*>& parameter_blocks); ResidualBlockId AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, - double* x0); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6, double* x7); - ResidualBlockId AddResidualBlock(CostFunction* cost_function, - LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8); + double* const* const parameter_blocks, + int num_parameter_blocks); + + template <typename... Ts> ResidualBlockId AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, - double* x0, double* x1, double* x2, - double* x3, double* x4, double* x5, - double* x6, double* x7, double* x8, - double* x9); + double* x0, + Ts*... xs) { + const std::array<double*, sizeof...(Ts) + 1> parameter_blocks{{x0, xs...}}; + return AddResidualBlock(cost_function, + loss_function, + parameter_blocks.data(), + static_cast<int>(parameter_blocks.size())); + } + void AddParameterBlock(double* values, int size); void AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization); void RemoveResidualBlock(ResidualBlock* residual_block); - void RemoveParameterBlock(double* values); + void RemoveParameterBlock(const double* values); - void SetParameterBlockConstant(double* values); + void SetParameterBlockConstant(const double* values); void SetParameterBlockVariable(double* values); - bool IsParameterBlockConstant(double* values) const; + bool IsParameterBlockConstant(const double* values) const; void SetParameterization(double* values, LocalParameterization* local_parameterization); - const LocalParameterization* GetParameterization(double* values) const; + const LocalParameterization* GetParameterization(const double* values) const; void SetParameterLowerBound(double* values, int index, double lower_bound); void SetParameterUpperBound(double* values, int index, double upper_bound); + double GetParameterLowerBound(const double* values, int index) const; + double GetParameterUpperBound(const double* values, int index) const; bool Evaluate(const Problem::EvaluateOptions& options, double* cost, @@ -143,6 +122,12 @@ class ProblemImpl { std::vector<double>* gradient, CRSMatrix* jacobian); + bool EvaluateResidualBlock(ResidualBlock* residual_block, + bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians) const; + int NumParameterBlocks() const; int NumParameters() const; int NumResidualBlocks() const; @@ -179,20 +164,16 @@ class ProblemImpl { return residual_block_set_; } + ContextImpl* context() { return context_impl_; } + private: ParameterBlock* InternalAddParameterBlock(double* values, int size); void InternalRemoveResidualBlock(ResidualBlock* residual_block); - bool InternalEvaluate(Program* program, - double* cost, - std::vector<double>* residuals, - std::vector<double>* gradient, - CRSMatrix* jacobian); - // Delete the arguments in question. These differ from the Remove* functions // in that they do not clean up references to the block to delete; they // merely delete them. - template<typename Block> + template <typename Block> void DeleteBlockInVector(std::vector<Block*>* mutable_blocks, Block* block_to_remove); void DeleteBlock(ResidualBlock* residual_block); @@ -200,26 +181,32 @@ class ProblemImpl { const Problem::Options options_; + bool context_impl_owned_; + ContextImpl* context_impl_; + // The mapping from user pointers to parameter blocks. - std::map<double*, ParameterBlock*> parameter_block_map_; + ParameterMap parameter_block_map_; // Iff enable_fast_removal is enabled, contains the current residual blocks. ResidualBlockSet residual_block_set_; // The actual parameter and residual blocks. - internal::scoped_ptr<internal::Program> program_; + std::unique_ptr<internal::Program> program_; - // When removing residual and parameter blocks, cost/loss functions and - // parameterizations have ambiguous ownership. Instead of scanning the entire - // problem to see if the cost/loss/parameterization is shared with other - // residual or parameter blocks, buffer them until destruction. + // When removing parameter blocks, parameterizations have ambiguous + // ownership. Instead of scanning the entire problem to see if the + // parameterization is shared with other parameter blocks, buffer + // them until destruction. // // TODO(keir): See if it makes sense to use sets instead. - std::vector<CostFunction*> cost_functions_to_delete_; - std::vector<LossFunction*> loss_functions_to_delete_; std::vector<LocalParameterization*> local_parameterizations_to_delete_; - CERES_DISALLOW_COPY_AND_ASSIGN(ProblemImpl); + // For each cost function and loss function in the problem, a count + // of the number of residual blocks that refer to them. When the + // count goes to zero and the problem owns these objects, they are + // destroyed. + CostFunctionRefCount cost_function_ref_count_; + LossFunctionRefCount loss_function_ref_count_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/program.cc b/extern/ceres/internal/ceres/program.cc index 8e97f072113..f1ded2e5d5a 100644 --- a/extern/ceres/internal/ceres/program.cc +++ b/extern/ceres/internal/ceres/program.cc @@ -30,8 +30,11 @@ #include "ceres/program.h" +#include <algorithm> #include <map> +#include <memory> #include <vector> + #include "ceres/array_utils.h" #include "ceres/casts.h" #include "ceres/compressed_row_sparse_matrix.h" @@ -59,8 +62,8 @@ Program::Program() {} Program::Program(const Program& program) : parameter_blocks_(program.parameter_blocks_), - residual_blocks_(program.residual_blocks_) { -} + residual_blocks_(program.residual_blocks_), + evaluation_callback_(program.evaluation_callback_) {} const vector<ParameterBlock*>& Program::parameter_blocks() const { return parameter_blocks_; @@ -78,7 +81,11 @@ vector<ResidualBlock*>* Program::mutable_residual_blocks() { return &residual_blocks_; } -bool Program::StateVectorToParameterBlocks(const double *state) { +EvaluationCallback* Program::mutable_evaluation_callback() { + return evaluation_callback_; +} + +bool Program::StateVectorToParameterBlocks(const double* state) { for (int i = 0; i < parameter_blocks_.size(); ++i) { if (!parameter_blocks_[i]->IsConstant() && !parameter_blocks_[i]->SetState(state)) { @@ -89,7 +96,7 @@ bool Program::StateVectorToParameterBlocks(const double *state) { return true; } -void Program::ParameterBlocksToStateVector(double *state) const { +void Program::ParameterBlocksToStateVector(double* state) const { for (int i = 0; i < parameter_blocks_.size(); ++i) { parameter_blocks_[i]->GetState(state); state += parameter_blocks_[i]->Size(); @@ -178,7 +185,7 @@ bool Program::IsValid() const { } bool Program::ParameterBlocksAreFinite(string* message) const { - CHECK_NOTNULL(message); + CHECK(message != nullptr); for (int i = 0; i < parameter_blocks_.size(); ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; const double* array = parameter_block->user_state(); @@ -189,7 +196,9 @@ bool Program::ParameterBlocksAreFinite(string* message) const { "ParameterBlock: %p with size %d has at least one invalid value.\n" "First invalid value is at index: %d.\n" "Parameter block values: ", - array, size, invalid_index); + array, + size, + invalid_index); AppendArrayToString(size, array, message); return false; } @@ -217,7 +226,7 @@ bool Program::IsBoundsConstrained() const { } bool Program::IsFeasible(string* message) const { - CHECK_NOTNULL(message); + CHECK(message != nullptr); for (int i = 0; i < parameter_blocks_.size(); ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; const double* parameters = parameter_block->user_state(); @@ -236,7 +245,12 @@ bool Program::IsFeasible(string* message) const { "\nFirst infeasible value is at index: %d." "\nLower bound: %e, value: %e, upper bound: %e" "\nParameter block values: ", - parameters, size, j, lower_bound, parameters[j], upper_bound); + parameters, + size, + j, + lower_bound, + parameters[j], + upper_bound); AppendArrayToString(size, parameters, message); return false; } @@ -255,7 +269,11 @@ bool Program::IsFeasible(string* message) const { "\nFirst infeasible bound is at index: %d." "\nLower bound: %e, upper bound: %e" "\nParameter block values: ", - parameters, size, j, lower_bound, upper_bound); + parameters, + size, + j, + lower_bound, + upper_bound); AppendArrayToString(size, parameters, message); return false; } @@ -270,15 +288,14 @@ Program* Program::CreateReducedProgram( vector<double*>* removed_parameter_blocks, double* fixed_cost, string* error) const { - CHECK_NOTNULL(removed_parameter_blocks); - CHECK_NOTNULL(fixed_cost); - CHECK_NOTNULL(error); - - scoped_ptr<Program> reduced_program(new Program(*this)); - if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks, - fixed_cost, - error)) { - return NULL; + CHECK(removed_parameter_blocks != nullptr); + CHECK(fixed_cost != nullptr); + CHECK(error != nullptr); + + std::unique_ptr<Program> reduced_program(new Program(*this)); + if (!reduced_program->RemoveFixedBlocks( + removed_parameter_blocks, fixed_cost, error)) { + return nullptr; } reduced_program->SetParameterOffsetsAndIndex(); @@ -288,15 +305,17 @@ Program* Program::CreateReducedProgram( bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, double* fixed_cost, string* error) { - CHECK_NOTNULL(removed_parameter_blocks); - CHECK_NOTNULL(fixed_cost); - CHECK_NOTNULL(error); + CHECK(removed_parameter_blocks != nullptr); + CHECK(fixed_cost != nullptr); + CHECK(error != nullptr); - scoped_array<double> residual_block_evaluate_scratch; + std::unique_ptr<double[]> residual_block_evaluate_scratch; residual_block_evaluate_scratch.reset( new double[MaxScratchDoublesNeededForEvaluate()]); *fixed_cost = 0.0; + bool need_to_call_prepare_for_evaluation = evaluation_callback_ != nullptr; + // Mark all the parameters as unused. Abuse the index member of the // parameter blocks for the marking. for (int i = 0; i < parameter_blocks_.size(); ++i) { @@ -326,18 +345,45 @@ bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, continue; } + // This is an exceedingly rare case, where the user has residual + // blocks which are effectively constant but they are also + // performance sensitive enough to add an EvaluationCallback. + // + // In this case before we evaluate the cost of the constant + // residual blocks, we must call + // EvaluationCallback::PrepareForEvaluation(). Because this call + // can be costly, we only call this if we actually encounter a + // residual block with all constant parameter blocks. + // + // It is worth nothing that there is a minor inefficiency here, + // that the iteration 0 of TrustRegionMinimizer will also cause + // PrepareForEvaluation to be called on the same point, but with + // evaluate_jacobians = true. We could try and optimize this here, + // but given the rarity of this case, the additional complexity + // and long range dependency is not worth it. + if (need_to_call_prepare_for_evaluation) { + constexpr bool kNewPoint = true; + constexpr bool kDoNotEvaluateJacobians = false; + evaluation_callback_->PrepareForEvaluation(kDoNotEvaluateJacobians, + kNewPoint); + need_to_call_prepare_for_evaluation = false; + } + // The residual is constant and will be removed, so its cost is // added to the variable fixed_cost. double cost = 0.0; if (!residual_block->Evaluate(true, &cost, - NULL, - NULL, + nullptr, + nullptr, residual_block_evaluate_scratch.get())) { - *error = StringPrintf("Evaluation of the residual %d failed during " - "removal of fixed residual blocks.", i); + *error = StringPrintf( + "Evaluation of the residual %d failed during " + "removal of fixed residual blocks.", + i); return false; } + *fixed_cost += cost; } residual_blocks_.resize(num_active_residual_blocks); @@ -356,11 +402,9 @@ bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, } parameter_blocks_.resize(num_active_parameter_blocks); - if (!(((NumResidualBlocks() == 0) && - (NumParameterBlocks() == 0)) || - ((NumResidualBlocks() != 0) && - (NumParameterBlocks() != 0)))) { - *error = "Congratulations, you found a bug in Ceres. Please report it."; + if (!(((NumResidualBlocks() == 0) && (NumParameterBlocks() == 0)) || + ((NumResidualBlocks() != 0) && (NumParameterBlocks() != 0)))) { + *error = "Congratulations, you found a bug in Ceres. Please report it."; return false; } @@ -373,15 +417,13 @@ bool Program::IsParameterBlockSetIndependent( // blocks in the same residual block are part of // parameter_block_ptrs as that would violate the assumption that it // is an independent set in the Hessian matrix. - for (vector<ResidualBlock*>::const_iterator it = residual_blocks_.begin(); - it != residual_blocks_.end(); - ++it) { - ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks(); - const int num_parameter_blocks = (*it)->NumParameterBlocks(); + for (const ResidualBlock* residual_block : residual_blocks_) { + ParameterBlock* const* parameter_blocks = + residual_block->parameter_blocks(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); int count = 0; for (int i = 0; i < num_parameter_blocks; ++i) { - count += independent_set.count( - parameter_blocks[i]->mutable_user_state()); + count += independent_set.count(parameter_blocks[i]->mutable_user_state()); } if (count > 1) { return false; @@ -390,18 +432,20 @@ bool Program::IsParameterBlockSetIndependent( return true; } -TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const { +std::unique_ptr<TripletSparseMatrix> +Program::CreateJacobianBlockSparsityTranspose(int start_residual_block) const { // Matrix to store the block sparsity structure of the Jacobian. - TripletSparseMatrix* tsm = - new TripletSparseMatrix(NumParameterBlocks(), - NumResidualBlocks(), - 10 * NumResidualBlocks()); + const int num_rows = NumParameterBlocks(); + const int num_cols = NumResidualBlocks() - start_residual_block; + + std::unique_ptr<TripletSparseMatrix> tsm( + new TripletSparseMatrix(num_rows, num_cols, 10 * num_cols)); int num_nonzeros = 0; int* rows = tsm->mutable_rows(); int* cols = tsm->mutable_cols(); double* values = tsm->mutable_values(); - for (int c = 0; c < residual_blocks_.size(); ++c) { + for (int c = start_residual_block; c < residual_blocks_.size(); ++c) { const ResidualBlock* residual_block = residual_blocks_[c]; const int num_parameter_blocks = residual_block->NumParameterBlocks(); ParameterBlock* const* parameter_blocks = @@ -423,7 +467,7 @@ TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const { const int r = parameter_blocks[j]->index(); rows[num_nonzeros] = r; - cols[num_nonzeros] = c; + cols[num_nonzeros] = c - start_residual_block; values[num_nonzeros] = 1.0; ++num_nonzeros; } @@ -433,13 +477,9 @@ TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const { return tsm; } -int Program::NumResidualBlocks() const { - return residual_blocks_.size(); -} +int Program::NumResidualBlocks() const { return residual_blocks_.size(); } -int Program::NumParameterBlocks() const { - return parameter_blocks_.size(); -} +int Program::NumParameterBlocks() const { return parameter_blocks_.size(); } int Program::NumResiduals() const { int num_residuals = 0; @@ -465,6 +505,9 @@ int Program::NumEffectiveParameters() const { return num_parameters; } +// TODO(sameeragarwal): The following methods should just be updated +// incrementally and the values cached, rather than the linear +// complexity we have right now on every call. int Program::MaxScratchDoublesNeededForEvaluate() const { // Compute the scratch space needed for evaluate. int max_scratch_bytes_for_evaluate = 0; @@ -494,8 +537,8 @@ int Program::MaxDerivativesPerResidualBlock() const { int Program::MaxParametersPerResidualBlock() const { int max_parameters = 0; for (int i = 0; i < residual_blocks_.size(); ++i) { - max_parameters = max(max_parameters, - residual_blocks_[i]->NumParameterBlocks()); + max_parameters = + max(max_parameters, residual_blocks_[i]->NumParameterBlocks()); } return max_parameters; } @@ -514,8 +557,8 @@ string Program::ToString() const { ret += StringPrintf("Number of parameters: %d\n", NumParameters()); ret += "Parameters:\n"; for (int i = 0; i < parameter_blocks_.size(); ++i) { - ret += StringPrintf("%d: %s\n", - i, parameter_blocks_[i]->ToString().c_str()); + ret += + StringPrintf("%d: %s\n", i, parameter_blocks_[i]->ToString().c_str()); } return ret; } diff --git a/extern/ceres/internal/ceres/program.h b/extern/ceres/internal/ceres/program.h index 38c958fe34a..797129980e3 100644 --- a/extern/ceres/internal/ceres/program.h +++ b/extern/ceres/internal/ceres/program.h @@ -31,10 +31,13 @@ #ifndef CERES_INTERNAL_PROGRAM_H_ #define CERES_INTERNAL_PROGRAM_H_ +#include <memory> #include <set> #include <string> #include <vector> + #include "ceres/internal/port.h" +#include "ceres/evaluation_callback.h" namespace ceres { namespace internal { @@ -64,6 +67,7 @@ class Program { const std::vector<ResidualBlock*>& residual_blocks() const; std::vector<ParameterBlock*>* mutable_parameter_blocks(); std::vector<ResidualBlock*>* mutable_residual_blocks(); + EvaluationCallback* mutable_evaluation_callback(); // Serialize to/from the program and update states. // @@ -71,8 +75,8 @@ class Program { // computation of the Jacobian of its local parameterization. If // this computation fails for some reason, then this method returns // false and the state of the parameter blocks cannot be trusted. - bool StateVectorToParameterBlocks(const double *state); - void ParameterBlocksToStateVector(double *state) const; + bool StateVectorToParameterBlocks(const double* state); + void ParameterBlocksToStateVector(double* state) const; // Copy internal state to the user's parameters. void CopyParameterBlockStateToUserState(); @@ -127,8 +131,10 @@ class Program { // structure corresponding to the block sparsity of the transpose of // the Jacobian matrix. // - // Caller owns the result. - TripletSparseMatrix* CreateJacobianBlockSparsityTranspose() const; + // start_residual_block which allows the user to ignore the first + // start_residual_block residuals. + std::unique_ptr<TripletSparseMatrix> CreateJacobianBlockSparsityTranspose( + int start_residual_block = 0) const; // Create a copy of this program and removes constant parameter // blocks and residual blocks with no varying parameter blocks while @@ -182,6 +188,7 @@ class Program { // The Program does not own the ParameterBlock or ResidualBlock objects. std::vector<ParameterBlock*> parameter_blocks_; std::vector<ResidualBlock*> residual_blocks_; + EvaluationCallback* evaluation_callback_ = nullptr; friend class ProblemImpl; }; diff --git a/extern/ceres/internal/ceres/program_evaluator.h b/extern/ceres/internal/ceres/program_evaluator.h index 74a812adeef..97ee590fbab 100644 --- a/extern/ceres/internal/ceres/program_evaluator.h +++ b/extern/ceres/internal/ceres/program_evaluator.h @@ -43,7 +43,7 @@ // residual jacobians are written directly into their final position in the // block sparse matrix by the user's CostFunction; there is no copying. // -// The evaluation is threaded with OpenMP. +// The evaluation is threaded with OpenMP or C++ threads. // // The EvaluatePreparer and JacobianWriter interfaces are as follows: // @@ -82,16 +82,16 @@ // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" -#ifdef CERES_USE_OPENMP -#include <omp.h> -#endif - +#include <atomic> #include <map> +#include <memory> #include <string> #include <vector> + +#include "ceres/evaluation_callback.h" #include "ceres/execution_summary.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/parallel_for.h" #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" @@ -104,34 +104,33 @@ struct NullJacobianFinalizer { void operator()(SparseMatrix* jacobian, int num_parameters) {} }; -template<typename EvaluatePreparer, - typename JacobianWriter, - typename JacobianFinalizer = NullJacobianFinalizer> +template <typename EvaluatePreparer, + typename JacobianWriter, + typename JacobianFinalizer = NullJacobianFinalizer> class ProgramEvaluator : public Evaluator { public: - ProgramEvaluator(const Evaluator::Options &options, Program* program) + ProgramEvaluator(const Evaluator::Options& options, Program* program) : options_(options), program_(program), jacobian_writer_(options, program), evaluate_preparers_( jacobian_writer_.CreateEvaluatePreparers(options.num_threads)) { -#ifndef CERES_USE_OPENMP +#ifdef CERES_NO_THREADS if (options_.num_threads > 1) { - LOG(WARNING) - << "OpenMP support is not compiled into this binary; " - << "only options.num_threads = 1 is supported. Switching " - << "to single threaded mode."; + LOG(WARNING) << "No threading support is compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; options_.num_threads = 1; } -#endif +#endif // CERES_NO_THREADS BuildResidualLayout(*program, &residual_layout_); - evaluate_scratch_.reset(CreateEvaluatorScratch(*program, - options.num_threads)); + evaluate_scratch_.reset( + CreateEvaluatorScratch(*program, options.num_threads)); } // Implementation of Evaluator interface. - SparseMatrix* CreateJacobian() const { + SparseMatrix* CreateJacobian() const final { return jacobian_writer_.CreateJacobian(); } @@ -140,133 +139,133 @@ class ProgramEvaluator : public Evaluator { double* cost, double* residuals, double* gradient, - SparseMatrix* jacobian) { + SparseMatrix* jacobian) final { ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); - ScopedExecutionTimer call_type_timer(gradient == NULL && jacobian == NULL - ? "Evaluator::Residual" - : "Evaluator::Jacobian", - &execution_summary_); + ScopedExecutionTimer call_type_timer( + gradient == nullptr && jacobian == nullptr ? "Evaluator::Residual" + : "Evaluator::Jacobian", + &execution_summary_); // The parameters are stateful, so set the state before evaluating. if (!program_->StateVectorToParameterBlocks(state)) { return false; } - if (residuals != NULL) { + // Notify the user about a new evaluation point if they are interested. + if (options_.evaluation_callback != nullptr) { + program_->CopyParameterBlockStateToUserState(); + options_.evaluation_callback->PrepareForEvaluation( + /*jacobians=*/(gradient != nullptr || jacobian != nullptr), + evaluate_options.new_evaluation_point); + } + + if (residuals != nullptr) { VectorRef(residuals, program_->NumResiduals()).setZero(); } - if (jacobian != NULL) { + if (jacobian != nullptr) { jacobian->SetZero(); } // Each thread gets it's own cost and evaluate scratch space. for (int i = 0; i < options_.num_threads; ++i) { evaluate_scratch_[i].cost = 0.0; - if (gradient != NULL) { + if (gradient != nullptr) { VectorRef(evaluate_scratch_[i].gradient.get(), - program_->NumEffectiveParameters()).setZero(); + program_->NumEffectiveParameters()) + .setZero(); } } - // This bool is used to disable the loop if an error is encountered - // without breaking out of it. The remaining loop iterations are still run, - // but with an empty body, and so will finish quickly. - bool abort = false; - int num_residual_blocks = program_->NumResidualBlocks(); -#pragma omp parallel for num_threads(options_.num_threads) - for (int i = 0; i < num_residual_blocks; ++i) { -// Disable the loop instead of breaking, as required by OpenMP. -#pragma omp flush(abort) - if (abort) { - continue; - } + const int num_residual_blocks = program_->NumResidualBlocks(); + // This bool is used to disable the loop if an error is encountered without + // breaking out of it. The remaining loop iterations are still run, but with + // an empty body, and so will finish quickly. + std::atomic_bool abort(false); + ParallelFor( + options_.context, + 0, + num_residual_blocks, + options_.num_threads, + [&](int thread_id, int i) { + if (abort) { + return; + } -#ifdef CERES_USE_OPENMP - int thread_id = omp_get_thread_num(); -#else - int thread_id = 0; -#endif - EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; - EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; - - // Prepare block residuals if requested. - const ResidualBlock* residual_block = program_->residual_blocks()[i]; - double* block_residuals = NULL; - if (residuals != NULL) { - block_residuals = residuals + residual_layout_[i]; - } else if (gradient != NULL) { - block_residuals = scratch->residual_block_residuals.get(); - } + EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; + EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; - // Prepare block jacobians if requested. - double** block_jacobians = NULL; - if (jacobian != NULL || gradient != NULL) { - preparer->Prepare(residual_block, - i, - jacobian, - scratch->jacobian_block_ptrs.get()); - block_jacobians = scratch->jacobian_block_ptrs.get(); - } + // Prepare block residuals if requested. + const ResidualBlock* residual_block = program_->residual_blocks()[i]; + double* block_residuals = nullptr; + if (residuals != nullptr) { + block_residuals = residuals + residual_layout_[i]; + } else if (gradient != nullptr) { + block_residuals = scratch->residual_block_residuals.get(); + } - // Evaluate the cost, residuals, and jacobians. - double block_cost; - if (!residual_block->Evaluate( - evaluate_options.apply_loss_function, - &block_cost, - block_residuals, - block_jacobians, - scratch->residual_block_evaluate_scratch.get())) { - abort = true; -// This ensures that the OpenMP threads have a consistent view of 'abort'. Do -// the flush inside the failure case so that there is usually only one -// synchronization point per loop iteration instead of two. -#pragma omp flush(abort) - continue; - } + // Prepare block jacobians if requested. + double** block_jacobians = nullptr; + if (jacobian != nullptr || gradient != nullptr) { + preparer->Prepare(residual_block, + i, + jacobian, + scratch->jacobian_block_ptrs.get()); + block_jacobians = scratch->jacobian_block_ptrs.get(); + } - scratch->cost += block_cost; + // Evaluate the cost, residuals, and jacobians. + double block_cost; + if (!residual_block->Evaluate( + evaluate_options.apply_loss_function, + &block_cost, + block_residuals, + block_jacobians, + scratch->residual_block_evaluate_scratch.get())) { + abort = true; + return; + } - // Store the jacobians, if they were requested. - if (jacobian != NULL) { - jacobian_writer_.Write(i, - residual_layout_[i], - block_jacobians, - jacobian); - } + scratch->cost += block_cost; - // Compute and store the gradient, if it was requested. - if (gradient != NULL) { - int num_residuals = residual_block->NumResiduals(); - int num_parameter_blocks = residual_block->NumParameterBlocks(); - for (int j = 0; j < num_parameter_blocks; ++j) { - const ParameterBlock* parameter_block = - residual_block->parameter_blocks()[j]; - if (parameter_block->IsConstant()) { - continue; + // Store the jacobians, if they were requested. + if (jacobian != nullptr) { + jacobian_writer_.Write( + i, residual_layout_[i], block_jacobians, jacobian); } - MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - block_jacobians[j], - num_residuals, - parameter_block->LocalSize(), - block_residuals, - scratch->gradient.get() + parameter_block->delta_offset()); - } - } - } + // Compute and store the gradient, if it was requested. + if (gradient != nullptr) { + int num_residuals = residual_block->NumResiduals(); + int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (parameter_block->IsConstant()) { + continue; + } + + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + block_jacobians[j], + num_residuals, + parameter_block->LocalSize(), + block_residuals, + scratch->gradient.get() + parameter_block->delta_offset()); + } + } + }); if (!abort) { const int num_parameters = program_->NumEffectiveParameters(); // Sum the cost and gradient (if requested) from each thread. (*cost) = 0.0; - if (gradient != NULL) { + if (gradient != nullptr) { VectorRef(gradient, num_parameters).setZero(); } for (int i = 0; i < options_.num_threads; ++i) { (*cost) += evaluate_scratch_[i].cost; - if (gradient != NULL) { + if (gradient != nullptr) { VectorRef(gradient, num_parameters) += VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters); } @@ -276,7 +275,7 @@ class ProgramEvaluator : public Evaluator { // `num_parameters` is passed to the finalizer so that additional // storage can be reserved for additional diagonal elements if // necessary. - if (jacobian != NULL) { + if (jacobian != nullptr) { JacobianFinalizer f; f(jacobian, num_parameters); } @@ -286,27 +285,19 @@ class ProgramEvaluator : public Evaluator { bool Plus(const double* state, const double* delta, - double* state_plus_delta) const { + double* state_plus_delta) const final { return program_->Plus(state, delta, state_plus_delta); } - int NumParameters() const { - return program_->NumParameters(); - } - int NumEffectiveParameters() const { + int NumParameters() const final { return program_->NumParameters(); } + int NumEffectiveParameters() const final { return program_->NumEffectiveParameters(); } - int NumResiduals() const { - return program_->NumResiduals(); - } - - virtual std::map<std::string, int> CallStatistics() const { - return execution_summary_.calls(); - } + int NumResiduals() const final { return program_->NumResiduals(); } - virtual std::map<std::string, double> TimeStatistics() const { - return execution_summary_.times(); + std::map<std::string, CallStatistics> Statistics() const final { + return execution_summary_.statistics(); } private: @@ -322,17 +313,16 @@ class ProgramEvaluator : public Evaluator { VectorRef(gradient.get(), num_parameters).setZero(); residual_block_residuals.reset( new double[max_residuals_per_residual_block]); - jacobian_block_ptrs.reset( - new double*[max_parameters_per_residual_block]); + jacobian_block_ptrs.reset(new double*[max_parameters_per_residual_block]); } double cost; - scoped_array<double> residual_block_evaluate_scratch; + std::unique_ptr<double[]> residual_block_evaluate_scratch; // The gradient in the local parameterization. - scoped_array<double> gradient; + std::unique_ptr<double[]> gradient; // Enough space to store the residual for the largest residual block. - scoped_array<double> residual_block_residuals; - scoped_array<double*> jacobian_block_ptrs; + std::unique_ptr<double[]> residual_block_residuals; + std::unique_ptr<double*[]> jacobian_block_ptrs; }; static void BuildResidualLayout(const Program& program, @@ -372,8 +362,8 @@ class ProgramEvaluator : public Evaluator { Evaluator::Options options_; Program* program_; JacobianWriter jacobian_writer_; - scoped_array<EvaluatePreparer> evaluate_preparers_; - scoped_array<EvaluateScratch> evaluate_scratch_; + std::unique_ptr<EvaluatePreparer[]> evaluate_preparers_; + std::unique_ptr<EvaluateScratch[]> evaluate_scratch_; std::vector<int> residual_layout_; ::ceres::internal::ExecutionSummary execution_summary_; }; diff --git a/extern/ceres/internal/ceres/random.h b/extern/ceres/internal/ceres/random.h index 2a025600609..87d9d77d90d 100644 --- a/extern/ceres/internal/ceres/random.h +++ b/extern/ceres/internal/ceres/random.h @@ -43,7 +43,11 @@ inline void SetRandomState(int state) { } inline int Uniform(int n) { - return rand() % n; + if (n) { + return rand() % n; + } else { + return 0; + } } inline double RandDouble() { diff --git a/extern/ceres/internal/ceres/reorder_program.cc b/extern/ceres/internal/ceres/reorder_program.cc index a7c37107591..aa6032a9e9e 100644 --- a/extern/ceres/internal/ceres/reorder_program.cc +++ b/extern/ceres/internal/ceres/reorder_program.cc @@ -31,6 +31,7 @@ #include "ceres/reorder_program.h" #include <algorithm> +#include <memory> #include <numeric> #include <vector> @@ -84,7 +85,7 @@ static int MinParameterBlock(const ResidualBlock* residual_block, return min_parameter_block_position; } -#if EIGEN_VERSION_AT_LEAST(3, 2, 2) && defined(CERES_USE_EIGEN_SPARSE) +#if defined(CERES_USE_EIGEN_SPARSE) Eigen::SparseMatrix<int> CreateBlockJacobian( const TripletSparseMatrix& block_jacobian_transpose) { typedef Eigen::SparseMatrix<int> SparseMatrix; @@ -178,7 +179,6 @@ void OrderingForSparseNormalCholeskyUsingCXSparse( } -#if EIGEN_VERSION_AT_LEAST(3, 2, 2) void OrderingForSparseNormalCholeskyUsingEigenSparse( const TripletSparseMatrix& tsm_block_jacobian_transpose, int* ordering) { @@ -211,7 +211,6 @@ void OrderingForSparseNormalCholeskyUsingEigenSparse( } #endif // CERES_USE_EIGEN_SPARSE } -#endif } // namespace @@ -233,24 +232,19 @@ bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map, 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()) { + const map<int, set<double*>>& groups = ordering.group_to_elements(); + for (const auto& p : groups) { + const set<double*>& group = p.second; + for (double* parameter_block_ptr : group) { + auto it = parameter_map.find(parameter_block_ptr); + if (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); + p.first); return false; } - parameter_blocks->push_back(parameter_block_it->second); + parameter_blocks->push_back(it->second); } } return true; @@ -339,7 +333,7 @@ bool LexicographicallyOrderResidualBlocks( // Pre-order the columns corresponding to the schur complement if // possible. -void MaybeReorderSchurComplementColumnsUsingSuiteSparse( +static void MaybeReorderSchurComplementColumnsUsingSuiteSparse( const ParameterBlockOrdering& parameter_block_ordering, Program* program) { #ifndef CERES_NO_SUITESPARSE @@ -364,7 +358,7 @@ void MaybeReorderSchurComplementColumnsUsingSuiteSparse( MapValuesToContiguousRange(constraints.size(), &constraints[0]); // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + std::unique_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( program->CreateJacobianBlockSparsityTranspose()); cholmod_sparse* block_jacobian_transpose = @@ -385,15 +379,12 @@ void MaybeReorderSchurComplementColumnsUsingSuiteSparse( #endif } -void MaybeReorderSchurComplementColumnsUsingEigen( +static void MaybeReorderSchurComplementColumnsUsingEigen( const int size_of_first_elimination_group, const ProblemImpl::ParameterMap& parameter_map, Program* program) { -#if !EIGEN_VERSION_AT_LEAST(3, 2, 2) || !defined(CERES_USE_EIGEN_SPARSE) - return; -#else - - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( +#if defined(CERES_USE_EIGEN_SPARSE) + std::unique_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( program->CreateJacobianBlockSparsityTranspose()); typedef Eigen::SparseMatrix<int> SparseMatrix; @@ -531,18 +522,14 @@ bool ReorderProgramForSchurTypeLinearSolver( // Schur type solvers also require that their residual blocks be // lexicographically ordered. - if (!LexicographicallyOrderResidualBlocks(size_of_first_elimination_group, - program, - error)) { - return false; - } - - return true; + return LexicographicallyOrderResidualBlocks( + size_of_first_elimination_group, program, error); } -bool ReorderProgramForSparseNormalCholesky( +bool ReorderProgramForSparseCholesky( const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, const ParameterBlockOrdering& parameter_block_ordering, + int start_row_block, Program* program, string* error) { if (parameter_block_ordering.NumElements() != program->NumParameterBlocks()) { @@ -555,8 +542,8 @@ bool ReorderProgramForSparseNormalCholesky( } // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( - program->CreateJacobianBlockSparsityTranspose()); + std::unique_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose(start_row_block)); vector<int> ordering(program->NumParameterBlocks(), 0); vector<ParameterBlock*>& parameter_blocks = @@ -572,19 +559,19 @@ bool ReorderProgramForSparseNormalCholesky( OrderingForSparseNormalCholeskyUsingCXSparse( *tsm_block_jacobian_transpose, &ordering[0]); + } else if (sparse_linear_algebra_library_type == ACCELERATE_SPARSE) { + // Accelerate does not provide a function to perform reordering without + // performing a full symbolic factorisation. As such, we have nothing + // to gain from trying to reorder the problem here, as it will happen + // in AppleAccelerateCholesky::Factorize() (once) and reordering here + // would involve performing two symbolic factorisations instead of one + // which would have a negative overall impact on performance. + return true; + } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) { -#if EIGEN_VERSION_AT_LEAST(3, 2, 2) - OrderingForSparseNormalCholeskyUsingEigenSparse( + OrderingForSparseNormalCholeskyUsingEigenSparse( *tsm_block_jacobian_transpose, &ordering[0]); -#else - // For Eigen versions less than 3.2.2, there is nothing to do as - // older versions of Eigen do not expose a method for doing - // symbolic analysis on pre-ordered matrices, so a block - // pre-ordering is a bit pointless. - - return true; -#endif } // Apply ordering. @@ -597,5 +584,17 @@ bool ReorderProgramForSparseNormalCholesky( return true; } +int ReorderResidualBlocksByPartition( + const std::unordered_set<ResidualBlockId>& bottom_residual_blocks, + Program* program) { + auto residual_blocks = program->mutable_residual_blocks(); + auto it = std::partition( + residual_blocks->begin(), residual_blocks->end(), + [&bottom_residual_blocks](ResidualBlock* r) { + return bottom_residual_blocks.count(r) == 0; + }); + return it - residual_blocks->begin(); +} + } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/reorder_program.h b/extern/ceres/internal/ceres/reorder_program.h index 36e5d1637a9..88cbee3af21 100644 --- a/extern/ceres/internal/ceres/reorder_program.h +++ b/extern/ceres/internal/ceres/reorder_program.h @@ -89,12 +89,27 @@ bool ReorderProgramForSchurTypeLinearSolver( // fill-reducing ordering is available in the sparse linear algebra // library (SuiteSparse version >= 4.2.0) then the fill reducing // ordering will take it into account, otherwise it will be ignored. -bool ReorderProgramForSparseNormalCholesky( +bool ReorderProgramForSparseCholesky( SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, const ParameterBlockOrdering& parameter_block_ordering, + int start_row_block, Program* program, std::string* error); +// Reorder the residual blocks in the program so that all the residual +// blocks in bottom_residual_blocks are at the bottom. The return +// value is the number of residual blocks in the program in "top" part +// of the Program, i.e., the ones not included in +// bottom_residual_blocks. +// +// This number can be different from program->NumResidualBlocks() - +// bottom_residual_blocks.size() because we allow +// bottom_residual_blocks to contain residual blocks not present in +// the Program. +int ReorderResidualBlocksByPartition( + const std::unordered_set<ResidualBlockId>& bottom_residual_blocks, + Program* program); + } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/residual_block.cc b/extern/ceres/internal/ceres/residual_block.cc index 9a123cf132e..0bf30bcf446 100644 --- a/extern/ceres/internal/ceres/residual_block.cc +++ b/extern/ceres/internal/ceres/residual_block.cc @@ -35,13 +35,13 @@ #include <cstddef> #include <vector> #include "ceres/corrector.h" -#include "ceres/parameter_block.h" -#include "ceres/residual_block_utils.h" #include "ceres/cost_function.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" #include "ceres/local_parameterization.h" #include "ceres/loss_function.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block_utils.h" #include "ceres/small_blas.h" using Eigen::Dynamic; @@ -50,16 +50,14 @@ namespace ceres { namespace internal { ResidualBlock::ResidualBlock( - const CostFunction* cost_function, - const LossFunction* loss_function, - const std::vector<ParameterBlock*>& parameter_blocks, - int index) + const CostFunction* cost_function, const LossFunction* loss_function, + const std::vector<ParameterBlock*>& parameter_blocks, int index) : cost_function_(cost_function), loss_function_(loss_function), parameter_blocks_( - new ParameterBlock* [ - cost_function->parameter_block_sizes().size()]), + new ParameterBlock*[cost_function->parameter_block_sizes().size()]), index_(index) { + CHECK(cost_function_ != nullptr); std::copy(parameter_blocks.begin(), parameter_blocks.end(), parameter_blocks_.get()); @@ -82,11 +80,11 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, // Put pointers into the scratch space into global_jacobians as appropriate. FixedArray<double*, 8> global_jacobians(num_parameter_blocks); - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; - if (jacobians[i] != NULL && - parameter_block->LocalParameterizationJacobian() != NULL) { + if (jacobians[i] != nullptr && + parameter_block->LocalParameterizationJacobian() != nullptr) { global_jacobians[i] = scratch; scratch += num_residuals * parameter_block->Size(); } else { @@ -96,7 +94,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, } // If the caller didn't request residuals, use the scratch space for them. - bool outputting_residuals = (residuals != NULL); + bool outputting_residuals = (residuals != nullptr); if (!outputting_residuals) { residuals = scratch; } @@ -104,16 +102,17 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, // Invalidate the evaluation buffers so that we can check them after // the CostFunction::Evaluate call, to see if all the return values // that were required were written to and that they are finite. - double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL; + double** eval_jacobians = + (jacobians != nullptr) ? global_jacobians.data() : nullptr; InvalidateEvaluation(*this, cost, residuals, eval_jacobians); - if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians)) { + if (!cost_function_->Evaluate(parameters.data(), residuals, eval_jacobians)) { return false; } if (!IsEvaluationValid(*this, - parameters.get(), + parameters.data(), cost, residuals, eval_jacobians)) { @@ -124,7 +123,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n" // NOLINT "generated during the or jacobian computation. \n\n" + EvaluationToString(*this, - parameters.get(), + parameters.data(), cost, residuals, eval_jacobians); @@ -135,13 +134,13 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); // Update the jacobians with the local parameterizations. - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { - if (jacobians[i] != NULL) { + if (jacobians[i] != nullptr) { const ParameterBlock* parameter_block = parameter_blocks_[i]; // Apply local reparameterization to the jacobians. - if (parameter_block->LocalParameterizationJacobian() != NULL) { + if (parameter_block->LocalParameterizationJacobian() != nullptr) { // jacobians[i] = global_jacobians[i] * global_to_local_jacobian. MatrixMatrixMultiply<Dynamic, Dynamic, Dynamic, Dynamic, 0>( global_jacobians[i], @@ -156,7 +155,7 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, } } - if (loss_function_ == NULL || !apply_loss_function) { + if (loss_function_ == nullptr || !apply_loss_function) { *cost = 0.5 * squared_norm; return true; } @@ -167,16 +166,16 @@ bool ResidualBlock::Evaluate(const bool apply_loss_function, // No jacobians and not outputting residuals? All done. Doing an early exit // here avoids constructing the "Corrector" object below in a common case. - if (jacobians == NULL && !outputting_residuals) { + if (jacobians == nullptr && !outputting_residuals) { return true; } // Correct for the effects of the loss function. The jacobians need to be // corrected before the residuals, since they use the uncorrected residuals. Corrector correct(squared_norm, rho); - if (jacobians != NULL) { + if (jacobians != nullptr) { for (int i = 0; i < num_parameter_blocks; ++i) { - if (jacobians[i] != NULL) { + if (jacobians[i] != nullptr) { const ParameterBlock* parameter_block = parameter_blocks_[i]; // Correct the jacobians for the loss function. @@ -206,8 +205,7 @@ int ResidualBlock::NumScratchDoublesForEvaluate() const { int scratch_doubles = 1; for (int i = 0; i < num_parameters; ++i) { const ParameterBlock* parameter_block = parameter_blocks_[i]; - if (!parameter_block->IsConstant() && - parameter_block->LocalParameterizationJacobian() != NULL) { + if (parameter_block->LocalParameterizationJacobian() != nullptr) { scratch_doubles += parameter_block->Size(); } } diff --git a/extern/ceres/internal/ceres/residual_block.h b/extern/ceres/internal/ceres/residual_block.h index a32f1c36cd3..a2e4425b911 100644 --- a/extern/ceres/internal/ceres/residual_block.h +++ b/extern/ceres/internal/ceres/residual_block.h @@ -34,12 +34,13 @@ #ifndef CERES_INTERNAL_RESIDUAL_BLOCK_H_ #define CERES_INTERNAL_RESIDUAL_BLOCK_H_ +#include <cstdint> +#include <memory> #include <string> #include <vector> #include "ceres/cost_function.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/stringprintf.h" #include "ceres/types.h" @@ -81,12 +82,14 @@ class ResidualBlock { // computed. If jacobians[i] is NULL, then the jacobian for that parameter is // not computed. // + // cost must not be null. + // // Evaluate needs scratch space which must be supplied by the caller via // scratch. The array should have at least NumScratchDoublesForEvaluate() // space available. // // The return value indicates the success or failure. If the function returns - // false, the caller should expect the the output memory locations to have + // false, the caller should expect the output memory locations to have // been modified. // // The returned cost and jacobians have had robustification and local @@ -134,12 +137,12 @@ class ResidualBlock { private: const CostFunction* cost_function_; const LossFunction* loss_function_; - scoped_array<ParameterBlock*> parameter_blocks_; + std::unique_ptr<ParameterBlock*[]> parameter_blocks_; // The index of the residual, typically in a Program. This is only to permit // switching from a ResidualBlock* to an index in the Program's array, needed // to do efficient removals. - int32 index_; + int32_t index_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/residual_block_utils.cc b/extern/ceres/internal/ceres/residual_block_utils.cc index dd2bd73a6ac..35e928bbcc1 100644 --- a/extern/ceres/internal/ceres/residual_block_utils.cc +++ b/extern/ceres/internal/ceres/residual_block_utils.cc @@ -68,8 +68,8 @@ string EvaluationToString(const ResidualBlock& block, double* cost, double* residuals, double** jacobians) { - CHECK_NOTNULL(cost); - CHECK_NOTNULL(residuals); + CHECK(cost != nullptr); + CHECK(residuals != nullptr); const int num_parameter_blocks = block.NumParameterBlocks(); const int num_residuals = block.NumResiduals(); diff --git a/extern/ceres/internal/ceres/schur_complement_solver.cc b/extern/ceres/internal/ceres/schur_complement_solver.cc index 65449832c4c..0083300b036 100644 --- a/extern/ceres/internal/ceres/schur_complement_solver.cc +++ b/extern/ceres/internal/ceres/schur_complement_solver.cc @@ -28,33 +28,30 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include "ceres/internal/port.h" +#include "ceres/schur_complement_solver.h" #include <algorithm> #include <ctime> +#include <memory> #include <set> -#include <sstream> #include <vector> +#include "Eigen/Dense" +#include "Eigen/SparseCore" #include "ceres/block_random_access_dense_matrix.h" #include "ceres/block_random_access_matrix.h" #include "ceres/block_random_access_sparse_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/conjugate_gradients_solver.h" -#include "ceres/cxsparse.h" #include "ceres/detect_structure.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/lapack.h" #include "ceres/linear_solver.h" -#include "ceres/schur_complement_solver.h" -#include "ceres/suitesparse.h" +#include "ceres/sparse_cholesky.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "ceres/wall_time.h" -#include "Eigen/Dense" -#include "Eigen/SparseCore" namespace ceres { namespace internal { @@ -70,23 +67,22 @@ class BlockRandomAccessSparseMatrixAdapter : public LinearOperator { public: explicit BlockRandomAccessSparseMatrixAdapter( const BlockRandomAccessSparseMatrix& m) - : m_(m) { - } + : m_(m) {} virtual ~BlockRandomAccessSparseMatrixAdapter() {} // y = y + Ax; - virtual void RightMultiply(const double* x, double* y) const { + void RightMultiply(const double* x, double* y) const final { m_.SymmetricRightMultiply(x, y); } // y = y + A'x; - virtual void LeftMultiply(const double* x, double* y) const { + void LeftMultiply(const double* x, double* y) const final { m_.SymmetricRightMultiply(x, y); } - virtual int num_rows() const { return m_.num_rows(); } - virtual int num_cols() const { return m_.num_rows(); } + int num_rows() const final { return m_.num_rows(); } + int num_cols() const final { return m_.num_rows(); } private: const BlockRandomAccessSparseMatrix& m_; @@ -96,29 +92,28 @@ class BlockRandomAccessDiagonalMatrixAdapter : public LinearOperator { public: explicit BlockRandomAccessDiagonalMatrixAdapter( const BlockRandomAccessDiagonalMatrix& m) - : m_(m) { - } + : m_(m) {} virtual ~BlockRandomAccessDiagonalMatrixAdapter() {} // y = y + Ax; - virtual void RightMultiply(const double* x, double* y) const { + void RightMultiply(const double* x, double* y) const final { m_.RightMultiply(x, y); } // y = y + A'x; - virtual void LeftMultiply(const double* x, double* y) const { + void LeftMultiply(const double* x, double* y) const final { m_.RightMultiply(x, y); } - virtual int num_rows() const { return m_.num_rows(); } - virtual int num_cols() const { return m_.num_rows(); } + int num_rows() const final { return m_.num_rows(); } + int num_cols() const final { return m_.num_rows(); } private: const BlockRandomAccessDiagonalMatrix& m_; }; -} // namespace +} // namespace LinearSolver::Summary SchurComplementSolver::SolveImpl( BlockSparseMatrix* A, @@ -127,20 +122,45 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( double* x) { EventLogger event_logger("SchurComplementSolver::Solve"); + const CompressedRowBlockStructure* bs = A->block_structure(); if (eliminator_.get() == NULL) { - InitStorage(A->block_structure()); - DetectStructure(*A->block_structure(), - options_.elimination_groups[0], + const int num_eliminate_blocks = options_.elimination_groups[0]; + const int num_f_blocks = bs->cols.size() - num_eliminate_blocks; + + InitStorage(bs); + DetectStructure(*bs, + num_eliminate_blocks, &options_.row_block_size, &options_.e_block_size, &options_.f_block_size); - eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_))); - eliminator_->Init(options_.elimination_groups[0], A->block_structure()); - }; + + // For the special case of the static structure <2,3,6> with + // exactly one f block use the SchurEliminatorForOneFBlock. + // + // TODO(sameeragarwal): A more scalable template specialization + // mechanism that does not cause binary bloat. + if (options_.row_block_size == 2 && + options_.e_block_size == 3 && + options_.f_block_size == 6 && + num_f_blocks == 1) { + eliminator_.reset(new SchurEliminatorForOneFBlock<2, 3, 6>); + } else { + eliminator_.reset(SchurEliminatorBase::Create(options_)); + } + + CHECK(eliminator_); + const bool kFullRankETE = true; + eliminator_->Init(num_eliminate_blocks, kFullRankETE, bs); + } + std::fill(x, x + A->num_cols(), 0.0); event_logger.AddEvent("Setup"); - eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get()); + eliminator_->Eliminate(BlockSparseMatrixData(*A), + b, + per_solve_options.D, + lhs_.get(), + rhs_.get()); event_logger.AddEvent("Eliminate"); double* reduced_solution = x + A->num_cols() - lhs_->num_cols(); @@ -149,7 +169,8 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( event_logger.AddEvent("ReducedSolve"); if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { - eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x); + eliminator_->BackSubstitute( + BlockSparseMatrixData(*A), b, per_solve_options.D, reduced_solution, x); event_logger.AddEvent("BackSubstitute"); } @@ -164,9 +185,7 @@ void DenseSchurComplementSolver::InitStorage( const int num_col_blocks = bs->cols.size(); vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); - for (int i = num_eliminate_blocks, j = 0; - i < num_col_blocks; - ++i, ++j) { + for (int i = num_eliminate_blocks, j = 0; i < num_col_blocks; ++i, ++j) { blocks[j] = bs->cols[i].size; } @@ -177,10 +196,8 @@ void DenseSchurComplementSolver::InitStorage( // Solve the system Sx = r, assuming that the matrix S is stored in a // BlockRandomAccessDenseMatrix. The linear system is solved using // Eigen's Cholesky factorization. -LinearSolver::Summary -DenseSchurComplementSolver::SolveReducedLinearSystem( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution) { +LinearSolver::Summary DenseSchurComplementSolver::SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution) { LinearSolver::Summary summary; summary.num_iterations = 0; summary.termination_type = LINEAR_SOLVER_SUCCESS; @@ -201,8 +218,8 @@ DenseSchurComplementSolver::SolveReducedLinearSystem( if (options().dense_linear_algebra_library_type == EIGEN) { Eigen::LLT<Matrix, Eigen::Upper> llt = ConstMatrixRef(m->values(), num_rows, num_rows) - .selfadjointView<Eigen::Upper>() - .llt(); + .selfadjointView<Eigen::Upper>() + .llt(); if (llt.info() != Eigen::Success) { summary.termination_type = LINEAR_SOLVER_FAILURE; summary.message = @@ -213,11 +230,8 @@ DenseSchurComplementSolver::SolveReducedLinearSystem( VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows)); } else { VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); - summary.termination_type = - LAPACK::SolveInPlaceUsingCholesky(num_rows, - m->values(), - solution, - &summary.message); + summary.termination_type = LAPACK::SolveInPlaceUsingCholesky( + num_rows, m->values(), solution, &summary.message); } return summary; @@ -225,23 +239,14 @@ DenseSchurComplementSolver::SolveReducedLinearSystem( SparseSchurComplementSolver::SparseSchurComplementSolver( const LinearSolver::Options& options) - : SchurComplementSolver(options), - factor_(NULL), - cxsparse_factor_(NULL) { -} - -SparseSchurComplementSolver::~SparseSchurComplementSolver() { - if (factor_ != NULL) { - ss_.Free(factor_); - factor_ = NULL; - } - - if (cxsparse_factor_ != NULL) { - cxsparse_.Free(cxsparse_factor_); - cxsparse_factor_ = NULL; + : SchurComplementSolver(options) { + if (options.type != ITERATIVE_SCHUR) { + sparse_cholesky_ = SparseCholesky::Create(options); } } +SparseSchurComplementSolver::~SparseSchurComplementSolver() {} + // Determine the non-zero blocks in the Schur Complement matrix, and // initialize a BlockRandomAccessSparseMatrix object. void SparseSchurComplementSolver::InitStorage( @@ -255,7 +260,7 @@ void SparseSchurComplementSolver::InitStorage( blocks_[i - num_eliminate_blocks] = bs->cols[i].size; } - set<pair<int, int> > block_pairs; + set<pair<int, int>> block_pairs; for (int i = 0; i < blocks_.size(); ++i) { block_pairs.insert(make_pair(i, i)); } @@ -293,7 +298,7 @@ void SparseSchurComplementSolver::InitStorage( } } - // Remaing rows do not contribute to the chunks and directly go + // Remaining rows do not contribute to the chunks and directly go // into the schur complement via an outer product. for (; r < num_row_blocks; ++r) { const CompressedRow& row = bs->rows[r]; @@ -313,296 +318,49 @@ void SparseSchurComplementSolver::InitStorage( set_rhs(new double[lhs()->num_rows()]); } -LinearSolver::Summary -SparseSchurComplementSolver::SolveReducedLinearSystem( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution) { +LinearSolver::Summary SparseSchurComplementSolver::SolveReducedLinearSystem( + const LinearSolver::PerSolveOptions& per_solve_options, double* solution) { if (options().type == ITERATIVE_SCHUR) { - CHECK(options().use_explicit_schur_complement); return SolveReducedLinearSystemUsingConjugateGradients(per_solve_options, solution); } - switch (options().sparse_linear_algebra_library_type) { - case SUITE_SPARSE: - return SolveReducedLinearSystemUsingSuiteSparse(per_solve_options, - solution); - case CX_SPARSE: - return SolveReducedLinearSystemUsingCXSparse(per_solve_options, - solution); - case EIGEN_SPARSE: - return SolveReducedLinearSystemUsingEigen(per_solve_options, - solution); - default: - LOG(FATAL) << "Unknown sparse linear algebra library : " - << options().sparse_linear_algebra_library_type; - } - - return LinearSolver::Summary(); -} - -// Solve the system Sx = r, assuming that the matrix S is stored in a -// BlockRandomAccessSparseMatrix. The linear system is solved using -// CHOLMOD's sparse cholesky factorization routines. -LinearSolver::Summary -SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution) { -#ifdef CERES_NO_SUITESPARSE - - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = "Ceres was not built with SuiteSparse support. " - "Therefore, SPARSE_SCHUR cannot be used with SUITE_SPARSE"; - return summary; - -#else - LinearSolver::Summary summary; summary.num_iterations = 0; summary.termination_type = LINEAR_SOLVER_SUCCESS; summary.message = "Success."; - TripletSparseMatrix* tsm = - const_cast<TripletSparseMatrix*>( - down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); - const int num_rows = tsm->num_rows(); - - // The case where there are no f blocks, and the system is block - // diagonal. - if (num_rows == 0) { + const TripletSparseMatrix* tsm = + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix(); + if (tsm->num_rows() == 0) { return summary; } - summary.num_iterations = 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. - - // Create a upper triangular symmetric matrix. - cholmod_lhs = ss_.CreateSparseMatrix(tsm); - cholmod_lhs->stype = 1; - - if (factor_ == NULL) { - factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, - blocks_, - blocks_, - &summary.message); - } + std::unique_ptr<CompressedRowSparseMatrix> lhs; + const CompressedRowSparseMatrix::StorageType storage_type = + sparse_cholesky_->StorageType(); + if (storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR) { + lhs.reset(CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm)); + lhs->set_storage_type(CompressedRowSparseMatrix::UPPER_TRIANGULAR); } 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, - &summary.message); - } - } - - if (factor_ == NULL) { - ss_.Free(cholmod_lhs); - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - // No need to set message as it has already been set by the - // symbolic analysis routines above. - return summary; + lhs.reset( + CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm)); + lhs->set_storage_type(CompressedRowSparseMatrix::LOWER_TRIANGULAR); } - summary.termination_type = - ss_.Cholesky(cholmod_lhs, factor_, &summary.message); - - ss_.Free(cholmod_lhs); - - if (summary.termination_type != LINEAR_SOLVER_SUCCESS) { - // No need to set message as it has already been set by the - // numeric factorization routine above. - return summary; - } - - cholmod_dense* cholmod_rhs = - ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); - cholmod_dense* cholmod_solution = ss_.Solve(factor_, - cholmod_rhs, - &summary.message); - ss_.Free(cholmod_rhs); - - if (cholmod_solution == NULL) { - summary.message = - "SuiteSparse failure. Unable to perform triangular solve."; - summary.termination_type = LINEAR_SOLVER_FAILURE; - return summary; - } - - VectorRef(solution, num_rows) - = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); - ss_.Free(cholmod_solution); - return summary; -#endif // CERES_NO_SUITESPARSE -} - -// Solve the system Sx = r, assuming that the matrix S is stored in a -// BlockRandomAccessSparseMatrix. The linear system is solved using -// CXSparse's sparse cholesky factorization routines. -LinearSolver::Summary -SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution) { -#ifdef CERES_NO_CXSPARSE - - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = "Ceres was not built with CXSparse support. " - "Therefore, SPARSE_SCHUR cannot be used with CX_SPARSE"; - return summary; - -#else - - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.message = "Success."; - - // Extract the TripletSparseMatrix that is used for actually storing S. - TripletSparseMatrix* tsm = - const_cast<TripletSparseMatrix*>( - down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); - const int num_rows = tsm->num_rows(); - - // The case where there are no f blocks, and the system is block - // diagonal. - if (num_rows == 0) { - return summary; - } - - cs_di* lhs = CHECK_NOTNULL(cxsparse_.CreateSparseMatrix(tsm)); - VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); - - // Compute symbolic factorization if not available. - if (cxsparse_factor_ == NULL) { - cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_); - } - - if (cxsparse_factor_ == NULL) { - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "CXSparse failure. Unable to find symbolic factorization."; - } else if (!cxsparse_.SolveCholesky(lhs, cxsparse_factor_, solution)) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "CXSparse::SolveCholesky failed."; - } - - cxsparse_.Free(lhs); - return summary; -#endif // CERES_NO_CXPARSE -} - -// Solve the system Sx = r, assuming that the matrix S is stored in a -// BlockRandomAccessSparseMatrix. The linear system is solved using -// Eigen's sparse cholesky factorization routines. -LinearSolver::Summary -SparseSchurComplementSolver::SolveReducedLinearSystemUsingEigen( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution) { -#ifndef CERES_USE_EIGEN_SPARSE - - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "SPARSE_SCHUR cannot be used with EIGEN_SPARSE. " - "Ceres was not built with support for " - "Eigen's SimplicialLDLT decomposition. " - "This requires enabling building with -DEIGENSPARSE=ON."; - return summary; - -#else - EventLogger event_logger("SchurComplementSolver::EigenSolve"); - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.message = "Success."; - - // Extract the TripletSparseMatrix that is used for actually storing S. - TripletSparseMatrix* tsm = - const_cast<TripletSparseMatrix*>( - down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); - const int num_rows = tsm->num_rows(); - - // The case where there are no f blocks, and the system is block - // diagonal. - if (num_rows == 0) { - return summary; - } - - // This is an upper triangular matrix. - CompressedRowSparseMatrix crsm(*tsm); - // Map this to a column major, lower triangular matrix. - Eigen::MappedSparseMatrix<double, Eigen::ColMajor> eigen_lhs( - crsm.num_rows(), - crsm.num_rows(), - crsm.num_nonzeros(), - crsm.mutable_rows(), - crsm.mutable_cols(), - crsm.mutable_values()); - event_logger.AddEvent("ToCompressedRowSparseMatrix"); - - // Compute symbolic factorization if one does not exist. - if (simplicial_ldlt_.get() == NULL) { - simplicial_ldlt_.reset(new SimplicialLDLT); - // This ordering is quite bad. The scalar ordering produced by the - // AMD algorithm is quite bad and can be an order of magnitude - // worse than the one computed using the block version of the - // algorithm. - simplicial_ldlt_->analyzePattern(eigen_lhs); - if (VLOG_IS_ON(2)) { - std::stringstream ss; - simplicial_ldlt_->dumpMemory(ss); - VLOG(2) << "Symbolic Analysis\n" - << ss.str(); - } - event_logger.AddEvent("Analysis"); - if (simplicial_ldlt_->info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "Eigen failure. Unable to find symbolic factorization."; - return summary; - } - } - - simplicial_ldlt_->factorize(eigen_lhs); - event_logger.AddEvent("Factorize"); - if (simplicial_ldlt_->info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "Eigen failure. Unable to find numeric factoriztion."; - return summary; - } - - VectorRef(solution, num_rows) = - simplicial_ldlt_->solve(ConstVectorRef(rhs(), num_rows)); - event_logger.AddEvent("Solve"); - if (simplicial_ldlt_->info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "Eigen failure. Unable to do triangular solve."; - } + *lhs->mutable_col_blocks() = blocks_; + *lhs->mutable_row_blocks() = blocks_; + summary.num_iterations = 1; + summary.termination_type = sparse_cholesky_->FactorAndSolve( + lhs.get(), rhs(), solution, &summary.message); return summary; -#endif // CERES_USE_EIGEN_SPARSE } LinearSolver::Summary SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution) { + const LinearSolver::PerSolveOptions& per_solve_options, double* solution) { + CHECK(options().use_explicit_schur_complement); const int num_rows = lhs()->num_rows(); // The case where there are no f blocks, and the system is block // diagonal. @@ -621,27 +379,24 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( preconditioner_.reset(new BlockRandomAccessDiagonalMatrix(blocks_)); } - BlockRandomAccessSparseMatrix* sc = - down_cast<BlockRandomAccessSparseMatrix*>( - const_cast<BlockRandomAccessMatrix*>(lhs())); + BlockRandomAccessSparseMatrix* sc = down_cast<BlockRandomAccessSparseMatrix*>( + const_cast<BlockRandomAccessMatrix*>(lhs())); // Extract block diagonal from the Schur complement to construct the // schur_jacobi preconditioner. - for (int i = 0; i < blocks_.size(); ++i) { + for (int i = 0; i < blocks_.size(); ++i) { const int block_size = blocks_[i]; int sc_r, sc_c, sc_row_stride, sc_col_stride; CellInfo* sc_cell_info = - CHECK_NOTNULL(sc->GetCell(i, i, - &sc_r, &sc_c, - &sc_row_stride, &sc_col_stride)); + sc->GetCell(i, i, &sc_r, &sc_c, &sc_row_stride, &sc_col_stride); + CHECK(sc_cell_info != nullptr); MatrixRef sc_m(sc_cell_info->values, sc_row_stride, sc_col_stride); int pre_r, pre_c, pre_row_stride, pre_col_stride; - CellInfo* pre_cell_info = CHECK_NOTNULL( - preconditioner_->GetCell(i, i, - &pre_r, &pre_c, - &pre_row_stride, &pre_col_stride)); + CellInfo* pre_cell_info = preconditioner_->GetCell( + i, i, &pre_r, &pre_c, &pre_row_stride, &pre_col_stride); + CHECK(pre_cell_info != nullptr); MatrixRef pre_m(pre_cell_info->values, pre_row_stride, pre_col_stride); pre_m.block(pre_r, pre_c, block_size, block_size) = @@ -651,12 +406,11 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( VectorRef(solution, num_rows).setZero(); - scoped_ptr<LinearOperator> lhs_adapter( + std::unique_ptr<LinearOperator> lhs_adapter( new BlockRandomAccessSparseMatrixAdapter(*sc)); - scoped_ptr<LinearOperator> preconditioner_adapter( + std::unique_ptr<LinearOperator> preconditioner_adapter( new BlockRandomAccessDiagonalMatrixAdapter(*preconditioner_)); - LinearSolver::Options cg_options; cg_options.min_num_iterations = options().min_num_iterations; cg_options.max_num_iterations = options().max_num_iterations; @@ -667,10 +421,8 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; cg_per_solve_options.preconditioner = preconditioner_adapter.get(); - return cg_solver.Solve(lhs_adapter.get(), - rhs(), - cg_per_solve_options, - solution); + return cg_solver.Solve( + lhs_adapter.get(), rhs(), cg_per_solve_options, solution); } } // namespace internal diff --git a/extern/ceres/internal/ceres/schur_complement_solver.h b/extern/ceres/internal/ceres/schur_complement_solver.h index 714dafc5b0c..87f04785794 100644 --- a/extern/ceres/internal/ceres/schur_complement_solver.h +++ b/extern/ceres/internal/ceres/schur_complement_solver.h @@ -31,22 +31,19 @@ #ifndef CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ #define CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ +#include <memory> #include <set> #include <utility> #include <vector> -#include "ceres/internal/port.h" - +#include "ceres/block_random_access_diagonal_matrix.h" #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" -#include "ceres/cxsparse.h" +#include "ceres/internal/port.h" #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" -#include "ceres/suitesparse.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" -#include "ceres/block_random_access_diagonal_matrix.h" #ifdef CERES_USE_EIGEN_SPARSE #include "Eigen/SparseCholesky" @@ -57,6 +54,7 @@ namespace ceres { namespace internal { class BlockSparseMatrix; +class SparseCholesky; // Base class for Schur complement based linear least squares // solvers. It assumes that the input linear system Ax = b can be @@ -96,33 +94,37 @@ class BlockSparseMatrix; // be used for problems with upto a few hundred cameras. // // SparseSchurComplementSolver: For problems where the Schur -// complement matrix is large and sparse. It requires that -// CHOLMOD/SuiteSparse be installed, as it uses CHOLMOD to find a -// sparse Cholesky factorization of the Schur complement. This solver -// can be used for solving structure from motion problems with tens of -// thousands of cameras, though depending on the exact sparsity -// structure, it maybe better to use an iterative solver. +// complement matrix is large and sparse. It requires that Ceres be +// build with at least one sparse linear algebra library, as it +// computes a sparse Cholesky factorization of the Schur complement. +// +// This solver can be used for solving structure from motion problems +// with tens of thousands of cameras, though depending on the exact +// sparsity structure, it maybe better to use an iterative solver. // // The two solvers can be instantiated by calling // LinearSolver::CreateLinearSolver with LinearSolver::Options::type // set to DENSE_SCHUR and SPARSE_SCHUR -// respectively. LinearSolver::Options::elimination_groups[0] should be -// at least 1. +// respectively. LinearSolver::Options::elimination_groups[0] should +// be at least 1. class SchurComplementSolver : public BlockSparseMatrixSolver { public: explicit SchurComplementSolver(const LinearSolver::Options& options) : options_(options) { CHECK_GT(options.elimination_groups.size(), 1); CHECK_GT(options.elimination_groups[0], 0); + CHECK(options.context != NULL); } + SchurComplementSolver(const SchurComplementSolver&) = delete; + void operator=(const SchurComplementSolver&) = delete; // LinearSolver methods virtual ~SchurComplementSolver() {} - virtual LinearSolver::Summary SolveImpl( + LinearSolver::Summary SolveImpl( BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, - double* x); + double* x) override; protected: const LinearSolver::Options& options() const { return options_; } @@ -140,11 +142,9 @@ class SchurComplementSolver : public BlockSparseMatrixSolver { LinearSolver::Options options_; - scoped_ptr<SchurEliminatorBase> eliminator_; - scoped_ptr<BlockRandomAccessMatrix> lhs_; - scoped_array<double> rhs_; - - CERES_DISALLOW_COPY_AND_ASSIGN(SchurComplementSolver); + std::unique_ptr<SchurEliminatorBase> eliminator_; + std::unique_ptr<BlockRandomAccessMatrix> lhs_; + std::unique_ptr<double[]> rhs_; }; // Dense Cholesky factorization based solver. @@ -152,72 +152,40 @@ class DenseSchurComplementSolver : public SchurComplementSolver { public: explicit DenseSchurComplementSolver(const LinearSolver::Options& options) : SchurComplementSolver(options) {} + DenseSchurComplementSolver(const DenseSchurComplementSolver&) = delete; + void operator=(const DenseSchurComplementSolver&) = delete; + virtual ~DenseSchurComplementSolver() {} private: - virtual void InitStorage(const CompressedRowBlockStructure* bs); - virtual LinearSolver::Summary SolveReducedLinearSystem( + void InitStorage(const CompressedRowBlockStructure* bs) final; + LinearSolver::Summary SolveReducedLinearSystem( const LinearSolver::PerSolveOptions& per_solve_options, - double* solution); - - CERES_DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); + double* solution) final; }; // Sparse Cholesky factorization based solver. class SparseSchurComplementSolver : public SchurComplementSolver { public: explicit SparseSchurComplementSolver(const LinearSolver::Options& options); + SparseSchurComplementSolver(const SparseSchurComplementSolver&) = delete; + void operator=(const SparseSchurComplementSolver&) = delete; + virtual ~SparseSchurComplementSolver(); private: - virtual void InitStorage(const CompressedRowBlockStructure* bs); - virtual LinearSolver::Summary SolveReducedLinearSystem( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution); - LinearSolver::Summary SolveReducedLinearSystemUsingSuiteSparse( + void InitStorage(const CompressedRowBlockStructure* bs) final; + LinearSolver::Summary SolveReducedLinearSystem( const LinearSolver::PerSolveOptions& per_solve_options, - double* solution); - LinearSolver::Summary SolveReducedLinearSystemUsingCXSparse( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution); - LinearSolver::Summary SolveReducedLinearSystemUsingEigen( - const LinearSolver::PerSolveOptions& per_solve_options, - double* solution); + double* solution) final; LinearSolver::Summary SolveReducedLinearSystemUsingConjugateGradients( const LinearSolver::PerSolveOptions& per_solve_options, double* solution); // Size of the blocks in the Schur complement. std::vector<int> blocks_; - - SuiteSparse ss_; - // Symbolic factorization of the reduced linear system. Precomputed - // once and reused in subsequent calls. - cholmod_factor* factor_; - - CXSparse cxsparse_; - // Cached factorization - cs_dis* cxsparse_factor_; - -#ifdef CERES_USE_EIGEN_SPARSE - - // The preprocessor gymnastics here are dealing with the fact that - // before version 3.2.2, Eigen did not support a third template - // parameter to specify the ordering. -#if EIGEN_VERSION_AT_LEAST(3,2,2) - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Lower, - Eigen::NaturalOrdering<int> > - SimplicialLDLT; -#else - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Lower> - SimplicialLDLT; -#endif - - scoped_ptr<SimplicialLDLT> simplicial_ldlt_; -#endif - - scoped_ptr<BlockRandomAccessDiagonalMatrix> preconditioner_; - CERES_DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); + std::unique_ptr<SparseCholesky> sparse_cholesky_; + std::unique_ptr<BlockRandomAccessDiagonalMatrix> preconditioner_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/schur_eliminator.cc b/extern/ceres/internal/ceres/schur_eliminator.cc index ec0e2a020e5..bc8ced4bc7c 100644 --- a/extern/ceres/internal/ceres/schur_eliminator.cc +++ b/extern/ceres/internal/ceres/schur_eliminator.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" @@ -50,106 +49,105 @@ namespace internal { SchurEliminatorBase* SchurEliminatorBase::Create(const LinearSolver::Options& options) { #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == 2)) { - return new SchurEliminator<2, 2, 2>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == 3)) { - return new SchurEliminator<2, 2, 3>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == 4)) { - return new SchurEliminator<2, 2, 4>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 2) && - (options.f_block_size == Eigen::Dynamic)) { - return new SchurEliminator<2, 2, Eigen::Dynamic>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 3)) { - return new SchurEliminator<2, 3, 3>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 4)) { - return new SchurEliminator<2, 3, 4>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 6)) { - return new SchurEliminator<2, 3, 6>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == 9)) { - return new SchurEliminator<2, 3, 9>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 3) && - (options.f_block_size == Eigen::Dynamic)) { - return new SchurEliminator<2, 3, Eigen::Dynamic>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 3)) { - return new SchurEliminator<2, 4, 3>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 4)) { - return new SchurEliminator<2, 4, 4>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 8)) { - return new SchurEliminator<2, 4, 8>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == 9)) { - return new SchurEliminator<2, 4, 9>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == 4) && - (options.f_block_size == Eigen::Dynamic)) { - return new SchurEliminator<2, 4, Eigen::Dynamic>(options); - } - if ((options.row_block_size == 2) && - (options.e_block_size == Eigen::Dynamic) && - (options.f_block_size == Eigen::Dynamic)) { - return new SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>(options); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == 2)) { - return new SchurEliminator<4, 4, 2>(options); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == 3)) { - return new SchurEliminator<4, 4, 3>(options); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == 4)) { - return new SchurEliminator<4, 4, 4>(options); - } - if ((options.row_block_size == 4) && - (options.e_block_size == 4) && - (options.f_block_size == Eigen::Dynamic)) { - return new SchurEliminator<4, 4, Eigen::Dynamic>(options); - } - if ((options.row_block_size == Eigen::Dynamic) && - (options.e_block_size == Eigen::Dynamic) && - (options.f_block_size == Eigen::Dynamic)) { - return new SchurEliminator<Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>(options); - } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + return new SchurEliminator<2, 2, 2>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 2, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 2, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2)) { + return new SchurEliminator<2, 2, Eigen::Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 3, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 3, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 6)) { + return new SchurEliminator<2, 3, 6>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + return new SchurEliminator<2, 3, 9>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3)) { + return new SchurEliminator<2, 3, Eigen::Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 4, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 4, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 6)) { + return new SchurEliminator<2, 4, 6>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 8)) { + return new SchurEliminator<2, 4, 8>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 9)) { + return new SchurEliminator<2, 4, 9>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4)) { + return new SchurEliminator<2, 4, Eigen::Dynamic>(options); + } + if (options.row_block_size == 2){ + return new SchurEliminator<2, Eigen::Dynamic, Eigen::Dynamic>(options); + } + if ((options.row_block_size == 3) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new SchurEliminator<3, 3, 3>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + return new SchurEliminator<4, 4, 2>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<4, 4, 3>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<4, 4, 4>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4)) { + return new SchurEliminator<4, 4, Eigen::Dynamic>(options); + } #endif VLOG(1) << "Template specializations not found for <" diff --git a/extern/ceres/internal/ceres/schur_eliminator.h b/extern/ceres/internal/ceres/schur_eliminator.h index 761b58adc7f..66fcb4d58e8 100644 --- a/extern/ceres/internal/ceres/schur_eliminator.h +++ b/extern/ceres/internal/ceres/schur_eliminator.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -32,14 +32,17 @@ #define CERES_INTERNAL_SCHUR_ELIMINATOR_H_ #include <map> +#include <memory> +#include <mutex> #include <vector> -#include "ceres/mutex.h" + +#include "Eigen/Dense" #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" -#include "ceres/linear_solver.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/port.h" +#include "ceres/linear_solver.h" namespace ceres { namespace internal { @@ -50,7 +53,7 @@ namespace internal { // // E y + F z = b // -// Where x = [y;z] is a partition of the variables. The paritioning +// Where x = [y;z] is a partition of the variables. The partitioning // of the variables is such that, E'E is a block diagonal matrix. Or // in other words, the parameter blocks in E form an independent set // of the of the graph implied by the block matrix A'A. Then, this @@ -147,7 +150,7 @@ namespace internal { // Where the sum is over chunks and E_k'E_k is dense matrix of size y1 // x y1. // -// Advanced usage. Uptil now it has been assumed that the user would +// Advanced usage. Until now it has been assumed that the user would // be interested in all of the Schur Complement S. However, it is also // possible to use this eliminator to obtain an arbitrary submatrix of // the full Schur complement. When the eliminator is generating the @@ -171,7 +174,14 @@ class SchurEliminatorBase { // CompressedRowBlockStructure object passed to this method is the // same one (or is equivalent to) the one associated with the // BlockSparseMatrix objects below. + // + // assume_full_rank_ete controls how the eliminator inverts with the + // diagonal blocks corresponding to e blocks in A'A. If + // assume_full_rank_ete is true, then a Cholesky factorization is + // used to compute the inverse, otherwise a singular value + // decomposition is used to compute the pseudo inverse. virtual void Init(int num_eliminate_blocks, + bool assume_full_rank_ete, const CompressedRowBlockStructure* bs) = 0; // Compute the Schur complement system from the augmented linear @@ -185,7 +195,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 BlockSparseMatrix* A, + virtual void Eliminate(const BlockSparseMatrixData& A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, @@ -194,7 +204,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 BlockSparseMatrix* A, + virtual void BackSubstitute(const BlockSparseMatrixData& A, const double* b, const double* D, const double* z, @@ -210,32 +220,31 @@ class SchurEliminatorBase { // parameters as template arguments so that they are visible to the // compiler and can be used for compile time optimization of the low // level linear algebra routines. -// -// This implementation is mulithreaded using OpenMP. The level of -// parallelism is controlled by LinearSolver::Options::num_threads. template <int kRowBlockSize = Eigen::Dynamic, int kEBlockSize = Eigen::Dynamic, - int kFBlockSize = Eigen::Dynamic > + int kFBlockSize = Eigen::Dynamic> class SchurEliminator : public SchurEliminatorBase { public: explicit SchurEliminator(const LinearSolver::Options& options) - : num_threads_(options.num_threads) { + : num_threads_(options.num_threads), context_(options.context) { + CHECK(context_ != nullptr); } // SchurEliminatorBase Interface virtual ~SchurEliminator(); - virtual void Init(int num_eliminate_blocks, - const CompressedRowBlockStructure* bs); - virtual void Eliminate(const BlockSparseMatrix* A, - const double* b, - const double* D, - BlockRandomAccessMatrix* lhs, - double* rhs); - virtual void BackSubstitute(const BlockSparseMatrix* A, - const double* b, - const double* D, - const double* z, - double* y); + void Init(int num_eliminate_blocks, + bool assume_full_rank_ete, + const CompressedRowBlockStructure* bs) final; + void Eliminate(const BlockSparseMatrixData& A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) final; + void BackSubstitute(const BlockSparseMatrixData& A, + const double* b, + const double* D, + const double* z, + double* y) final; private: // Chunk objects store combinatorial information needed to @@ -273,7 +282,7 @@ class SchurEliminator : public SchurEliminatorBase { void ChunkDiagonalBlockAndGradient( const Chunk& chunk, - const BlockSparseMatrix* A, + const BlockSparseMatrixData& A, const double* b, int row_block_counter, typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* eet, @@ -282,33 +291,36 @@ class SchurEliminator : public SchurEliminatorBase { BlockRandomAccessMatrix* lhs); void UpdateRhs(const Chunk& chunk, - const BlockSparseMatrix* A, + const BlockSparseMatrixData& A, const double* b, int row_block_counter, const double* inverse_ete_g, double* rhs); - void ChunkOuterProduct(const CompressedRowBlockStructure* bs, + void ChunkOuterProduct(int thread_id, + const CompressedRowBlockStructure* bs, const Matrix& inverse_eet, const double* buffer, const BufferLayoutType& buffer_layout, BlockRandomAccessMatrix* lhs); - void EBlockRowOuterProduct(const BlockSparseMatrix* A, + void EBlockRowOuterProduct(const BlockSparseMatrixData& A, int row_block_index, BlockRandomAccessMatrix* lhs); + void NoEBlockRowsUpdate(const BlockSparseMatrixData& A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs); - void NoEBlockRowsUpdate(const BlockSparseMatrix* A, - const double* b, - int row_block_counter, - BlockRandomAccessMatrix* lhs, - double* rhs); - - void NoEBlockRowOuterProduct(const BlockSparseMatrix* A, + void NoEBlockRowOuterProduct(const BlockSparseMatrixData& A, int row_block_index, BlockRandomAccessMatrix* lhs); + int num_threads_; + ContextImpl* context_; int num_eliminate_blocks_; + bool assume_full_rank_ete_; // Block layout of the columns of the reduced linear system. Since // the f blocks can be of varying size, this vector stores the @@ -330,7 +342,7 @@ class SchurEliminator : public SchurEliminatorBase { // // [thread_id * buffer_size_ , (thread_id + 1) * buffer_size_] // - scoped_array<double> buffer_; + std::unique_ptr<double[]> buffer_; // Buffer to store per thread matrix matrix products used by // ChunkOuterProduct. Like buffer_ it is of size num_threads * @@ -338,15 +350,275 @@ class SchurEliminator : public SchurEliminatorBase { // // [thread_id * buffer_size_ , (thread_id + 1) * buffer_size_ -1] // - scoped_array<double> chunk_outer_product_buffer_; + std::unique_ptr<double[]> chunk_outer_product_buffer_; int buffer_size_; - int num_threads_; int uneliminated_row_begins_; // Locks for the blocks in the right hand side of the reduced linear // system. - std::vector<Mutex*> rhs_locks_; + std::vector<std::mutex*> rhs_locks_; +}; + +// SchurEliminatorForOneFBlock specializes the SchurEliminatorBase interface for +// the case where there is exactly one f-block and all three parameters +// kRowBlockSize, kEBlockSize and KFBlockSize are known at compile time. This is +// the case for some two view bundle adjustment problems which have very +// stringent latency requirements. +// +// Under these assumptions, we can simplify the more general algorithm +// implemented by SchurEliminatorImpl significantly. Two of the major +// contributors to the increased performance are: +// +// 1. Simpler loop structure and less use of dynamic memory. Almost everything +// is on the stack and benefits from aligned memory as well as fixed sized +// vectorization. We are also able to reason about temporaries and control +// their lifetimes better. +// 2. Use of inverse() over llt().solve(Identity). +template <int kRowBlockSize = Eigen::Dynamic, + int kEBlockSize = Eigen::Dynamic, + int kFBlockSize = Eigen::Dynamic> +class SchurEliminatorForOneFBlock : public SchurEliminatorBase { + public: + virtual ~SchurEliminatorForOneFBlock() {} + void Init(int num_eliminate_blocks, + bool assume_full_rank_ete, + const CompressedRowBlockStructure* bs) override { + CHECK_GT(num_eliminate_blocks, 0) + << "SchurComplementSolver cannot be initialized with " + << "num_eliminate_blocks = 0."; + CHECK_EQ(bs->cols.size() - num_eliminate_blocks, 1); + + num_eliminate_blocks_ = num_eliminate_blocks; + const int num_row_blocks = bs->rows.size(); + chunks_.clear(); + int r = 0; + // Iterate over the row blocks of A, and detect the chunks. The + // matrix should already have been ordered so that all rows + // containing the same y block are vertically contiguous. + while (r < num_row_blocks) { + const int e_block_id = bs->rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks_) { + break; + } + + chunks_.push_back(Chunk()); + Chunk& chunk = chunks_.back(); + chunk.num_rows = 0; + chunk.start = r; + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + while (r + chunk.num_rows < num_row_blocks) { + const CompressedRow& row = bs->rows[r + chunk.num_rows]; + if (row.cells.front().block_id != e_block_id) { + break; + } + ++chunk.num_rows; + } + r += chunk.num_rows; + } + + const Chunk& last_chunk = chunks_.back(); + uneliminated_row_begins_ = last_chunk.start + last_chunk.num_rows; + e_t_e_inverse_matrices_.resize(kEBlockSize * kEBlockSize * chunks_.size()); + std::fill( + e_t_e_inverse_matrices_.begin(), e_t_e_inverse_matrices_.end(), 0.0); + } + + void Eliminate(const BlockSparseMatrixData& A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs_bram, + double* rhs_ptr) override { + // Since there is only one f-block, we can call GetCell once, and cache its + // output. + int r, c, row_stride, col_stride; + CellInfo* cell_info = + lhs_bram->GetCell(0, 0, &r, &c, &row_stride, &col_stride); + typename EigenTypes<kFBlockSize, kFBlockSize>::MatrixRef lhs( + cell_info->values, kFBlockSize, kFBlockSize); + typename EigenTypes<kFBlockSize>::VectorRef rhs(rhs_ptr, kFBlockSize); + + lhs.setZero(); + rhs.setZero(); + + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); + + // Add the diagonal to the schur complement. + if (D != nullptr) { + typename EigenTypes<kFBlockSize>::ConstVectorRef diag( + D + bs->cols[num_eliminate_blocks_].position, kFBlockSize); + lhs.diagonal() = diag.array().square().matrix(); + } + + Eigen::Matrix<double, kEBlockSize, kFBlockSize> tmp; + Eigen::Matrix<double, kEBlockSize, 1> tmp2; + + // The following loop works on a block matrix which looks as follows + // (number of rows can be anything): + // + // [e_1 | f_1] = [b1] + // [e_2 | f_2] = [b2] + // [e_3 | f_3] = [b3] + // [e_4 | f_4] = [b4] + // + // and computes the following + // + // e_t_e = sum_i e_i^T * e_i + // e_t_e_inverse = inverse(e_t_e) + // e_t_f = sum_i e_i^T f_i + // e_t_b = sum_i e_i^T b_i + // f_t_b = sum_i f_i^T b_i + // + // lhs += sum_i f_i^T * f_i - e_t_f^T * e_t_e_inverse * e_t_f + // rhs += f_t_b - e_t_f^T * e_t_e_inverse * e_t_b + for (int i = 0; i < chunks_.size(); ++i) { + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + + // Naming covention, e_t_e = e_block.transpose() * e_block; + Eigen::Matrix<double, kEBlockSize, kEBlockSize> e_t_e; + Eigen::Matrix<double, kEBlockSize, kFBlockSize> e_t_f; + Eigen::Matrix<double, kEBlockSize, 1> e_t_b; + Eigen::Matrix<double, kFBlockSize, 1> f_t_b; + + // Add the square of the diagonal to e_t_e. + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef diag( + D + bs->cols[e_block_id].position, kEBlockSize); + e_t_e = diag.array().square().matrix().asDiagonal(); + } else { + e_t_e.setZero(); + } + e_t_f.setZero(); + e_t_b.setZero(); + f_t_b.setZero(); + + for (int j = 0; j < chunk.num_rows; ++j) { + const int row_id = chunk.start + j; + const auto& row = bs->rows[row_id]; + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(values + row.cells[0].position, kRowBlockSize, kEBlockSize); + const typename EigenTypes<kRowBlockSize>::ConstVectorRef b_block( + b + row.block.position, kRowBlockSize); + + e_t_e.noalias() += e_block.transpose() * e_block; + e_t_b.noalias() += e_block.transpose() * b_block; + + if (row.cells.size() == 1) { + // There is no f block, so there is nothing more to do. + continue; + } + + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block(values + row.cells[1].position, kRowBlockSize, kFBlockSize); + e_t_f.noalias() += e_block.transpose() * f_block; + lhs.noalias() += f_block.transpose() * f_block; + f_t_b.noalias() += f_block.transpose() * b_block; + } + + // BackSubstitute computes the same inverse, and this is the key workload + // there, so caching these inverses makes BackSubstitute essentially free. + typename EigenTypes<kEBlockSize, kEBlockSize>::MatrixRef e_t_e_inverse( + &e_t_e_inverse_matrices_[kEBlockSize * kEBlockSize * i], + kEBlockSize, + kEBlockSize); + + // e_t_e is a symmetric positive definite matrix, so the standard way to + // compute its inverse is via the Cholesky factorization by calling + // e_t_e.llt().solve(Identity()). However, the inverse() method even + // though it is not optimized for symmetric matrices is significantly + // faster for small fixed size (up to 4x4) matrices. + // + // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title3 + e_t_e_inverse.noalias() = e_t_e.inverse(); + + // The use of these two pre-allocated tmp vectors saves temporaries in the + // expressions for lhs and rhs updates below and has a significant impact + // on the performance of this method. + tmp.noalias() = e_t_e_inverse * e_t_f; + tmp2.noalias() = e_t_e_inverse * e_t_b; + + lhs.noalias() -= e_t_f.transpose() * tmp; + rhs.noalias() += f_t_b - e_t_f.transpose() * tmp2; + } + + // The rows without any e-blocks can have arbitrary size but only contain + // the f-block. + // + // lhs += f_i^T f_i + // rhs += f_i^T b_i + for (int row_id = uneliminated_row_begins_; row_id < bs->rows.size(); + ++row_id) { + const auto& row = bs->rows[row_id]; + const auto& cell = row.cells[0]; + const typename EigenTypes<Eigen::Dynamic, kFBlockSize>::ConstMatrixRef + f_block(values + cell.position, row.block.size, kFBlockSize); + const typename EigenTypes<Eigen::Dynamic>::ConstVectorRef b_block( + b + row.block.position, row.block.size); + lhs.noalias() += f_block.transpose() * f_block; + rhs.noalias() += f_block.transpose() * b_block; + } + } + + // This implementation of BackSubstitute depends on Eliminate being called + // before this. SchurComplementSolver always does this. + // + // y_i = e_t_e_inverse * sum_i e_i^T * (b_i - f_i * z); + void BackSubstitute(const BlockSparseMatrixData& A, + const double* b, + const double* D, + const double* z_ptr, + double* y) override { + typename EigenTypes<kFBlockSize>::ConstVectorRef z(z_ptr, kFBlockSize); + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); + Eigen::Matrix<double, kEBlockSize, 1> tmp; + for (int i = 0; i < chunks_.size(); ++i) { + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + tmp.setZero(); + for (int j = 0; j < chunk.num_rows; ++j) { + const int row_id = chunk.start + j; + const auto& row = bs->rows[row_id]; + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(values + row.cells[0].position, kRowBlockSize, kEBlockSize); + const typename EigenTypes<kRowBlockSize>::ConstVectorRef b_block( + b + row.block.position, kRowBlockSize); + + if (row.cells.size() == 1) { + // There is no f block. + tmp += e_block.transpose() * b_block; + } else { + typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block( + values + row.cells[1].position, kRowBlockSize, kFBlockSize); + tmp += e_block.transpose() * (b_block - f_block * z); + } + } + + typename EigenTypes<kEBlockSize, kEBlockSize>::MatrixRef e_t_e_inverse( + &e_t_e_inverse_matrices_[kEBlockSize * kEBlockSize * i], + kEBlockSize, + kEBlockSize); + + typename EigenTypes<kEBlockSize>::VectorRef y_block( + y + bs->cols[e_block_id].position, kEBlockSize); + y_block.noalias() = e_t_e_inverse * tmp; + } + } + + private: + struct Chunk { + int start = 0; + int num_rows = 0; + }; + + std::vector<Chunk> chunks_; + int num_eliminate_blocks_; + int uneliminated_row_begins_; + std::vector<double> e_t_e_inverse_matrices_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/schur_eliminator_impl.h b/extern/ceres/internal/ceres/schur_eliminator_impl.h index f2535880f15..bd0881eec1e 100644 --- a/extern/ceres/internal/ceres/schur_eliminator_impl.h +++ b/extern/ceres/internal/ceres/schur_eliminator_impl.h @@ -48,23 +48,23 @@ // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" -#ifdef CERES_USE_OPENMP -#include <omp.h> -#endif - #include <algorithm> #include <map> + +#include "Eigen/Dense" #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/invert_psd_matrix.h" #include "ceres/map_util.h" +#include "ceres/parallel_for.h" #include "ceres/schur_eliminator.h" +#include "ceres/scoped_thread_token.h" #include "ceres/small_blas.h" #include "ceres/stl_util.h" -#include "Eigen/Dense" +#include "ceres/thread_token_provider.h" #include "glog/logging.h" namespace ceres { @@ -76,14 +76,16 @@ SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::~SchurEliminator() { } template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> -void -SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { +void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::Init( + int num_eliminate_blocks, + bool assume_full_rank_ete, + const CompressedRowBlockStructure* bs) { CHECK_GT(num_eliminate_blocks, 0) << "SchurComplementSolver cannot be initialized with " << "num_eliminate_blocks = 0."; num_eliminate_blocks_ = num_eliminate_blocks; + assume_full_rank_ete_ = assume_full_rank_ete; const int num_col_blocks = bs->cols.size(); const int num_row_blocks = bs->rows.size(); @@ -102,6 +104,13 @@ Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { lhs_num_rows += bs->cols[i].size; } + // TODO(sameeragarwal): Now that we may have subset block structure, + // we need to make sure that we account for the fact that somep + // point blocks only have a "diagonal" row and nothing more. + // + // This likely requires a slightly different algorithm, which works + // off of the number of elimination blocks. + int r = 0; // Iterate over the row blocks of A, and detect the chunks. The // matrix should already have been ordered so that all rows @@ -143,15 +152,12 @@ Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { ++chunk.size; } - CHECK_GT(chunk.size, 0); + CHECK_GT(chunk.size, 0); // This check will need to be resolved. r += chunk.size; } const Chunk& chunk = chunks_.back(); uneliminated_row_begins_ = chunk.start + chunk.size; - if (num_threads_ > 1) { - random_shuffle(chunks_.begin(), chunks_.end()); - } buffer_.reset(new double[buffer_size_ * num_threads_]); @@ -163,46 +169,51 @@ Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { STLDeleteElements(&rhs_locks_); rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_); for (int i = 0; i < num_col_blocks - num_eliminate_blocks_; ++i) { - rhs_locks_[i] = new Mutex; + rhs_locks_[i] = new std::mutex; } } template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -Eliminate(const BlockSparseMatrix* A, +Eliminate(const BlockSparseMatrixData& A, const double* b, const double* D, BlockRandomAccessMatrix* lhs, double* rhs) { if (lhs->num_rows() > 0) { lhs->SetZero(); - VectorRef(rhs, lhs->num_rows()).setZero(); + if (rhs) { + VectorRef(rhs, lhs->num_rows()).setZero(); + } } - const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRowBlockStructure* bs = A.block_structure(); const int num_col_blocks = bs->cols.size(); // Add the diagonal to the schur complement. if (D != NULL) { -#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) - for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { - const int block_id = i - num_eliminate_blocks_; - int r, c, row_stride, col_stride; - CellInfo* cell_info = lhs->GetCell(block_id, block_id, - &r, &c, - &row_stride, &col_stride); - if (cell_info != NULL) { - const int block_size = bs->cols[i].size; - typename EigenTypes<Eigen::Dynamic>::ConstVectorRef - diag(D + bs->cols[i].position, block_size); - - CeresMutexLock l(&cell_info->m); - MatrixRef m(cell_info->values, row_stride, col_stride); - m.block(r, c, block_size, block_size).diagonal() - += diag.array().square().matrix(); - } - } + ParallelFor( + context_, + num_eliminate_blocks_, + num_col_blocks, + num_threads_, + [&](int i) { + const int block_id = i - num_eliminate_blocks_; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block_id, block_id, &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + const int block_size = bs->cols[i].size; + typename EigenTypes<Eigen::Dynamic>::ConstVectorRef diag( + D + bs->cols[i].position, block_size); + + std::lock_guard<std::mutex> l(cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() += + diag.array().square().matrix(); + } + }); } // Eliminate y blocks one chunk at a time. For each chunk, compute @@ -218,79 +229,78 @@ Eliminate(const BlockSparseMatrix* A, // z blocks that share a row block/residual term with the y // block. EliminateRowOuterProduct does the corresponding operation // for the lhs of the reduced linear system. -#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) - for (int i = 0; i < chunks_.size(); ++i) { -#ifdef CERES_USE_OPENMP - int thread_id = omp_get_thread_num(); -#else - int thread_id = 0; -#endif - double* buffer = buffer_.get() + thread_id * buffer_size_; - const Chunk& chunk = chunks_[i]; - const int e_block_id = bs->rows[chunk.start].cells.front().block_id; - const int e_block_size = bs->cols[e_block_id].size; - - VectorRef(buffer, buffer_size_).setZero(); - - typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix - ete(e_block_size, e_block_size); + ParallelFor( + context_, + 0, + int(chunks_.size()), + num_threads_, + [&](int thread_id, int i) { + double* buffer = buffer_.get() + thread_id * buffer_size_; + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + VectorRef(buffer, buffer_size_).setZero(); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } - if (D != NULL) { - const typename EigenTypes<kEBlockSize>::ConstVectorRef - diag(D + bs->cols[e_block_id].position, e_block_size); - ete = diag.array().square().matrix().asDiagonal(); - } else { - ete.setZero(); - } + FixedArray<double, 8> g(e_block_size); + typename EigenTypes<kEBlockSize>::VectorRef gref(g.data(), + e_block_size); + gref.setZero(); + + // We are going to be computing + // + // S += F'F - F'E(E'E)^{-1}E'F + // + // for each Chunk. The computation is broken down into a number of + // function calls as below. + + // Compute the outer product of the e_blocks with themselves (ete + // = E'E). Compute the product of the e_blocks with the + // corresponding f_blocks (buffer = E'F), the gradient of the terms + // in this chunk (g) and add the outer product of the f_blocks to + // Schur complement (S += F'F). + ChunkDiagonalBlockAndGradient( + chunk, A, b, chunk.start, &ete, g.data(), buffer, lhs); + + // Normally one wouldn't compute the inverse explicitly, but + // e_block_size will typically be a small number like 3, in + // which case its much faster to compute the inverse once and + // use it to multiply other matrices/vectors instead of doing a + // Solve call over and over again. + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete = + InvertPSDMatrix<kEBlockSize>(assume_full_rank_ete_, ete); + + // For the current chunk compute and update the rhs of the reduced + // linear system. + // + // rhs = F'b - F'E(E'E)^(-1) E'b + + if (rhs) { + FixedArray<double, 8> inverse_ete_g(e_block_size); + MatrixVectorMultiply<kEBlockSize, kEBlockSize, 0>( + inverse_ete.data(), + e_block_size, + e_block_size, + g.data(), + inverse_ete_g.data()); + UpdateRhs(chunk, A, b, chunk.start, inverse_ete_g.data(), rhs); + } - FixedArray<double, 8> g(e_block_size); - typename EigenTypes<kEBlockSize>::VectorRef gref(g.get(), e_block_size); - gref.setZero(); - - // We are going to be computing - // - // S += F'F - F'E(E'E)^{-1}E'F - // - // for each Chunk. The computation is broken down into a number of - // function calls as below. - - // Compute the outer product of the e_blocks with themselves (ete - // = E'E). Compute the product of the e_blocks with the - // corresonding f_blocks (buffer = E'F), the gradient of the terms - // in this chunk (g) and add the outer product of the f_blocks to - // Schur complement (S += F'F). - ChunkDiagonalBlockAndGradient( - chunk, A, b, chunk.start, &ete, g.get(), buffer, lhs); - - // Normally one wouldn't compute the inverse explicitly, but - // e_block_size will typically be a small number like 3, in - // which case its much faster to compute the inverse once and - // use it to multiply other matrices/vectors instead of doing a - // Solve call over and over again. - typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete = - ete - .template selfadjointView<Eigen::Upper>() - .llt() - .solve(Matrix::Identity(e_block_size, e_block_size)); - - // For the current chunk compute and update the rhs of the reduced - // linear system. - // - // rhs = F'b - F'E(E'E)^(-1) E'b - - FixedArray<double, 8> inverse_ete_g(e_block_size); - MatrixVectorMultiply<kEBlockSize, kEBlockSize, 0>( - inverse_ete.data(), - e_block_size, - e_block_size, - g.get(), - inverse_ete_g.get()); - - UpdateRhs(chunk, A, b, chunk.start, inverse_ete_g.get(), rhs); - - // S -= F'E(E'E)^{-1}E'F - ChunkOuterProduct(bs, inverse_ete, buffer, chunk.buffer_layout, lhs); - } + // S -= F'E(E'E)^{-1}E'F + ChunkOuterProduct( + thread_id, bs, inverse_ete, buffer, chunk.buffer_layout, lhs); + }); // For rows with no e_blocks, the schur complement update reduces to // S += F'F. @@ -300,19 +310,25 @@ Eliminate(const BlockSparseMatrix* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -BackSubstitute(const BlockSparseMatrix* A, +BackSubstitute(const BlockSparseMatrixData& A, const double* b, const double* D, const double* z, double* y) { - const CompressedRowBlockStructure* bs = A->block_structure(); -#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) - for (int i = 0; i < chunks_.size(); ++i) { + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); + + ParallelFor( + context_, + 0, + int(chunks_.size()), + num_threads_, + [&](int i) { const Chunk& chunk = chunks_[i]; const int e_block_id = bs->rows[chunk.start].cells.front().block_id; const int e_block_size = bs->cols[e_block_id].size; - double* y_ptr = y + bs->cols[e_block_id].position; + double* y_ptr = y + bs->cols[e_block_id].position; typename EigenTypes<kEBlockSize>::VectorRef y_block(y_ptr, e_block_size); typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix @@ -325,7 +341,6 @@ BackSubstitute(const BlockSparseMatrix* 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 Cell& e_cell = row.cells.front(); @@ -333,9 +348,9 @@ BackSubstitute(const BlockSparseMatrix* A, FixedArray<double, 8> sj(row.block.size); - typename EigenTypes<kRowBlockSize>::VectorRef(sj.get(), row.block.size) = - typename EigenTypes<kRowBlockSize>::ConstVectorRef - (b + bs->rows[chunk.start + j].block.position, row.block.size); + typename EigenTypes<kRowBlockSize>::VectorRef(sj.data(), row.block.size) = + typename EigenTypes<kRowBlockSize>::ConstVectorRef( + b + bs->rows[chunk.start + j].block.position, row.block.size); for (int c = 1; c < row.cells.size(); ++c) { const int f_block_id = row.cells[c].block_id; @@ -345,23 +360,24 @@ BackSubstitute(const BlockSparseMatrix* A, MatrixVectorMultiply<kRowBlockSize, kFBlockSize, -1>( values + row.cells[c].position, row.block.size, f_block_size, z + lhs_row_layout_[r_block], - sj.get()); + sj.data()); } MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( values + e_cell.position, row.block.size, e_block_size, - sj.get(), + sj.data(), y_ptr); MatrixTransposeMatrixMultiply <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>( - 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); + 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); } - ete.llt().solveInPlace(y_block); - } + y_block = + InvertPSDMatrix<kEBlockSize>(assume_full_rank_ete_, ete) * y_block; + }); } // Update the rhs of the reduced linear system. Compute @@ -371,17 +387,17 @@ template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: UpdateRhs(const Chunk& chunk, - const BlockSparseMatrix* A, + const BlockSparseMatrixData& A, const double* b, int row_block_counter, const double* inverse_ete_g, double* rhs) { - const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; 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 Cell& e_cell = row.cells.front(); @@ -398,7 +414,7 @@ UpdateRhs(const Chunk& chunk, 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_; - CeresMutexLock l(rhs_locks_[block]); + std::lock_guard<std::mutex> l(*rhs_locks_[block]); MatrixTransposeVectorMultiply<kRowBlockSize, kFBlockSize, 1>( values + row.cells[c].position, row.block.size, block_size, @@ -432,14 +448,15 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: ChunkDiagonalBlockAndGradient( const Chunk& chunk, - const BlockSparseMatrix* A, + const BlockSparseMatrixData& A, const double* b, int row_block_counter, typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete, double* g, double* buffer, BlockRandomAccessMatrix* lhs) { - const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); int b_pos = bs->rows[row_block_counter].block.position; const int e_block_size = ete->rows(); @@ -448,7 +465,6 @@ 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]; @@ -464,12 +480,13 @@ ChunkDiagonalBlockAndGradient( 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>( - values + e_cell.position, row.block.size, e_block_size, - b + b_pos, - g); - + if (b) { + // g += E_i' b_i + MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>( + values + e_cell.position, row.block.size, e_block_size, + b + b_pos, + g); + } // buffer = E'F. This computation is done by iterating over the // f_blocks for each row in the chunk. @@ -495,7 +512,8 @@ ChunkDiagonalBlockAndGradient( template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -ChunkOuterProduct(const CompressedRowBlockStructure* bs, +ChunkOuterProduct(int thread_id, + const CompressedRowBlockStructure* bs, const Matrix& inverse_ete, const double* buffer, const BufferLayoutType& buffer_layout, @@ -507,11 +525,6 @@ ChunkOuterProduct(const CompressedRowBlockStructure* bs, const int e_block_size = inverse_ete.rows(); BufferLayoutType::const_iterator it1 = buffer_layout.begin(); -#ifdef CERES_USE_OPENMP - int thread_id = omp_get_thread_num(); -#else - int thread_id = 0; -#endif double* b1_transpose_inverse_ete = chunk_outer_product_buffer_.get() + thread_id * buffer_size_; @@ -535,7 +548,7 @@ ChunkOuterProduct(const CompressedRowBlockStructure* bs, &row_stride, &col_stride); if (cell_info != NULL) { const int block2_size = bs->cols[it2->first].size; - CeresMutexLock l(&cell_info->m); + std::lock_guard<std::mutex> l(cell_info->m); MatrixMatrixMultiply <kFBlockSize, kEBlockSize, kEBlockSize, kFBlockSize, -1>( b1_transpose_inverse_ete, block1_size, e_block_size, @@ -552,14 +565,18 @@ ChunkOuterProduct(const CompressedRowBlockStructure* bs, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -NoEBlockRowsUpdate(const BlockSparseMatrix* A, +NoEBlockRowsUpdate(const BlockSparseMatrixData& A, const double* b, int row_block_counter, BlockRandomAccessMatrix* lhs, double* rhs) { - const CompressedRowBlockStructure* bs = A->block_structure(); - const double* values = A->values(); + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); for (; row_block_counter < bs->rows.size(); ++row_block_counter) { + NoEBlockRowOuterProduct(A, row_block_counter, lhs); + if (!rhs) { + continue; + } const CompressedRow& row = bs->rows[row_block_counter]; for (int c = 0; c < row.cells.size(); ++c) { const int block_id = row.cells[c].block_id; @@ -570,7 +587,6 @@ NoEBlockRowsUpdate(const BlockSparseMatrix* A, b + row.block.position, rhs + lhs_row_layout_[block]); } - NoEBlockRowOuterProduct(A, row_block_counter, lhs); } } @@ -582,7 +598,7 @@ NoEBlockRowsUpdate(const BlockSparseMatrix* A, // one difference. It does not use any of the template // parameters. This is because the algorithm used for detecting the // static structure of the matrix A only pays attention to rows with -// e_blocks. This is becase rows without e_blocks are rare and +// e_blocks. This is because rows without e_blocks are rare and // typically arise from regularization terms in the original // optimization problem, and have a very different structure than the // rows with e_blocks. Including them in the static structure @@ -592,12 +608,13 @@ NoEBlockRowsUpdate(const BlockSparseMatrix* A, template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -NoEBlockRowOuterProduct(const BlockSparseMatrix* A, +NoEBlockRowOuterProduct(const BlockSparseMatrixData& A, int row_block_index, BlockRandomAccessMatrix* lhs) { - const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); + const CompressedRow& row = bs->rows[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); @@ -608,7 +625,7 @@ NoEBlockRowOuterProduct(const BlockSparseMatrix* A, &r, &c, &row_stride, &col_stride); if (cell_info != NULL) { - CeresMutexLock l(&cell_info->m); + std::lock_guard<std::mutex> l(cell_info->m); // This multiply currently ignores the fact that this is a // symmetric outer product. MatrixTransposeMatrixMultiply @@ -628,7 +645,7 @@ NoEBlockRowOuterProduct(const BlockSparseMatrix* A, &row_stride, &col_stride); if (cell_info != NULL) { const int block2_size = bs->cols[row.cells[j].block_id].size; - CeresMutexLock l(&cell_info->m); + std::lock_guard<std::mutex> l(cell_info->m); MatrixTransposeMatrixMultiply <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( values + row.cells[i].position, row.block.size, block1_size, @@ -639,18 +656,19 @@ NoEBlockRowOuterProduct(const BlockSparseMatrix* A, } } -// For a row with an e_block, compute the contribition S += F'F. This +// For a row with an e_block, compute the contribution S += F'F. This // function has the same structure as NoEBlockRowOuterProduct, except // that this function uses the template parameters. template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: -EBlockRowOuterProduct(const BlockSparseMatrix* A, +EBlockRowOuterProduct(const BlockSparseMatrixData& A, int row_block_index, BlockRandomAccessMatrix* lhs) { - const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRowBlockStructure* bs = A.block_structure(); + const double* values = A.values(); + const CompressedRow& row = bs->rows[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); @@ -661,7 +679,7 @@ EBlockRowOuterProduct(const BlockSparseMatrix* A, &r, &c, &row_stride, &col_stride); if (cell_info != NULL) { - CeresMutexLock l(&cell_info->m); + std::lock_guard<std::mutex> l(cell_info->m); // block += b1.transpose() * b1; MatrixTransposeMatrixMultiply <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( @@ -681,7 +699,7 @@ EBlockRowOuterProduct(const BlockSparseMatrix* A, &row_stride, &col_stride); if (cell_info != NULL) { // block += b1.transpose() * b2; - CeresMutexLock l(&cell_info->m); + std::lock_guard<std::mutex> l(cell_info->m); MatrixTransposeMatrixMultiply <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>( values + row.cells[i].position, row.block.size, block1_size, diff --git a/extern/ceres/internal/ceres/generate_eliminator_specialization.py b/extern/ceres/internal/ceres/schur_eliminator_template.py index e89e7a48c98..2f38cf5ad8f 100644 --- a/extern/ceres/internal/ceres/generate_eliminator_specialization.py +++ b/extern/ceres/internal/ceres/schur_eliminator_template.py @@ -1,5 +1,5 @@ # Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2015 Google Inc. All rights reserved. +# Copyright 2017 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without @@ -49,28 +49,9 @@ # specializations that is generated. # Set of template specializations to generate -SPECIALIZATIONS = [(2, 2, 2), - (2, 2, 3), - (2, 2, 4), - (2, 2, "Eigen::Dynamic"), - (2, 3, 3), - (2, 3, 4), - (2, 3, 6), - (2, 3, 9), - (2, 3, "Eigen::Dynamic"), - (2, 4, 3), - (2, 4, 4), - (2, 4, 8), - (2, 4, 9), - (2, 4, "Eigen::Dynamic"), - (2, "Eigen::Dynamic", "Eigen::Dynamic"), - (4, 4, 2), - (4, 4, 3), - (4, 4, 4), - (4, 4, "Eigen::Dynamic"), - ("Eigen::Dynamic", "Eigen::Dynamic", "Eigen::Dynamic")] + HEADER = """// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -108,8 +89,7 @@ HEADER = """// Ceres Solver - A fast non-linear least squares minimizer // THIS FILE IS AUTOGENERATED. DO NOT EDIT. //========================================= // -// This file is generated using generate_eliminator_specialization.py. -// Editing it manually is not recommended. +// This file is generated using generate_template_specializations.py. """ DYNAMIC_FILE = """ @@ -159,12 +139,7 @@ SchurEliminatorBase::Create(const LinearSolver::Options& options) { #ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION """ -FACTORY_CONDITIONAL = """ if ((options.row_block_size == %s) && - (options.e_block_size == %s) && - (options.f_block_size == %s)) { - return new SchurEliminator<%s, %s, %s>(options); - } -""" +FACTORY = """ return new SchurEliminator<%s, %s, %s>(options);""" FACTORY_FOOTER = """ #endif @@ -178,54 +153,3 @@ FACTORY_FOOTER = """ } // namespace internal } // namespace ceres """ - - -def SuffixForSize(size): - if size == "Eigen::Dynamic": - return "d" - return str(size) - - -def SpecializationFilename(prefix, row_block_size, e_block_size, f_block_size): - return "_".join([prefix] + map(SuffixForSize, (row_block_size, - e_block_size, - f_block_size))) - - -def Specialize(): - """ - Generate specialization code and the conditionals to instantiate it. - """ - f = open("schur_eliminator.cc", "w") - f.write(HEADER) - f.write(FACTORY_FILE_HEADER) - - for row_block_size, e_block_size, f_block_size in SPECIALIZATIONS: - output = SpecializationFilename("generated/schur_eliminator", - row_block_size, - e_block_size, - f_block_size) + ".cc" - fptr = open(output, "w") - fptr.write(HEADER) - - template = SPECIALIZATION_FILE - if (row_block_size == "Eigen::Dynamic" and - e_block_size == "Eigen::Dynamic" and - f_block_size == "Eigen::Dynamic"): - template = DYNAMIC_FILE - - fptr.write(template % (row_block_size, e_block_size, f_block_size)) - fptr.close() - - f.write(FACTORY_CONDITIONAL % (row_block_size, - e_block_size, - f_block_size, - row_block_size, - e_block_size, - f_block_size)) - f.write(FACTORY_FOOTER) - f.close() - - -if __name__ == "__main__": - Specialize() diff --git a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc index 3e6cc90f63c..89d770b405a 100644 --- a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc +++ b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.cc @@ -32,10 +32,9 @@ #include <utility> #include <vector> + #include "ceres/block_random_access_diagonal_matrix.h" #include "ceres/block_sparse_matrix.h" -#include "ceres/collections_port.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" #include "glog/logging.h" @@ -50,9 +49,9 @@ SchurJacobiPreconditioner::SchurJacobiPreconditioner( CHECK_GT(options_.elimination_groups.size(), 1); CHECK_GT(options_.elimination_groups[0], 0); const int num_blocks = bs.cols.size() - options_.elimination_groups[0]; - CHECK_GT(num_blocks, 0) - << "Jacobian should have atleast 1 f_block for " - << "SCHUR_JACOBI preconditioner."; + CHECK_GT(num_blocks, 0) << "Jacobian should have at least 1 f_block for " + << "SCHUR_JACOBI preconditioner."; + CHECK(options_.context != NULL); std::vector<int> blocks(num_blocks); for (int i = 0; i < num_blocks; ++i) { @@ -63,8 +62,7 @@ SchurJacobiPreconditioner::SchurJacobiPreconditioner( InitEliminator(bs); } -SchurJacobiPreconditioner::~SchurJacobiPreconditioner() { -} +SchurJacobiPreconditioner::~SchurJacobiPreconditioner() {} // Initialize the SchurEliminator. void SchurJacobiPreconditioner::InitEliminator( @@ -75,8 +73,11 @@ void SchurJacobiPreconditioner::InitEliminator( eliminator_options.e_block_size = options_.e_block_size; eliminator_options.f_block_size = options_.f_block_size; eliminator_options.row_block_size = options_.row_block_size; + eliminator_options.context = options_.context; eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); - eliminator_->Init(eliminator_options.elimination_groups[0], &bs); + const bool kFullRankETE = true; + eliminator_->Init( + eliminator_options.elimination_groups[0], kFullRankETE, &bs); } // Update the values of the preconditioner matrix and factorize it. @@ -85,19 +86,9 @@ bool SchurJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, const int num_rows = m_->num_rows(); CHECK_GT(num_rows, 0); - // We need a dummy rhs vector and a dummy b vector since the Schur - // eliminator combines the computation of the reduced camera matrix - // with the computation of the right hand side of that linear - // system. - // - // TODO(sameeragarwal): Perhaps its worth refactoring the - // SchurEliminator::Eliminate function to allow NULL for the rhs. As - // of now it does not seem to be worth the effort. - Vector rhs = Vector::Zero(m_->num_rows()); - Vector b = Vector::Zero(A.num_rows()); - // Compute a subset of the entries of the Schur complement. - eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); + eliminator_->Eliminate( + BlockSparseMatrixData(A), nullptr, D, m_.get(), nullptr); m_->Invert(); return true; } @@ -107,9 +98,7 @@ void SchurJacobiPreconditioner::RightMultiply(const double* x, m_->RightMultiply(x, y); } -int SchurJacobiPreconditioner::num_rows() const { - return m_->num_rows(); -} +int SchurJacobiPreconditioner::num_rows() const { return m_->num_rows(); } } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h index 5398f3ff35d..372b790b82f 100644 --- a/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h +++ b/extern/ceres/internal/ceres/schur_jacobi_preconditioner.h @@ -38,12 +38,11 @@ #ifndef CERES_INTERNAL_SCHUR_JACOBI_PRECONDITIONER_H_ #define CERES_INTERNAL_SCHUR_JACOBI_PRECONDITIONER_H_ +#include <memory> #include <set> -#include <vector> #include <utility> -#include "ceres/collections_port.h" -#include "ceres/internal/macros.h" -#include "ceres/internal/scoped_ptr.h" +#include <vector> + #include "ceres/preconditioner.h" namespace ceres { @@ -83,21 +82,23 @@ class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { // based solvers. Please see schur_eliminator.h for more details. SchurJacobiPreconditioner(const CompressedRowBlockStructure& bs, const Preconditioner::Options& options); + SchurJacobiPreconditioner(const SchurJacobiPreconditioner&) = delete; + void operator=(const SchurJacobiPreconditioner&) = delete; + virtual ~SchurJacobiPreconditioner(); // Preconditioner interface. - virtual void RightMultiply(const double* x, double* y) const; - virtual int num_rows() const; + void RightMultiply(const double* x, double* y) const final; + int num_rows() const final; private: void InitEliminator(const CompressedRowBlockStructure& bs); - virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); + bool UpdateImpl(const BlockSparseMatrix& A, const double* D) final; Preconditioner::Options options_; - scoped_ptr<SchurEliminatorBase> eliminator_; + std::unique_ptr<SchurEliminatorBase> eliminator_; // Preconditioner matrix. - scoped_ptr<BlockRandomAccessDiagonalMatrix> m_; - CERES_DISALLOW_COPY_AND_ASSIGN(SchurJacobiPreconditioner); + std::unique_ptr<BlockRandomAccessDiagonalMatrix> m_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/schur_templates.cc b/extern/ceres/internal/ceres/schur_templates.cc new file mode 100644 index 00000000000..01528619b1b --- /dev/null +++ b/extern/ceres/internal/ceres/schur_templates.cc @@ -0,0 +1,227 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) +// +// What template specializations are available. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. + +#include "ceres/internal/eigen.h" +#include "ceres/schur_templates.h" + +namespace ceres { +namespace internal { + +void GetBestSchurTemplateSpecialization(int* row_block_size, + int* e_block_size, + int* f_block_size) { + LinearSolver::Options options; + options.row_block_size = *row_block_size; + options.e_block_size = *e_block_size; + options.f_block_size = *f_block_size; + *row_block_size = Eigen::Dynamic; + *e_block_size = Eigen::Dynamic; + *f_block_size = Eigen::Dynamic; +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + *row_block_size = 2; + *e_block_size = 2; + *f_block_size = 2; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + *row_block_size = 2; + *e_block_size = 2; + *f_block_size = 3; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + *row_block_size = 2; + *e_block_size = 2; + *f_block_size = 4; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2)) { + *row_block_size = 2; + *e_block_size = 2; + *f_block_size = Eigen::Dynamic; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + *row_block_size = 2; + *e_block_size = 3; + *f_block_size = 3; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + *row_block_size = 2; + *e_block_size = 3; + *f_block_size = 4; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 6)) { + *row_block_size = 2; + *e_block_size = 3; + *f_block_size = 6; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + *row_block_size = 2; + *e_block_size = 3; + *f_block_size = 9; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3)) { + *row_block_size = 2; + *e_block_size = 3; + *f_block_size = Eigen::Dynamic; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + *row_block_size = 2; + *e_block_size = 4; + *f_block_size = 3; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + *row_block_size = 2; + *e_block_size = 4; + *f_block_size = 4; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 6)) { + *row_block_size = 2; + *e_block_size = 4; + *f_block_size = 6; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 8)) { + *row_block_size = 2; + *e_block_size = 4; + *f_block_size = 8; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 9)) { + *row_block_size = 2; + *e_block_size = 4; + *f_block_size = 9; + return; + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4)) { + *row_block_size = 2; + *e_block_size = 4; + *f_block_size = Eigen::Dynamic; + return; + } + if (options.row_block_size == 2){ + *row_block_size = 2; + *e_block_size = Eigen::Dynamic; + *f_block_size = Eigen::Dynamic; + return; + } + if ((options.row_block_size == 3) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + *row_block_size = 3; + *e_block_size = 3; + *f_block_size = 3; + return; + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + *row_block_size = 4; + *e_block_size = 4; + *f_block_size = 2; + return; + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + *row_block_size = 4; + *e_block_size = 4; + *f_block_size = 3; + return; + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + *row_block_size = 4; + *e_block_size = 4; + *f_block_size = 4; + return; + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4)) { + *row_block_size = 4; + *e_block_size = 4; + *f_block_size = Eigen::Dynamic; + return; + } + +#endif + return; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/schur_templates.h b/extern/ceres/internal/ceres/schur_templates.h new file mode 100644 index 00000000000..90aee0a1afc --- /dev/null +++ b/extern/ceres/internal/ceres/schur_templates.h @@ -0,0 +1,46 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_SCHUR_TEMPLATES_H_ +#define CERES_INTERNAL_SCHUR_TEMPLATES_H_ + +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +void GetBestSchurTemplateSpecialization(int* row_block_size, + int* e_block_size, + int* f_block_size); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_TEMPLATES_H_ diff --git a/extern/ceres/internal/ceres/scoped_thread_token.h b/extern/ceres/internal/ceres/scoped_thread_token.h new file mode 100644 index 00000000000..c167397cce9 --- /dev/null +++ b/extern/ceres/internal/ceres/scoped_thread_token.h @@ -0,0 +1,61 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: yp@photonscore.de (Yury Prokazov) + +#ifndef CERES_INTERNAL_SCOPED_THREAD_TOKEN_H_ +#define CERES_INTERNAL_SCOPED_THREAD_TOKEN_H_ + +#include "ceres/thread_token_provider.h" + +namespace ceres { +namespace internal { + +// Helper class for ThreadTokenProvider. This object acquires a token in its +// constructor and puts that token back with destruction. +class ScopedThreadToken { + public: + ScopedThreadToken(ThreadTokenProvider* provider) + : provider_(provider), token_(provider->Acquire()) {} + + ~ScopedThreadToken() { provider_->Release(token_); } + + int token() const { return token_; } + + private: + ThreadTokenProvider* provider_; + int token_; + + ScopedThreadToken(ScopedThreadToken&); + ScopedThreadToken& operator=(ScopedThreadToken&); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCOPED_THREAD_TOKEN_H_ diff --git a/extern/ceres/internal/ceres/scratch_evaluate_preparer.h b/extern/ceres/internal/ceres/scratch_evaluate_preparer.h index fa9ebd0e50e..c8d9b937b47 100644 --- a/extern/ceres/internal/ceres/scratch_evaluate_preparer.h +++ b/extern/ceres/internal/ceres/scratch_evaluate_preparer.h @@ -35,7 +35,7 @@ #ifndef CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ #define CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ -#include "ceres/internal/scoped_ptr.h" +#include <memory> namespace ceres { namespace internal { @@ -60,7 +60,7 @@ class ScratchEvaluatePreparer { private: // Scratch space for the jacobians; each jacobian is packed one after another. // There is enough scratch to hold all the jacobians for the largest residual. - scoped_array<double> jacobian_scratch_; + std::unique_ptr<double[]> jacobian_scratch_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/single_linkage_clustering.cc b/extern/ceres/internal/ceres/single_linkage_clustering.cc new file mode 100644 index 00000000000..394492cdf23 --- /dev/null +++ b/extern/ceres/internal/ceres/single_linkage_clustering.cc @@ -0,0 +1,94 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/single_linkage_clustering.h" + +#include <unordered_set> +#include <unordered_map> +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" + +namespace ceres { +namespace internal { + +int ComputeSingleLinkageClustering( + const SingleLinkageClusteringOptions& options, + const WeightedGraph<int>& graph, + std::unordered_map<int, int>* membership) { + CHECK(membership != nullptr); + membership->clear(); + + // Initially each vertex is in its own cluster. + const std::unordered_set<int>& vertices = graph.vertices(); + for (const int v : vertices) { + (*membership)[v] = v; + } + + for (const int vertex1 : vertices) { + const std::unordered_set<int>& neighbors = graph.Neighbors(vertex1); + for (const int vertex2 : neighbors) { + // Since the graph is undirected, only pay attention to one side + // of the edge and ignore weak edges. + if ((vertex1 > vertex2) || + (graph.EdgeWeight(vertex1, vertex2) < options.min_similarity)) { + continue; + } + + // Use a union-find algorithm to keep track of the clusters. + const int c1 = FindConnectedComponent(vertex1, membership); + const int c2 = FindConnectedComponent(vertex2, membership); + + if (c1 == c2) { + continue; + } + + if (c1 < c2) { + (*membership)[c2] = c1; + } else { + (*membership)[c1] = c2; + } + } + } + + // Make sure that every vertex is connected directly to the vertex + // identifying the cluster. + int num_clusters = 0; + for (auto& m : *membership) { + m.second = FindConnectedComponent(m.first, membership); + if (m.first == m.second) { + ++num_clusters; + } + } + + return num_clusters; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/integral_types.h b/extern/ceres/internal/ceres/single_linkage_clustering.h index 98a746f13ff..ccd6f8ea37d 100644 --- a/extern/ceres/internal/ceres/integral_types.h +++ b/extern/ceres/internal/ceres/single_linkage_clustering.h @@ -26,66 +26,39 @@ // 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) -// -// Portable typedefs for various fixed-size integers. Uses template -// metaprogramming instead of fragile compiler defines. +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SINGLE_LINKAGE_CLUSTERING_H_ +#define CERES_INTERNAL_SINGLE_LINKAGE_CLUSTERING_H_ -#ifndef CERES_INTERNAL_INTEGRAL_TYPES_H_ -#define CERES_INTERNAL_INTEGRAL_TYPES_H_ +#include <unordered_map> +#include "ceres/graph.h" namespace ceres { namespace internal { -// Compile time ternary on types. -template<bool kCondition, typename kTrueType, typename kFalseType> -struct Ternary { - typedef kTrueType type; -}; -template<typename kTrueType, typename kFalseType> -struct Ternary<false, kTrueType, kFalseType> { - typedef kFalseType type; -}; - -#define CERES_INTSIZE(TYPE) \ - typename Ternary<sizeof(TYPE) * 8 == kBits, TYPE, - -template<int kBits> -struct Integer { - typedef - CERES_INTSIZE(char) - CERES_INTSIZE(short) - CERES_INTSIZE(int) - CERES_INTSIZE(long int) - CERES_INTSIZE(long long) - void>::type >::type >::type >::type >::type - type; +struct SingleLinkageClusteringOptions { + // Graph edges with edge weight less than min_similarity are ignored + // during the clustering process. + double min_similarity = 0.99; }; -template<int kBits> -struct UnsignedInteger { - typedef - CERES_INTSIZE(unsigned char) - CERES_INTSIZE(unsigned short) - CERES_INTSIZE(unsigned int) - CERES_INTSIZE(unsigned long int) - CERES_INTSIZE(unsigned long long) - void>::type >::type >::type >::type >::type - type; -}; - -#undef CERES_INTSIZE - -typedef Integer< 8>::type int8; -typedef Integer<32>::type int32; -typedef Integer<64>::type int64; - -typedef UnsignedInteger< 8>::type uint8; -typedef UnsignedInteger<16>::type uint16; -typedef UnsignedInteger<32>::type uint32; -typedef UnsignedInteger<64>::type uint64; +// Compute a partitioning of the vertices of the graph using the +// single linkage clustering algorithm. Edges with weight less than +// SingleLinkageClusteringOptions::min_similarity will be ignored. +// +// membership upon return will contain a mapping from the vertices of +// the graph to an integer indicating the identity of the cluster that +// it belongs to. +// +// The return value of this function is the number of clusters +// identified by the algorithm. +int ComputeSingleLinkageClustering( + const SingleLinkageClusteringOptions& options, + const WeightedGraph<int>& graph, + std::unordered_map<int, int>* membership); } // namespace internal } // namespace ceres -#endif // CERES_INTERNAL_INTEGRAL_TYPES_H_ +#endif // CERES_INTERNAL_SINGLE_LINKAGE_CLUSTERING_H_ diff --git a/extern/ceres/internal/ceres/small_blas.h b/extern/ceres/internal/ceres/small_blas.h index 264ac53047d..81c58722d5b 100644 --- a/extern/ceres/internal/ceres/small_blas.h +++ b/extern/ceres/internal/ceres/small_blas.h @@ -38,6 +38,7 @@ #include "ceres/internal/port.h" #include "ceres/internal/eigen.h" #include "glog/logging.h" +#include "small_blas_generic.h" namespace ceres { namespace internal { @@ -89,6 +90,26 @@ namespace internal { B, num_row_b, num_col_b, \ C, start_row_c, start_col_c, row_stride_c, col_stride_c); +#define CERES_GEMM_STORE_SINGLE(p, index, value) \ + if (kOperation > 0) { \ + p[index] += value; \ + } else if (kOperation < 0) { \ + p[index] -= value; \ + } else { \ + p[index] = value; \ + } + +#define CERES_GEMM_STORE_PAIR(p, index, v1, v2) \ + if (kOperation > 0) { \ + p[index] += v1; \ + p[index + 1] += v2; \ + } else if (kOperation < 0) { \ + p[index] -= v1; \ + p[index + 1] -= v2; \ + } else { \ + p[index] = v1; \ + p[index + 1] = v2; \ + } // For the matrix-matrix functions below, there are three variants for // each functionality. Foo, FooNaive and FooEigen. Foo is the one to @@ -160,24 +181,64 @@ CERES_GEMM_BEGIN(MatrixMatrixMultiplyNaive) { 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); + const int span = 4; + + // Calculate the remainder part first. - for (int row = 0; row < NUM_ROW_C; ++row) { - for (int col = 0; col < NUM_COL_C; ++col) { + // Process the last odd column if present. + if (NUM_COL_C & 1) { + int col = NUM_COL_C - 1; + const double* pa = &A[0]; + for (int row = 0; row < NUM_ROW_C; ++row, pa += NUM_COL_A) { + const double* pb = &B[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]; + for (int k = 0; k < NUM_COL_A; ++k, pb += NUM_COL_B) { + tmp += pa[k] * pb[0]; } 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_STORE_SINGLE(C, index, tmp); + } + + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_COL_C == 1) { + return; + } + } + + // Process the couple columns in remainder if present. + if (NUM_COL_C & 2) { + int col = NUM_COL_C & (int)(~(span - 1)) ; + const double* pa = &A[0]; + for (int row = 0; row < NUM_ROW_C; ++row, pa += NUM_COL_A) { + const double* pb = &B[col]; + double tmp1 = 0.0, tmp2 = 0.0; + for (int k = 0; k < NUM_COL_A; ++k, pb += NUM_COL_B) { + double av = pa[k]; + tmp1 += av * pb[0]; + tmp2 += av * pb[1]; } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + CERES_GEMM_STORE_PAIR(C, index, tmp1, tmp2); + } + + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_COL_C < span) { + return; } } + + // Calculate the main part with multiples of 4. + int col_m = NUM_COL_C & (int)(~(span - 1)); + for (int col = 0; col < col_m; col += span) { + for (int row = 0; row < NUM_ROW_C; ++row) { + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + MMM_mat1x4(NUM_COL_A, &A[row * NUM_COL_A], + &B[col], NUM_COL_B, &C[index], kOperation); + } + } + } CERES_GEMM_BEGIN(MatrixMatrixMultiply) { @@ -220,24 +281,68 @@ CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiplyNaive) { 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); + const int span = 4; - for (int row = 0; row < NUM_ROW_C; ++row) { - for (int col = 0; col < NUM_COL_C; ++col) { + // Process the remainder part first. + + // Process the last odd column if present. + if (NUM_COL_C & 1) { + int col = NUM_COL_C - 1; + for (int row = 0; row < NUM_ROW_C; ++row) { + const double* pa = &A[row]; + const double* pb = &B[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]; + tmp += pa[0] * pb[0]; + pa += NUM_COL_A; + pb += NUM_COL_B; } 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_STORE_SINGLE(C, index, tmp); + } + + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_COL_C == 1) { + return; + } + } + + // Process the couple columns in remainder if present. + if (NUM_COL_C & 2) { + int col = NUM_COL_C & (int)(~(span - 1)) ; + for (int row = 0; row < NUM_ROW_C; ++row) { + const double* pa = &A[row]; + const double* pb = &B[col]; + double tmp1 = 0.0, tmp2 = 0.0; + for (int k = 0; k < NUM_ROW_A; ++k) { + double av = *pa; + tmp1 += av * pb[0]; + tmp2 += av * pb[1]; + pa += NUM_COL_A; + pb += NUM_COL_B; } + + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + CERES_GEMM_STORE_PAIR(C, index, tmp1, tmp2); + } + + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_COL_C < span) { + return; } } + + // Process the main part with multiples of 4. + int col_m = NUM_COL_C & (int)(~(span - 1)); + for (int col = 0; col < col_m; col += span) { + for (int row = 0; row < NUM_ROW_C; ++row) { + const int index = (row + start_row_c) * col_stride_c + start_col_c + col; + MTM_mat1x4(NUM_ROW_A, &A[row], NUM_COL_A, + &B[col], NUM_COL_B, &C[index], kOperation); + } + } + } CERES_GEMM_BEGIN(MatrixTransposeMatrixMultiply) { @@ -301,21 +406,54 @@ inline void MatrixVectorMultiply(const double* 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); + const int span = 4; - for (int row = 0; row < NUM_ROW_A; ++row) { + // Calculate the remainder part first. + + // Process the last odd row if present. + if (NUM_ROW_A & 1) { + int row = NUM_ROW_A - 1; + const double* pa = &A[row * NUM_COL_A]; + const double* pb = &b[0]; double tmp = 0.0; for (int col = 0; col < NUM_COL_A; ++col) { - tmp += A[row * NUM_COL_A + col] * b[col]; + tmp += (*pa++) * (*pb++); + } + CERES_GEMM_STORE_SINGLE(c, row, tmp); + + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_ROW_A == 1) { + return; + } + } + + // Process the couple rows in remainder if present. + if (NUM_ROW_A & 2) { + int row = NUM_ROW_A & (int)(~(span - 1)); + const double* pa1 = &A[row * NUM_COL_A]; + const double* pa2 = pa1 + NUM_COL_A; + const double* pb = &b[0]; + double tmp1 = 0.0, tmp2 = 0.0; + for (int col = 0; col < NUM_COL_A; ++col) { + double bv = *pb++; + tmp1 += *(pa1++) * bv; + tmp2 += *(pa2++) * bv; } + CERES_GEMM_STORE_PAIR(c, row, tmp1, tmp2); - if (kOperation > 0) { - c[row] += tmp; - } else if (kOperation < 0) { - c[row] -= tmp; - } else { - c[row] = tmp; + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_ROW_A < span) { + return; } } + + // Calculate the main part with multiples of 4. + int row_m = NUM_ROW_A & (int)(~(span - 1)); + for (int row = 0; row < row_m; row += span) { + MVM_mat4x1(NUM_COL_A, &A[row * NUM_COL_A], NUM_COL_A, + &b[0], &c[row], kOperation); + } + #endif // CERES_NO_CUSTOM_BLAS } @@ -352,21 +490,55 @@ inline void MatrixTransposeVectorMultiply(const double* 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); + const int span = 4; + + // Calculate the remainder part first. - for (int row = 0; row < NUM_COL_A; ++row) { + // Process the last odd column if present. + if (NUM_COL_A & 1) { + int row = NUM_COL_A - 1; + const double* pa = &A[row]; + const double* pb = &b[0]; double tmp = 0.0; for (int col = 0; col < NUM_ROW_A; ++col) { - tmp += A[col * NUM_COL_A + row] * b[col]; + tmp += *pa * (*pb++); + pa += NUM_COL_A; } + CERES_GEMM_STORE_SINGLE(c, row, tmp); - if (kOperation > 0) { - c[row] += tmp; - } else if (kOperation < 0) { - c[row] -= tmp; - } else { - c[row] = tmp; + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_COL_A == 1) { + return; } } + + // Process the couple columns in remainder if present. + if (NUM_COL_A & 2) { + int row = NUM_COL_A & (int)(~(span - 1)); + const double* pa = &A[row]; + const double* pb = &b[0]; + double tmp1 = 0.0, tmp2 = 0.0; + for (int col = 0; col < NUM_ROW_A; ++col) { + double bv = *pb++; + tmp1 += *(pa ) * bv; + tmp2 += *(pa + 1) * bv; + pa += NUM_COL_A; + } + CERES_GEMM_STORE_PAIR(c, row, tmp1, tmp2); + + // Return directly for efficiency of extremely small matrix multiply. + if (NUM_COL_A < span) { + return; + } + } + + // Calculate the main part with multiples of 4. + int row_m = NUM_COL_A & (int)(~(span - 1)); + for (int row = 0; row < row_m; row += span) { + MTV_mat4x1(NUM_ROW_A, &A[row], NUM_COL_A, + &b[0], &c[row], kOperation); + } + #endif // CERES_NO_CUSTOM_BLAS } @@ -374,6 +546,8 @@ inline void MatrixTransposeVectorMultiply(const double* A, #undef CERES_GEMM_EIGEN_HEADER #undef CERES_GEMM_NAIVE_HEADER #undef CERES_CALL_GEMM +#undef CERES_GEMM_STORE_SINGLE +#undef CERES_GEMM_STORE_PAIR } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/small_blas_generic.h b/extern/ceres/internal/ceres/small_blas_generic.h new file mode 100644 index 00000000000..978c5d56a84 --- /dev/null +++ b/extern/ceres/internal/ceres/small_blas_generic.h @@ -0,0 +1,315 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: yangfan34@lenovo.com (Lenovo Research Device+ Lab - Shanghai) +// +// Optimization for simple blas functions used in the Schur Eliminator. +// These are fairly basic implementations which already yield a significant +// speedup in the eliminator performance. + +#ifndef CERES_INTERNAL_SMALL_BLAS_GENERIC_H_ +#define CERES_INTERNAL_SMALL_BLAS_GENERIC_H_ + +namespace ceres { +namespace internal { + +// The following macros are used to share code +#define CERES_GEMM_OPT_NAIVE_HEADER \ + double c0 = 0.0; \ + double c1 = 0.0; \ + double c2 = 0.0; \ + double c3 = 0.0; \ + const double* pa = a; \ + const double* pb = b; \ + const int span = 4; \ + int col_r = col_a & (span - 1); \ + int col_m = col_a - col_r; + +#define CERES_GEMM_OPT_STORE_MAT1X4 \ + if (kOperation > 0) { \ + *c++ += c0; \ + *c++ += c1; \ + *c++ += c2; \ + *c++ += c3; \ + } else if (kOperation < 0) { \ + *c++ -= c0; \ + *c++ -= c1; \ + *c++ -= c2; \ + *c++ -= c3; \ + } else { \ + *c++ = c0; \ + *c++ = c1; \ + *c++ = c2; \ + *c++ = c3; \ + } + +// Matrix-Matrix Multiplication +// Figure out 1x4 of Matrix C in one batch +// +// c op a * B; +// where op can be +=, -=, or =, indicated by kOperation. +// +// Matrix C Matrix A Matrix B +// +// C0, C1, C2, C3 op A0, A1, A2, A3, ... * B0, B1, B2, B3 +// B4, B5, B6, B7 +// B8, B9, Ba, Bb +// Bc, Bd, Be, Bf +// . , . , . , . +// . , . , . , . +// . , . , . , . +// +// unroll for loops +// utilize the data resided in cache +// NOTE: col_a means the columns of A +static inline void MMM_mat1x4(const int col_a, + const double* a, + const double* b, + const int col_stride_b, + double* c, + const int kOperation) { + CERES_GEMM_OPT_NAIVE_HEADER + double av = 0.0; + int bi = 0; + +#define CERES_GEMM_OPT_MMM_MAT1X4_MUL \ + av = pa[k]; \ + pb = b + bi; \ + c0 += av * *pb++; \ + c1 += av * *pb++; \ + c2 += av * *pb++; \ + c3 += av * *pb++; \ + bi += col_stride_b; \ + k++; + + for (int k = 0; k < col_m;) { + CERES_GEMM_OPT_MMM_MAT1X4_MUL + CERES_GEMM_OPT_MMM_MAT1X4_MUL + CERES_GEMM_OPT_MMM_MAT1X4_MUL + CERES_GEMM_OPT_MMM_MAT1X4_MUL + } + + for (int k = col_m; k < col_a;) { + CERES_GEMM_OPT_MMM_MAT1X4_MUL + } + + CERES_GEMM_OPT_STORE_MAT1X4 + +#undef CERES_GEMM_OPT_MMM_MAT1X4_MUL +} + +// Matrix Transpose-Matrix multiplication +// Figure out 1x4 of Matrix C in one batch +// +// c op a' * B; +// where op can be +=, -=, or = indicated by kOperation. +// +// Matrix A +// +// A0 +// A1 +// A2 +// A3 +// . +// . +// . +// +// Matrix C Matrix A' Matrix B +// +// C0, C1, C2, C3 op A0, A1, A2, A3, ... * B0, B1, B2, B3 +// B4, B5, B6, B7 +// B8, B9, Ba, Bb +// Bc, Bd, Be, Bf +// . , . , . , . +// . , . , . , . +// . , . , . , . +// +// unroll for loops +// utilize the data resided in cache +// NOTE: col_a means the columns of A' +static inline void MTM_mat1x4(const int col_a, + const double* a, + const int col_stride_a, + const double* b, + const int col_stride_b, + double* c, + const int kOperation) { + CERES_GEMM_OPT_NAIVE_HEADER + double av = 0.0; + int ai = 0; + int bi = 0; + +#define CERES_GEMM_OPT_MTM_MAT1X4_MUL \ + av = pa[ai]; \ + pb = b + bi; \ + c0 += av * *pb++; \ + c1 += av * *pb++; \ + c2 += av * *pb++; \ + c3 += av * *pb++; \ + ai += col_stride_a; \ + bi += col_stride_b; + + for (int k = 0; k < col_m; k += span) { + CERES_GEMM_OPT_MTM_MAT1X4_MUL + CERES_GEMM_OPT_MTM_MAT1X4_MUL + CERES_GEMM_OPT_MTM_MAT1X4_MUL + CERES_GEMM_OPT_MTM_MAT1X4_MUL + } + + for (int k = col_m; k < col_a; k++) { + CERES_GEMM_OPT_MTM_MAT1X4_MUL + } + + CERES_GEMM_OPT_STORE_MAT1X4 + +#undef CERES_GEMM_OPT_MTM_MAT1X4_MUL +} + +// Matrix-Vector Multiplication +// Figure out 4x1 of vector c in one batch +// +// c op A * b; +// where op can be +=, -=, or =, indicated by kOperation. +// +// Vector c Matrix A Vector b +// +// C0 op A0, A1, A2, A3, ... * B0 +// C1 A4, A5, A6, A7, ... B1 +// C2 A8, A9, Aa, Ab, ... B2 +// C3 Ac, Ad, Ae, Af, ... B3 +// . +// . +// . +// +// unroll for loops +// utilize the data resided in cache +// NOTE: col_a means the columns of A +static inline void MVM_mat4x1(const int col_a, + const double* a, + const int col_stride_a, + const double* b, + double* c, + const int kOperation) { + CERES_GEMM_OPT_NAIVE_HEADER + double bv = 0.0; + +#define CERES_GEMM_OPT_MVM_MAT4X1_MUL \ + bv = *pb; \ + c0 += *(pa ) * bv; \ + c1 += *(pa + col_stride_a ) * bv; \ + c2 += *(pa + col_stride_a * 2) * bv; \ + c3 += *(pa + col_stride_a * 3) * bv; \ + pa++; \ + pb++; + + for (int k = 0; k < col_m; k += span) { + CERES_GEMM_OPT_MVM_MAT4X1_MUL + CERES_GEMM_OPT_MVM_MAT4X1_MUL + CERES_GEMM_OPT_MVM_MAT4X1_MUL + CERES_GEMM_OPT_MVM_MAT4X1_MUL + } + + for (int k = col_m; k < col_a; k++) { + CERES_GEMM_OPT_MVM_MAT4X1_MUL + } + + CERES_GEMM_OPT_STORE_MAT1X4 + +#undef CERES_GEMM_OPT_MVM_MAT4X1_MUL +} + +// Matrix Transpose-Vector multiplication +// Figure out 4x1 of vector c in one batch +// +// c op A' * b; +// where op can be +=, -=, or =, indicated by kOperation. +// +// Matrix A +// +// A0, A4, A8, Ac +// A1, A5, A9, Ad +// A2, A6, Aa, Ae +// A3, A7, Ab, Af +// . , . , . , . +// . , . , . , . +// . , . , . , . +// +// Vector c Matrix A' Vector b +// +// C0 op A0, A1, A2, A3, ... * B0 +// C1 A4, A5, A6, A7, ... B1 +// C2 A8, A9, Aa, Ab, ... B2 +// C3 Ac, Ad, Ae, Af, ... B3 +// . +// . +// . +// +// unroll for loops +// utilize the data resided in cache +// NOTE: col_a means the columns of A' +static inline void MTV_mat4x1(const int col_a, + const double* a, + const int col_stride_a, + const double* b, + double* c, + const int kOperation) { + CERES_GEMM_OPT_NAIVE_HEADER + double bv = 0.0; + +#define CERES_GEMM_OPT_MTV_MAT4X1_MUL \ + bv = *pb; \ + c0 += *(pa ) * bv; \ + c1 += *(pa + 1) * bv; \ + c2 += *(pa + 2) * bv; \ + c3 += *(pa + 3) * bv; \ + pa += col_stride_a; \ + pb++; + + for (int k = 0; k < col_m; k += span) { + CERES_GEMM_OPT_MTV_MAT4X1_MUL + CERES_GEMM_OPT_MTV_MAT4X1_MUL + CERES_GEMM_OPT_MTV_MAT4X1_MUL + CERES_GEMM_OPT_MTV_MAT4X1_MUL + } + + for (int k = col_m; k < col_a; k++) { + CERES_GEMM_OPT_MTV_MAT4X1_MUL + } + + CERES_GEMM_OPT_STORE_MAT1X4 + +#undef CERES_GEMM_OPT_MTV_MAT4X1_MUL +} + +#undef CERES_GEMM_OPT_NAIVE_HEADER +#undef CERES_GEMM_OPT_STORE_MAT1X4 + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SMALL_BLAS_GENERIC_H_ diff --git a/extern/ceres/internal/ceres/solver.cc b/extern/ceres/internal/ceres/solver.cc index 8411350986a..861d8d30485 100644 --- a/extern/ceres/internal/ceres/solver.cc +++ b/extern/ceres/internal/ceres/solver.cc @@ -32,8 +32,14 @@ #include "ceres/solver.h" #include <algorithm> -#include <sstream> // NOLINT +#include <memory> +#include <sstream> // NOLINT #include <vector> + +#include "ceres/casts.h" +#include "ceres/context.h" +#include "ceres/context_impl.h" +#include "ceres/detect_structure.h" #include "ceres/gradient_checking_cost_function.h" #include "ceres/internal/port.h" #include "ceres/parameter_block_ordering.h" @@ -41,6 +47,7 @@ #include "ceres/problem.h" #include "ceres/problem_impl.h" #include "ceres/program.h" +#include "ceres/schur_templates.h" #include "ceres/solver_utils.h" #include "ceres/stringprintf.h" #include "ceres/types.h" @@ -52,6 +59,8 @@ namespace { using std::map; using std::string; using std::vector; +using internal::StringAppendF; +using internal::StringPrintf; #define OPTION_OP(x, y, OP) \ if (!(options.x OP y)) { \ @@ -91,7 +100,6 @@ bool CommonOptionsAreValid(const Solver::Options& options, string* error) { OPTION_GE(gradient_tolerance, 0.0); OPTION_GE(parameter_tolerance, 0.0); OPTION_GT(num_threads, 0); - OPTION_GT(num_linear_solver_threads, 0); if (options.check_gradients) { OPTION_GT(gradient_check_relative_precision, 0.0); OPTION_GT(gradient_check_numeric_derivative_relative_step_size, 0.0); @@ -132,101 +140,51 @@ bool TrustRegionOptionsAreValid(const Solver::Options& options, string* error) { return false; } - if (options.preconditioner_type == CLUSTER_JACOBI && - options.sparse_linear_algebra_library_type != SUITE_SPARSE) { - *error = "CLUSTER_JACOBI requires " - "Solver::Options::sparse_linear_algebra_library_type to be " - "SUITE_SPARSE"; - return false; - } - - if (options.preconditioner_type == CLUSTER_TRIDIAGONAL && - options.sparse_linear_algebra_library_type != SUITE_SPARSE) { - *error = "CLUSTER_TRIDIAGONAL requires " - "Solver::Options::sparse_linear_algebra_library_type to be " - "SUITE_SPARSE"; - return false; - } - -#ifdef CERES_NO_LAPACK - if (options.dense_linear_algebra_library_type == LAPACK) { - if (options.linear_solver_type == DENSE_NORMAL_CHOLESKY) { - *error = "Can't use DENSE_NORMAL_CHOLESKY with LAPACK because " - "LAPACK was not enabled when Ceres was built."; - return false; - } else if (options.linear_solver_type == DENSE_QR) { - *error = "Can't use DENSE_QR with LAPACK because " - "LAPACK was not enabled when Ceres was built."; - return false; - } else if (options.linear_solver_type == DENSE_SCHUR) { - *error = "Can't use DENSE_SCHUR with LAPACK because " - "LAPACK was not enabled when Ceres was built."; - return false; - } - } -#endif - -#ifdef CERES_NO_SUITESPARSE - if (options.sparse_linear_algebra_library_type == SUITE_SPARSE) { - if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { - *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " - "SuiteSparse was not enabled when Ceres was built."; - return false; - } else if (options.linear_solver_type == SPARSE_SCHUR) { - *error = "Can't use SPARSE_SCHUR with SUITESPARSE because " - "SuiteSparse was not enabled when Ceres was built."; - return false; - } else if (options.preconditioner_type == CLUSTER_JACOBI) { - *error = "CLUSTER_JACOBI preconditioner not supported. " - "SuiteSparse was not enabled when Ceres was built."; - return false; - } else if (options.preconditioner_type == CLUSTER_TRIDIAGONAL) { - *error = "CLUSTER_TRIDIAGONAL preconditioner not supported. " - "SuiteSparse was not enabled when Ceres was built."; + if (options.dense_linear_algebra_library_type == LAPACK && + !IsDenseLinearAlgebraLibraryTypeAvailable(LAPACK) && + (options.linear_solver_type == DENSE_NORMAL_CHOLESKY || + options.linear_solver_type == DENSE_QR || + options.linear_solver_type == DENSE_SCHUR)) { + *error = StringPrintf( + "Can't use %s with " + "Solver::Options::dense_linear_algebra_library_type = LAPACK " + "because LAPACK was not enabled when Ceres was built.", + LinearSolverTypeToString(options.linear_solver_type)); return false; - } } -#endif -#ifdef CERES_NO_CXSPARSE - if (options.sparse_linear_algebra_library_type == CX_SPARSE) { - if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { - *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because " - "CXSparse was not enabled when Ceres was built."; - return false; - } else if (options.linear_solver_type == SPARSE_SCHUR) { - *error = "Can't use SPARSE_SCHUR with CX_SPARSE because " - "CXSparse was not enabled when Ceres was built."; - return false; + { + const char* sparse_linear_algebra_library_name = + SparseLinearAlgebraLibraryTypeToString( + options.sparse_linear_algebra_library_type); + const char* name = nullptr; + if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY || + options.linear_solver_type == SPARSE_SCHUR) { + name = LinearSolverTypeToString(options.linear_solver_type); + } else if ((options.linear_solver_type == ITERATIVE_SCHUR && + (options.preconditioner_type == CLUSTER_JACOBI || + options.preconditioner_type == CLUSTER_TRIDIAGONAL)) || + (options.linear_solver_type == CGNR && + options.preconditioner_type == SUBSET)) { + name = PreconditionerTypeToString(options.preconditioner_type); } - } -#endif -#ifndef CERES_USE_EIGEN_SPARSE - if (options.sparse_linear_algebra_library_type == EIGEN_SPARSE) { - if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { - *error = "Can't use SPARSE_NORMAL_CHOLESKY with EIGEN_SPARSE because " - "Eigen's sparse linear algebra was not enabled when Ceres was " - "built."; - return false; - } else if (options.linear_solver_type == SPARSE_SCHUR) { - *error = "Can't use SPARSE_SCHUR with EIGEN_SPARSE because " - "Eigen's sparse linear algebra was not enabled when Ceres was " - "built."; - return false; - } - } -#endif - - if (options.sparse_linear_algebra_library_type == NO_SPARSE) { - if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY) { - *error = "Can't use SPARSE_NORMAL_CHOLESKY as " - "sparse_linear_algebra_library_type is NO_SPARSE."; - return false; - } else if (options.linear_solver_type == SPARSE_SCHUR) { - *error = "Can't use SPARSE_SCHUR as " - "sparse_linear_algebra_library_type is NO_SPARSE."; - return false; + if (name) { + if (options.sparse_linear_algebra_library_type == NO_SPARSE) { + *error = StringPrintf( + "Can't use %s with " + "Solver::Options::sparse_linear_algebra_library_type = %s.", + name, sparse_linear_algebra_library_name); + return false; + } else if (!IsSparseLinearAlgebraLibraryTypeAvailable( + options.sparse_linear_algebra_library_type)) { + *error = StringPrintf( + "Can't use %s with " + "Solver::Options::sparse_linear_algebra_library_type = %s, " + "because support was not enabled when Ceres Solver was built.", + name, sparse_linear_algebra_library_name); + return false; + } } } @@ -240,16 +198,32 @@ bool TrustRegionOptionsAreValid(const Solver::Options& options, string* error) { } } - if (options.trust_region_minimizer_iterations_to_dump.size() > 0 && + if (!options.trust_region_minimizer_iterations_to_dump.empty() && options.trust_region_problem_dump_format_type != CONSOLE && options.trust_region_problem_dump_directory.empty()) { *error = "Solver::Options::trust_region_problem_dump_directory is empty."; return false; } - if (options.dynamic_sparsity && - options.linear_solver_type != SPARSE_NORMAL_CHOLESKY) { - *error = "Dynamic sparsity is only supported with SPARSE_NORMAL_CHOLESKY."; + if (options.dynamic_sparsity) { + if (options.linear_solver_type != SPARSE_NORMAL_CHOLESKY) { + *error = "Dynamic sparsity is only supported with SPARSE_NORMAL_CHOLESKY."; + return false; + } + if (options.sparse_linear_algebra_library_type == ACCELERATE_SPARSE) { + *error = "ACCELERATE_SPARSE is not currently supported with dynamic " + "sparsity."; + return false; + } + } + + if (options.linear_solver_type == CGNR && + options.preconditioner_type == SUBSET && + options.residual_blocks_for_subset_preconditioner.empty()) { + *error = + "When using SUBSET preconditioner, " + "Solver::Options::residual_blocks_for_subset_preconditioner cannot be " + "empty"; return false; } @@ -264,7 +238,8 @@ bool LineSearchOptionsAreValid(const Solver::Options& options, string* error) { OPTION_LT_OPTION(max_line_search_step_contraction, min_line_search_step_contraction); OPTION_LE(min_line_search_step_contraction, 1.0); - OPTION_GT(max_num_line_search_step_size_iterations, 0); + OPTION_GE(max_num_line_search_step_size_iterations, + (options.minimizer_type == ceres::TRUST_REGION ? 0 : 1)); OPTION_GT(line_search_sufficient_function_decrease, 0.0); OPTION_LT_OPTION(line_search_sufficient_function_decrease, line_search_sufficient_curvature_decrease); @@ -310,13 +285,13 @@ bool LineSearchOptionsAreValid(const Solver::Options& options, string* error) { #undef OPTION_LT_OPTION void StringifyOrdering(const vector<int>& ordering, string* report) { - if (ordering.size() == 0) { + if (ordering.empty()) { internal::StringAppendF(report, "AUTOMATIC"); return; } for (int i = 0; i < ordering.size() - 1; ++i) { - internal::StringAppendF(report, "%d, ", ordering[i]); + internal::StringAppendF(report, "%d,", ordering[i]); } internal::StringAppendF(report, "%d", ordering.back()); } @@ -364,7 +339,6 @@ void PreSolveSummarize(const Solver::Options& options, summary->max_lbfgs_rank = options.max_lbfgs_rank; summary->minimizer_type = options.minimizer_type; summary->nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; // NOLINT - summary->num_linear_solver_threads_given = options.num_linear_solver_threads; // NOLINT summary->num_threads_given = options.num_threads; summary->preconditioner_type_given = options.preconditioner_type; summary->sparse_linear_algebra_library_type = options.sparse_linear_algebra_library_type; // NOLINT @@ -380,10 +354,9 @@ void PostSolveSummarize(const internal::PreprocessedProblem& pp, &(summary->inner_iteration_ordering_used)); summary->inner_iterations_used = pp.inner_iteration_minimizer.get() != NULL; // NOLINT - summary->linear_solver_type_used = pp.options.linear_solver_type; - summary->num_linear_solver_threads_used = pp.options.num_linear_solver_threads; // NOLINT + summary->linear_solver_type_used = pp.linear_solver_options.type; summary->num_threads_used = pp.options.num_threads; - summary->preconditioner_type_used = pp.options.preconditioner_type; // NOLINT + summary->preconditioner_type_used = pp.options.preconditioner_type; internal::SetSummaryFinalCost(summary); @@ -391,36 +364,47 @@ void PostSolveSummarize(const internal::PreprocessedProblem& pp, SummarizeReducedProgram(*pp.reduced_program, summary); } + using internal::CallStatistics; + // It is possible that no evaluator was created. This would be the // case if the preprocessor failed, or if the reduced problem did // not contain any parameter blocks. Thus, only extract the // evaluator statistics if one exists. if (pp.evaluator.get() != NULL) { - const map<string, double>& evaluator_time_statistics = - pp.evaluator->TimeStatistics(); - summary->residual_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Residual", 0.0); - summary->jacobian_evaluation_time_in_seconds = - FindWithDefault(evaluator_time_statistics, "Evaluator::Jacobian", 0.0); + const map<string, CallStatistics>& evaluator_statistics = + pp.evaluator->Statistics(); + { + const CallStatistics& call_stats = FindWithDefault( + evaluator_statistics, "Evaluator::Residual", CallStatistics()); + + summary->residual_evaluation_time_in_seconds = call_stats.time; + summary->num_residual_evaluations = call_stats.calls; + } + { + const CallStatistics& call_stats = FindWithDefault( + evaluator_statistics, "Evaluator::Jacobian", CallStatistics()); + + summary->jacobian_evaluation_time_in_seconds = call_stats.time; + summary->num_jacobian_evaluations = call_stats.calls; + } } // Again, like the evaluator, there may or may not be a linear // solver from which we can extract run time statistics. In // particular the line search solver does not use a linear solver. if (pp.linear_solver.get() != NULL) { - const map<string, double>& linear_solver_time_statistics = - pp.linear_solver->TimeStatistics(); - summary->linear_solver_time_in_seconds = - FindWithDefault(linear_solver_time_statistics, - "LinearSolver::Solve", - 0.0); + const map<string, CallStatistics>& linear_solver_statistics = + pp.linear_solver->Statistics(); + const CallStatistics& call_stats = FindWithDefault( + linear_solver_statistics, "LinearSolver::Solve", CallStatistics()); + summary->num_linear_solves = call_stats.calls; + summary->linear_solver_time_in_seconds = call_stats.time; } } void Minimize(internal::PreprocessedProblem* pp, Solver::Summary* summary) { using internal::Program; - using internal::scoped_ptr; using internal::Minimizer; Program* program = pp->reduced_program.get(); @@ -434,16 +418,36 @@ void Minimize(internal::PreprocessedProblem* pp, return; } - scoped_ptr<Minimizer> minimizer( + const Vector original_reduced_parameters = pp->reduced_parameters; + std::unique_ptr<Minimizer> minimizer( Minimizer::Create(pp->options.minimizer_type)); minimizer->Minimize(pp->minimizer_options, pp->reduced_parameters.data(), summary); - if (summary->IsSolutionUsable()) { - program->StateVectorToParameterBlocks(pp->reduced_parameters.data()); - program->CopyParameterBlockStateToUserState(); - } + program->StateVectorToParameterBlocks( + summary->IsSolutionUsable() + ? pp->reduced_parameters.data() + : original_reduced_parameters.data()); + program->CopyParameterBlockStateToUserState(); +} + +std::string SchurStructureToString(const int row_block_size, + const int e_block_size, + const int f_block_size) { + const std::string row = + (row_block_size == Eigen::Dynamic) + ? "d" : internal::StringPrintf("%d", row_block_size); + + const std::string e = + (e_block_size == Eigen::Dynamic) + ? "d" : internal::StringPrintf("%d", e_block_size); + + const std::string f = + (f_block_size == Eigen::Dynamic) + ? "d" : internal::StringPrintf("%d", f_block_size); + + return internal::StringPrintf("%s,%s,%s", row.c_str(), e.c_str(), f.c_str()); } } // namespace @@ -475,11 +479,10 @@ void Solver::Solve(const Solver::Options& options, using internal::Preprocessor; using internal::ProblemImpl; using internal::Program; - using internal::scoped_ptr; using internal::WallTimeInSeconds; - CHECK_NOTNULL(problem); - CHECK_NOTNULL(summary); + CHECK(problem != nullptr); + CHECK(summary != nullptr); double start_time = WallTimeInSeconds(); *summary = Summary(); @@ -488,18 +491,14 @@ void Solver::Solve(const Solver::Options& options, return; } - ProblemImpl* problem_impl = problem->problem_impl_.get(); + ProblemImpl* problem_impl = problem->impl_.get(); Program* program = problem_impl->mutable_program(); PreSolveSummarize(options, problem_impl, summary); - // Make sure that all the parameter blocks states are set to the - // values provided by the user. - program->SetParameterBlockStatePtrsToUserStatePtrs(); - // If gradient_checking is enabled, wrap all cost functions in a // gradient checker and install a callback that terminates if any gradient // error is detected. - scoped_ptr<internal::ProblemImpl> gradient_checking_problem; + std::unique_ptr<internal::ProblemImpl> gradient_checking_problem; internal::GradientCheckingIterationCallback gradient_checking_callback; Solver::Options modified_options = options; if (options.check_gradients) { @@ -514,10 +513,46 @@ void Solver::Solve(const Solver::Options& options, program = problem_impl->mutable_program(); } - scoped_ptr<Preprocessor> preprocessor( + // Make sure that all the parameter blocks states are set to the + // values provided by the user. + program->SetParameterBlockStatePtrsToUserStatePtrs(); + + // The main thread also does work so we only need to launch num_threads - 1. + problem_impl->context()->EnsureMinimumThreads(options.num_threads - 1); + + std::unique_ptr<Preprocessor> preprocessor( Preprocessor::Create(modified_options.minimizer_type)); PreprocessedProblem pp; + const bool status = preprocessor->Preprocess(modified_options, problem_impl, &pp); + + // We check the linear_solver_options.type rather than + // modified_options.linear_solver_type because, depending on the + // lack of a Schur structure, the preprocessor may change the linear + // solver type. + if (IsSchurType(pp.linear_solver_options.type)) { + // TODO(sameeragarwal): We can likely eliminate the duplicate call + // to DetectStructure here and inside the linear solver, by + // calling this in the preprocessor. + int row_block_size; + int e_block_size; + int f_block_size; + DetectStructure(*static_cast<internal::BlockSparseMatrix*>( + pp.minimizer_options.jacobian.get()) + ->block_structure(), + pp.linear_solver_options.elimination_groups[0], + &row_block_size, + &e_block_size, + &f_block_size); + summary->schur_structure_given = + SchurStructureToString(row_block_size, e_block_size, f_block_size); + internal::GetBestSchurTemplateSpecialization(&row_block_size, + &e_block_size, + &f_block_size); + summary->schur_structure_used = + SchurStructureToString(row_block_size, e_block_size, f_block_size); + } + summary->fixed_cost = pp.fixed_cost; summary->preprocessor_time_in_seconds = WallTimeInSeconds() - start_time; @@ -531,7 +566,7 @@ void Solver::Solve(const Solver::Options& options, } const double postprocessor_start_time = WallTimeInSeconds(); - problem_impl = problem->problem_impl_.get(); + problem_impl = problem->impl_.get(); program = problem_impl->mutable_program(); // On exit, ensure that the parameter blocks again point at the user // provided values and the parameter blocks are numbered according @@ -559,66 +594,6 @@ void Solve(const Solver::Options& options, solver.Solve(options, problem, summary); } -Solver::Summary::Summary() - // Invalid values for most fields, to ensure that we are not - // accidentally reporting default values. - : minimizer_type(TRUST_REGION), - termination_type(FAILURE), - message("ceres::Solve was not called."), - initial_cost(-1.0), - final_cost(-1.0), - fixed_cost(-1.0), - num_successful_steps(-1), - num_unsuccessful_steps(-1), - num_inner_iteration_steps(-1), - num_line_search_steps(-1), - preprocessor_time_in_seconds(-1.0), - minimizer_time_in_seconds(-1.0), - postprocessor_time_in_seconds(-1.0), - total_time_in_seconds(-1.0), - 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), - line_search_cost_evaluation_time_in_seconds(-1.0), - line_search_gradient_evaluation_time_in_seconds(-1.0), - line_search_polynomial_minimization_time_in_seconds(-1.0), - line_search_total_time_in_seconds(-1.0), - num_parameter_blocks(-1), - num_parameters(-1), - num_effective_parameters(-1), - num_residual_blocks(-1), - num_residuals(-1), - num_parameter_blocks_reduced(-1), - num_parameters_reduced(-1), - num_effective_parameters_reduced(-1), - num_residual_blocks_reduced(-1), - num_residuals_reduced(-1), - is_constrained(false), - num_threads_given(-1), - num_threads_used(-1), - num_linear_solver_threads_given(-1), - 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_given(IDENTITY), - preconditioner_type_used(IDENTITY), - visibility_clustering_type(CANONICAL_VIEWS), - trust_region_strategy_type(LEVENBERG_MARQUARDT), - dense_linear_algebra_library_type(EIGEN), - sparse_linear_algebra_library_type(SUITE_SPARSE), - line_search_direction_type(LBFGS), - line_search_type(ARMIJO), - line_search_interpolation_type(BISECTION), - nonlinear_conjugate_gradient_type(FLETCHER_REEVES), - max_lbfgs_rank(-1) { -} - -using internal::StringAppendF; -using internal::StringPrintf; - string Solver::Summary::BriefReport() const { return StringPrintf("Ceres Solver Report: " "Iterations: %d, " @@ -647,7 +622,7 @@ string Solver::Summary::FullReport() const { } StringAppendF(&report, "Residual blocks % 25d% 25d\n", num_residual_blocks, num_residual_blocks_reduced); - StringAppendF(&report, "Residual % 25d% 25d\n", + StringAppendF(&report, "Residuals % 25d% 25d\n", num_residuals, num_residuals_reduced); if (minimizer_type == TRUST_REGION) { @@ -708,9 +683,6 @@ string Solver::Summary::FullReport() const { } 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, - num_linear_solver_threads_used); string given; StringifyOrdering(linear_solver_ordering_given, &given); @@ -720,6 +692,12 @@ string Solver::Summary::FullReport() const { "Linear solver ordering %22s %24s\n", given.c_str(), used.c_str()); + if (IsSchurType(linear_solver_type_used)) { + StringAppendF(&report, + "Schur structure %22s %24s\n", + schur_structure_given.c_str(), + schur_structure_used.c_str()); + } if (inner_iterations_given) { StringAppendF(&report, @@ -808,44 +786,44 @@ string Solver::Summary::FullReport() const { } StringAppendF(&report, "\nTime (in seconds):\n"); - StringAppendF(&report, "Preprocessor %25.4f\n", + StringAppendF(&report, "Preprocessor %25.6f\n", preprocessor_time_in_seconds); - StringAppendF(&report, "\n Residual evaluation %23.4f\n", - residual_evaluation_time_in_seconds); + StringAppendF(&report, "\n Residual only evaluation %18.6f (%d)\n", + residual_evaluation_time_in_seconds, num_residual_evaluations); if (line_search_used) { - StringAppendF(&report, " Line search cost evaluation %10.4f\n", + StringAppendF(&report, " Line search cost evaluation %10.6f\n", line_search_cost_evaluation_time_in_seconds); } - StringAppendF(&report, " Jacobian evaluation %23.4f\n", - jacobian_evaluation_time_in_seconds); + StringAppendF(&report, " Jacobian & residual evaluation %12.6f (%d)\n", + jacobian_evaluation_time_in_seconds, num_jacobian_evaluations); if (line_search_used) { - StringAppendF(&report, " Line search gradient evaluation %6.4f\n", + StringAppendF(&report, " Line search gradient evaluation %6.6f\n", line_search_gradient_evaluation_time_in_seconds); } if (minimizer_type == TRUST_REGION) { - StringAppendF(&report, " Linear solver %23.4f\n", - linear_solver_time_in_seconds); + StringAppendF(&report, " Linear solver %23.6f (%d)\n", + linear_solver_time_in_seconds, num_linear_solves); } if (inner_iterations_used) { - StringAppendF(&report, " Inner iterations %23.4f\n", + StringAppendF(&report, " Inner iterations %23.6f\n", inner_iteration_time_in_seconds); } if (line_search_used) { - StringAppendF(&report, " Line search polynomial minimization %.4f\n", + StringAppendF(&report, " Line search polynomial minimization %.6f\n", line_search_polynomial_minimization_time_in_seconds); } - StringAppendF(&report, "Minimizer %25.4f\n\n", + StringAppendF(&report, "Minimizer %25.6f\n\n", minimizer_time_in_seconds); - StringAppendF(&report, "Postprocessor %24.4f\n", + StringAppendF(&report, "Postprocessor %24.6f\n", postprocessor_time_in_seconds); - StringAppendF(&report, "Total %25.4f\n\n", + StringAppendF(&report, "Total %25.6f\n\n", total_time_in_seconds); StringAppendF(&report, "Termination: %25s (%s)\n", diff --git a/extern/ceres/internal/ceres/solver_utils.cc b/extern/ceres/internal/ceres/solver_utils.cc index 7f4ff7eb940..177a928e090 100644 --- a/extern/ceres/internal/ceres/solver_utils.cc +++ b/extern/ceres/internal/ceres/solver_utils.cc @@ -30,6 +30,8 @@ #include <string> +#include "ceres/internal/config.h" + #include "Eigen/Core" #include "ceres/internal/port.h" #include "ceres/solver_utils.h" @@ -61,6 +63,10 @@ std::string VersionString() { value += "-cxsparse-(" + std::string(CERES_CXSPARSE_VERSION) + ")"; #endif +#ifndef CERES_NO_ACCELERATE_SPARSE + value += "-acceleratesparse"; +#endif + #ifdef CERES_USE_EIGEN_SPARSE value += "-eigensparse"; #endif diff --git a/extern/ceres/internal/ceres/sparse_cholesky.cc b/extern/ceres/internal/ceres/sparse_cholesky.cc new file mode 100644 index 00000000000..d9d2100d3f9 --- /dev/null +++ b/extern/ceres/internal/ceres/sparse_cholesky.cc @@ -0,0 +1,163 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/sparse_cholesky.h" + +#include "ceres/accelerate_sparse.h" +#include "ceres/cxsparse.h" +#include "ceres/eigensparse.h" +#include "ceres/float_cxsparse.h" +#include "ceres/float_suitesparse.h" +#include "ceres/iterative_refiner.h" +#include "ceres/suitesparse.h" + +namespace ceres { +namespace internal { + +std::unique_ptr<SparseCholesky> SparseCholesky::Create( + const LinearSolver::Options& options) { + const OrderingType ordering_type = options.use_postordering ? AMD : NATURAL; + std::unique_ptr<SparseCholesky> sparse_cholesky; + + switch (options.sparse_linear_algebra_library_type) { + case SUITE_SPARSE: +#ifndef CERES_NO_SUITESPARSE + if (options.use_mixed_precision_solves) { + sparse_cholesky = FloatSuiteSparseCholesky::Create(ordering_type); + } else { + sparse_cholesky = SuiteSparseCholesky::Create(ordering_type); + } + break; +#else + (void)ordering_type; + LOG(FATAL) << "Ceres was compiled without support for SuiteSparse."; +#endif + + case EIGEN_SPARSE: +#ifdef CERES_USE_EIGEN_SPARSE + if (options.use_mixed_precision_solves) { + sparse_cholesky = FloatEigenSparseCholesky::Create(ordering_type); + } else { + sparse_cholesky = EigenSparseCholesky::Create(ordering_type); + } + break; +#else + LOG(FATAL) << "Ceres was compiled without support for " + << "Eigen's sparse Cholesky factorization routines."; +#endif + + case CX_SPARSE: +#ifndef CERES_NO_CXSPARSE + if (options.use_mixed_precision_solves) { + sparse_cholesky = FloatCXSparseCholesky::Create(ordering_type); + } else { + sparse_cholesky = CXSparseCholesky::Create(ordering_type); + } + break; +#else + LOG(FATAL) << "Ceres was compiled without support for CXSparse."; +#endif + + case ACCELERATE_SPARSE: +#ifndef CERES_NO_ACCELERATE_SPARSE + if (options.use_mixed_precision_solves) { + sparse_cholesky = AppleAccelerateCholesky<float>::Create(ordering_type); + } else { + sparse_cholesky = AppleAccelerateCholesky<double>::Create(ordering_type); + } + break; +#else + LOG(FATAL) << "Ceres was compiled without support for Apple's Accelerate " + << "framework solvers."; +#endif + + default: + LOG(FATAL) << "Unknown sparse linear algebra library type : " + << SparseLinearAlgebraLibraryTypeToString( + options.sparse_linear_algebra_library_type); + } + + if (options.max_num_refinement_iterations > 0) { + std::unique_ptr<IterativeRefiner> refiner( + new IterativeRefiner(options.max_num_refinement_iterations)); + sparse_cholesky = std::unique_ptr<SparseCholesky>(new RefinedSparseCholesky( + std::move(sparse_cholesky), std::move(refiner))); + } + return sparse_cholesky; +} + +SparseCholesky::~SparseCholesky() {} + +LinearSolverTerminationType SparseCholesky::FactorAndSolve( + CompressedRowSparseMatrix* lhs, + const double* rhs, + double* solution, + std::string* message) { + LinearSolverTerminationType termination_type = Factorize(lhs, message); + if (termination_type == LINEAR_SOLVER_SUCCESS) { + termination_type = Solve(rhs, solution, message); + } + return termination_type; +} + +RefinedSparseCholesky::RefinedSparseCholesky( + std::unique_ptr<SparseCholesky> sparse_cholesky, + std::unique_ptr<IterativeRefiner> iterative_refiner) + : sparse_cholesky_(std::move(sparse_cholesky)), + iterative_refiner_(std::move(iterative_refiner)) {} + +RefinedSparseCholesky::~RefinedSparseCholesky() {} + +CompressedRowSparseMatrix::StorageType RefinedSparseCholesky::StorageType() + const { + return sparse_cholesky_->StorageType(); +} + +LinearSolverTerminationType RefinedSparseCholesky::Factorize( + CompressedRowSparseMatrix* lhs, std::string* message) { + lhs_ = lhs; + return sparse_cholesky_->Factorize(lhs, message); +} + +LinearSolverTerminationType RefinedSparseCholesky::Solve(const double* rhs, + double* solution, + std::string* message) { + CHECK(lhs_ != nullptr); + auto termination_type = sparse_cholesky_->Solve(rhs, solution, message); + if (termination_type != LINEAR_SOLVER_SUCCESS) { + return termination_type; + } + + iterative_refiner_->Refine(*lhs_, rhs, sparse_cholesky_.get(), solution); + return LINEAR_SOLVER_SUCCESS; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/sparse_cholesky.h b/extern/ceres/internal/ceres/sparse_cholesky.h new file mode 100644 index 00000000000..bbe42370505 --- /dev/null +++ b/extern/ceres/internal/ceres/sparse_cholesky.h @@ -0,0 +1,138 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_SPARSE_CHOLESKY_H_ +#define CERES_INTERNAL_SPARSE_CHOLESKY_H_ + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#include <memory> +#include "ceres/linear_solver.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// An interface that abstracts away the internal details of various +// sparse linear algebra libraries and offers a simple API for solving +// symmetric positive definite linear systems using a sparse Cholesky +// factorization. +// +// Instances of SparseCholesky are expected to cache the symbolic +// factorization of the linear system. They do this on the first call +// to Factorize or FactorAndSolve. Subsequent calls to Factorize and +// FactorAndSolve are expected to have the same sparsity structure. +// +// Example usage: +// +// std::unique_ptr<SparseCholesky> +// sparse_cholesky(SparseCholesky::Create(SUITE_SPARSE, AMD)); +// +// CompressedRowSparseMatrix lhs = ...; +// std::string message; +// CHECK_EQ(sparse_cholesky->Factorize(&lhs, &message), LINEAR_SOLVER_SUCCESS); +// Vector rhs = ...; +// Vector solution = ...; +// CHECK_EQ(sparse_cholesky->Solve(rhs.data(), solution.data(), &message), +// LINEAR_SOLVER_SUCCESS); + +class SparseCholesky { + public: + static std::unique_ptr<SparseCholesky> Create( + const LinearSolver::Options& options); + + virtual ~SparseCholesky(); + + // Due to the symmetry of the linear system, sparse linear algebra + // libraries only use one half of the input matrix. Whether it is + // the upper or the lower triangular part of the matrix depends on + // the library and the re-ordering strategy being used. This + // function tells the user the storage type expected of the input + // matrix for the sparse linear algebra library and reordering + // strategy used. + virtual CompressedRowSparseMatrix::StorageType StorageType() const = 0; + + // Computes the numeric factorization of the given matrix. If this + // is the first call to Factorize, first the symbolic factorization + // will be computed and cached and the numeric factorization will be + // computed based on that. + // + // Subsequent calls to Factorize will use that symbolic + // factorization assuming that the sparsity of the matrix has + // remained constant. + virtual LinearSolverTerminationType Factorize( + CompressedRowSparseMatrix* lhs, std::string* message) = 0; + + // Computes the solution to the equation + // + // lhs * solution = rhs + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) = 0; + + // Convenience method which combines a call to Factorize and + // Solve. Solve is only called if Factorize returns + // LINEAR_SOLVER_SUCCESS. + virtual LinearSolverTerminationType FactorAndSolve( + CompressedRowSparseMatrix* lhs, + const double* rhs, + double* solution, + std::string* message); + +}; + +class IterativeRefiner; + +// Computes an initial solution using the given instance of +// SparseCholesky, and then refines it using the IterativeRefiner. +class RefinedSparseCholesky : public SparseCholesky { + public: + RefinedSparseCholesky(std::unique_ptr<SparseCholesky> sparse_cholesky, + std::unique_ptr<IterativeRefiner> iterative_refiner); + virtual ~RefinedSparseCholesky(); + + virtual CompressedRowSparseMatrix::StorageType StorageType() const; + virtual LinearSolverTerminationType Factorize( + CompressedRowSparseMatrix* lhs, std::string* message); + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message); + + private: + std::unique_ptr<SparseCholesky> sparse_cholesky_; + std::unique_ptr<IterativeRefiner> iterative_refiner_; + CompressedRowSparseMatrix* lhs_ = nullptr; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SPARSE_CHOLESKY_H_ diff --git a/extern/ceres/internal/ceres/sparse_matrix.h b/extern/ceres/internal/ceres/sparse_matrix.h index b3af1d06440..074d847807e 100644 --- a/extern/ceres/internal/ceres/sparse_matrix.h +++ b/extern/ceres/internal/ceres/sparse_matrix.h @@ -90,7 +90,7 @@ class SparseMatrix : public LinearOperator { virtual void ToTextFile(FILE* file) const = 0; // Accessors for the values array that stores the entries of the - // sparse matrix. The exact interpreptation of the values of this + // sparse matrix. The exact interpretation of the values of this // array depends on the particular kind of SparseMatrix being // accessed. virtual double* mutable_values() = 0; diff --git a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc index a4c2c766ddc..0f2e589d041 100644 --- a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc +++ b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,461 +33,82 @@ #include <algorithm> #include <cstring> #include <ctime> -#include <sstream> +#include <memory> -#include "ceres/compressed_row_sparse_matrix.h" -#include "ceres/cxsparse.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/inner_product_computer.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/iterative_refiner.h" #include "ceres/linear_solver.h" -#include "ceres/suitesparse.h" +#include "ceres/sparse_cholesky.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" #include "ceres/wall_time.h" -#include "Eigen/SparseCore" - -#ifdef CERES_USE_EIGEN_SPARSE -#include "Eigen/SparseCholesky" -#endif namespace ceres { namespace internal { -namespace { - -#ifdef CERES_USE_EIGEN_SPARSE -// A templated factorized and solve function, which allows us to use -// the same code independent of whether a AMD or a Natural ordering is -// used. -template <typename SimplicialCholeskySolver, typename SparseMatrixType> -LinearSolver::Summary SimplicialLDLTSolve( - const SparseMatrixType& lhs, - const bool do_symbolic_analysis, - SimplicialCholeskySolver* solver, - double* rhs_and_solution, - EventLogger* event_logger) { - LinearSolver::Summary summary; - summary.num_iterations = 1; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.message = "Success."; - - if (do_symbolic_analysis) { - solver->analyzePattern(lhs); - if (VLOG_IS_ON(2)) { - std::stringstream ss; - solver->dumpMemory(ss); - VLOG(2) << "Symbolic Analysis\n" - << ss.str(); - } - event_logger->AddEvent("Analyze"); - if (solver->info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "Eigen failure. Unable to find symbolic factorization."; - return summary; - } - } - - solver->factorize(lhs); - event_logger->AddEvent("Factorize"); - if (solver->info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "Eigen failure. Unable to find numeric factorization."; - return summary; - } - - const Vector rhs = VectorRef(rhs_and_solution, lhs.cols()); - - VectorRef(rhs_and_solution, lhs.cols()) = solver->solve(rhs); - event_logger->AddEvent("Solve"); - if (solver->info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "Eigen failure. Unable to do triangular solve."; - return summary; - } - - return summary; -} - -#endif // CERES_USE_EIGEN_SPARSE - -#ifndef CERES_NO_CXSPARSE -LinearSolver::Summary ComputeNormalEquationsAndSolveUsingCXSparse( - CompressedRowSparseMatrix* A, - double * rhs_and_solution, - EventLogger* event_logger) { - LinearSolver::Summary summary; - summary.num_iterations = 1; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.message = "Success."; - - CXSparse cxsparse; - - // Wrap the augmented Jacobian in a compressed sparse column matrix. - cs_di a_transpose = cxsparse.CreateSparseMatrixTransposeView(A); - - // Compute the normal equations. J'J delta = J'f and solve them - // using a sparse Cholesky factorization. Notice that when compared - // to SuiteSparse we have to explicitly compute the transpose of Jt, - // and then the normal equations before they can be - // factorized. CHOLMOD/SuiteSparse on the other hand can just work - // off of Jt to compute the Cholesky factorization of the normal - // equations. - cs_di* a = cxsparse.TransposeMatrix(&a_transpose); - cs_di* lhs = cxsparse.MatrixMatrixMultiply(&a_transpose, a); - cxsparse.Free(a); - event_logger->AddEvent("NormalEquations"); - - cs_dis* factor = cxsparse.AnalyzeCholesky(lhs); - event_logger->AddEvent("Analysis"); - - if (factor == NULL) { - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = "CXSparse::AnalyzeCholesky failed."; - } else if (!cxsparse.SolveCholesky(lhs, factor, rhs_and_solution)) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "CXSparse::SolveCholesky failed."; - } - event_logger->AddEvent("Solve"); - - cxsparse.Free(lhs); - cxsparse.Free(factor); - event_logger->AddEvent("TearDown"); - return summary; -} - -#endif // CERES_NO_CXSPARSE - -} // namespace SparseNormalCholeskySolver::SparseNormalCholeskySolver( const LinearSolver::Options& options) - : factor_(NULL), - cxsparse_factor_(NULL), - options_(options) { + : options_(options) { + sparse_cholesky_ = SparseCholesky::Create(options); } -void SparseNormalCholeskySolver::FreeFactorization() { - if (factor_ != NULL) { - ss_.Free(factor_); - factor_ = NULL; - } - - if (cxsparse_factor_ != NULL) { - cxsparse_.Free(cxsparse_factor_); - cxsparse_factor_ = NULL; - } -} - -SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { - FreeFactorization(); -} +SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {} LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( - CompressedRowSparseMatrix* A, + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& per_solve_options, - double * x) { - - const int num_cols = A->num_cols(); - VectorRef(x, num_cols).setZero(); - A->LeftMultiply(b, x); - - 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. - scoped_ptr<CompressedRowSparseMatrix> regularizer; - if (A->col_blocks().size() > 0) { - regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( - per_solve_options.D, A->col_blocks())); - } else { - regularizer.reset(new CompressedRowSparseMatrix( - per_solve_options.D, num_cols)); - } - A->AppendRows(*regularizer); - } - - LinearSolver::Summary summary; - switch (options_.sparse_linear_algebra_library_type) { - case SUITE_SPARSE: - summary = SolveImplUsingSuiteSparse(A, x); - break; - case CX_SPARSE: - summary = SolveImplUsingCXSparse(A, x); - break; - case EIGEN_SPARSE: - summary = SolveImplUsingEigen(A, x); - break; - default: - LOG(FATAL) << "Unknown sparse linear algebra library : " - << options_.sparse_linear_algebra_library_type; - } - - if (per_solve_options.D != NULL) { - A->DeleteRows(num_cols); - } - - return summary; -} - -LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( - CompressedRowSparseMatrix* A, - double * rhs_and_solution) { -#ifndef CERES_USE_EIGEN_SPARSE - - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE " - "because Ceres was not built with support for " - "Eigen's SimplicialLDLT decomposition. " - "This requires enabling building with -DEIGENSPARSE=ON."; - return summary; - -#else - - EventLogger event_logger("SparseNormalCholeskySolver::Eigen::Solve"); - // Compute the normal equations. J'J delta = J'f and solve them - // using a sparse Cholesky factorization. Notice that when compared - // to SuiteSparse we have to explicitly compute the normal equations - // before they can be factorized. CHOLMOD/SuiteSparse on the other - // hand can just work off of Jt to compute the Cholesky - // factorization of the normal equations. - - if (options_.dynamic_sparsity) { - // In the case where the problem has dynamic sparsity, it is not - // worth using the ComputeOuterProduct routine, as the setup cost - // is not amortized over multiple calls to Solve. - Eigen::MappedSparseMatrix<double, Eigen::RowMajor> a( - A->num_rows(), - A->num_cols(), - A->num_nonzeros(), - A->mutable_rows(), - A->mutable_cols(), - A->mutable_values()); - - Eigen::SparseMatrix<double> lhs = a.transpose() * a; - Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver; - return SimplicialLDLTSolve(lhs, - true, - &solver, - rhs_and_solution, - &event_logger); - } - - if (outer_product_.get() == NULL) { - outer_product_.reset( - CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( - *A, &pattern_)); - } - - CompressedRowSparseMatrix::ComputeOuterProduct( - *A, pattern_, outer_product_.get()); - - // Map to an upper triangular column major matrix. - // - // outer_product_ is a compressed row sparse matrix and in lower - // triangular form, when mapped to a compressed column sparse - // matrix, it becomes an upper triangular matrix. - Eigen::MappedSparseMatrix<double, Eigen::ColMajor> lhs( - outer_product_->num_rows(), - outer_product_->num_rows(), - outer_product_->num_nonzeros(), - outer_product_->mutable_rows(), - outer_product_->mutable_cols(), - outer_product_->mutable_values()); - - bool do_symbolic_analysis = false; - - // If using post ordering or an old version of Eigen, we cannot - // depend on a preordered jacobian, so we work with a SimplicialLDLT - // decomposition with AMD ordering. - if (options_.use_postordering || - !EIGEN_VERSION_AT_LEAST(3, 2, 2)) { - if (amd_ldlt_.get() == NULL) { - amd_ldlt_.reset(new SimplicialLDLTWithAMDOrdering); - do_symbolic_analysis = true; - } - - return SimplicialLDLTSolve(lhs, - do_symbolic_analysis, - amd_ldlt_.get(), - rhs_and_solution, - &event_logger); - } - -#if EIGEN_VERSION_AT_LEAST(3,2,2) - // The common case - if (natural_ldlt_.get() == NULL) { - natural_ldlt_.reset(new SimplicialLDLTWithNaturalOrdering); - do_symbolic_analysis = true; - } - - return SimplicialLDLTSolve(lhs, - do_symbolic_analysis, - natural_ldlt_.get(), - rhs_and_solution, - &event_logger); -#endif - -#endif // EIGEN_USE_EIGEN_SPARSE -} - -LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( - CompressedRowSparseMatrix* A, - double * rhs_and_solution) { -#ifdef CERES_NO_CXSPARSE - - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE " - "because Ceres was not built with support for CXSparse. " - "This requires enabling building with -DCXSPARSE=ON."; - - return summary; - -#else - - EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve"); - if (options_.dynamic_sparsity) { - return ComputeNormalEquationsAndSolveUsingCXSparse(A, - rhs_and_solution, - &event_logger); - } - + double* x) { + EventLogger event_logger("SparseNormalCholeskySolver::Solve"); LinearSolver::Summary summary; summary.num_iterations = 1; summary.termination_type = LINEAR_SOLVER_SUCCESS; summary.message = "Success."; - // Compute the normal equations. J'J delta = J'f and solve them - // using a sparse Cholesky factorization. Notice that when compared - // to SuiteSparse we have to explicitly compute the normal equations - // before they can be factorized. CHOLMOD/SuiteSparse on the other - // hand can just work off of Jt to compute the Cholesky - // factorization of the normal equations. - if (outer_product_.get() == NULL) { - outer_product_.reset( - CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( - *A, &pattern_)); - } - - CompressedRowSparseMatrix::ComputeOuterProduct( - *A, pattern_, outer_product_.get()); - cs_di lhs = - cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get()); - - event_logger.AddEvent("Setup"); - - // Compute symbolic factorization if not available. - if (cxsparse_factor_ == NULL) { - if (options_.use_postordering) { - cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(&lhs, - A->col_blocks(), - A->col_blocks()); - } else { - cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(&lhs); - } - } - event_logger.AddEvent("Analysis"); - - if (cxsparse_factor_ == NULL) { - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "CXSparse failure. Unable to find symbolic factorization."; - } else if (!cxsparse_.SolveCholesky(&lhs, - cxsparse_factor_, - rhs_and_solution)) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = "CXSparse::SolveCholesky failed."; - } - event_logger.AddEvent("Solve"); - - return summary; -#endif -} - -LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( - CompressedRowSparseMatrix* A, - double * rhs_and_solution) { -#ifdef CERES_NO_SUITESPARSE - - LinearSolver::Summary summary; - summary.num_iterations = 0; - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - summary.message = - "SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE " - "because Ceres was not built with support for SuiteSparse. " - "This requires enabling building with -DSUITESPARSE=ON."; - return summary; - -#else - - EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve"); - LinearSolver::Summary summary; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - summary.num_iterations = 1; - summary.message = "Success."; - const int num_cols = A->num_cols(); - cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A); - event_logger.AddEvent("Setup"); - - if (options_.dynamic_sparsity) { - FreeFactorization(); - } + VectorRef xref(x, num_cols); + xref.setZero(); + rhs_.resize(num_cols); + rhs_.setZero(); + A->LeftMultiply(b, rhs_.data()); + event_logger.AddEvent("Compute RHS"); - if (factor_ == NULL) { - if (options_.use_postordering) { - factor_ = ss_.BlockAnalyzeCholesky(&lhs, - A->col_blocks(), - A->row_blocks(), - &summary.message); - } else { - if (options_.dynamic_sparsity) { - factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message); - } else { - factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs, - &summary.message); - } - } + 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. + std::unique_ptr<BlockSparseMatrix> regularizer; + regularizer.reset(BlockSparseMatrix::CreateDiagonalMatrix( + per_solve_options.D, A->block_structure()->cols)); + event_logger.AddEvent("Diagonal"); + A->AppendRows(*regularizer); + event_logger.AddEvent("Append"); } - event_logger.AddEvent("Analysis"); + event_logger.AddEvent("Append Rows"); - if (factor_ == NULL) { - summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; - // No need to set message as it has already been set by the - // symbolic analysis routines above. - return summary; - } + if (inner_product_computer_.get() == NULL) { + inner_product_computer_.reset( + InnerProductComputer::Create(*A, sparse_cholesky_->StorageType())); - summary.termination_type = ss_.Cholesky(&lhs, factor_, &summary.message); - if (summary.termination_type != LINEAR_SOLVER_SUCCESS) { - return summary; + event_logger.AddEvent("InnerProductComputer::Create"); } - cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution, - num_cols, - num_cols); - cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message); - event_logger.AddEvent("Solve"); + inner_product_computer_->Compute(); + event_logger.AddEvent("InnerProductComputer::Compute"); - ss_.Free(rhs); - if (solution != NULL) { - memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution)); - ss_.Free(solution); - } else { - // No need to set message as it has already been set by the - // numeric factorization routine above. - summary.termination_type = LINEAR_SOLVER_FAILURE; + if (per_solve_options.D != NULL) { + A->DeleteRowBlocks(A->block_structure()->cols.size()); } - event_logger.AddEvent("Teardown"); + summary.termination_type = sparse_cholesky_->FactorAndSolve( + inner_product_computer_->mutable_result(), + rhs_.data(), + x, + &summary.message); + event_logger.AddEvent("SparseCholesky::FactorAndSolve"); return summary; -#endif } -} // namespace internal -} // namespace ceres +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h index 2a93bc56d29..cbff2bdb3f6 100644 --- a/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h +++ b/extern/ceres/internal/ceres/sparse_normal_cholesky_solver.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -34,91 +34,40 @@ #ifndef CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ #define CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ -#include <vector> - // This include must come before any #ifndef check on Ceres compile options. #include "ceres/internal/port.h" -#include "ceres/internal/macros.h" +#include <vector> #include "ceres/linear_solver.h" -#include "ceres/suitesparse.h" -#include "ceres/cxsparse.h" - -#ifdef CERES_USE_EIGEN_SPARSE -#include "Eigen/SparseCholesky" -#endif namespace ceres { namespace internal { class CompressedRowSparseMatrix; +class InnerProductComputer; +class SparseCholesky; -// Solves the normal equations (A'A + D'D) x = A'b, using the CHOLMOD sparse -// cholesky solver. -class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { +// Solves the normal equations (A'A + D'D) x = A'b, using the sparse +// linear algebra library of the user's choice. +class SparseNormalCholeskySolver : public BlockSparseMatrixSolver { public: explicit SparseNormalCholeskySolver(const LinearSolver::Options& options); + SparseNormalCholeskySolver(const SparseNormalCholeskySolver&) = delete; + void operator=(const SparseNormalCholeskySolver&) = delete; + virtual ~SparseNormalCholeskySolver(); private: - virtual LinearSolver::Summary SolveImpl( - CompressedRowSparseMatrix* A, + LinearSolver::Summary SolveImpl( + BlockSparseMatrix* A, const double* b, const LinearSolver::PerSolveOptions& options, - double* x); - - LinearSolver::Summary SolveImplUsingSuiteSparse( - CompressedRowSparseMatrix* A, - double* rhs_and_solution); - - - LinearSolver::Summary SolveImplUsingCXSparse( - CompressedRowSparseMatrix* A, - double* rhs_and_solution); - - LinearSolver::Summary SolveImplUsingEigen( - CompressedRowSparseMatrix* A, - double* rhs_and_solution); - - void FreeFactorization(); - - SuiteSparse ss_; - // Cached factorization - cholmod_factor* factor_; - - CXSparse cxsparse_; - // Cached factorization - cs_dis* cxsparse_factor_; - -#ifdef CERES_USE_EIGEN_SPARSE - - // The preprocessor gymnastics here are dealing with the fact that - // before version 3.2.2, Eigen did not support a third template - // parameter to specify the ordering. -#if EIGEN_VERSION_AT_LEAST(3,2,2) - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper, - Eigen::NaturalOrdering<int> > - SimplicialLDLTWithNaturalOrdering; - scoped_ptr<SimplicialLDLTWithNaturalOrdering> natural_ldlt_; - - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper, - Eigen::AMDOrdering<int> > - SimplicialLDLTWithAMDOrdering; - scoped_ptr<SimplicialLDLTWithAMDOrdering> amd_ldlt_; - -#else - typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper> - SimplicialLDLTWithAMDOrdering; - - scoped_ptr<SimplicialLDLTWithAMDOrdering> amd_ldlt_; -#endif - -#endif + double* x) final; - scoped_ptr<CompressedRowSparseMatrix> outer_product_; - std::vector<int> pattern_; const LinearSolver::Options options_; - CERES_DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); + Vector rhs_; + std::unique_ptr<SparseCholesky> sparse_cholesky_; + std::unique_ptr<InnerProductComputer> inner_product_computer_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/split.cc b/extern/ceres/internal/ceres/split.cc index 296c09a6440..3a09e866839 100644 --- a/extern/ceres/internal/ceres/split.cc +++ b/extern/ceres/internal/ceres/split.cc @@ -115,7 +115,7 @@ void SplitStringUsing(const string& full, const char* delim, vector<string>* result) { result->reserve(result->size() + CalculateReserveForVector(full, delim)); - std::back_insert_iterator<vector<string> > it(*result); + std::back_insert_iterator<vector<string>> it(*result); SplitStringToIteratorUsing(full, delim, it); } diff --git a/extern/ceres/internal/ceres/stringprintf.cc b/extern/ceres/internal/ceres/stringprintf.cc index b3b7474d8f8..7a21f0e2118 100644 --- a/extern/ceres/internal/ceres/stringprintf.cc +++ b/extern/ceres/internal/ceres/stringprintf.cc @@ -43,28 +43,6 @@ namespace internal { using std::string; -// va_copy() was defined in the C99 standard. However, it did not appear in the -// C++ standard until C++11. This means that if Ceres is being compiled with a -// strict pre-C++11 standard (e.g. -std=c++03), va_copy() will NOT be defined, -// as we are using the C++ compiler (it would however be defined if we were -// using the C compiler). Note however that both GCC & Clang will in fact -// define va_copy() when compiling for C++ if the C++ standard is not explicitly -// specified (i.e. no -std=c++<XX> arg), even though it should not strictly be -// defined unless -std=c++11 (or greater) was passed. -#if !defined(va_copy) -#if defined (__GNUC__) -// On GCC/Clang, if va_copy() is not defined (C++ standard < C++11 explicitly -// specified), use the internal __va_copy() version, which should be present -// in even very old GCC versions. -#define va_copy(d, s) __va_copy(d, s) -#else -// Some older versions of MSVC do not have va_copy(), in which case define it. -// Although this is required for older MSVC versions, it should also work for -// other non-GCC/Clang compilers which also do not defined va_copy(). -#define va_copy(d, s) ((d) = (s)) -#endif // defined (__GNUC__) -#endif // !defined(va_copy) - void StringAppendV(string* dst, const char* format, va_list ap) { // First try with a small fixed size buffer char space[1024]; diff --git a/extern/ceres/internal/ceres/subset_preconditioner.cc b/extern/ceres/internal/ceres/subset_preconditioner.cc new file mode 100644 index 00000000000..7c24ae9f288 --- /dev/null +++ b/extern/ceres/internal/ceres/subset_preconditioner.cc @@ -0,0 +1,117 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/subset_preconditioner.h" + +#include <memory> +#include <string> +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/inner_product_computer.h" +#include "ceres/linear_solver.h" +#include "ceres/sparse_cholesky.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +SubsetPreconditioner::SubsetPreconditioner( + const Preconditioner::Options& options, const BlockSparseMatrix& A) + : options_(options), num_cols_(A.num_cols()) { + CHECK_GE(options_.subset_preconditioner_start_row_block, 0) + << "Congratulations, you found a bug in Ceres. Please report it."; + + LinearSolver::Options sparse_cholesky_options; + sparse_cholesky_options.sparse_linear_algebra_library_type = + options_.sparse_linear_algebra_library_type; + sparse_cholesky_options.use_postordering = + options_.use_postordering; + sparse_cholesky_ = SparseCholesky::Create(sparse_cholesky_options); +} + +SubsetPreconditioner::~SubsetPreconditioner() {} + +void SubsetPreconditioner::RightMultiply(const double* x, double* y) const { + CHECK(x != nullptr); + CHECK(y != nullptr); + std::string message; + sparse_cholesky_->Solve(x, y, &message); +} + +bool SubsetPreconditioner::UpdateImpl(const BlockSparseMatrix& A, + const double* D) { + BlockSparseMatrix* m = const_cast<BlockSparseMatrix*>(&A); + const CompressedRowBlockStructure* bs = m->block_structure(); + + // A = [P] + // [Q] + + // Now add D to A if needed. + if (D != NULL) { + // A = [P] + // [Q] + // [D] + std::unique_ptr<BlockSparseMatrix> regularizer( + BlockSparseMatrix::CreateDiagonalMatrix(D, bs->cols)); + m->AppendRows(*regularizer); + } + + if (inner_product_computer_.get() == NULL) { + inner_product_computer_.reset(InnerProductComputer::Create( + *m, + options_.subset_preconditioner_start_row_block, + bs->rows.size(), + sparse_cholesky_->StorageType())); + } + + // Compute inner_product = [Q'*Q + D'*D] + inner_product_computer_->Compute(); + + // Unappend D if needed. + if (D != NULL) { + // A = [P] + // [Q] + m->DeleteRowBlocks(bs->cols.size()); + } + + std::string message; + // Compute L. s.t., LL' = Q'*Q + D'*D + const LinearSolverTerminationType termination_type = + sparse_cholesky_->Factorize(inner_product_computer_->mutable_result(), + &message); + if (termination_type != LINEAR_SOLVER_SUCCESS) { + LOG(ERROR) << "Preconditioner factorization failed: " << message; + return false; + } + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/subset_preconditioner.h b/extern/ceres/internal/ceres/subset_preconditioner.h new file mode 100644 index 00000000000..6f3c9ecd052 --- /dev/null +++ b/extern/ceres/internal/ceres/subset_preconditioner.h @@ -0,0 +1,91 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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_SUBSET_PRECONDITIONER_H_ +#define CERES_INTERNAL_SUBSET_PRECONDITIONER_H_ + +#include <memory> +#include "ceres/preconditioner.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class SparseCholesky; +class InnerProductComputer; + +// Subset preconditioning, uses a subset of the rows of the Jacobian +// to construct a preconditioner for the normal equations. +// +// To keep the interface simple, we assume that the matrix A has +// already been re-ordered that the user wishes to some subset of the +// bottom row blocks of the matrix as the preconditioner. This is +// controlled by +// Preconditioner::Options::subset_preconditioner_start_row_block. +// +// When using the subset preconditioner, all row blocks starting +// from this row block are used to construct the preconditioner. +// +// More precisely the matrix A is horizontally partitioned as +// +// A = [P] +// [Q] +// +// where P has subset_preconditioner_start_row_block row blocks, +// and the preconditioner is the inverse of the matrix Q'Q. +// +// Obviously, the smaller this number, the more accurate and +// computationally expensive this preconditioner will be. +// +// See the tests for example usage. +class SubsetPreconditioner : public BlockSparseMatrixPreconditioner { + public: + SubsetPreconditioner(const Preconditioner::Options& options, + const BlockSparseMatrix& A); + virtual ~SubsetPreconditioner(); + + // Preconditioner interface + void RightMultiply(const double* x, double* y) const final; + int num_rows() const final { return num_cols_; } + int num_cols() const final { return num_cols_; } + + private: + bool UpdateImpl(const BlockSparseMatrix& A, const double* D) final; + + const Preconditioner::Options options_; + const int num_cols_; + std::unique_ptr<SparseCholesky> sparse_cholesky_; + std::unique_ptr<InnerProductComputer> inner_product_computer_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SUBSET_PRECONDITIONER_H_ diff --git a/extern/ceres/internal/ceres/suitesparse.cc b/extern/ceres/internal/ceres/suitesparse.cc new file mode 100644 index 00000000000..190d1755add --- /dev/null +++ b/extern/ceres/internal/ceres/suitesparse.cc @@ -0,0 +1,430 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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) + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifndef CERES_NO_SUITESPARSE +#include "ceres/suitesparse.h" + +#include <vector> + +#include "ceres/compressed_col_sparse_matrix_utils.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "cholmod.h" + +namespace ceres { +namespace internal { + +using std::string; +using std::vector; + +SuiteSparse::SuiteSparse() { cholmod_start(&cc_); } + +SuiteSparse::~SuiteSparse() { cholmod_finish(&cc_); } + +cholmod_sparse* SuiteSparse::CreateSparseMatrix(TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.nrow = A->num_rows(); + triplet.ncol = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + triplet.i = reinterpret_cast<void*>(A->mutable_rows()); + triplet.j = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + +cholmod_sparse* SuiteSparse::CreateSparseMatrixTranspose( + TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.ncol = A->num_rows(); // swap row and columns + triplet.nrow = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + + // swap rows and columns + triplet.j = reinterpret_cast<void*>(A->mutable_rows()); + triplet.i = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + +cholmod_sparse SuiteSparse::CreateSparseMatrixTransposeView( + CompressedRowSparseMatrix* A) { + cholmod_sparse m; + m.nrow = A->num_cols(); + m.ncol = A->num_rows(); + m.nzmax = A->num_nonzeros(); + m.nz = nullptr; + m.p = reinterpret_cast<void*>(A->mutable_rows()); + m.i = reinterpret_cast<void*>(A->mutable_cols()); + m.x = reinterpret_cast<void*>(A->mutable_values()); + m.z = nullptr; + + if (A->storage_type() == CompressedRowSparseMatrix::LOWER_TRIANGULAR) { + m.stype = 1; + } else if (A->storage_type() == CompressedRowSparseMatrix::UPPER_TRIANGULAR) { + m.stype = -1; + } else { + m.stype = 0; + } + + m.itype = CHOLMOD_INT; + m.xtype = CHOLMOD_REAL; + m.dtype = CHOLMOD_DOUBLE; + m.sorted = 1; + m.packed = 1; + + return m; +} + +cholmod_dense SuiteSparse::CreateDenseVectorView(const double* x, int size) { + cholmod_dense v; + v.nrow = size; + v.ncol = 1; + v.nzmax = size; + v.d = size; + v.x = const_cast<void*>(reinterpret_cast<const void*>(x)); + v.xtype = CHOLMOD_REAL; + v.dtype = CHOLMOD_DOUBLE; + return v; +} + +cholmod_dense* SuiteSparse::CreateDenseVector(const double* x, + int in_size, + int out_size) { + CHECK_LE(in_size, out_size); + cholmod_dense* v = cholmod_zeros(out_size, 1, CHOLMOD_REAL, &cc_); + if (x != nullptr) { + memcpy(v->x, x, in_size * sizeof(*x)); + } + return v; +} + +cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A, + string* message) { + // Cholmod can try multiple re-ordering strategies to find a fill + // reducing ordering. Here we just tell it use AMD with automatic + // matrix dependence choice of supernodal versus simplicial + // factorization. + cc_.nmethods = 1; + cc_.method[0].ordering = CHOLMOD_AMD; + cc_.supernodal = CHOLMOD_AUTO; + + cholmod_factor* factor = cholmod_analyze(A, &cc_); + if (VLOG_IS_ON(2)) { + cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_); + } + + if (cc_.status != CHOLMOD_OK) { + *message = + StringPrintf("cholmod_analyze failed. error code: %d", cc_.status); + return nullptr; + } + + CHECK(factor != nullptr); + return factor; +} + +cholmod_factor* SuiteSparse::BlockAnalyzeCholesky(cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + string* message) { + vector<int> ordering; + if (!BlockAMDOrdering(A, row_blocks, col_blocks, &ordering)) { + return nullptr; + } + return AnalyzeCholeskyWithUserOrdering(A, ordering, message); +} + +cholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering( + cholmod_sparse* A, const vector<int>& ordering, string* message) { + CHECK_EQ(ordering.size(), A->nrow); + + cc_.nmethods = 1; + cc_.method[0].ordering = CHOLMOD_GIVEN; + + cholmod_factor* factor = + cholmod_analyze_p(A, const_cast<int*>(&ordering[0]), nullptr, 0, &cc_); + if (VLOG_IS_ON(2)) { + cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_); + } + if (cc_.status != CHOLMOD_OK) { + *message = + StringPrintf("cholmod_analyze failed. error code: %d", cc_.status); + return nullptr; + } + + CHECK(factor != nullptr); + return factor; +} + +cholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering( + cholmod_sparse* A, string* message) { + cc_.nmethods = 1; + cc_.method[0].ordering = CHOLMOD_NATURAL; + cc_.postorder = 0; + + cholmod_factor* factor = cholmod_analyze(A, &cc_); + if (VLOG_IS_ON(2)) { + cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_); + } + if (cc_.status != CHOLMOD_OK) { + *message = + StringPrintf("cholmod_analyze failed. error code: %d", cc_.status); + return nullptr; + } + + CHECK(factor != nullptr); + return factor; +} + +bool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A, + const vector<int>& row_blocks, + const vector<int>& col_blocks, + vector<int>* ordering) { + const int num_row_blocks = row_blocks.size(); + const int num_col_blocks = col_blocks.size(); + + // Arrays storing the compressed column structure of the matrix + // incoding the block sparsity of A. + vector<int> block_cols; + vector<int> block_rows; + + 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; + block_matrix.ncol = num_col_blocks; + block_matrix.nzmax = block_rows.size(); + block_matrix.p = reinterpret_cast<void*>(&block_cols[0]); + block_matrix.i = reinterpret_cast<void*>(&block_rows[0]); + block_matrix.x = nullptr; + block_matrix.stype = A->stype; + block_matrix.itype = CHOLMOD_INT; + block_matrix.xtype = CHOLMOD_PATTERN; + block_matrix.dtype = CHOLMOD_DOUBLE; + block_matrix.sorted = 1; + block_matrix.packed = 1; + + vector<int> block_ordering(num_row_blocks); + if (!cholmod_amd(&block_matrix, nullptr, 0, &block_ordering[0], &cc_)) { + return false; + } + + BlockOrderingToScalarOrdering(row_blocks, block_ordering, ordering); + return true; +} + +LinearSolverTerminationType SuiteSparse::Cholesky(cholmod_sparse* A, + cholmod_factor* L, + string* message) { + CHECK(A != nullptr); + CHECK(L != nullptr); + + // Save the current print level and silence CHOLMOD, otherwise + // CHOLMOD is prone to dumping stuff to stderr, which can be + // distracting when the error (matrix is indefinite) is not a fatal + // failure. + const int old_print_level = cc_.print; + cc_.print = 0; + + cc_.quick_return_if_not_posdef = 1; + int cholmod_status = cholmod_factorize(A, L, &cc_); + cc_.print = old_print_level; + + switch (cc_.status) { + case CHOLMOD_NOT_INSTALLED: + *message = "CHOLMOD failure: Method not installed."; + return LINEAR_SOLVER_FATAL_ERROR; + case CHOLMOD_OUT_OF_MEMORY: + *message = "CHOLMOD failure: Out of memory."; + return LINEAR_SOLVER_FATAL_ERROR; + case CHOLMOD_TOO_LARGE: + *message = "CHOLMOD failure: Integer overflow occurred."; + return LINEAR_SOLVER_FATAL_ERROR; + case CHOLMOD_INVALID: + *message = "CHOLMOD failure: Invalid input."; + return LINEAR_SOLVER_FATAL_ERROR; + case CHOLMOD_NOT_POSDEF: + *message = "CHOLMOD warning: Matrix not positive definite."; + return LINEAR_SOLVER_FAILURE; + case CHOLMOD_DSMALL: + *message = + "CHOLMOD warning: D for LDL' or diag(L) or " + "LL' has tiny absolute value."; + return LINEAR_SOLVER_FAILURE; + case CHOLMOD_OK: + if (cholmod_status != 0) { + return LINEAR_SOLVER_SUCCESS; + } + + *message = + "CHOLMOD failure: cholmod_factorize returned false " + "but cholmod_common::status is CHOLMOD_OK." + "Please report this to ceres-solver@googlegroups.com."; + return LINEAR_SOLVER_FATAL_ERROR; + default: + *message = StringPrintf( + "Unknown cholmod return code: %d. " + "Please report this to ceres-solver@googlegroups.com.", + cc_.status); + return LINEAR_SOLVER_FATAL_ERROR; + } + + return LINEAR_SOLVER_FATAL_ERROR; +} + +cholmod_dense* SuiteSparse::Solve(cholmod_factor* L, + cholmod_dense* b, + string* message) { + if (cc_.status != CHOLMOD_OK) { + *message = "cholmod_solve failed. CHOLMOD status is not CHOLMOD_OK"; + return nullptr; + } + + return cholmod_solve(CHOLMOD_A, L, b, &cc_); +} + +bool SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, + int* ordering) { + return cholmod_amd(matrix, nullptr, 0, ordering, &cc_); +} + +bool SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering( + cholmod_sparse* matrix, int* constraints, int* ordering) { +#ifndef CERES_NO_CAMD + return cholmod_camd(matrix, nullptr, 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."; + return false; +#endif +} + +std::unique_ptr<SparseCholesky> SuiteSparseCholesky::Create( + const OrderingType ordering_type) { + return std::unique_ptr<SparseCholesky>(new SuiteSparseCholesky(ordering_type)); +} + +SuiteSparseCholesky::SuiteSparseCholesky(const OrderingType ordering_type) + : ordering_type_(ordering_type), factor_(nullptr) {} + +SuiteSparseCholesky::~SuiteSparseCholesky() { + if (factor_ != nullptr) { + ss_.Free(factor_); + } +} + +LinearSolverTerminationType SuiteSparseCholesky::Factorize( + CompressedRowSparseMatrix* lhs, string* message) { + if (lhs == nullptr) { + *message = "Failure: Input lhs is NULL."; + return LINEAR_SOLVER_FATAL_ERROR; + } + + cholmod_sparse cholmod_lhs = ss_.CreateSparseMatrixTransposeView(lhs); + + if (factor_ == nullptr) { + if (ordering_type_ == NATURAL) { + factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&cholmod_lhs, message); + } else { + if (!lhs->col_blocks().empty() && !(lhs->row_blocks().empty())) { + factor_ = ss_.BlockAnalyzeCholesky( + &cholmod_lhs, lhs->col_blocks(), lhs->row_blocks(), message); + } else { + factor_ = ss_.AnalyzeCholesky(&cholmod_lhs, message); + } + } + + if (factor_ == nullptr) { + return LINEAR_SOLVER_FATAL_ERROR; + } + } + + return ss_.Cholesky(&cholmod_lhs, factor_, message); +} + +CompressedRowSparseMatrix::StorageType SuiteSparseCholesky::StorageType() + const { + return ((ordering_type_ == NATURAL) + ? CompressedRowSparseMatrix::UPPER_TRIANGULAR + : CompressedRowSparseMatrix::LOWER_TRIANGULAR); +} + +LinearSolverTerminationType SuiteSparseCholesky::Solve(const double* rhs, + double* solution, + string* message) { + // Error checking + if (factor_ == nullptr) { + *message = "Solve called without a call to Factorize first."; + return LINEAR_SOLVER_FATAL_ERROR; + } + + const int num_cols = factor_->n; + cholmod_dense cholmod_rhs = ss_.CreateDenseVectorView(rhs, num_cols); + cholmod_dense* cholmod_dense_solution = + ss_.Solve(factor_, &cholmod_rhs, message); + + if (cholmod_dense_solution == nullptr) { + return LINEAR_SOLVER_FAILURE; + } + + memcpy(solution, cholmod_dense_solution->x, num_cols * sizeof(*solution)); + ss_.Free(cholmod_dense_solution); + return LINEAR_SOLVER_SUCCESS; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE diff --git a/extern/ceres/internal/ceres/suitesparse.h b/extern/ceres/internal/ceres/suitesparse.h index 380d76e003a..b77b296a66e 100644 --- a/extern/ceres/internal/ceres/suitesparse.h +++ b/extern/ceres/internal/ceres/suitesparse.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -41,11 +41,11 @@ #include <cstring> #include <string> #include <vector> - +#include "SuiteSparseQR.hpp" #include "ceres/linear_solver.h" +#include "ceres/sparse_cholesky.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 @@ -99,6 +99,11 @@ class SuiteSparse { // use the SuiteSparse machinery to allocate memory. cholmod_sparse CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + // Create a cholmod_dense vector around the contents of the array x. + // This is a shallow object, which refers to the contents of x and + // does not use the SuiteSparse machinery to allocate memory. + cholmod_dense CreateDenseVectorView(const double* x, int size); + // Given a vector x, build a cholmod_dense vector of size out_size // with the first in_size entries copied from x. If x is NULL, then // an all zeros vector is returned. Caller owns the result. @@ -197,12 +202,12 @@ class SuiteSparse { // doing sparse direct factorization of these matrices the // fill-reducing ordering algorithms (in particular AMD) can either // be run on the block or the scalar form of these matrices. The two - // SuiteSparse::AnalyzeCholesky methods allows the the client to + // SuiteSparse::AnalyzeCholesky methods allows the client to // compute the symbolic factorization of a matrix by either using // AMD on the matrix or a user provided ordering of the rows. // // But since the underlying matrices are block oriented, it is worth - // running AMD on just the block structre of these matrices and then + // running AMD on just the block structure of these matrices and then // lifting these block orderings to a full scalar ordering. This // preserves the block structure of the permuted matrix, and exposes // more of the super-nodal structure of the matrix to the numerical @@ -278,6 +283,27 @@ class SuiteSparse { cholmod_common cc_; }; +class SuiteSparseCholesky : public SparseCholesky { + public: + static std::unique_ptr<SparseCholesky> Create( + OrderingType ordering_type); + + // SparseCholesky interface. + virtual ~SuiteSparseCholesky(); + CompressedRowSparseMatrix::StorageType StorageType() const final; + LinearSolverTerminationType Factorize( + CompressedRowSparseMatrix* lhs, std::string* message) final; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) final; + private: + SuiteSparseCholesky(const OrderingType ordering_type); + + const OrderingType ordering_type_; + SuiteSparse ss_; + cholmod_factor* factor_; +}; + } // namespace internal } // namespace ceres @@ -285,6 +311,9 @@ class SuiteSparse { typedef void cholmod_factor; +namespace ceres { +namespace internal { + class SuiteSparse { public: // Defining this static function even when SuiteSparse is not @@ -292,7 +321,7 @@ class SuiteSparse { // without checking for the absence of the CERES_NO_CAMD symbol. // // This is safer because the symbol maybe missing due to a user - // accidently not including suitesparse.h in their code when + // accidentally not including suitesparse.h in their code when // checking for the symbol. static bool IsConstrainedApproximateMinimumDegreeOrderingAvailable() { return false; @@ -301,6 +330,9 @@ class SuiteSparse { void Free(void* arg) {} }; +} // namespace internal +} // namespace ceres + #endif // CERES_NO_SUITESPARSE #endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/ceres/internal/ceres/thread_pool.cc b/extern/ceres/internal/ceres/thread_pool.cc new file mode 100644 index 00000000000..5a52c9d06a6 --- /dev/null +++ b/extern/ceres/internal/ceres/thread_pool.cc @@ -0,0 +1,116 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +// This include must come before any #ifndef check on Ceres compile options. +#include "ceres/internal/port.h" + +#ifdef CERES_USE_CXX_THREADS + +#include "ceres/thread_pool.h" + +#include <cmath> +#include <limits> + +namespace ceres { +namespace internal { +namespace { + +// Constrain the total number of threads to the amount the hardware can support. +int GetNumAllowedThreads(int requested_num_threads) { + return std::min(requested_num_threads, ThreadPool::MaxNumThreadsAvailable()); +} + +} // namespace + +int ThreadPool::MaxNumThreadsAvailable() { + const int num_hardware_threads = std::thread::hardware_concurrency(); + // hardware_concurrency() can return 0 if the value is not well defined or not + // computable. + return num_hardware_threads == 0 + ? std::numeric_limits<int>::max() + : num_hardware_threads; +} + +ThreadPool::ThreadPool() { } + +ThreadPool::ThreadPool(int num_threads) { + Resize(num_threads); +} + +ThreadPool::~ThreadPool() { + std::lock_guard<std::mutex> lock(thread_pool_mutex_); + // Signal the thread workers to stop and wait for them to finish all scheduled + // tasks. + Stop(); + for (std::thread& thread : thread_pool_) { + thread.join(); + } +} + +void ThreadPool::Resize(int num_threads) { + std::lock_guard<std::mutex> lock(thread_pool_mutex_); + + const int num_current_threads = thread_pool_.size(); + if (num_current_threads >= num_threads) { + return; + } + + const int create_num_threads = + GetNumAllowedThreads(num_threads) - num_current_threads; + + for (int i = 0; i < create_num_threads; ++i) { + thread_pool_.push_back(std::thread(&ThreadPool::ThreadMainLoop, this)); + } +} + +void ThreadPool::AddTask(const std::function<void()>& func) { + task_queue_.Push(func); +} + +int ThreadPool::Size() { + std::lock_guard<std::mutex> lock(thread_pool_mutex_); + return thread_pool_.size(); +} + +void ThreadPool::ThreadMainLoop() { + std::function<void()> task; + while (task_queue_.Wait(&task)) { + task(); + } +} + +void ThreadPool::Stop() { + task_queue_.StopWaiters(); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_USE_CXX_THREADS diff --git a/extern/ceres/internal/ceres/thread_pool.h b/extern/ceres/internal/ceres/thread_pool.h new file mode 100644 index 00000000000..1ebb52eb6b4 --- /dev/null +++ b/extern/ceres/internal/ceres/thread_pool.h @@ -0,0 +1,120 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: vitus@google.com (Michael Vitus) + +#ifndef CERES_INTERNAL_THREAD_POOL_H_ +#define CERES_INTERNAL_THREAD_POOL_H_ + +#include <functional> +#include <mutex> +#include <thread> +#include <vector> + +#include "ceres/concurrent_queue.h" + +namespace ceres { +namespace internal { + +// A thread-safe thread pool with an unbounded task queue and a resizable number +// of workers. The size of the thread pool can be increased but never decreased +// in order to support the largest number of threads requested. The ThreadPool +// has three states: +// +// (1) The thread pool size is zero. Tasks may be added to the thread pool via +// AddTask but they will not be executed until the thread pool is resized. +// +// (2) The thread pool size is greater than zero. Tasks may be added to the +// thread pool and will be executed as soon as a worker is available. The +// thread pool may be resized while the thread pool is running. +// +// (3) The thread pool is destructing. The thread pool will signal all the +// workers to stop. The workers will finish all of the tasks that have already +// been added to the thread pool. +// +class ThreadPool { + public: + // Returns the maximum number of hardware threads. + static int MaxNumThreadsAvailable(); + + // Default constructor with no active threads. We allow instantiating a + // thread pool with no threads to support the use case of single threaded + // Ceres where everything will be executed on the main thread. For single + // threaded execution this has two benefits: avoid any overhead as threads + // are expensive to create, and no unused threads shown in the debugger. + ThreadPool(); + + // Instantiates a thread pool with min(MaxNumThreadsAvailable, num_threads) + // number of threads. + explicit ThreadPool(int num_threads); + + // Signals the workers to stop and waits for them to finish any tasks that + // have been scheduled. + ~ThreadPool(); + + // Resizes the thread pool if it is currently less than the requested number + // of threads. The thread pool will be resized to min(MaxNumThreadsAvailable, + // num_threads) number of threads. Resize does not support reducing the + // thread pool size. If a smaller number of threads is requested, the thread + // pool remains the same size. The thread pool is reused within Ceres with + // different number of threads, and we need to ensure we can support the + // largest number of threads requested. It is safe to resize the thread pool + // while the workers are executing tasks, and the resizing is guaranteed to + // complete upon return. + void Resize(int num_threads); + + // Adds a task to the queue and wakes up a blocked thread. If the thread pool + // size is greater than zero, then the task will be executed by a currently + // idle thread or when a thread becomes available. If the thread pool has no + // threads, then the task will never be executed and the user should use + // Resize() to create a non-empty thread pool. + void AddTask(const std::function<void()>& func); + + // Returns the current size of the thread pool. + int Size(); + + private: + // Main loop for the threads which blocks on the task queue until work becomes + // available. It will return if and only if Stop has been called. + void ThreadMainLoop(); + + // Signal all the threads to stop. It does not block until the threads are + // finished. + void Stop(); + + // The queue that stores the units of work available for the thread pool. The + // task queue maintains its own thread safety. + ConcurrentQueue<std::function<void()>> task_queue_; + std::vector<std::thread> thread_pool_; + std::mutex thread_pool_mutex_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_THREAD_POOL_H_ diff --git a/extern/ceres/internal/ceres/thread_token_provider.cc b/extern/ceres/internal/ceres/thread_token_provider.cc new file mode 100644 index 00000000000..b04cf844488 --- /dev/null +++ b/extern/ceres/internal/ceres/thread_token_provider.cc @@ -0,0 +1,76 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: yp@photonscore.de (Yury Prokazov) + +#include "ceres/thread_token_provider.h" + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +namespace ceres { +namespace internal { + +ThreadTokenProvider::ThreadTokenProvider(int num_threads) { + (void)num_threads; +#ifdef CERES_USE_CXX_THREADS + for (int i = 0; i < num_threads; i++) { + pool_.Push(i); + } +#endif + +} + +int ThreadTokenProvider::Acquire() { +#ifdef CERES_USE_OPENMP + return omp_get_thread_num(); +#endif + +#ifdef CERES_NO_THREADS + return 0; +#endif + +#ifdef CERES_USE_CXX_THREADS + int thread_id; + CHECK(pool_.Wait(&thread_id)); + return thread_id; +#endif + +} + +void ThreadTokenProvider::Release(int thread_id) { + (void)thread_id; +#ifdef CERES_USE_CXX_THREADS + pool_.Push(thread_id); +#endif + +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/thread_token_provider.h b/extern/ceres/internal/ceres/thread_token_provider.h new file mode 100644 index 00000000000..06dc0438572 --- /dev/null +++ b/extern/ceres/internal/ceres/thread_token_provider.h @@ -0,0 +1,97 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: yp@photonscore.de (Yury Prokazov) + +#ifndef CERES_INTERNAL_THREAD_TOKEN_PROVIDER_H_ +#define CERES_INTERNAL_THREAD_TOKEN_PROVIDER_H_ + +#include "ceres/internal/config.h" +#include "ceres/internal/port.h" + +#ifdef CERES_USE_CXX_THREADS +#include "ceres/concurrent_queue.h" +#endif + +namespace ceres { +namespace internal { + +// Helper for C++ thread number identification that is similar to +// omp_get_thread_num() behaviour. This is necessary to support C++ +// threading with a sequential thread id. This is used to access preallocated +// resources in the parallelized code parts. The sequence of tokens varies from +// 0 to num_threads - 1 that can be acquired to identify the thread in a thread +// pool. +// +// If CERES_NO_THREADS is defined, Acquire() always returns 0 and Release() +// takes no action. +// +// If CERES_USE_OPENMP, omp_get_thread_num() is used to Acquire() with no action +// in Release() +// +// +// Example usage pseudocode: +// +// ThreadTokenProvider ttp(N); // allocate N tokens +// Spawn N threads { +// int token = ttp.Acquire(); // get unique token +// ... +// ... use token to access resources bound to the thread +// ... +// ttp.Release(token); // return token to the pool +// } +// +class ThreadTokenProvider { + public: + ThreadTokenProvider(int num_threads); + + // Returns the first token from the queue. The acquired value must be + // given back by Release(). + int Acquire(); + + // Makes previously acquired token available for other threads. + void Release(int thread_id); + + private: +#ifdef CERES_USE_CXX_THREADS + // This queue initially holds a sequence from 0..num_threads-1. Every + // Acquire() call the first number is removed from here. When the token is not + // needed anymore it shall be given back with corresponding Release() + // call. This concurrent queue is more expensive than TBB's version, so you + // should not acquire the thread ID on every for loop iteration. + ConcurrentQueue<int> pool_; +#endif + + ThreadTokenProvider(ThreadTokenProvider&); + ThreadTokenProvider& operator=(ThreadTokenProvider&); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_THREAD_TOKEN_PROVIDER_H_ diff --git a/extern/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc index 8df405ca115..54b588ba466 100644 --- a/extern/ceres/internal/ceres/triplet_sparse_matrix.cc +++ b/extern/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -32,9 +32,10 @@ #include <algorithm> #include <cstddef> + #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/random.h" #include "ceres/types.h" #include "glog/logging.h" @@ -45,10 +46,8 @@ TripletSparseMatrix::TripletSparseMatrix() : num_rows_(0), num_cols_(0), max_num_nonzeros_(0), - num_nonzeros_(0), - rows_(NULL), - cols_(NULL), - values_(NULL) {} + num_nonzeros_(0) {} + TripletSparseMatrix::~TripletSparseMatrix() {} @@ -58,10 +57,7 @@ TripletSparseMatrix::TripletSparseMatrix(int num_rows, : num_rows_(num_rows), num_cols_(num_cols), max_num_nonzeros_(max_num_nonzeros), - num_nonzeros_(0), - rows_(NULL), - cols_(NULL), - values_(NULL) { + num_nonzeros_(0) { // All the sizes should at least be zero CHECK_GE(num_rows, 0); CHECK_GE(num_cols, 0); @@ -69,21 +65,41 @@ TripletSparseMatrix::TripletSparseMatrix(int num_rows, AllocateMemory(); } +TripletSparseMatrix::TripletSparseMatrix(const int num_rows, + const int num_cols, + const std::vector<int>& rows, + const std::vector<int>& cols, + const std::vector<double>& values) + : num_rows_(num_rows), + num_cols_(num_cols), + max_num_nonzeros_(values.size()), + num_nonzeros_(values.size()) { + // All the sizes should at least be zero + CHECK_GE(num_rows, 0); + CHECK_GE(num_cols, 0); + CHECK_EQ(rows.size(), cols.size()); + CHECK_EQ(rows.size(), values.size()); + AllocateMemory(); + std::copy(rows.begin(), rows.end(), rows_.get()); + std::copy(cols.begin(), cols.end(), cols_.get()); + std::copy(values.begin(), values.end(), values_.get()); +} + TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) : SparseMatrix(), num_rows_(orig.num_rows_), num_cols_(orig.num_cols_), max_num_nonzeros_(orig.max_num_nonzeros_), - num_nonzeros_(orig.num_nonzeros_), - rows_(NULL), - cols_(NULL), - values_(NULL) { + num_nonzeros_(orig.num_nonzeros_) { AllocateMemory(); CopyData(orig); } TripletSparseMatrix& TripletSparseMatrix::operator=( const TripletSparseMatrix& rhs) { + if (this == &rhs) { + return *this; + } num_rows_ = rhs.num_rows_; num_cols_ = rhs.num_cols_; num_nonzeros_ = rhs.num_nonzeros_; @@ -165,7 +181,7 @@ void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const { } void TripletSparseMatrix::SquaredColumnNorm(double* x) const { - CHECK_NOTNULL(x); + CHECK(x != nullptr); VectorRef(x, num_cols_).setZero(); for (int i = 0; i < num_nonzeros_; ++i) { x[cols_[i]] += values_[i] * values_[i]; @@ -173,7 +189,7 @@ void TripletSparseMatrix::SquaredColumnNorm(double* x) const { } void TripletSparseMatrix::ScaleColumns(const double* scale) { - CHECK_NOTNULL(scale); + CHECK(scale != nullptr); for (int i = 0; i < num_nonzeros_; ++i) { values_[i] = values_[i] * scale[cols_[i]]; } @@ -254,11 +270,40 @@ TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( } void TripletSparseMatrix::ToTextFile(FILE* file) const { - CHECK_NOTNULL(file); + CHECK(file != nullptr); for (int i = 0; i < num_nonzeros_; ++i) { fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]); } } +TripletSparseMatrix* TripletSparseMatrix::CreateRandomMatrix( + const TripletSparseMatrix::RandomMatrixOptions& options) { + CHECK_GT(options.num_rows, 0); + CHECK_GT(options.num_cols, 0); + CHECK_GT(options.density, 0.0); + CHECK_LE(options.density, 1.0); + + std::vector<int> rows; + std::vector<int> cols; + std::vector<double> values; + while (rows.empty()) { + rows.clear(); + cols.clear(); + values.clear(); + for (int r = 0; r < options.num_rows; ++r) { + for (int c = 0; c < options.num_cols; ++c) { + if (RandDouble() <= options.density) { + rows.push_back(r); + cols.push_back(c); + values.push_back(RandNormal()); + } + } + } + } + + return new TripletSparseMatrix( + options.num_rows, options.num_cols, rows, cols, values); +} + } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/triplet_sparse_matrix.h b/extern/ceres/internal/ceres/triplet_sparse_matrix.h index f3f5370df6f..2ee0fa992b5 100644 --- a/extern/ceres/internal/ceres/triplet_sparse_matrix.h +++ b/extern/ceres/internal/ceres/triplet_sparse_matrix.h @@ -31,9 +31,10 @@ #ifndef CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ #define CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ +#include <memory> +#include <vector> #include "ceres/sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" namespace ceres { @@ -47,26 +48,32 @@ class TripletSparseMatrix : public SparseMatrix { public: TripletSparseMatrix(); TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); + TripletSparseMatrix(int num_rows, + int num_cols, + const std::vector<int>& rows, + const std::vector<int>& cols, + const std::vector<double>& values); + explicit TripletSparseMatrix(const TripletSparseMatrix& orig); TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); - ~TripletSparseMatrix(); + virtual ~TripletSparseMatrix(); // Implementation of the SparseMatrix interface. - virtual void SetZero(); - virtual void RightMultiply(const double* x, double* y) const; - virtual void LeftMultiply(const double* x, double* y) const; - virtual void SquaredColumnNorm(double* x) const; - virtual void ScaleColumns(const double* scale); - virtual void ToDenseMatrix(Matrix* dense_matrix) const; - 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 num_nonzeros_; } - virtual const double* values() const { return values_.get(); } - virtual double* mutable_values() { return values_.get(); } - virtual void set_num_nonzeros(int num_nonzeros); + void SetZero() final; + void RightMultiply(const double* x, double* y) const final; + void LeftMultiply(const double* x, double* y) const final; + void SquaredColumnNorm(double* x) const final; + void ScaleColumns(const double* scale) final; + void ToDenseMatrix(Matrix* dense_matrix) const final; + void ToTextFile(FILE* file) const final; + int num_rows() const final { return num_rows_; } + int num_cols() const final { return num_cols_; } + int num_nonzeros() const final { return num_nonzeros_; } + const double* values() const final { return values_.get(); } + double* mutable_values() final { return values_.get(); } + void set_num_nonzeros(int num_nonzeros); // Increase max_num_nonzeros and correspondingly increase the size // of rows_, cols_ and values_. If new_max_num_nonzeros is smaller @@ -105,6 +112,25 @@ class TripletSparseMatrix : public SparseMatrix { static TripletSparseMatrix* CreateSparseDiagonalMatrix(const double* values, int num_rows); + // Options struct to control the generation of random + // TripletSparseMatrix objects. + struct RandomMatrixOptions { + int num_rows; + int num_cols; + // 0 < density <= 1 is the probability of an entry being + // structurally non-zero. A given random matrix will not have + // precisely this density. + double density; + }; + + // Create a random CompressedRowSparseMatrix whose entries are + // normally distributed and whose structure is determined by + // RandomMatrixOptions. + // + // Caller owns the result. + static TripletSparseMatrix* CreateRandomMatrix( + const TripletSparseMatrix::RandomMatrixOptions& options); + private: void AllocateMemory(); void CopyData(const TripletSparseMatrix& orig); @@ -118,9 +144,9 @@ class TripletSparseMatrix : public SparseMatrix { // stored at the location (rows_[i], cols_[i]). If the there are // multiple entries with the same (rows_[i], cols_[i]), the values_ // entries corresponding to them are summed up. - scoped_array<int> rows_; - scoped_array<int> cols_; - scoped_array<double> values_; + std::unique_ptr<int[]> rows_; + std::unique_ptr<int[]> cols_; + std::unique_ptr<double[]> values_; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.cc b/extern/ceres/internal/ceres/trust_region_minimizer.cc index d809906ab54..7065977ad77 100644 --- a/extern/ceres/internal/ceres/trust_region_minimizer.cc +++ b/extern/ceres/internal/ceres/trust_region_minimizer.cc @@ -35,6 +35,7 @@ #include <cstdlib> #include <cstring> #include <limits> +#include <memory> #include <string> #include <vector> @@ -92,7 +93,8 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, continue; } - if (options_.is_constrained) { + if (options_.is_constrained && + options_.max_num_line_search_step_size_iterations > 0) { // Use a projected line search to enforce the bounds constraints // and improve the quality of the step. DoLineSearch(x_, gradient_, x_cost_, &delta_); @@ -135,13 +137,16 @@ void TrustRegionMinimizer::Init(const Minimizer::Options& options, solver_summary_->num_unsuccessful_steps = 0; solver_summary_->is_constrained = options.is_constrained; - evaluator_ = CHECK_NOTNULL(options_.evaluator.get()); - jacobian_ = CHECK_NOTNULL(options_.jacobian.get()); - strategy_ = CHECK_NOTNULL(options_.trust_region_strategy.get()); + CHECK(options_.evaluator != nullptr); + CHECK(options_.jacobian != nullptr); + CHECK(options_.trust_region_strategy != nullptr); + evaluator_ = options_.evaluator.get(); + jacobian_ = options_.jacobian.get(); + strategy_ = options_.trust_region_strategy.get(); is_not_silent_ = !options.is_silent; inner_iterations_are_enabled_ = - options.inner_iteration_minimizer.get() != NULL; + options.inner_iteration_minimizer.get() != nullptr; inner_iterations_were_useful_ = false; num_parameters_ = evaluator_->NumParameters(); @@ -201,7 +206,7 @@ bool TrustRegionMinimizer::IterationZero() { x_norm_ = x_.norm(); } - if (!EvaluateGradientAndJacobian()) { + if (!EvaluateGradientAndJacobian(/*new_evaluation_point=*/true)) { return false; } @@ -223,8 +228,12 @@ bool TrustRegionMinimizer::IterationZero() { // Returns true if all computations could be performed // successfully. Any failures are considered fatal and the // Solver::Summary is updated to indicate this. -bool TrustRegionMinimizer::EvaluateGradientAndJacobian() { - if (!evaluator_->Evaluate(x_.data(), +bool TrustRegionMinimizer::EvaluateGradientAndJacobian( + bool new_evaluation_point) { + Evaluator::EvaluateOptions evaluate_options; + evaluate_options.new_evaluation_point = new_evaluation_point; + if (!evaluator_->Evaluate(evaluate_options, + x_.data(), &x_cost_, residuals_.data(), gradient_.data(), @@ -482,8 +491,11 @@ void TrustRegionMinimizer::DoInnerIterationsIfNeeded() { options_.inner_iteration_minimizer->Minimize( options_, inner_iteration_x_.data(), &inner_iteration_summary); double inner_iteration_cost; - if (!evaluator_->Evaluate( - inner_iteration_x_.data(), &inner_iteration_cost, NULL, NULL, NULL)) { + if (!evaluator_->Evaluate(inner_iteration_x_.data(), + &inner_iteration_cost, + nullptr, + nullptr, + nullptr)) { VLOG_IF(2, is_not_silent_) << "Inner iteration failed."; return; } @@ -569,8 +581,8 @@ void TrustRegionMinimizer::DoLineSearch(const Vector& x, line_search_options.function = &line_search_function; std::string message; - scoped_ptr<LineSearch> line_search(CHECK_NOTNULL( - LineSearch::Create(ceres::ARMIJO, line_search_options, &message))); + std::unique_ptr<LineSearch> line_search( + LineSearch::Create(ceres::ARMIJO, line_search_options, &message)); LineSearch::Summary line_search_summary; line_search_function.Init(x, *delta); line_search->Search(1.0, cost, gradient.dot(*delta), &line_search_summary); @@ -586,7 +598,7 @@ void TrustRegionMinimizer::DoLineSearch(const Vector& x, line_search_summary.total_time_in_seconds; if (line_search_summary.success) { - *delta *= line_search_summary.optimal_step_size; + *delta *= line_search_summary.optimal_point.x; } } @@ -601,10 +613,11 @@ bool TrustRegionMinimizer::MaxSolverTimeReached() { return false; } - solver_summary_->message = StringPrintf("Maximum solver time reached. " - "Total solver time: %e >= %e.", - total_solver_time, - options_.max_solver_time_in_seconds); + solver_summary_->message = StringPrintf( + "Maximum solver time reached. " + "Total solver time: %e >= %e.", + total_solver_time, + options_.max_solver_time_in_seconds); solver_summary_->termination_type = NO_CONVERGENCE; VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; return true; @@ -618,10 +631,10 @@ bool TrustRegionMinimizer::MaxSolverIterationsReached() { return false; } - solver_summary_->message = - StringPrintf("Maximum number of iterations reached. " - "Number of iterations: %d.", - iteration_summary_.iteration); + solver_summary_->message = StringPrintf( + "Maximum number of iterations reached. " + "Number of iterations: %d.", + iteration_summary_.iteration); solver_summary_->termination_type = NO_CONVERGENCE; VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; @@ -653,11 +666,11 @@ bool TrustRegionMinimizer::MinTrustRegionRadiusReached() { return false; } - solver_summary_->message = - StringPrintf("Minimum trust region radius reached. " - "Trust region radius: %e <= %e", - iteration_summary_.trust_region_radius, - options_.min_trust_region_radius); + solver_summary_->message = StringPrintf( + "Minimum trust region radius reached. " + "Trust region radius: %e <= %e", + iteration_summary_.trust_region_radius, + options_.min_trust_region_radius); solver_summary_->termination_type = CONVERGENCE; VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message; return true; @@ -725,7 +738,7 @@ void TrustRegionMinimizer::ComputeCandidatePointAndEvaluateCost() { } if (!evaluator_->Evaluate( - candidate_x_.data(), &candidate_cost_, NULL, NULL, NULL)) { + candidate_x_.data(), &candidate_cost_, nullptr, nullptr, nullptr)) { LOG_IF(WARNING, is_not_silent_) << "Step failed to evaluate. " << "Treating it as a step with infinite cost"; @@ -746,7 +759,7 @@ bool TrustRegionMinimizer::IsStepSuccessful() { // small. // // This can cause the trust region loop to reject this step. To - // get around this, we expicitly check if the inner iterations + // get around this, we explicitly 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. @@ -768,7 +781,9 @@ bool TrustRegionMinimizer::HandleSuccessfulStep() { x_ = candidate_x_; x_norm_ = x_.norm(); - if (!EvaluateGradientAndJacobian()) { + // Since the step was successful, this point has already had the residual + // evaluated (but not the jacobian). So indicate that to the evaluator. + if (!EvaluateGradientAndJacobian(/*new_evaluation_point=*/false)) { return false; } diff --git a/extern/ceres/internal/ceres/trust_region_minimizer.h b/extern/ceres/internal/ceres/trust_region_minimizer.h index 43141da58a1..b5c41225ddc 100644 --- a/extern/ceres/internal/ceres/trust_region_minimizer.h +++ b/extern/ceres/internal/ceres/trust_region_minimizer.h @@ -31,8 +31,8 @@ #ifndef CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ #define CERES_INTERNAL_TRUST_REGION_MINIMIZER_H_ +#include <memory> #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/minimizer.h" #include "ceres/solver.h" #include "ceres/sparse_matrix.h" @@ -51,9 +51,9 @@ class TrustRegionMinimizer : public Minimizer { ~TrustRegionMinimizer(); // This method is not thread safe. - virtual void Minimize(const Minimizer::Options& options, - double* parameters, - Solver::Summary* solver_summary); + void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* solver_summary) override; private: void Init(const Minimizer::Options& options, @@ -63,7 +63,7 @@ class TrustRegionMinimizer : public Minimizer { bool FinalizeIterationAndCheckIfMinimizerCanContinue(); bool ComputeTrustRegionStep(); - bool EvaluateGradientAndJacobian(); + bool EvaluateGradientAndJacobian(bool new_evaluation_point); void ComputeCandidatePointAndEvaluateCost(); void DoLineSearch(const Vector& x, @@ -94,7 +94,7 @@ class TrustRegionMinimizer : public Minimizer { SparseMatrix* jacobian_; TrustRegionStrategy* strategy_; - scoped_ptr<TrustRegionStepEvaluator> step_evaluator_; + std::unique_ptr<TrustRegionStepEvaluator> step_evaluator_; bool is_not_silent_; bool inner_iterations_are_enabled_; diff --git a/extern/ceres/internal/ceres/trust_region_preprocessor.cc b/extern/ceres/internal/ceres/trust_region_preprocessor.cc index 4020e4ca115..b8c6b49d1ca 100644 --- a/extern/ceres/internal/ceres/trust_region_preprocessor.cc +++ b/extern/ceres/internal/ceres/trust_region_preprocessor.cc @@ -32,7 +32,9 @@ #include <numeric> #include <string> + #include "ceres/callbacks.h" +#include "ceres/context_impl.h" #include "ceres/evaluator.h" #include "ceres/linear_solver.h" #include "ceres/minimizer.h" @@ -56,8 +58,7 @@ namespace { ParameterBlockOrdering* CreateDefaultLinearSolverOrdering( const Program& program) { ParameterBlockOrdering* ordering = new ParameterBlockOrdering; - const vector<ParameterBlock*>& parameter_blocks = - program.parameter_blocks(); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); for (int i = 0; i < parameter_blocks.size(); ++i) { ordering->AddElementToGroup( const_cast<double*>(parameter_blocks[i]->user_state()), 0); @@ -68,8 +69,7 @@ ParameterBlockOrdering* CreateDefaultLinearSolverOrdering( // Check if all the user supplied values in the parameter blocks are // sane or not, and if the program is feasible or not. bool IsProgramValid(const Program& program, std::string* error) { - return (program.ParameterBlocksAreFinite(error) && - program.IsFeasible(error)); + return (program.ParameterBlocksAreFinite(error) && program.IsFeasible(error)); } void AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( @@ -81,36 +81,33 @@ void AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( const LinearSolverType linear_solver_type_given = options->linear_solver_type; const PreconditionerType preconditioner_type_given = options->preconditioner_type; - options->linear_solver_type = LinearSolver::LinearSolverForZeroEBlocks( - linear_solver_type_given); + options->linear_solver_type = + LinearSolver::LinearSolverForZeroEBlocks(linear_solver_type_given); std::string message; if (linear_solver_type_given == ITERATIVE_SCHUR) { - options->preconditioner_type = Preconditioner::PreconditionerForZeroEBlocks( - preconditioner_type_given); + options->preconditioner_type = + Preconditioner::PreconditionerForZeroEBlocks(preconditioner_type_given); message = - StringPrintf( - "No E blocks. Switching from %s(%s) to %s(%s).", - LinearSolverTypeToString(linear_solver_type_given), - PreconditionerTypeToString(preconditioner_type_given), - LinearSolverTypeToString(options->linear_solver_type), - PreconditionerTypeToString(options->preconditioner_type)); + StringPrintf("No E blocks. Switching from %s(%s) to %s(%s).", + LinearSolverTypeToString(linear_solver_type_given), + PreconditionerTypeToString(preconditioner_type_given), + LinearSolverTypeToString(options->linear_solver_type), + PreconditionerTypeToString(options->preconditioner_type)); } else { message = - StringPrintf( - "No E blocks. Switching from %s to %s.", - LinearSolverTypeToString(linear_solver_type_given), - LinearSolverTypeToString(options->linear_solver_type)); + StringPrintf("No E blocks. Switching from %s to %s.", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(options->linear_solver_type)); } VLOG_IF(1, options->logging_type != SILENT) << message; } -// For Schur type and SPARSE_NORMAL_CHOLESKY linear solvers, reorder -// the program to reduce fill-in and increase cache coherency. +// Reorder the program to reduce fill-in and increase cache coherency. bool ReorderProgram(PreprocessedProblem* pp) { - Solver::Options& options = pp->options; + const Solver::Options& options = pp->options; if (IsSchurType(options.linear_solver_type)) { return ReorderProgramForSchurTypeLinearSolver( options.linear_solver_type, @@ -123,9 +120,25 @@ bool ReorderProgram(PreprocessedProblem* pp) { if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY && !options.dynamic_sparsity) { - return ReorderProgramForSparseNormalCholesky( + return ReorderProgramForSparseCholesky( options.sparse_linear_algebra_library_type, *options.linear_solver_ordering, + 0, /* use all the rows of the jacobian */ + pp->reduced_program.get(), + &pp->error); + } + + if (options.linear_solver_type == CGNR && + options.preconditioner_type == SUBSET) { + pp->linear_solver_options.subset_preconditioner_start_row_block = + ReorderResidualBlocksByPartition( + options.residual_blocks_for_subset_preconditioner, + pp->reduced_program.get()); + + return ReorderProgramForSparseCholesky( + options.sparse_linear_algebra_library_type, + *options.linear_solver_ordering, + pp->linear_solver_options.subset_preconditioner_start_row_block, pp->reduced_program.get(), &pp->error); } @@ -139,7 +152,9 @@ bool ReorderProgram(PreprocessedProblem* pp) { // too. bool SetupLinearSolver(PreprocessedProblem* pp) { Solver::Options& options = pp->options; - if (options.linear_solver_ordering.get() == NULL) { + pp->linear_solver_options = LinearSolver::Options(); + + if (!options.linear_solver_ordering) { // If the user has not supplied a linear solver ordering, then we // assume that they are giving all the freedom to us in choosing // the best possible ordering. This intent can be indicated by @@ -163,8 +178,7 @@ bool SetupLinearSolver(PreprocessedProblem* pp) { ordering->Remove(pp->removed_parameter_blocks); if (IsSchurType(options.linear_solver_type) && min_group_id != ordering->MinNonZeroGroup()) { - AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( - &options); + AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver(&options); } } @@ -175,7 +189,6 @@ bool SetupLinearSolver(PreprocessedProblem* pp) { } // Configure the linear solver. - pp->linear_solver_options = LinearSolver::Options(); pp->linear_solver_options.min_num_iterations = options.min_linear_solver_iterations; pp->linear_solver_options.max_num_iterations = @@ -191,33 +204,50 @@ bool SetupLinearSolver(PreprocessedProblem* pp) { pp->linear_solver_options.use_explicit_schur_complement = options.use_explicit_schur_complement; pp->linear_solver_options.dynamic_sparsity = options.dynamic_sparsity; - pp->linear_solver_options.num_threads = options.num_linear_solver_threads; - - // Ignore user's postordering preferences and force it to be true if - // cholmod_camd is not available. This ensures that the linear - // solver does not assume that a fill-reducing pre-ordering has been - // done. + pp->linear_solver_options.use_mixed_precision_solves = + options.use_mixed_precision_solves; + pp->linear_solver_options.max_num_refinement_iterations = + options.max_num_refinement_iterations; + pp->linear_solver_options.num_threads = options.num_threads; pp->linear_solver_options.use_postordering = options.use_postordering; - if (options.linear_solver_type == SPARSE_SCHUR && - options.sparse_linear_algebra_library_type == SUITE_SPARSE && - !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { - pp->linear_solver_options.use_postordering = true; - } - - OrderingToGroupSizes(options.linear_solver_ordering.get(), - &pp->linear_solver_options.elimination_groups); + pp->linear_solver_options.context = pp->problem->context(); + + if (IsSchurType(pp->linear_solver_options.type)) { + OrderingToGroupSizes(options.linear_solver_ordering.get(), + &pp->linear_solver_options.elimination_groups); + + // Schur type solvers expect at least two elimination groups. If + // there is only one elimination group, then it is guaranteed that + // this group only contains e_blocks. Thus we add a dummy + // elimination group with zero blocks in it. + if (pp->linear_solver_options.elimination_groups.size() == 1) { + pp->linear_solver_options.elimination_groups.push_back(0); + } - // Schur type solvers expect at least two elimination groups. If - // there is only one elimination group, then it is guaranteed that - // this group only contains e_blocks. Thus we add a dummy - // elimination group with zero blocks in it. - if (IsSchurType(pp->linear_solver_options.type) && - pp->linear_solver_options.elimination_groups.size() == 1) { - pp->linear_solver_options.elimination_groups.push_back(0); + if (options.linear_solver_type == SPARSE_SCHUR) { + // When using SPARSE_SCHUR, we ignore the user's postordering + // preferences in certain cases. + // + // 1. SUITE_SPARSE is the sparse linear algebra library requested + // but cholmod_camd is not available. + // 2. CX_SPARSE is the sparse linear algebra library requested. + // + // This ensures that the linear solver does not assume that a + // fill-reducing pre-ordering has been done. + // + // TODO(sameeragarwal): Implement the reordering of parameter + // blocks for CX_SPARSE. + if ((options.sparse_linear_algebra_library_type == SUITE_SPARSE && + !SuiteSparse:: + IsConstrainedApproximateMinimumDegreeOrderingAvailable()) || + (options.sparse_linear_algebra_library_type == CX_SPARSE)) { + pp->linear_solver_options.use_postordering = true; + } + } } pp->linear_solver.reset(LinearSolver::Create(pp->linear_solver_options)); - return (pp->linear_solver.get() != NULL); + return (pp->linear_solver != nullptr); } // Configure and create the evaluator. @@ -228,19 +258,20 @@ bool SetupEvaluator(PreprocessedProblem* pp) { pp->evaluator_options.num_eliminate_blocks = 0; if (IsSchurType(options.linear_solver_type)) { pp->evaluator_options.num_eliminate_blocks = - options - .linear_solver_ordering - ->group_to_elements().begin() - ->second.size(); + options.linear_solver_ordering->group_to_elements() + .begin() + ->second.size(); } pp->evaluator_options.num_threads = options.num_threads; pp->evaluator_options.dynamic_sparsity = options.dynamic_sparsity; - pp->evaluator.reset(Evaluator::Create(pp->evaluator_options, - pp->reduced_program.get(), - &pp->error)); + pp->evaluator_options.context = pp->problem->context(); + pp->evaluator_options.evaluation_callback = + pp->reduced_program->mutable_evaluation_callback(); + pp->evaluator.reset(Evaluator::Create( + pp->evaluator_options, pp->reduced_program.get(), &pp->error)); - return (pp->evaluator.get() != NULL); + return (pp->evaluator != nullptr); } // If the user requested inner iterations, then find an inner @@ -252,6 +283,11 @@ bool SetupInnerIterationMinimizer(PreprocessedProblem* pp) { return true; } + if (pp->reduced_program->mutable_evaluation_callback()) { + pp->error = "Inner iterations cannot be used with EvaluationCallbacks"; + return false; + } + // With just one parameter block, the outer iteration of the trust // region method and inner iterations are doing exactly the same // thing, and thus inner iterations are not needed. @@ -261,7 +297,7 @@ bool SetupInnerIterationMinimizer(PreprocessedProblem* pp) { return true; } - if (options.inner_iteration_ordering.get() != NULL) { + if (options.inner_iteration_ordering != nullptr) { // If the user supplied an ordering, then remove the set of // inactive parameter blocks from it options.inner_iteration_ordering->Remove(pp->removed_parameter_blocks); @@ -283,7 +319,8 @@ bool SetupInnerIterationMinimizer(PreprocessedProblem* pp) { CoordinateDescentMinimizer::CreateOrdering(*pp->reduced_program)); } - pp->inner_iteration_minimizer.reset(new CoordinateDescentMinimizer); + pp->inner_iteration_minimizer.reset( + new CoordinateDescentMinimizer(pp->problem->context())); return pp->inner_iteration_minimizer->Init(*pp->reduced_program, pp->problem->parameter_map(), *options.inner_iteration_ordering, @@ -303,8 +340,7 @@ void SetupMinimizerOptions(PreprocessedProblem* pp) { TrustRegionStrategy::Options strategy_options; strategy_options.linear_solver = pp->linear_solver.get(); - strategy_options.initial_radius = - options.initial_trust_region_radius; + strategy_options.initial_radius = options.initial_trust_region_radius; strategy_options.max_radius = options.max_trust_region_radius; strategy_options.min_lm_diagonal = options.min_lm_diagonal; strategy_options.max_lm_diagonal = options.max_lm_diagonal; @@ -312,18 +348,18 @@ void SetupMinimizerOptions(PreprocessedProblem* pp) { options.trust_region_strategy_type; strategy_options.dogleg_type = options.dogleg_type; pp->minimizer_options.trust_region_strategy.reset( - CHECK_NOTNULL(TrustRegionStrategy::Create(strategy_options))); + TrustRegionStrategy::Create(strategy_options)); + CHECK(pp->minimizer_options.trust_region_strategy != nullptr); } } // namespace -TrustRegionPreprocessor::~TrustRegionPreprocessor() { -} +TrustRegionPreprocessor::~TrustRegionPreprocessor() {} bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, ProblemImpl* problem, PreprocessedProblem* pp) { - CHECK_NOTNULL(pp); + CHECK(pp != nullptr); pp->options = options; ChangeNumThreadsIfNeeded(&pp->options); @@ -333,10 +369,8 @@ bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, return false; } - pp->reduced_program.reset( - program->CreateReducedProgram(&pp->removed_parameter_blocks, - &pp->fixed_cost, - &pp->error)); + pp->reduced_program.reset(program->CreateReducedProgram( + &pp->removed_parameter_blocks, &pp->fixed_cost, &pp->error)); if (pp->reduced_program.get() == NULL) { return false; @@ -348,8 +382,7 @@ bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, return true; } - if (!SetupLinearSolver(pp) || - !SetupEvaluator(pp) || + if (!SetupLinearSolver(pp) || !SetupEvaluator(pp) || !SetupInnerIterationMinimizer(pp)) { return false; } diff --git a/extern/ceres/internal/ceres/trust_region_preprocessor.h b/extern/ceres/internal/ceres/trust_region_preprocessor.h index a6631ab3d40..9597905ae9a 100644 --- a/extern/ceres/internal/ceres/trust_region_preprocessor.h +++ b/extern/ceres/internal/ceres/trust_region_preprocessor.h @@ -39,9 +39,9 @@ namespace internal { class TrustRegionPreprocessor : public Preprocessor { public: virtual ~TrustRegionPreprocessor(); - virtual bool Preprocess(const Solver::Options& options, - ProblemImpl* problem, - PreprocessedProblem* preprocessed_problem); + bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* preprocessed_problem) override; }; } // namespace internal diff --git a/extern/ceres/internal/ceres/trust_region_step_evaluator.cc b/extern/ceres/internal/ceres/trust_region_step_evaluator.cc index c9167e623ef..33b0c41ead6 100644 --- a/extern/ceres/internal/ceres/trust_region_step_evaluator.cc +++ b/extern/ceres/internal/ceres/trust_region_step_evaluator.cc @@ -29,6 +29,7 @@ // Author: sameeragarwal@google.com (Sameer Agarwal) #include <algorithm> +#include <limits> #include "ceres/trust_region_step_evaluator.h" #include "glog/logging.h" @@ -51,6 +52,15 @@ TrustRegionStepEvaluator::TrustRegionStepEvaluator( double TrustRegionStepEvaluator::StepQuality( const double cost, const double model_cost_change) const { + // If the function evaluation for this step was a failure, in which + // case the TrustRegionMinimizer would have set the cost to + // std::numeric_limits<double>::max(). In this case, the division by + // model_cost_change can result in an overflow. To prevent that from + // happening, we will deal with this case explicitly. + if (cost >= std::numeric_limits<double>::max()) { + return std::numeric_limits<double>::lowest(); + } + const double relative_decrease = (current_cost_ - cost) / model_cost_change; const double historical_relative_decrease = (reference_cost_ - cost) / diff --git a/extern/ceres/internal/ceres/trust_region_step_evaluator.h b/extern/ceres/internal/ceres/trust_region_step_evaluator.h index 06df102a308..03c00362dac 100644 --- a/extern/ceres/internal/ceres/trust_region_step_evaluator.h +++ b/extern/ceres/internal/ceres/trust_region_step_evaluator.h @@ -56,7 +56,7 @@ namespace internal { // The parameter max_consecutive_nonmonotonic_steps controls the // window size used by the step selection algorithm to accept // non-monotonic steps. Setting this parameter to zero, recovers the -// classic montonic descent algorithm. +// classic monotonic descent algorithm. // // Based on algorithm 10.1.2 (page 357) of "Trust Region // Methods" by Conn Gould & Toint, or equations 33-40 of @@ -82,7 +82,7 @@ class TrustRegionStepEvaluator { // max_consecutive_nonmonotonic_steps controls the window size used // by the step selection algorithm to accept non-monotonic // steps. Setting this parameter to zero, recovers the classic - // montonic descent algorithm. + // monotonic descent algorithm. TrustRegionStepEvaluator(double initial_cost, int max_consecutive_nonmonotonic_steps); diff --git a/extern/ceres/internal/ceres/trust_region_strategy.h b/extern/ceres/internal/ceres/trust_region_strategy.h index 36e8e981cc0..5751691e5ec 100644 --- a/extern/ceres/internal/ceres/trust_region_strategy.h +++ b/extern/ceres/internal/ceres/trust_region_strategy.h @@ -56,43 +56,34 @@ class SparseMatrix; class TrustRegionStrategy { public: struct Options { - Options() - : trust_region_strategy_type(LEVENBERG_MARQUARDT), - initial_radius(1e4), - max_radius(1e32), - min_lm_diagonal(1e-6), - max_lm_diagonal(1e32), - dogleg_type(TRADITIONAL_DOGLEG) { - } - - TrustRegionStrategyType trust_region_strategy_type; + TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT; // Linear solver used for actually solving the trust region step. - LinearSolver* linear_solver; - double initial_radius; - double max_radius; + LinearSolver* linear_solver = nullptr; + double initial_radius = 1e4; + double max_radius = 1e32; // Minimum and maximum values of the diagonal damping matrix used // 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 min_lm_diagonal; - double max_lm_diagonal; + double min_lm_diagonal = 1e-6; + double max_lm_diagonal = 1e32; // Further specify which dogleg method to use - DoglegType dogleg_type; + DoglegType dogleg_type = TRADITIONAL_DOGLEG; }; + // Factory. + static TrustRegionStrategy* Create(const Options& options); + + virtual ~TrustRegionStrategy(); + // Per solve options. struct PerSolveOptions { - PerSolveOptions() - : eta(0), - dump_format_type(TEXTFILE) { - } - // Forcing sequence for inexact solves. - double eta; + double eta = 1e-1; - DumpFormatType dump_format_type; + DumpFormatType dump_format_type = TEXTFILE; // If non-empty and dump_format_type is not CONSOLE, the trust // regions strategy will write the linear system to file(s) with @@ -103,12 +94,6 @@ class TrustRegionStrategy { }; struct Summary { - Summary() - : residual_norm(0.0), - num_iterations(-1), - termination_type(LINEAR_SOLVER_FAILURE) { - } - // If the trust region problem is, // // 1/2 x'Ax + b'x + c, @@ -116,19 +101,17 @@ class TrustRegionStrategy { // then // // residual_norm = |Ax -b| - double residual_norm; + double residual_norm = -1; // Number of iterations used by the linear solver. If a linear // solver was not called (e.g., DogLegStrategy after an // unsuccessful step), then this would be zero. - int num_iterations; + int num_iterations = -1; // Status of the linear solver used to solve the Newton system. - LinearSolverTerminationType termination_type; + LinearSolverTerminationType termination_type = LINEAR_SOLVER_FAILURE; }; - virtual ~TrustRegionStrategy(); - // Use the current radius to solve for the trust region step. virtual Summary ComputeStep(const PerSolveOptions& per_solve_options, SparseMatrix* jacobian, @@ -153,9 +136,6 @@ class TrustRegionStrategy { // Current trust region radius. virtual double Radius() const = 0; - - // Factory. - static TrustRegionStrategy* Create(const Options& options); }; } // namespace internal diff --git a/extern/ceres/internal/ceres/types.cc b/extern/ceres/internal/ceres/types.cc index f86fb78eb8c..93c4cfcb027 100644 --- a/extern/ceres/internal/ceres/types.cc +++ b/extern/ceres/internal/ceres/types.cc @@ -78,6 +78,7 @@ const char* PreconditionerTypeToString(PreconditionerType type) { CASESTR(SCHUR_JACOBI); CASESTR(CLUSTER_JACOBI); CASESTR(CLUSTER_TRIDIAGONAL); + CASESTR(SUBSET); default: return "UNKNOWN"; } @@ -90,6 +91,7 @@ bool StringToPreconditionerType(string value, PreconditionerType* type) { STRENUM(SCHUR_JACOBI); STRENUM(CLUSTER_JACOBI); STRENUM(CLUSTER_TRIDIAGONAL); + STRENUM(SUBSET); return false; } @@ -99,6 +101,7 @@ const char* SparseLinearAlgebraLibraryTypeToString( CASESTR(SUITE_SPARSE); CASESTR(CX_SPARSE); CASESTR(EIGEN_SPARSE); + CASESTR(ACCELERATE_SPARSE); CASESTR(NO_SPARSE); default: return "UNKNOWN"; @@ -112,6 +115,7 @@ bool StringToSparseLinearAlgebraLibraryType( STRENUM(SUITE_SPARSE); STRENUM(CX_SPARSE); STRENUM(EIGEN_SPARSE); + STRENUM(ACCELERATE_SPARSE); STRENUM(NO_SPARSE); return false; } @@ -267,8 +271,7 @@ const char* CovarianceAlgorithmTypeToString( CovarianceAlgorithmType type) { switch (type) { CASESTR(DENSE_SVD); - CASESTR(EIGEN_SPARSE_QR); - CASESTR(SUITE_SPARSE_QR); + CASESTR(SPARSE_QR); default: return "UNKNOWN"; } @@ -279,8 +282,7 @@ bool StringToCovarianceAlgorithmType( CovarianceAlgorithmType* type) { UpperCase(&value); STRENUM(DENSE_SVD); - STRENUM(EIGEN_SPARSE_QR); - STRENUM(SUITE_SPARSE_QR); + STRENUM(SPARSE_QR); return false; } @@ -336,6 +338,39 @@ const char* TerminationTypeToString(TerminationType type) { } } +const char* LoggingTypeToString(LoggingType type) { + switch (type) { + CASESTR(SILENT); + CASESTR(PER_MINIMIZER_ITERATION); + default: + return "UNKNOWN"; + } +} + +bool StringtoLoggingType(std::string value, LoggingType* type) { + UpperCase(&value); + STRENUM(SILENT); + STRENUM(PER_MINIMIZER_ITERATION); + return false; +} + + +const char* DumpFormatTypeToString(DumpFormatType type) { + switch (type) { + CASESTR(CONSOLE); + CASESTR(TEXTFILE); + default: + return "UNKNOWN"; + } +} + +bool StringtoDumpFormatType(std::string value, DumpFormatType* type) { + UpperCase(&value); + STRENUM(CONSOLE); + STRENUM(TEXTFILE); + return false; +} + #undef CASESTR #undef STRENUM @@ -363,6 +398,14 @@ bool IsSparseLinearAlgebraLibraryTypeAvailable( #endif } + if (type == ACCELERATE_SPARSE) { +#ifdef CERES_NO_ACCELERATE_SPARSE + return false; +#else + return true; +#endif + } + if (type == EIGEN_SPARSE) { #ifdef CERES_USE_EIGEN_SPARSE return true; diff --git a/extern/ceres/internal/ceres/visibility.cc b/extern/ceres/internal/ceres/visibility.cc new file mode 100644 index 00000000000..0981eeddcbf --- /dev/null +++ b/extern/ceres/internal/ceres/visibility.cc @@ -0,0 +1,152 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: kushalav@google.com (Avanish Kushal) + +#include "ceres/visibility.h" + +#include <cmath> +#include <ctime> +#include <algorithm> +#include <set> +#include <vector> +#include <unordered_map> +#include <utility> +#include "ceres/block_structure.h" +#include "ceres/graph.h" +#include "ceres/pair_hash.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::make_pair; +using std::max; +using std::pair; +using std::set; +using std::vector; + +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + const int num_eliminate_blocks, + vector<set<int>>* visibility) { + CHECK(visibility != nullptr); + + // Clear the visibility vector and resize it to hold a + // vector for each camera. + visibility->resize(0); + visibility->resize(block_structure.cols.size() - num_eliminate_blocks); + + for (int i = 0; i < block_structure.rows.size(); ++i) { + const vector<Cell>& cells = block_structure.rows[i].cells; + int block_id = cells[0].block_id; + // If the first block is not an e_block, then skip this row block. + if (block_id >= num_eliminate_blocks) { + continue; + } + + for (int j = 1; j < cells.size(); ++j) { + int camera_block_id = cells[j].block_id - num_eliminate_blocks; + DCHECK_GE(camera_block_id, 0); + DCHECK_LT(camera_block_id, visibility->size()); + (*visibility)[camera_block_id].insert(block_id); + } + } +} + +WeightedGraph<int>* CreateSchurComplementGraph( + const vector<set<int>>& visibility) { + const time_t start_time = time(NULL); + // Compute the number of e_blocks/point blocks. Since the visibility + // set for each e_block/camera contains the set of e_blocks/points + // visible to it, we find the maximum across all visibility sets. + int num_points = 0; + for (int i = 0; i < visibility.size(); i++) { + if (visibility[i].size() > 0) { + num_points = max(num_points, (*visibility[i].rbegin()) + 1); + } + } + + // Invert the visibility. The input is a camera->point mapping, + // which tells us which points are visible in which + // cameras. However, to compute the sparsity structure of the Schur + // Complement efficiently, its better to have the point->camera + // mapping. + vector<set<int>> inverse_visibility(num_points); + for (int i = 0; i < visibility.size(); i++) { + const set<int>& visibility_set = visibility[i]; + for (const int v : visibility_set) { + inverse_visibility[v].insert(i); + } + } + + // Map from camera pairs to number of points visible to both cameras + // in the pair. + std::unordered_map<pair<int, int>, int, pair_hash> camera_pairs; + + // Count the number of points visible to each camera/f_block pair. + for (const auto& inverse_visibility_set : inverse_visibility) { + for (set<int>::const_iterator camera1 = inverse_visibility_set.begin(); + camera1 != inverse_visibility_set.end(); + ++camera1) { + set<int>::const_iterator camera2 = camera1; + for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) { + ++(camera_pairs[make_pair(*camera1, *camera2)]); + } + } + } + + WeightedGraph<int>* graph = new WeightedGraph<int>; + + // Add vertices and initialize the pairs for self edges so that self + // edges are guaranteed. This is needed for the Canonical views + // algorithm to work correctly. + static constexpr double kSelfEdgeWeight = 1.0; + for (int i = 0; i < visibility.size(); ++i) { + graph->AddVertex(i); + graph->AddEdge(i, i, kSelfEdgeWeight); + } + + // Add an edge for each camera pair. + for (const auto& camera_pair_count : camera_pairs) { + const int camera1 = camera_pair_count.first.first; + const int camera2 = camera_pair_count.first.second; + const int count = camera_pair_count.second; + DCHECK_NE(camera1, camera2); + // Static cast necessary for Windows. + const double weight = static_cast<double>(count) / + (sqrt(static_cast<double>( + visibility[camera1].size() * visibility[camera2].size()))); + graph->AddEdge(camera1, camera2, weight); + } + + VLOG(2) << "Schur complement graph time: " << (time(NULL) - start_time); + return graph; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/visibility.h b/extern/ceres/internal/ceres/visibility.h new file mode 100644 index 00000000000..115d45f7cf6 --- /dev/null +++ b/extern/ceres/internal/ceres/visibility.h @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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: kushalav@google.com (Avanish Kushal) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Functions to manipulate visibility information from the block +// structure of sparse matrices. + +#ifndef CERES_INTERNAL_VISIBILITY_H_ +#define CERES_INTERNAL_VISIBILITY_H_ + +#include <set> +#include <vector> +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +struct CompressedRowBlockStructure; + +// Given a compressed row block structure, computes the set of +// e_blocks "visible" to each f_block. If an e_block co-occurs with an +// f_block in a residual block, it is visible to the f_block. The +// first num_eliminate_blocks columns blocks are e_blocks and the rest +// f_blocks. +// +// In a structure from motion problem, e_blocks correspond to 3D +// points and f_blocks correspond to cameras. +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + int num_eliminate_blocks, + std::vector<std::set<int>>* visibility); + +// Given f_block visibility as computed by the ComputeVisibility +// function above, construct and return a graph whose vertices are +// f_blocks and an edge connects two vertices if they have at least one +// e_block in common. The weight of this edge is normalized dot +// product between the visibility vectors of the two +// vertices/f_blocks. +// +// This graph reflects the sparsity structure of reduced camera +// matrix/Schur complement matrix obtained by eliminating the e_blocks +// from the normal equations. +// +// Caller acquires ownership of the returned WeightedGraph pointer +// (heap-allocated). +WeightedGraph<int>* CreateSchurComplementGraph( + const std::vector<std::set<int>>& visibility); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_H_ diff --git a/extern/ceres/internal/ceres/visibility_based_preconditioner.cc b/extern/ceres/internal/ceres/visibility_based_preconditioner.cc new file mode 100644 index 00000000000..3372e82d1e1 --- /dev/null +++ b/extern/ceres/internal/ceres/visibility_based_preconditioner.cc @@ -0,0 +1,585 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// 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/visibility_based_preconditioner.h" + +#include <algorithm> +#include <functional> +#include <iterator> +#include <memory> +#include <set> +#include <utility> +#include <vector> + +#include "Eigen/Dense" +#include "ceres/block_random_access_sparse_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/canonical_views_clustering.h" +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/single_linkage_clustering.h" +#include "ceres/visibility.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +using std::make_pair; +using std::pair; +using std::set; +using std::swap; +using std::vector; + +// TODO(sameeragarwal): Currently these are magic weights for the +// preconditioner construction. Move these higher up into the Options +// struct and provide some guidelines for choosing them. +// +// This will require some more work on the clustering algorithm and +// possibly some more refactoring of the code. +static constexpr double kCanonicalViewsSizePenaltyWeight = 3.0; +static constexpr double kCanonicalViewsSimilarityPenaltyWeight = 0.0; +static constexpr double kSingleLinkageMinSimilarity = 0.9; + +VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( + const CompressedRowBlockStructure& bs, + const Preconditioner::Options& options) + : options_(options), num_blocks_(0), num_clusters_(0) { + CHECK_GT(options_.elimination_groups.size(), 1); + CHECK_GT(options_.elimination_groups[0], 0); + CHECK(options_.type == CLUSTER_JACOBI || options_.type == CLUSTER_TRIDIAGONAL) + << "Unknown preconditioner type: " << options_.type; + num_blocks_ = bs.cols.size() - options_.elimination_groups[0]; + CHECK_GT(num_blocks_, 0) << "Jacobian should have at least 1 f_block for " + << "visibility based preconditioning."; + CHECK(options_.context != NULL); + + // Vector of camera block sizes + block_size_.resize(num_blocks_); + for (int i = 0; i < num_blocks_; ++i) { + block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size; + } + + const time_t start_time = time(NULL); + switch (options_.type) { + case CLUSTER_JACOBI: + ComputeClusterJacobiSparsity(bs); + break; + case CLUSTER_TRIDIAGONAL: + ComputeClusterTridiagonalSparsity(bs); + break; + default: + LOG(FATAL) << "Unknown preconditioner type"; + } + const time_t structure_time = time(NULL); + InitStorage(bs); + const time_t storage_time = time(NULL); + InitEliminator(bs); + const time_t eliminator_time = time(NULL); + + LinearSolver::Options sparse_cholesky_options; + sparse_cholesky_options.sparse_linear_algebra_library_type = + options_.sparse_linear_algebra_library_type; + + // The preconditioner's sparsity is not available in the + // preprocessor, so the columns of the Jacobian have not been + // reordered to minimize fill in when computing its sparse Cholesky + // factorization. So we must tell the SparseCholesky object to + // perform approximate minimum-degree reordering, which is done by + // setting use_postordering to true. + sparse_cholesky_options.use_postordering = true; + sparse_cholesky_ = SparseCholesky::Create(sparse_cholesky_options); + + const time_t init_time = time(NULL); + VLOG(2) << "init time: " << init_time - start_time + << " structure time: " << structure_time - start_time + << " storage time:" << storage_time - structure_time + << " eliminator time: " << eliminator_time - storage_time; +} + +VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() {} + +// Determine the sparsity structure of the CLUSTER_JACOBI +// preconditioner. It clusters cameras using their scene +// visibility. The clusters form the diagonal blocks of the +// preconditioner matrix. +void VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int>> visibility; + ComputeVisibility(bs, options_.elimination_groups[0], &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + cluster_pairs_.clear(); + for (int i = 0; i < num_clusters_; ++i) { + cluster_pairs_.insert(make_pair(i, i)); + } +} + +// Determine the sparsity structure of the CLUSTER_TRIDIAGONAL +// preconditioner. It clusters cameras using using the scene +// visibility and then finds the strongly interacting pairs of +// clusters by constructing another graph with the clusters as +// vertices and approximating it with a degree-2 maximum spanning +// forest. The set of edges in this forest are the cluster pairs. +void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int>> visibility; + ComputeVisibility(bs, options_.elimination_groups[0], &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + + // Construct a weighted graph on the set of clusters, where the + // edges are the number of 3D points/e_blocks visible in both the + // clusters at the ends of the edge. Return an approximate degree-2 + // maximum spanning forest of this graph. + vector<set<int>> cluster_visibility; + ComputeClusterVisibility(visibility, &cluster_visibility); + std::unique_ptr<WeightedGraph<int>> cluster_graph( + CreateClusterGraph(cluster_visibility)); + CHECK(cluster_graph != nullptr); + std::unique_ptr<WeightedGraph<int>> forest( + Degree2MaximumSpanningForest(*cluster_graph)); + CHECK(forest != nullptr); + ForestToClusterPairs(*forest, &cluster_pairs_); +} + +// Allocate storage for the preconditioner matrix. +void VisibilityBasedPreconditioner::InitStorage( + const CompressedRowBlockStructure& bs) { + ComputeBlockPairsInPreconditioner(bs); + m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_)); +} + +// Call the canonical views algorithm and cluster the cameras based on +// their visibility sets. The visibility set of a camera is the set of +// e_blocks/3D points in the scene that are seen by it. +// +// The cluster_membership_ vector is updated to indicate cluster +// memberships for each camera block. +void VisibilityBasedPreconditioner::ClusterCameras( + const vector<set<int>>& visibility) { + std::unique_ptr<WeightedGraph<int>> schur_complement_graph( + CreateSchurComplementGraph(visibility)); + CHECK(schur_complement_graph != nullptr); + + std::unordered_map<int, int> membership; + + if (options_.visibility_clustering_type == CANONICAL_VIEWS) { + vector<int> centers; + CanonicalViewsClusteringOptions clustering_options; + clustering_options.size_penalty_weight = kCanonicalViewsSizePenaltyWeight; + clustering_options.similarity_penalty_weight = + kCanonicalViewsSimilarityPenaltyWeight; + ComputeCanonicalViewsClustering( + clustering_options, *schur_complement_graph, ¢ers, &membership); + num_clusters_ = centers.size(); + } else if (options_.visibility_clustering_type == SINGLE_LINKAGE) { + SingleLinkageClusteringOptions clustering_options; + clustering_options.min_similarity = kSingleLinkageMinSimilarity; + num_clusters_ = ComputeSingleLinkageClustering( + clustering_options, *schur_complement_graph, &membership); + } else { + LOG(FATAL) << "Unknown visibility clustering algorithm."; + } + + CHECK_GT(num_clusters_, 0); + VLOG(2) << "num_clusters: " << num_clusters_; + FlattenMembershipMap(membership, &cluster_membership_); +} + +// Compute the block sparsity structure of the Schur complement +// matrix. For each pair of cameras contributing a non-zero cell to +// the schur complement, determine if that cell is present in the +// preconditioner or not. +// +// A pair of cameras contribute a cell to the preconditioner if they +// are part of the same cluster or if the two clusters that they +// belong have an edge connecting them in the degree-2 maximum +// spanning forest. +// +// For example, a camera pair (i,j) where i belongs to cluster1 and +// j belongs to cluster2 (assume that cluster1 < cluster2). +// +// The cell corresponding to (i,j) is present in the preconditioner +// if cluster1 == cluster2 or the pair (cluster1, cluster2) were +// connected by an edge in the degree-2 maximum spanning forest. +// +// Since we have already expanded the forest into a set of camera +// pairs/edges, including self edges, the check can be reduced to +// checking membership of (cluster1, cluster2) in cluster_pairs_. +void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( + const CompressedRowBlockStructure& bs) { + block_pairs_.clear(); + for (int i = 0; i < num_blocks_; ++i) { + block_pairs_.insert(make_pair(i, i)); + } + + int r = 0; + const int num_row_blocks = bs.rows.size(); + const int num_eliminate_blocks = options_.elimination_groups[0]; + + // Iterate over each row of the matrix. The block structure of the + // matrix is assumed to be sorted in order of the e_blocks/point + // blocks. Thus all row blocks containing an e_block/point occur + // contiguously. Further, if present, an e_block is always the first + // parameter block in each row block. These structural assumptions + // are common to all Schur complement based solvers in Ceres. + // + // For each e_block/point block we identify the set of cameras + // seeing it. The cross product of this set with itself is the set + // of non-zero cells contributed by this e_block. + // + // The time complexity of this is O(nm^2) where, n is the number of + // 3d points and m is the maximum number of cameras seeing any + // point, which for most scenes is a fairly small number. + while (r < num_row_blocks) { + int e_block_id = bs.rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + // Skip the rows whose first block is an f_block. + break; + } + + set<int> f_blocks; + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first block + // since it is the one to be eliminated and adding the rest to + // the list of f_blocks associated with this e_block. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + const int f_block_id = cell.block_id - num_eliminate_blocks; + CHECK_GE(f_block_id, 0); + f_blocks.insert(f_block_id); + } + } + + for (set<int>::const_iterator block1 = f_blocks.begin(); + block1 != f_blocks.end(); + ++block1) { + set<int>::const_iterator block2 = block1; + ++block2; + for (; block2 != f_blocks.end(); ++block2) { + if (IsBlockPairInPreconditioner(*block1, *block2)) { + block_pairs_.insert(make_pair(*block1, *block2)); + } + } + } + } + + // The remaining rows which do not contain any e_blocks. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks; + if (block1 <= block2) { + if (IsBlockPairInPreconditioner(block1, block2)) { + block_pairs_.insert(make_pair(block1, block2)); + } + } + } + } + } + + VLOG(1) << "Block pair stats: " << block_pairs_.size(); +} + +// Initialize the SchurEliminator. +void VisibilityBasedPreconditioner::InitEliminator( + const CompressedRowBlockStructure& bs) { + LinearSolver::Options eliminator_options; + eliminator_options.elimination_groups = options_.elimination_groups; + eliminator_options.num_threads = options_.num_threads; + eliminator_options.e_block_size = options_.e_block_size; + eliminator_options.f_block_size = options_.f_block_size; + eliminator_options.row_block_size = options_.row_block_size; + eliminator_options.context = options_.context; + eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); + const bool kFullRankETE = true; + eliminator_->Init( + eliminator_options.elimination_groups[0], kFullRankETE, &bs); +} + +// Update the values of the preconditioner matrix and factorize it. +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); + + // Compute a subset of the entries of the Schur complement. + eliminator_->Eliminate( + BlockSparseMatrixData(A), nullptr, D, m_.get(), nullptr); + + // Try factorizing the matrix. For CLUSTER_JACOBI, this should + // always succeed modulo some numerical/conditioning problems. For + // CLUSTER_TRIDIAGONAL, in general the preconditioner matrix as + // constructed is not positive definite. However, we will go ahead + // and try factorizing it. If it works, great, otherwise we scale + // all the cells in the preconditioner corresponding to the edges in + // the degree-2 forest and that guarantees positive + // definiteness. The proof of this fact can be found in Lemma 1 in + // "Visibility Based Preconditioning for Bundle Adjustment". + // + // Doing the factorization like this saves us matrix mass when + // scaling is not needed, which is quite often in our experience. + LinearSolverTerminationType status = Factorize(); + + if (status == LINEAR_SOLVER_FATAL_ERROR) { + return false; + } + + // The scaling only affects the tri-diagonal case, since + // ScaleOffDiagonalBlocks only pays attention to the cells that + // belong to the edges of the degree-2 forest. In the CLUSTER_JACOBI + // case, the preconditioner is guaranteed to be positive + // semidefinite. + if (status == LINEAR_SOLVER_FAILURE && options_.type == CLUSTER_TRIDIAGONAL) { + VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal " + << "scaling"; + ScaleOffDiagonalCells(); + status = Factorize(); + } + + VLOG(2) << "Compute time: " << time(NULL) - start_time; + return (status == LINEAR_SOLVER_SUCCESS); +} + +// Consider the preconditioner matrix as meta-block matrix, whose +// blocks correspond to the clusters. Then cluster pairs corresponding +// to edges in the degree-2 forest are off diagonal entries of this +// matrix. Scaling these off-diagonal entries by 1/2 forces this +// matrix to be positive definite. +void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() { + for (const auto& block_pair : block_pairs_) { + const int block1 = block_pair.first; + const int block2 = block_pair.second; + if (!IsBlockPairOffDiagonal(block1, block2)) { + continue; + } + + int r, c, row_stride, col_stride; + CellInfo* cell_info = + m_->GetCell(block1, block2, &r, &c, &row_stride, &col_stride); + CHECK(cell_info != NULL) + << "Cell missing for block pair (" << block1 << "," << block2 << ")" + << " cluster pair (" << cluster_membership_[block1] << " " + << cluster_membership_[block2] << ")"; + + // Ah the magic of tri-diagonal matrices and diagonal + // dominance. See Lemma 1 in "Visibility Based Preconditioning + // For Bundle Adjustment". + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5; + } +} + +// Compute the sparse Cholesky factorization of the preconditioner +// matrix. +LinearSolverTerminationType VisibilityBasedPreconditioner::Factorize() { + // Extract the TripletSparseMatrix that is used for actually storing + // S and convert it into a CompressedRowSparseMatrix. + const TripletSparseMatrix* tsm = + down_cast<BlockRandomAccessSparseMatrix*>(m_.get())->mutable_matrix(); + + std::unique_ptr<CompressedRowSparseMatrix> lhs; + const CompressedRowSparseMatrix::StorageType storage_type = + sparse_cholesky_->StorageType(); + if (storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR) { + lhs.reset(CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm)); + lhs->set_storage_type(CompressedRowSparseMatrix::UPPER_TRIANGULAR); + } else { + lhs.reset( + CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm)); + lhs->set_storage_type(CompressedRowSparseMatrix::LOWER_TRIANGULAR); + } + + std::string message; + return sparse_cholesky_->Factorize(lhs.get(), &message); +} + +void VisibilityBasedPreconditioner::RightMultiply(const double* x, + double* y) const { + CHECK(x != nullptr); + CHECK(y != nullptr); + CHECK(sparse_cholesky_ != nullptr); + std::string message; + sparse_cholesky_->Solve(x, y, &message); +} + +int VisibilityBasedPreconditioner::num_rows() const { return m_->num_rows(); } + +// Classify camera/f_block pairs as in and out of the preconditioner, +// based on whether the cluster pair that they belong to is in the +// preconditioner or not. +bool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner( + const int block1, const int block2) const { + int cluster1 = cluster_membership_[block1]; + int cluster2 = cluster_membership_[block2]; + if (cluster1 > cluster2) { + swap(cluster1, cluster2); + } + return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0); +} + +bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal( + const int block1, const int block2) const { + return (cluster_membership_[block1] != cluster_membership_[block2]); +} + +// Convert a graph into a list of edges that includes self edges for +// each vertex. +void VisibilityBasedPreconditioner::ForestToClusterPairs( + const WeightedGraph<int>& forest, + std::unordered_set<pair<int, int>, pair_hash>* cluster_pairs) const { + CHECK(cluster_pairs != nullptr); + cluster_pairs->clear(); + const std::unordered_set<int>& vertices = forest.vertices(); + CHECK_EQ(vertices.size(), num_clusters_); + + // Add all the cluster pairs corresponding to the edges in the + // forest. + for (const int cluster1 : vertices) { + cluster_pairs->insert(make_pair(cluster1, cluster1)); + const std::unordered_set<int>& neighbors = forest.Neighbors(cluster1); + for (const int cluster2 : neighbors) { + if (cluster1 < cluster2) { + cluster_pairs->insert(make_pair(cluster1, cluster2)); + } + } + } +} + +// The visibility set of a cluster is the union of the visibility sets +// of all its cameras. In other words, the set of points visible to +// any camera in the cluster. +void VisibilityBasedPreconditioner::ComputeClusterVisibility( + const vector<set<int>>& visibility, + vector<set<int>>* cluster_visibility) const { + CHECK(cluster_visibility != nullptr); + cluster_visibility->resize(0); + cluster_visibility->resize(num_clusters_); + for (int i = 0; i < num_blocks_; ++i) { + const int cluster_id = cluster_membership_[i]; + (*cluster_visibility)[cluster_id].insert(visibility[i].begin(), + visibility[i].end()); + } +} + +// Construct a graph whose vertices are the clusters, and the edge +// weights are the number of 3D points visible to cameras in both the +// vertices. +WeightedGraph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( + const vector<set<int>>& cluster_visibility) const { + WeightedGraph<int>* cluster_graph = new WeightedGraph<int>; + + for (int i = 0; i < num_clusters_; ++i) { + cluster_graph->AddVertex(i); + } + + for (int i = 0; i < num_clusters_; ++i) { + const set<int>& cluster_i = cluster_visibility[i]; + for (int j = i + 1; j < num_clusters_; ++j) { + vector<int> intersection; + const set<int>& cluster_j = cluster_visibility[j]; + set_intersection(cluster_i.begin(), + cluster_i.end(), + cluster_j.begin(), + cluster_j.end(), + back_inserter(intersection)); + + if (intersection.size() > 0) { + // Clusters interact strongly when they share a large number + // of 3D points. The degree-2 maximum spanning forest + // algorithm, iterates on the edges in decreasing order of + // their weight, which is the number of points shared by the + // two cameras that it connects. + cluster_graph->AddEdge(i, j, intersection.size()); + } + } + } + return cluster_graph; +} + +// Canonical views clustering returns a std::unordered_map from vertices to +// cluster ids. Convert this into a flat array for quick lookup. It is +// possible that some of the vertices may not be associated with any +// cluster. In that case, randomly assign them to one of the clusters. +// +// The cluster ids can be non-contiguous integers. So as we flatten +// the membership_map, we also map the cluster ids to a contiguous set +// of integers so that the cluster ids are in [0, num_clusters_). +void VisibilityBasedPreconditioner::FlattenMembershipMap( + const std::unordered_map<int, int>& membership_map, + vector<int>* membership_vector) const { + CHECK(membership_vector != nullptr); + membership_vector->resize(0); + membership_vector->resize(num_blocks_, -1); + + std::unordered_map<int, int> cluster_id_to_index; + // Iterate over the cluster membership map and update the + // cluster_membership_ vector assigning arbitrary cluster ids to + // the few cameras that have not been clustered. + for (const auto& m : membership_map) { + const int camera_id = m.first; + int cluster_id = m.second; + + // If the view was not clustered, randomly assign it to one of the + // clusters. This preserves the mathematical correctness of the + // preconditioner. If there are too many views which are not + // clustered, it may lead to some quality degradation though. + // + // TODO(sameeragarwal): Check if a large number of views have not + // been clustered and deal with it? + if (cluster_id == -1) { + cluster_id = camera_id % num_clusters_; + } + + const int index = FindWithDefault( + cluster_id_to_index, cluster_id, cluster_id_to_index.size()); + + if (index == cluster_id_to_index.size()) { + cluster_id_to_index[cluster_id] = index; + } + + CHECK_LT(index, num_clusters_); + membership_vector->at(camera_id) = index; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/ceres/internal/ceres/visibility_based_preconditioner.h b/extern/ceres/internal/ceres/visibility_based_preconditioner.h index a627c13523c..aa582d5e7ef 100644 --- a/extern/ceres/internal/ceres/visibility_based_preconditioner.h +++ b/extern/ceres/internal/ceres/visibility_based_preconditioner.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2017 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -48,16 +48,18 @@ #ifndef CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ #define CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ +#include <memory> #include <set> -#include <vector> +#include <unordered_map> +#include <unordered_set> #include <utility> -#include "ceres/collections_port.h" +#include <vector> + #include "ceres/graph.h" -#include "ceres/internal/macros.h" -#include "ceres/internal/scoped_ptr.h" #include "ceres/linear_solver.h" +#include "ceres/pair_hash.h" #include "ceres/preconditioner.h" -#include "ceres/suitesparse.h" +#include "ceres/sparse_cholesky.h" namespace ceres { namespace internal { @@ -122,8 +124,6 @@ class SchurEliminatorBase; // *A.block_structure(), options); // preconditioner.Update(A, NULL); // preconditioner.RightMultiply(x, y); -// -#ifndef CERES_NO_SUITESPARSE class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { public: // Initialize the symbolic structure of the preconditioner. bs is @@ -134,16 +134,19 @@ class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { // based solvers. Please see schur_eliminator.h for more details. VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, const Preconditioner::Options& options); + VisibilityBasedPreconditioner(const VisibilityBasedPreconditioner&) = delete; + void operator=(const VisibilityBasedPreconditioner&) = delete; + virtual ~VisibilityBasedPreconditioner(); // Preconditioner interface - virtual void RightMultiply(const double* x, double* y) const; - virtual int num_rows() const; + void RightMultiply(const double* x, double* y) const final; + int num_rows() const final; friend class VisibilityBasedPreconditionerTest; private: - virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); + bool UpdateImpl(const BlockSparseMatrix& A, const double* D) final; void ComputeClusterJacobiSparsity(const CompressedRowBlockStructure& bs); void ComputeClusterTridiagonalSparsity(const CompressedRowBlockStructure& bs); void InitStorage(const CompressedRowBlockStructure& bs); @@ -151,16 +154,16 @@ class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { LinearSolverTerminationType Factorize(); void ScaleOffDiagonalCells(); - void ClusterCameras(const std::vector<std::set<int> >& visibility); - void FlattenMembershipMap(const HashMap<int, int>& membership_map, + void ClusterCameras(const std::vector<std::set<int>>& visibility); + void FlattenMembershipMap(const std::unordered_map<int, int>& membership_map, std::vector<int>* membership_vector) const; void ComputeClusterVisibility( - const std::vector<std::set<int> >& visibility, - std::vector<std::set<int> >* cluster_visibility) const; + const std::vector<std::set<int>>& visibility, + std::vector<std::set<int>>* cluster_visibility) const; WeightedGraph<int>* CreateClusterGraph( - const std::vector<std::set<int> >& visibility) const; + const std::vector<std::set<int>>& visibility) const; void ForestToClusterPairs(const WeightedGraph<int>& forest, - HashSet<std::pair<int, int> >* cluster_pairs) const; + std::unordered_set<std::pair<int, int>, pair_hash>* cluster_pairs) const; void ComputeBlockPairsInPreconditioner(const CompressedRowBlockStructure& bs); bool IsBlockPairInPreconditioner(int block1, int block2) const; bool IsBlockPairOffDiagonal(int block1, int block2) const; @@ -180,52 +183,17 @@ class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { // Non-zero camera pairs from the schur complement matrix that are // present in the preconditioner, sorted by row (first element of // each pair), then column (second). - std::set<std::pair<int, int> > block_pairs_; + std::set<std::pair<int, int>> block_pairs_; // Set of cluster pairs (including self pairs (i,i)) in the // preconditioner. - HashSet<std::pair<int, int> > cluster_pairs_; - scoped_ptr<SchurEliminatorBase> eliminator_; + std::unordered_set<std::pair<int, int>, pair_hash> cluster_pairs_; + std::unique_ptr<SchurEliminatorBase> eliminator_; // Preconditioner matrix. - scoped_ptr<BlockRandomAccessSparseMatrix> m_; - - // RightMultiply is a const method for LinearOperators. It is - // implemented using CHOLMOD's sparse triangular matrix solve - // function. This however requires non-const access to the - // SuiteSparse context object, even though it does not result in any - // of the state of the preconditioner being modified. - SuiteSparse ss_; - - // Symbolic and numeric factorization of the preconditioner. - cholmod_factor* factor_; - - // Temporary vector used by RightMultiply. - cholmod_dense* tmp_rhs_; - CERES_DISALLOW_COPY_AND_ASSIGN(VisibilityBasedPreconditioner); -}; -#else // SuiteSparse -// If SuiteSparse is not compiled in, the preconditioner is not -// available. -class VisibilityBasedPreconditioner : public BlockSparseMatrixPreconditioner { - public: - VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, - const Preconditioner::Options& options) { - LOG(FATAL) << "Visibility based preconditioning is not available. Please " - "build Ceres with SuiteSparse."; - } - virtual ~VisibilityBasedPreconditioner() {} - virtual void RightMultiply(const double* x, double* y) const {} - virtual void LeftMultiply(const double* x, double* y) const {} - virtual int num_rows() const { return -1; } - virtual int num_cols() const { return -1; } - - private: - bool UpdateImpl(const BlockSparseMatrix& A, const double* D) { - return false; - } + std::unique_ptr<BlockRandomAccessSparseMatrix> m_; + std::unique_ptr<SparseCholesky> sparse_cholesky_; }; -#endif // CERES_NO_SUITESPARSE } // namespace internal } // namespace ceres diff --git a/extern/ceres/internal/ceres/wall_time.cc b/extern/ceres/internal/ceres/wall_time.cc index c353973cc3e..716392741e9 100644 --- a/extern/ceres/internal/ceres/wall_time.cc +++ b/extern/ceres/internal/ceres/wall_time.cc @@ -50,7 +50,12 @@ double WallTimeInSeconds() { return omp_get_wtime(); #else #ifdef _WIN32 - return static_cast<double>(std::time(NULL)); + LARGE_INTEGER count; + LARGE_INTEGER frequency; + QueryPerformanceCounter(&count); + QueryPerformanceFrequency(&frequency); + return static_cast<double>(count.QuadPart) / + static_cast<double>(frequency.QuadPart); #else timeval time_val; gettimeofday(&time_val, NULL); @@ -59,20 +64,24 @@ double WallTimeInSeconds() { #endif } -EventLogger::EventLogger(const std::string& logger_name) - : start_time_(WallTimeInSeconds()), - last_event_time_(start_time_), - events_("") { - StringAppendF(&events_, - "\n%s\n Delta Cumulative\n", - logger_name.c_str()); +EventLogger::EventLogger(const std::string& logger_name) { + if (!VLOG_IS_ON(3)) { + return; + } + + start_time_ = WallTimeInSeconds(); + last_event_time_ = start_time_; + events_ = StringPrintf( + "\n%s\n Delta Cumulative\n", + logger_name.c_str()); } EventLogger::~EventLogger() { - if (VLOG_IS_ON(3)) { - AddEvent("Total"); - VLOG(2) << "\n" << events_ << "\n"; + if (!VLOG_IS_ON(3)) { + return; } + AddEvent("Total"); + VLOG(3) << "\n" << events_ << "\n"; } void EventLogger::AddEvent(const std::string& event_name) { diff --git a/extern/ceres/internal/ceres/wall_time.h b/extern/ceres/internal/ceres/wall_time.h index 966aa67cab6..ed0610f27da 100644 --- a/extern/ceres/internal/ceres/wall_time.h +++ b/extern/ceres/internal/ceres/wall_time.h @@ -77,7 +77,7 @@ class EventLogger { void AddEvent(const std::string& event_name); private: - const double start_time_; + double start_time_; double last_event_time_; std::string events_; }; |