diff options
Diffstat (limited to 'extern/ceres/internal/ceres/reorder_program.cc')
| -rw-r--r-- | extern/ceres/internal/ceres/reorder_program.cc | 87 |
1 files changed, 43 insertions, 44 deletions
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 |