Refactor reordering routines. Move all the program reordering programs into their own file. Also split the reordering routines for SPARSE_NORMAL_CHOLESKY into individual library dependent routines. Also get rid of RemovedFixedBlocksFromProgram. Change-Id: Ie969f529e6d20dded9da021b9df1a040e08287c1
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index dd48ab7..89757b3 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -87,6 +87,7 @@ problem.cc problem_impl.cc program.cc + reorder_program.cc residual_block.cc residual_block_utils.cc schur_complement_solver.cc @@ -269,6 +270,7 @@ CERES_TEST(polynomial) CERES_TEST(problem) CERES_TEST(program) + CERES_TEST(reorder_program) CERES_TEST(residual_block) CERES_TEST(residual_block_utils) CERES_TEST(rotation)
diff --git a/internal/ceres/reorder_program.cc b/internal/ceres/reorder_program.cc new file mode 100644 index 0000000..c1afba7 --- /dev/null +++ b/internal/ceres/reorder_program.cc
@@ -0,0 +1,418 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/reorder_program.h" + +#include <algorithm> +#include <numeric> +#include <vector> + +#include "glog/logging.h" +#include "ceres/cxsparse.h" +#include "ceres/ordered_groups.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/program.h" +#include "ceres/solver.h" +#include "ceres/suitesparse.h" +#include "ceres/types.h" +#include "ceres/internal/port.h" +#include "ceres/residual_block.h" +#include "ceres/parameter_block.h" + +namespace ceres { +namespace internal { +namespace { + +// Find the minimum index of any parameter block to the given residual. +// Parameter blocks that have indices greater than num_eliminate_blocks are +// considered to have an index equal to num_eliminate_blocks. +static int MinParameterBlock(const ResidualBlock* residual_block, + int num_eliminate_blocks) { + int min_parameter_block_position = num_eliminate_blocks; + for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; + if (!parameter_block->IsConstant()) { + CHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " + << "This is a Ceres bug; please contact the developers!"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } + } + return min_parameter_block_position; +} + +void OrderingForSparseNormalCholeskyUsingSuiteSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + const vector<ParameterBlock*>& parameter_blocks, + const ParameterBlockOrdering& parameter_block_ordering, + int* ordering) { +#ifdef CERES_NO_SUITESPARSE + LOG(FATAL) << "Congratulations, you found a Ceres bug! " + << "Please report this error to the developers."; +#else + SuiteSparse ss; + cholmod_sparse* block_jacobian_transpose = + ss.CreateSparseMatrix( + const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose)); + + // No CAMD or the user did not supply a useful ordering, then just + // use regular AMD. + if (parameter_block_ordering.NumGroups() <= 1 || + !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); + } else { + vector<int> constraints; + for (int i = 0; i < parameter_blocks.size(); ++i) { + constraints.push_back( + parameter_block_ordering.GroupId( + parameter_blocks[i]->mutable_user_state())); + } + ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, + &constraints[0], + ordering); + } + + ss.Free(block_jacobian_transpose); +#endif // CERES_NO_SUITESPARSE +} + +void OrderingForSparseNormalCholeskyUsingCXSparse( + const TripletSparseMatrix& tsm_block_jacobian_transpose, + int* ordering) { +#ifdef CERES_NO_CXSPARSE + LOG(FATAL) << "Congratulations, you found a Ceres bug! " + << "Please report this error to the developers."; +#else // CERES_NO_CXSPARSE + // CXSparse works with J'J instead of J'. So compute the block + // sparsity for J'J and compute an approximate minimum degree + // ordering. + CXSparse cxsparse; + cs_di* block_jacobian_transpose; + block_jacobian_transpose = + cxsparse.CreateSparseMatrix( + const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose)); + cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose); + cs_di* block_hessian = + cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian); + cxsparse.Free(block_jacobian); + cxsparse.Free(block_jacobian_transpose); + + cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, ordering); + cxsparse.Free(block_hessian); +#endif // CERES_NO_CXSPARSE +} + +} // namespace + +bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + Program* program, + string* error) { + const int num_parameter_blocks = program->NumParameterBlocks(); + if (ordering.NumElements() != num_parameter_blocks) { + *error = StringPrintf("User specified ordering does not have the same " + "number of parameters as the problem. The problem" + "has %d blocks while the ordering has %d blocks.", + num_parameter_blocks, + ordering.NumElements()); + return false; + } + + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + parameter_blocks->clear(); + + const map<int, set<double*> >& groups = + ordering.group_to_elements(); + + for (map<int, set<double*> >::const_iterator group_it = groups.begin(); + group_it != groups.end(); + ++group_it) { + const set<double*>& group = group_it->second; + for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); + parameter_block_ptr_it != group.end(); + ++parameter_block_ptr_it) { + ProblemImpl::ParameterMap::const_iterator parameter_block_it = + parameter_map.find(*parameter_block_ptr_it); + if (parameter_block_it == parameter_map.end()) { + *error = StringPrintf("User specified ordering contains a pointer " + "to a double that is not a parameter block in " + "the problem. The invalid double is in group: %d", + group_it->first); + return false; + } + parameter_blocks->push_back(parameter_block_it->second); + } + } + return true; +} + +bool LexicographicallyOrderResidualBlocks(const int num_eliminate_blocks, + Program* program, + string* error) { + CHECK_GE(num_eliminate_blocks, 1) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Create a histogram of the number of residuals for each E block. There is an + // extra bucket at the end to catch all non-eliminated F blocks. + vector<int> residual_blocks_per_e_block(num_eliminate_blocks + 1); + vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); + vector<int> min_position_per_residual(residual_blocks->size()); + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int position = MinParameterBlock(residual_block, num_eliminate_blocks); + min_position_per_residual[i] = position; + DCHECK_LE(position, num_eliminate_blocks); + residual_blocks_per_e_block[position]++; + } + + // Run a cumulative sum on the histogram, to obtain offsets to the start of + // each histogram bucket (where each bucket is for the residuals for that + // E-block). + vector<int> offsets(num_eliminate_blocks + 1); + std::partial_sum(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end(), + offsets.begin()); + CHECK_EQ(offsets.back(), residual_blocks->size()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + CHECK(find(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end() - 1, 0) != + residual_blocks_per_e_block.end()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Fill in each bucket with the residual blocks for its corresponding E block. + // Each bucket is individually filled from the back of the bucket to the front + // of the bucket. The filling order among the buckets is dictated by the + // residual blocks. This loop uses the offsets as counters; subtracting one + // from each offset as a residual block is placed in the bucket. When the + // filling is finished, the offset pointerts should have shifted down one + // entry (this is verified below). + vector<ResidualBlock*> reordered_residual_blocks( + (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + for (int i = 0; i < residual_blocks->size(); ++i) { + int bucket = min_position_per_residual[i]; + + // Decrement the cursor, which should now point at the next empty position. + offsets[bucket]--; + + // Sanity. + CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; + } + + // Sanity check #1: The difference in bucket offsets should match the + // histogram sizes. + for (int i = 0; i < num_eliminate_blocks; ++i) { + CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + // Sanity check #2: No NULL's left behind. + for (int i = 0; i < reordered_residual_blocks.size(); ++i) { + CHECK(reordered_residual_blocks[i] != NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + + // Now that the residuals are collected by E block, swap them in place. + swap(*program->mutable_residual_blocks(), reordered_residual_blocks); + return true; +} + +void MaybeReorderSchurComplementColumnsUsingSuiteSparse( + const ParameterBlockOrdering& parameter_block_ordering, + Program* program) { + // Pre-order the columns corresponding to the schur complement if + // possible. +#ifndef CERES_NO_SUITESPARSE + SuiteSparse ss; + if (!SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) { + return; + } + + vector<int> constraints; + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); + + for (int i = 0; i < parameter_blocks.size(); ++i) { + constraints.push_back( + parameter_block_ordering.GroupId( + parameter_blocks[i]->mutable_user_state())); + } + + // Renumber the entries of constraints to be contiguous integers + // as camd requires that the group ids be in the range [0, + // parameter_blocks.size() - 1]. + MapValuesToContiguousRange(constraints.size(), &constraints[0]); + + // Set the offsets and index for CreateJacobianSparsityTranspose. + program->SetParameterOffsetsAndIndex(); + // Compute a block sparse presentation of J'. + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + + cholmod_sparse* block_jacobian_transpose = + ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); + + vector<int> ordering(parameter_blocks.size(), 0); + ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, + &constraints[0], + &ordering[0]); + ss.Free(block_jacobian_transpose); + + const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); + for (int i = 0; i < program->NumParameterBlocks(); ++i) { + parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; + } +#endif +} + +bool ReorderProgramForSchurTypeLinearSolver( + const LinearSolverType linear_solver_type, + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ProblemImpl::ParameterMap& parameter_map, + ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error) { + if (parameter_block_ordering->NumGroups() == 1) { + // If the user supplied an parameter_block_ordering with just one + // group, it is equivalent to the user supplying NULL as an + // parameter_block_ordering. Ceres is completely free to choose the + // parameter block ordering as it sees fit. For Schur type solvers, + // this means that the user wishes for Ceres to identify the + // e_blocks, which we do by computing a maximal independent set. + vector<ParameterBlock*> schur_ordering; + const int num_eliminate_blocks = + ComputeStableSchurOrdering(*program, &schur_ordering); + + CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Update the parameter_block_ordering object. + for (int i = 0; i < schur_ordering.size(); ++i) { + double* parameter_block = schur_ordering[i]->mutable_user_state(); + const int group_id = (i < num_eliminate_blocks) ? 0 : 1; + parameter_block_ordering->AddElementToGroup(parameter_block, group_id); + } + + // We could call ApplyOrdering but this is cheaper and + // simpler. + swap(*program->mutable_parameter_blocks(), schur_ordering); + } else { + // The user provided an ordering with more than one elimination + // group. Trust the user and apply the ordering. + if (!ApplyOrdering(parameter_map, + *parameter_block_ordering, + program, + error)) { + return false; + } + } + + if (linear_solver_type == SPARSE_SCHUR && + sparse_linear_algebra_library_type == SUITE_SPARSE) { + MaybeReorderSchurComplementColumnsUsingSuiteSparse( + *parameter_block_ordering, + program); + } + + program->SetParameterOffsetsAndIndex(); + // Schur type solvers also require that their residual blocks be + // lexicographically ordered. + const int num_eliminate_blocks = + parameter_block_ordering->group_to_elements().begin()->second.size(); + if (!LexicographicallyOrderResidualBlocks(num_eliminate_blocks, + program, + error)) { + return false; + } + + program->SetParameterOffsetsAndIndex(); + return true; +} + +bool ReorderProgramForSparseNormalCholesky( + const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering& parameter_block_ordering, + Program* program, + string* error) { + + if (sparse_linear_algebra_library_type != SUITE_SPARSE && + sparse_linear_algebra_library_type != CX_SPARSE) { + *error = "Unknown sparse linear algebra library."; + return false; + } + + // Set the offsets and index for CreateJacobianSparsityTranspose. + program->SetParameterOffsetsAndIndex(); + // Compute a block sparse presentation of J'. + scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( + program->CreateJacobianBlockSparsityTranspose()); + + vector<int> ordering(program->NumParameterBlocks(), 0); + vector<ParameterBlock*>& parameter_blocks = + *(program->mutable_parameter_blocks()); + + if (sparse_linear_algebra_library_type == SUITE_SPARSE) { + OrderingForSparseNormalCholeskyUsingSuiteSparse( + *tsm_block_jacobian_transpose, + parameter_blocks, + parameter_block_ordering, + &ordering[0]); + } else if (sparse_linear_algebra_library_type == CX_SPARSE){ + OrderingForSparseNormalCholeskyUsingCXSparse( + *tsm_block_jacobian_transpose, + &ordering[0]); + } + + // Apply ordering. + const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); + for (int i = 0; i < program->NumParameterBlocks(); ++i) { + parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; + } + + program->SetParameterOffsetsAndIndex(); + return true; +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/reorder_program.h b/internal/ceres/reorder_program.h new file mode 100644 index 0000000..d3962f9 --- /dev/null +++ b/internal/ceres/reorder_program.h
@@ -0,0 +1,101 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_REORDER_PROGRAM_H_ +#define CERES_INTERNAL_REORDER_PROGRAM_H_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem_impl.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; + +// Reorder the parameter blocks in program using the ordering +bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + Program* program, + string* error); + +// Reorder the residuals for program, if necessary, so that the residuals +// involving each E block occur together. This is a necessary condition for the +// Schur eliminator, which works on these "row blocks" in the jacobian. +bool LexicographicallyOrderResidualBlocks(int num_eliminate_blocks, + Program* program, + string* error); + +// Schur type solvers require that all parameter blocks eliminated +// by the Schur eliminator occur before others and the residuals be +// sorted in lexicographic order of their parameter blocks. +// +// If the parameter_block_ordering only contains one elimination +// group then a maximal independent set is computed and used as the +// first elimination group, otherwise the user's ordering is used. +// +// If the linear solver type is SPARSE_SCHUR and support for +// constrained fill-reducing ordering is available in the sparse +// linear algebra library (SuiteSparse version >= 4.2.0) then +// columns of the schur complement matrix are ordered to reduce the +// fill-in the Cholesky factorization. +// +// Upon return, ordering contains the parameter block ordering that +// was used to order the program. +bool ReorderProgramForSchurTypeLinearSolver( + LinearSolverType linear_solver_type, + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ProblemImpl::ParameterMap& parameter_map, + ParameterBlockOrdering* parameter_block_ordering, + Program* program, + string* error); + +// Sparse cholesky factorization routines when doing the sparse +// cholesky factorization of the Jacobian matrix, reorders its +// columns to reduce the fill-in. Compute this permutation and +// re-order the parameter blocks. +// +// When using SuiteSparse, if the parameter_block_ordering contains +// more than one elimination group and support for constrained +// fill-reducing ordering is available in the sparse linear algebra +// library (SuiteSparse version >= 4.2.0) then the fill reducing +// ordering will take it into account, otherwise it will be ignored. +bool ReorderProgramForSparseNormalCholesky( + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, + const ParameterBlockOrdering& parameter_block_ordering, + Program* program, + string* error); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_REORDER_PROGRAM_
diff --git a/internal/ceres/reorder_program_test.cc b/internal/ceres/reorder_program_test.cc new file mode 100644 index 0000000..2a0c4eb --- /dev/null +++ b/internal/ceres/reorder_program_test.cc
@@ -0,0 +1,170 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/reorder_program.h" + +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/sized_cost_function.h" +#include "ceres/solver.h" + +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +// Templated base class for the CostFunction signatures. +template <int kNumResiduals, int N0, int N1, int N2> +class MockCostFunctionBase : public +SizedCostFunction<kNumResiduals, N0, N1, N2> { + public: + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + // Do nothing. This is never called. + return true; + } +}; + +class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {}; +class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {}; +class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {}; + +TEST(_, ReorderResidualBlockNormalFunction) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); + + ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering; + linear_solver_ordering->AddElementToGroup(&x, 0); + linear_solver_ordering->AddElementToGroup(&y, 0); + linear_solver_ordering->AddElementToGroup(&z, 1); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering.reset(linear_solver_ordering); + + const vector<ResidualBlock*>& residual_blocks = + problem.program().residual_blocks(); + + vector<ResidualBlock*> expected_residual_blocks; + + // This is a bit fragile, but it serves the purpose. We know the + // bucketing algorithm that the reordering function uses, so we + // expect the order for residual blocks for each e_block to be + // filled in reverse. + expected_residual_blocks.push_back(residual_blocks[4]); + expected_residual_blocks.push_back(residual_blocks[1]); + expected_residual_blocks.push_back(residual_blocks[0]); + expected_residual_blocks.push_back(residual_blocks[5]); + expected_residual_blocks.push_back(residual_blocks[2]); + expected_residual_blocks.push_back(residual_blocks[3]); + + Program* program = problem.mutable_program(); + program->SetParameterOffsetsAndIndex(); + + string message; + EXPECT_TRUE(LexicographicallyOrderResidualBlocks( + 2, + problem.mutable_program(), + &message)); + EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size()); + for (int i = 0; i < expected_residual_blocks.size(); ++i) { + EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]); + } +} + +TEST(_, ApplyOrderingOrderingTooSmall) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering linear_solver_ordering; + linear_solver_ordering.AddElementToGroup(&x, 0); + linear_solver_ordering.AddElementToGroup(&y, 1); + + Program program(problem.program()); + string message; + EXPECT_FALSE(ApplyOrdering(problem.parameter_map(), + linear_solver_ordering, + &program, + &message)); +} + +TEST(_, ApplyOrderingNormal) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering linear_solver_ordering; + linear_solver_ordering.AddElementToGroup(&x, 0); + linear_solver_ordering.AddElementToGroup(&y, 2); + linear_solver_ordering.AddElementToGroup(&z, 1); + + Program* program = problem.mutable_program(); + string message; + + EXPECT_TRUE(ApplyOrdering(problem.parameter_map(), + linear_solver_ordering, + program, + &message)); + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + + EXPECT_EQ(parameter_blocks.size(), 3); + EXPECT_EQ(parameter_blocks[0]->user_state(), &x); + EXPECT_EQ(parameter_blocks[1]->user_state(), &z); + EXPECT_EQ(parameter_blocks[2]->user_state(), &y); +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index dfdfecb..a690eed 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc
@@ -53,6 +53,7 @@ #include "ceres/problem.h" #include "ceres/problem_impl.h" #include "ceres/program.h" +#include "ceres/reorder_program.h" #include "ceres/residual_block.h" #include "ceres/stringprintf.h" #include "ceres/suitesparse.h" @@ -743,7 +744,7 @@ !options->dynamic_sparsity) { if (!ReorderProgramForSparseNormalCholesky( options->sparse_linear_algebra_library_type, - linear_solver_ordering, + *linear_solver_ordering, reduced_program.get(), error)) { return NULL; @@ -892,109 +893,6 @@ return LinearSolver::Create(linear_solver_options); } - -// Find the minimum index of any parameter block to the given residual. -// Parameter blocks that have indices greater than num_eliminate_blocks are -// considered to have an index equal to num_eliminate_blocks. -static int MinParameterBlock(const ResidualBlock* residual_block, - int num_eliminate_blocks) { - int min_parameter_block_position = num_eliminate_blocks; - for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { - ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; - if (!parameter_block->IsConstant()) { - CHECK_NE(parameter_block->index(), -1) - << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " - << "This is a Ceres bug; please contact the developers!"; - min_parameter_block_position = std::min(parameter_block->index(), - min_parameter_block_position); - } - } - return min_parameter_block_position; -} - -// Reorder the residuals for program, if necessary, so that the residuals -// involving each E block occur together. This is a necessary condition for the -// Schur eliminator, which works on these "row blocks" in the jacobian. -bool SolverImpl::LexicographicallyOrderResidualBlocks( - const int num_eliminate_blocks, - Program* program, - string* error) { - CHECK_GE(num_eliminate_blocks, 1) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - // Create a histogram of the number of residuals for each E block. There is an - // extra bucket at the end to catch all non-eliminated F blocks. - vector<int> residual_blocks_per_e_block(num_eliminate_blocks + 1); - vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); - vector<int> min_position_per_residual(residual_blocks->size()); - for (int i = 0; i < residual_blocks->size(); ++i) { - ResidualBlock* residual_block = (*residual_blocks)[i]; - int position = MinParameterBlock(residual_block, num_eliminate_blocks); - min_position_per_residual[i] = position; - DCHECK_LE(position, num_eliminate_blocks); - residual_blocks_per_e_block[position]++; - } - - // Run a cumulative sum on the histogram, to obtain offsets to the start of - // each histogram bucket (where each bucket is for the residuals for that - // E-block). - vector<int> offsets(num_eliminate_blocks + 1); - std::partial_sum(residual_blocks_per_e_block.begin(), - residual_blocks_per_e_block.end(), - offsets.begin()); - CHECK_EQ(offsets.back(), residual_blocks->size()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - CHECK(find(residual_blocks_per_e_block.begin(), - residual_blocks_per_e_block.end() - 1, 0) != - residual_blocks_per_e_block.end()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - // Fill in each bucket with the residual blocks for its corresponding E block. - // Each bucket is individually filled from the back of the bucket to the front - // of the bucket. The filling order among the buckets is dictated by the - // residual blocks. This loop uses the offsets as counters; subtracting one - // from each offset as a residual block is placed in the bucket. When the - // filling is finished, the offset pointerts should have shifted down one - // entry (this is verified below). - vector<ResidualBlock*> reordered_residual_blocks( - (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); - for (int i = 0; i < residual_blocks->size(); ++i) { - int bucket = min_position_per_residual[i]; - - // Decrement the cursor, which should now point at the next empty position. - offsets[bucket]--; - - // Sanity. - CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; - } - - // Sanity check #1: The difference in bucket offsets should match the - // histogram sizes. - for (int i = 0; i < num_eliminate_blocks; ++i) { - CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - } - // Sanity check #2: No NULL's left behind. - for (int i = 0; i < reordered_residual_blocks.size(); ++i) { - CHECK(reordered_residual_blocks[i] != NULL) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - } - - // Now that the residuals are collected by E block, swap them in place. - swap(*program->mutable_residual_blocks(), reordered_residual_blocks); - return true; -} - Evaluator* SolverImpl::CreateEvaluator( const Solver::Options& options, const ProblemImpl::ParameterMap& parameter_map, @@ -1053,235 +951,5 @@ return inner_iteration_minimizer.release(); } -bool SolverImpl::ApplyUserOrdering( - const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - const int num_parameter_blocks = program->NumParameterBlocks(); - if (parameter_block_ordering->NumElements() != num_parameter_blocks) { - *error = StringPrintf("User specified ordering does not have the same " - "number of parameters as the problem. The problem" - "has %d blocks while the ordering has %d blocks.", - num_parameter_blocks, - parameter_block_ordering->NumElements()); - return false; - } - - vector<ParameterBlock*>* parameter_blocks = - program->mutable_parameter_blocks(); - parameter_blocks->clear(); - - const map<int, set<double*> >& groups = - parameter_block_ordering->group_to_elements(); - - for (map<int, set<double*> >::const_iterator group_it = groups.begin(); - group_it != groups.end(); - ++group_it) { - const set<double*>& group = group_it->second; - for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); - parameter_block_ptr_it != group.end(); - ++parameter_block_ptr_it) { - ProblemImpl::ParameterMap::const_iterator parameter_block_it = - parameter_map.find(*parameter_block_ptr_it); - if (parameter_block_it == parameter_map.end()) { - *error = StringPrintf("User specified ordering contains a pointer " - "to a double that is not a parameter block in " - "the problem. The invalid double is in group: %d", - group_it->first); - return false; - } - parameter_blocks->push_back(parameter_block_it->second); - } - } - return true; -} - -bool SolverImpl::ReorderProgramForSchurTypeLinearSolver( - const LinearSolverType linear_solver_type, - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - if (parameter_block_ordering->NumGroups() == 1) { - // If the user supplied an parameter_block_ordering with just one - // group, it is equivalent to the user supplying NULL as an - // parameter_block_ordering. Ceres is completely free to choose the - // parameter block ordering as it sees fit. For Schur type solvers, - // this means that the user wishes for Ceres to identify the - // e_blocks, which we do by computing a maximal independent set. - vector<ParameterBlock*> schur_ordering; - const int num_eliminate_blocks = - ComputeStableSchurOrdering(*program, &schur_ordering); - - CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - // Update the parameter_block_ordering object. - for (int i = 0; i < schur_ordering.size(); ++i) { - double* parameter_block = schur_ordering[i]->mutable_user_state(); - const int group_id = (i < num_eliminate_blocks) ? 0 : 1; - parameter_block_ordering->AddElementToGroup(parameter_block, group_id); - } - - // We could call ApplyUserOrdering but this is cheaper and - // simpler. - swap(*program->mutable_parameter_blocks(), schur_ordering); - } else { - // The user provided an ordering with more than one elimination - // group. Trust the user and apply the ordering. - if (!ApplyUserOrdering(parameter_map, - parameter_block_ordering, - program, - error)) { - return false; - } - } - - // Pre-order the columns corresponding to the schur complement if - // possible. -#if !defined(CERES_NO_SUITESPARSE) && !defined(CERES_NO_CAMD) - if (linear_solver_type == SPARSE_SCHUR && - sparse_linear_algebra_library_type == SUITE_SPARSE) { - vector<int> constraints; - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); - - for (int i = 0; i < parameter_blocks.size(); ++i) { - constraints.push_back( - parameter_block_ordering->GroupId( - parameter_blocks[i]->mutable_user_state())); - } - - // Renumber the entries of constraints to be contiguous integers - // as camd requires that the group ids be in the range [0, - // parameter_blocks.size() - 1]. - MapValuesToContiguousRange(constraints.size(), &constraints[0]); - - // Set the offsets and index for CreateJacobianSparsityTranspose. - program->SetParameterOffsetsAndIndex(); - // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( - program->CreateJacobianBlockSparsityTranspose()); - - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - - vector<int> ordering(parameter_blocks.size(), 0); - ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, - &constraints[0], - &ordering[0]); - ss.Free(block_jacobian_transpose); - - const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); - for (int i = 0; i < program->NumParameterBlocks(); ++i) { - parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; - } - } -#endif - - program->SetParameterOffsetsAndIndex(); - // Schur type solvers also require that their residual blocks be - // lexicographically ordered. - const int num_eliminate_blocks = - parameter_block_ordering->group_to_elements().begin()->second.size(); - return LexicographicallyOrderResidualBlocks(num_eliminate_blocks, - program, - error); -} - -bool SolverImpl::ReorderProgramForSparseNormalCholesky( - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - // Set the offsets and index for CreateJacobianSparsityTranspose. - program->SetParameterOffsetsAndIndex(); - // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( - program->CreateJacobianBlockSparsityTranspose()); - - vector<int> ordering(program->NumParameterBlocks(), 0); - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); - - if (sparse_linear_algebra_library_type == SUITE_SPARSE) { -#ifdef CERES_NO_SUITESPARSE - *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITE_SPARSE because " - "SuiteSparse was not enabled when Ceres was built."; - return false; -#else - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - -# ifdef CERES_NO_CAMD - // No cholmod_camd, so ignore user's parameter_block_ordering and - // use plain old AMD. - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); -# else - if (parameter_block_ordering->NumGroups() > 1) { - // If the user specified more than one elimination groups use them - // to constrain the ordering. - vector<int> constraints; - for (int i = 0; i < parameter_blocks.size(); ++i) { - constraints.push_back( - parameter_block_ordering->GroupId( - parameter_blocks[i]->mutable_user_state())); - } - ss.ConstrainedApproximateMinimumDegreeOrdering( - block_jacobian_transpose, - &constraints[0], - &ordering[0]); - } else { - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, - &ordering[0]); - } -# endif // CERES_NO_CAMD - - ss.Free(block_jacobian_transpose); -#endif // CERES_NO_SUITESPARSE - - } else if (sparse_linear_algebra_library_type == CX_SPARSE) { -#ifndef CERES_NO_CXSPARSE - - // CXSparse works with J'J instead of J'. So compute the block - // sparsity for J'J and compute an approximate minimum degree - // ordering. - CXSparse cxsparse; - cs_di* block_jacobian_transpose; - block_jacobian_transpose = - cxsparse.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose); - cs_di* block_hessian = - cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian); - cxsparse.Free(block_jacobian); - cxsparse.Free(block_jacobian_transpose); - - cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, &ordering[0]); - cxsparse.Free(block_hessian); -#else // CERES_NO_CXSPARSE - *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because " - "CXSparse was not enabled when Ceres was built."; - return false; -#endif // CERES_NO_CXSPARSE - } else { - *error = "Unknown sparse linear algebra library."; - return false; - } - - // Apply ordering. - const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); - for (int i = 0; i < program->NumParameterBlocks(); ++i) { - parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; - } - - program->SetParameterOffsetsAndIndex(); - return true; -} - } // namespace internal } // namespace ceres
diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h index 53b87a7..c42c32a 100644 --- a/internal/ceres/solver_impl.h +++ b/internal/ceres/solver_impl.h
@@ -99,15 +99,6 @@ static LinearSolver* CreateLinearSolver(Solver::Options* options, string* message); - // Reorder the residuals for program, if necessary, so that the - // residuals involving e block (i.e., the first num_eliminate_block - // parameter blocks) occur together. This is a necessary condition - // for the Schur eliminator. - static bool LexicographicallyOrderResidualBlocks( - const int num_eliminate_blocks, - Program* program, - string* message); - // Create the appropriate evaluator for the transformed program. static Evaluator* CreateEvaluator( const Solver::Options& options, @@ -115,26 +106,6 @@ Program* program, string* message); - // Remove the fixed or unused parameter blocks and residuals - // depending only on fixed parameters from the program. - // - // If either linear_solver_ordering or inner_iteration_ordering are - // not NULL, the constant parameter blocks are removed from them - // too. - // - // If fixed_cost is not NULL, the residual blocks that are removed - // are evaluated and the sum of their cost is returned in - // fixed_cost. - // - // If a failure is encountered, the function returns false with a - // description of the failure in message. - static bool RemoveFixedBlocksFromProgram( - Program* program, - ParameterBlockOrdering* linear_solver_ordering, - ParameterBlockOrdering* inner_iteration_ordering, - double* fixed_cost, - string* message); - static bool IsOrderingValid(const Solver::Options& options, const ProblemImpl* problem_impl, string* message); @@ -148,53 +119,6 @@ const Program& program, const ProblemImpl::ParameterMap& parameter_map, Solver::Summary* summary); - - // Reorder the parameter blocks in program using the ordering - static bool ApplyUserOrdering( - const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* message); - - // Sparse cholesky factorization routines when doing the sparse - // cholesky factorization of the Jacobian matrix, reorders its - // columns to reduce the fill-in. Compute this permutation and - // re-order the parameter blocks. - // - // If the parameter_block_ordering contains more than one - // elimination group and support for constrained fill-reducing - // ordering is available in the sparse linear algebra library - // (SuiteSparse version >= 4.2.0) then the fill reducing - // ordering will take it into account, otherwise it will be ignored. - static bool ReorderProgramForSparseNormalCholesky( - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* message); - - // Schur type solvers require that all parameter blocks eliminated - // by the Schur eliminator occur before others and the residuals be - // sorted in lexicographic order of their parameter blocks. - // - // If the parameter_block_ordering only contains one elimination - // group then a maximal independent set is computed and used as the - // first elimination group, otherwise the user's ordering is used. - // - // If the linear solver type is SPARSE_SCHUR and support for - // constrained fill-reducing ordering is available in the sparse - // linear algebra library (SuiteSparse version >= 4.2.0) then - // columns of the schur complement matrix are ordered to reduce the - // fill-in the Cholesky factorization. - // - // Upon return, ordering contains the parameter block ordering that - // was used to order the program. - static bool ReorderProgramForSchurTypeLinearSolver( - const LinearSolverType linear_solver_type, - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* message); }; } // namespace internal
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc index 51e8cbf..2d517c6 100644 --- a/internal/ceres/solver_impl_test.cc +++ b/internal/ceres/solver_impl_test.cc
@@ -42,281 +42,6 @@ namespace ceres { namespace internal { -// A cost function that sipmply returns its argument. -class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> { - public: - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - residuals[0] = parameters[0][0]; - if (jacobians != NULL && jacobians[0] != NULL) { - jacobians[0][0] = 1.0; - } - return true; - } -}; - -// Templated base class for the CostFunction signatures. -template <int kNumResiduals, int N0, int N1, int N2> -class MockCostFunctionBase : public -SizedCostFunction<kNumResiduals, N0, N1, N2> { - public: - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - for (int i = 0; i < kNumResiduals; ++i) { - residuals[i] = 0.0; - } - return true; - } -}; - -class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {}; -class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {}; -class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {}; - -TEST(SolverImpl, ReorderResidualBlockNormalFunction) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); - - ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering; - linear_solver_ordering->AddElementToGroup(&x, 0); - linear_solver_ordering->AddElementToGroup(&y, 0); - linear_solver_ordering->AddElementToGroup(&z, 1); - - Solver::Options options; - options.linear_solver_type = DENSE_SCHUR; - options.linear_solver_ordering.reset(linear_solver_ordering); - - const vector<ResidualBlock*>& residual_blocks = - problem.program().residual_blocks(); - - vector<ResidualBlock*> expected_residual_blocks; - - // This is a bit fragile, but it serves the purpose. We know the - // bucketing algorithm that the reordering function uses, so we - // expect the order for residual blocks for each e_block to be - // filled in reverse. - expected_residual_blocks.push_back(residual_blocks[4]); - expected_residual_blocks.push_back(residual_blocks[1]); - expected_residual_blocks.push_back(residual_blocks[0]); - expected_residual_blocks.push_back(residual_blocks[5]); - expected_residual_blocks.push_back(residual_blocks[2]); - expected_residual_blocks.push_back(residual_blocks[3]); - - Program* program = problem.mutable_program(); - program->SetParameterOffsetsAndIndex(); - - string message; - EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks( - 2, - problem.mutable_program(), - &message)); - EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size()); - for (int i = 0; i < expected_residual_blocks.size(); ++i) { - EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]); - } -} - -TEST(SolverImpl, ReorderResidualBlockNormalFunctionWithFixedBlocks) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - - // Set one parameter block constant. - problem.SetParameterBlockConstant(&z); - - // Mark residuals for x's row block with "x" for readability. - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); // 0 x - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); // 1 x - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 2 - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 3 - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 4 x - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 5 - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 6 x - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); // 7 - - ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering; - linear_solver_ordering->AddElementToGroup(&x, 0); - linear_solver_ordering->AddElementToGroup(&z, 0); - linear_solver_ordering->AddElementToGroup(&y, 1); - - Solver::Options options; - options.linear_solver_type = DENSE_SCHUR; - options.linear_solver_ordering.reset(linear_solver_ordering); - - // Create the reduced program. This should remove the fixed block "z", - // marking the index to -1 at the same time. x and y also get indices. - string message; - double fixed_cost; - scoped_ptr<Program> reduced_program( - SolverImpl::CreateReducedProgram(&options, - &problem, - &fixed_cost, - &message)); - - const vector<ResidualBlock*>& residual_blocks = - problem.program().residual_blocks(); - - // This is a bit fragile, but it serves the purpose. We know the - // bucketing algorithm that the reordering function uses, so we - // expect the order for residual blocks for each e_block to be - // filled in reverse. - - vector<ResidualBlock*> expected_residual_blocks; - - // Row block for residuals involving "x". These are marked "x" in the block - // of code calling AddResidual() above. - expected_residual_blocks.push_back(residual_blocks[6]); - expected_residual_blocks.push_back(residual_blocks[4]); - expected_residual_blocks.push_back(residual_blocks[1]); - expected_residual_blocks.push_back(residual_blocks[0]); - - // Row block for residuals involving "y". - expected_residual_blocks.push_back(residual_blocks[7]); - expected_residual_blocks.push_back(residual_blocks[5]); - expected_residual_blocks.push_back(residual_blocks[3]); - expected_residual_blocks.push_back(residual_blocks[2]); - - EXPECT_EQ(reduced_program->residual_blocks().size(), - expected_residual_blocks.size()); - for (int i = 0; i < expected_residual_blocks.size(); ++i) { - EXPECT_EQ(reduced_program->residual_blocks()[i], - expected_residual_blocks[i]); - } -} - -TEST(SolverImpl, AutomaticSchurReorderingRespectsConstantBlocks) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - - // Set one parameter block constant. - problem.SetParameterBlockConstant(&z); - - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); - problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); - problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); - - ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering; - linear_solver_ordering->AddElementToGroup(&x, 0); - linear_solver_ordering->AddElementToGroup(&z, 0); - linear_solver_ordering->AddElementToGroup(&y, 0); - - Solver::Options options; - options.linear_solver_type = DENSE_SCHUR; - options.linear_solver_ordering.reset(linear_solver_ordering); - - string message; - double fixed_cost; - scoped_ptr<Program> reduced_program( - SolverImpl::CreateReducedProgram(&options, - &problem, - &fixed_cost, - &message)); - - const vector<ResidualBlock*>& residual_blocks = - reduced_program->residual_blocks(); - const vector<ParameterBlock*>& parameter_blocks = - reduced_program->parameter_blocks(); - - const vector<ResidualBlock*>& original_residual_blocks = - problem.program().residual_blocks(); - - EXPECT_EQ(residual_blocks.size(), 8); - EXPECT_EQ(reduced_program->parameter_blocks().size(), 2); - - // Verify that right parmeter block and the residual blocks have - // been removed. - for (int i = 0; i < 8; ++i) { - EXPECT_NE(residual_blocks[i], original_residual_blocks.back()); - } - for (int i = 0; i < 2; ++i) { - EXPECT_NE(parameter_blocks[i]->mutable_user_state(), &z); - } -} - -TEST(SolverImpl, ApplyUserOrderingOrderingTooSmall) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - linear_solver_ordering.AddElementToGroup(&y, 1); - - Program program(problem.program()); - string message; - EXPECT_FALSE(SolverImpl::ApplyUserOrdering(problem.parameter_map(), - &linear_solver_ordering, - &program, - &message)); -} - -TEST(SolverImpl, ApplyUserOrderingNormal) { - ProblemImpl problem; - double x; - double y; - double z; - - problem.AddParameterBlock(&x, 1); - problem.AddParameterBlock(&y, 1); - problem.AddParameterBlock(&z, 1); - - ParameterBlockOrdering linear_solver_ordering; - linear_solver_ordering.AddElementToGroup(&x, 0); - linear_solver_ordering.AddElementToGroup(&y, 2); - linear_solver_ordering.AddElementToGroup(&z, 1); - - Program* program = problem.mutable_program(); - string message; - - EXPECT_TRUE(SolverImpl::ApplyUserOrdering(problem.parameter_map(), - &linear_solver_ordering, - program, - &message)); - const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); - - EXPECT_EQ(parameter_blocks.size(), 3); - EXPECT_EQ(parameter_blocks[0]->user_state(), &x); - EXPECT_EQ(parameter_blocks[1]->user_state(), &z); - EXPECT_EQ(parameter_blocks[2]->user_state(), &y); -} - // The parameters must be in separate blocks so that they can be individually // set constant or not. struct Quadratic4DCostFunction {