internal/ceres/reorder_program.cc - ceres-solver - Git at Google

 // Ceres Solver - A fast non-linear least squares minimizer
 // Copyright 2014 Google Inc. All rights reserved.
 // http://code.google.com/p/ceres-solver/
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
 //
 // * Redistributions of source code must retain the above copyright notice,
 //   this list of conditions and the following disclaimer.
 // * Redistributions in binary form must reproduce the above copyright notice,
 //   this list of conditions and the following disclaimer in the documentation
 //   and/or other materials provided with the distribution.
 // * Neither the name of Google Inc. nor the names of its contributors may be
 //   used to endorse or promote products derived from this software without
 //   specific prior written permission.
 //
 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
 // Author: sameeragarwal@google.com (Sameer Agarwal)

 #include "ceres/reorder_program.h"

 #include <algorithm>
 #include <numeric>
 #include <vector>

 #include "ceres/cxsparse.h"
 #include "ceres/internal/port.h"
 #include "ceres/ordered_groups.h"
 #include "ceres/parameter_block.h"
 #include "ceres/parameter_block_ordering.h"
 #include "ceres/problem_impl.h"
 #include "ceres/program.h"
 #include "ceres/residual_block.h"
 #include "ceres/solver.h"
 #include "ceres/suitesparse.h"
 #include "ceres/triplet_sparse_matrix.h"
 #include "ceres/types.h"
 #include "Eigen/SparseCore"

 #ifdef CERES_USE_EIGEN_SPARSE
 #include "Eigen/OrderingMethods"
 #endif

 #include "glog/logging.h"

 namespace ceres {
 namespace internal {

 using std::map;
 using std::set;
 using std::string;
 using std::vector;

 namespace {

 // Find the minimum index of any parameter block to the given
 // residual.  Parameter blocks that have indices greater than
 // size_of_first_elimination_group are considered to have an index
 // equal to size_of_first_elimination_group.
 static int MinParameterBlock(const ResidualBlock* residual_block,
                              int size_of_first_elimination_group) {
   int min_parameter_block_position = size_of_first_elimination_group;
   for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) {
     ParameterBlock* parameter_block = residual_block->parameter_blocks()[i];
     if (!parameter_block->IsConstant()) {
       CHECK_NE(parameter_block->index(), -1)
           << "Did you forget to call Program::SetParameterOffsetsAndIndex()? "
           << "This is a Ceres bug; please contact the developers!";
       min_parameter_block_position = std::min(parameter_block->index(),
                                               min_parameter_block_position);
     }
   }
   return min_parameter_block_position;
 }

 #if EIGEN_VERSION_AT_LEAST(3, 2, 2) && defined(CERES_USE_EIGEN_SPARSE)
 Eigen::SparseMatrix<int> CreateBlockJacobian(
     const TripletSparseMatrix& block_jacobian_transpose) {
   typedef Eigen::SparseMatrix<int> SparseMatrix;
   typedef Eigen::Triplet<int> Triplet;

   const int* rows = block_jacobian_transpose.rows();
   const int* cols = block_jacobian_transpose.cols();
   int num_nonzeros = block_jacobian_transpose.num_nonzeros();
   vector<Triplet> triplets;
   triplets.reserve(num_nonzeros);
   for (int i = 0; i < num_nonzeros; ++i) {
     triplets.push_back(Triplet(cols[i], rows[i], 1));
   }

   SparseMatrix block_jacobian(block_jacobian_transpose.num_cols(),
                               block_jacobian_transpose.num_rows());
   block_jacobian.setFromTriplets(triplets.begin(), triplets.end());
   return block_jacobian;
 }
 #endif

 void OrderingForSparseNormalCholeskyUsingSuiteSparse(
     const TripletSparseMatrix& tsm_block_jacobian_transpose,
     const vector<ParameterBlock*>& parameter_blocks,
     const ParameterBlockOrdering& parameter_block_ordering,
     int* ordering) {
 #ifdef CERES_NO_SUITESPARSE
   LOG(FATAL) << "Congratulations, you found a Ceres bug! "
              << "Please report this error to the developers.";
 #else
   SuiteSparse ss;
   cholmod_sparse* block_jacobian_transpose =
       ss.CreateSparseMatrix(
           const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose));

   // No CAMD or the user did not supply a useful ordering, then just
   // use regular AMD.
   if (parameter_block_ordering.NumGroups() <= 1 ||
       !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) {
     ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]);
   } else {
     vector<int> constraints;
     for (int i = 0; i < parameter_blocks.size(); ++i) {
       constraints.push_back(
           parameter_block_ordering.GroupId(
               parameter_blocks[i]->mutable_user_state()));
     }
     ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose,
                                                    &constraints[0],
                                                    ordering);
   }

   ss.Free(block_jacobian_transpose);
 #endif  // CERES_NO_SUITESPARSE
 }

 void OrderingForSparseNormalCholeskyUsingCXSparse(
     const TripletSparseMatrix& tsm_block_jacobian_transpose,
     int* ordering) {
 #ifdef CERES_NO_CXSPARSE
   LOG(FATAL) << "Congratulations, you found a Ceres bug! "
              << "Please report this error to the developers.";
 #else  // CERES_NO_CXSPARSE
   // CXSparse works with J'J instead of J'. So compute the block
   // sparsity for J'J and compute an approximate minimum degree
   // ordering.
   CXSparse cxsparse;
   cs_di* block_jacobian_transpose;
   block_jacobian_transpose =
       cxsparse.CreateSparseMatrix(
             const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose));
   cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose);
   cs_di* block_hessian =
       cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian);
   cxsparse.Free(block_jacobian);
   cxsparse.Free(block_jacobian_transpose);

   cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, ordering);
   cxsparse.Free(block_hessian);
 #endif  // CERES_NO_CXSPARSE
 }


 #if EIGEN_VERSION_AT_LEAST(3, 2, 2)
 void OrderingForSparseNormalCholeskyUsingEigenSparse(
     const TripletSparseMatrix& tsm_block_jacobian_transpose,
     int* ordering) {
 #ifndef CERES_USE_EIGEN_SPARSE
   LOG(FATAL) <<
       "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE "
       "because Ceres was not built with support for "
       "Eigen's SimplicialLDLT decomposition. "
       "This requires enabling building with -DEIGENSPARSE=ON.";
 #else

   // This conversion from a TripletSparseMatrix to a Eigen::Triplet
   // matrix is unfortunate, but unavoidable for now. It is not a
   // significant performance penalty in the grand scheme of
   // things. The right thing to do here would be to get a compressed
   // row sparse matrix representation of the jacobian and go from
   // there. But that is a project for another day.
   typedef Eigen::SparseMatrix<int> SparseMatrix;

   const SparseMatrix block_jacobian =
       CreateBlockJacobian(tsm_block_jacobian_transpose);
   const SparseMatrix block_hessian =
       block_jacobian.transpose() * block_jacobian;

   Eigen::AMDOrdering<int> amd_ordering;
   Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm;
   amd_ordering(block_hessian, perm);
   for (int i = 0; i < block_hessian.rows(); ++i) {
     ordering[i] = perm.indices()[i];
   }
 #endif  // CERES_USE_EIGEN_SPARSE
 }
 #endif

 }  // namespace

 bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map,
                    const ParameterBlockOrdering& ordering,
                    Program* program,
                    string* error) {
   const int num_parameter_blocks =  program->NumParameterBlocks();
   if (ordering.NumElements() != num_parameter_blocks) {
     *error = StringPrintf("User specified ordering does not have the same "
                           "number of parameters as the problem. The problem"
                           "has %d blocks while the ordering has %d blocks.",
                           num_parameter_blocks,
                           ordering.NumElements());
     return false;
   }

   vector<ParameterBlock*>* parameter_blocks =
       program->mutable_parameter_blocks();
   parameter_blocks->clear();

   const map<int, set<double*> >& groups = ordering.group_to_elements();
   for (map<int, set<double*> >::const_iterator group_it = groups.begin();
        group_it != groups.end();
        ++group_it) {
     const set<double*>& group = group_it->second;
     for (set<double*>::const_iterator parameter_block_ptr_it = group.begin();
          parameter_block_ptr_it != group.end();
          ++parameter_block_ptr_it) {
       ProblemImpl::ParameterMap::const_iterator parameter_block_it =
           parameter_map.find(*parameter_block_ptr_it);
       if (parameter_block_it == parameter_map.end()) {
         *error = StringPrintf("User specified ordering contains a pointer "
                               "to a double that is not a parameter block in "
                               "the problem. The invalid double is in group: %d",
                               group_it->first);
         return false;
       }
       parameter_blocks->push_back(parameter_block_it->second);
     }
   }
   return true;
 }

 bool LexicographicallyOrderResidualBlocks(
     const int size_of_first_elimination_group,
     Program* program,
     string* error) {
   CHECK_GE(size_of_first_elimination_group, 1)
       << "Congratulations, you found a Ceres bug! Please report this error "
       << "to the developers.";

   // Create a histogram of the number of residuals for each E block. There is an
   // extra bucket at the end to catch all non-eliminated F blocks.
   vector<int> residual_blocks_per_e_block(size_of_first_elimination_group + 1);
   vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks();
   vector<int> min_position_per_residual(residual_blocks->size());
   for (int i = 0; i < residual_blocks->size(); ++i) {
     ResidualBlock* residual_block = (*residual_blocks)[i];
     int position = MinParameterBlock(residual_block,
                                      size_of_first_elimination_group);
     min_position_per_residual[i] = position;
     DCHECK_LE(position, size_of_first_elimination_group);
     residual_blocks_per_e_block[position]++;
   }

   // Run a cumulative sum on the histogram, to obtain offsets to the start of
   // each histogram bucket (where each bucket is for the residuals for that
   // E-block).
   vector<int> offsets(size_of_first_elimination_group + 1);
   std::partial_sum(residual_blocks_per_e_block.begin(),
                    residual_blocks_per_e_block.end(),
                    offsets.begin());
   CHECK_EQ(offsets.back(), residual_blocks->size())
       << "Congratulations, you found a Ceres bug! Please report this error "
       << "to the developers.";

   CHECK(find(residual_blocks_per_e_block.begin(),
              residual_blocks_per_e_block.end() - 1, 0) !=
         residual_blocks_per_e_block.end())
       << "Congratulations, you found a Ceres bug! Please report this error "
       << "to the developers.";

   // Fill in each bucket with the residual blocks for its corresponding E block.
   // Each bucket is individually filled from the back of the bucket to the front
   // of the bucket. The filling order among the buckets is dictated by the
   // residual blocks. This loop uses the offsets as counters; subtracting one
   // from each offset as a residual block is placed in the bucket. When the
   // filling is finished, the offset pointerts should have shifted down one
   // entry (this is verified below).
   vector<ResidualBlock*> reordered_residual_blocks(
       (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL));
   for (int i = 0; i < residual_blocks->size(); ++i) {
     int bucket = min_position_per_residual[i];

     // Decrement the cursor, which should now point at the next empty position.
     offsets[bucket]--;

     // Sanity.
     CHECK(reordered_residual_blocks[offsets[bucket]] == NULL)
         << "Congratulations, you found a Ceres bug! Please report this error "
         << "to the developers.";

     reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i];
   }

   // Sanity check #1: The difference in bucket offsets should match the
   // histogram sizes.
   for (int i = 0; i < size_of_first_elimination_group; ++i) {
     CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i])
         << "Congratulations, you found a Ceres bug! Please report this error "
         << "to the developers.";
   }
   // Sanity check #2: No NULL's left behind.
   for (int i = 0; i < reordered_residual_blocks.size(); ++i) {
     CHECK(reordered_residual_blocks[i] != NULL)
         << "Congratulations, you found a Ceres bug! Please report this error "
         << "to the developers.";
   }

   // Now that the residuals are collected by E block, swap them in place.
   swap(*program->mutable_residual_blocks(), reordered_residual_blocks);
   return true;
 }

 // Pre-order the columns corresponding to the schur complement if
 // possible.
 void MaybeReorderSchurComplementColumnsUsingSuiteSparse(
     const ParameterBlockOrdering& parameter_block_ordering,
     Program* program) {
 #ifndef CERES_NO_SUITESPARSE
   SuiteSparse ss;
   if (!SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) {
     return;
   }

   vector<int> constraints;
   vector<ParameterBlock*>& parameter_blocks =
       *(program->mutable_parameter_blocks());

   for (int i = 0; i < parameter_blocks.size(); ++i) {
     constraints.push_back(
         parameter_block_ordering.GroupId(
             parameter_blocks[i]->mutable_user_state()));
   }

   // Renumber the entries of constraints to be contiguous integers
   // as camd requires that the group ids be in the range [0,
   // parameter_blocks.size() - 1].
   MapValuesToContiguousRange(constraints.size(), &constraints[0]);

   // Compute a block sparse presentation of J'.
   scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
       program->CreateJacobianBlockSparsityTranspose());

   cholmod_sparse* block_jacobian_transpose =
       ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get());

   vector<int> ordering(parameter_blocks.size(), 0);
   ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose,
                                                  &constraints[0],
                                                  &ordering[0]);
   ss.Free(block_jacobian_transpose);

   const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
   for (int i = 0; i < program->NumParameterBlocks(); ++i) {
     parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
   }

   program->SetParameterOffsetsAndIndex();
 #endif
 }

 void MaybeReorderSchurComplementColumnsUsingEigen(
     const int size_of_first_elimination_group,
     const ProblemImpl::ParameterMap& parameter_map,
     Program* program) {
 #if !EIGEN_VERSION_AT_LEAST(3, 2, 2) || !defined(CERES_USE_EIGEN_SPARSE)
   return;
 #else

   scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
       program->CreateJacobianBlockSparsityTranspose());

   typedef Eigen::SparseMatrix<int> SparseMatrix;
   const SparseMatrix block_jacobian =
       CreateBlockJacobian(*tsm_block_jacobian_transpose);
   const int num_rows = block_jacobian.rows();
   const int num_cols = block_jacobian.cols();

   // Vertically partition the jacobian in parameter blocks of type E
   // and F.
   const SparseMatrix E =
       block_jacobian.block(0,
                            0,
                            num_rows,
                            size_of_first_elimination_group);
   const SparseMatrix F =
       block_jacobian.block(0,
                            size_of_first_elimination_group,
                            num_rows,
                            num_cols - size_of_first_elimination_group);

   // Block sparsity pattern of the schur complement.
   const SparseMatrix block_schur_complement =
       F.transpose() * F - F.transpose() * E * E.transpose() * F;

   Eigen::AMDOrdering<int> amd_ordering;
   Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm;
   amd_ordering(block_schur_complement, perm);

   const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
   vector<ParameterBlock*> ordering(num_cols);

   // The ordering of the first size_of_first_elimination_group does
   // not matter, so we preserve the existing ordering.
   for (int i = 0; i < size_of_first_elimination_group; ++i) {
     ordering[i] = parameter_blocks[i];
   }

   // For the rest of the blocks, use the ordering computed using AMD.
   for (int i = 0; i < block_schur_complement.cols(); ++i) {
     ordering[size_of_first_elimination_group + i] =
         parameter_blocks[size_of_first_elimination_group + perm.indices()[i]];
   }

   swap(*program->mutable_parameter_blocks(), ordering);
   program->SetParameterOffsetsAndIndex();
 #endif
 }

 bool ReorderProgramForSchurTypeLinearSolver(
     const LinearSolverType linear_solver_type,
     const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
     const ProblemImpl::ParameterMap& parameter_map,
     ParameterBlockOrdering* parameter_block_ordering,
     Program* program,
     string* error) {
   if (parameter_block_ordering->NumElements() !=
       program->NumParameterBlocks()) {
     *error = StringPrintf(
         "The program has %d parameter blocks, but the parameter block "
         "ordering has %d parameter blocks.",
         program->NumParameterBlocks(),
         parameter_block_ordering->NumElements());
     return false;
   }

   if (parameter_block_ordering->NumGroups() == 1) {
     // If the user supplied an parameter_block_ordering with just one
     // group, it is equivalent to the user supplying NULL as an
     // parameter_block_ordering. Ceres is completely free to choose the
     // parameter block ordering as it sees fit. For Schur type solvers,
     // this means that the user wishes for Ceres to identify the
     // e_blocks, which we do by computing a maximal independent set.
     vector<ParameterBlock*> schur_ordering;
     const int size_of_first_elimination_group =
         ComputeStableSchurOrdering(*program, &schur_ordering);

     CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks())
         << "Congratulations, you found a Ceres bug! Please report this error "
         << "to the developers.";

     // Update the parameter_block_ordering object.
     for (int i = 0; i < schur_ordering.size(); ++i) {
       double* parameter_block = schur_ordering[i]->mutable_user_state();
       const int group_id = (i < size_of_first_elimination_group) ? 0 : 1;
       parameter_block_ordering->AddElementToGroup(parameter_block, group_id);
     }

     // We could call ApplyOrdering but this is cheaper and
     // simpler.
     swap(*program->mutable_parameter_blocks(), schur_ordering);
   } else {
     // The user provided an ordering with more than one elimination
     // group.

     // Verify that the first elimination group is an independent set.
     const set<double*>& first_elimination_group =
         parameter_block_ordering
         ->group_to_elements()
         .begin()
         ->second;
     if (!program->IsParameterBlockSetIndependent(first_elimination_group)) {
       *error =
           StringPrintf("The first elimination group in the parameter block "
                        "ordering of size %zd is not an independent set",
                        first_elimination_group.size());
       return false;
     }

     if (!ApplyOrdering(parameter_map,
                        *parameter_block_ordering,
                        program,
                        error)) {
       return false;
     }
   }

   program->SetParameterOffsetsAndIndex();

   const int size_of_first_elimination_group =
       parameter_block_ordering->group_to_elements().begin()->second.size();

   if (linear_solver_type == SPARSE_SCHUR) {
     if (sparse_linear_algebra_library_type == SUITE_SPARSE) {
       MaybeReorderSchurComplementColumnsUsingSuiteSparse(
           *parameter_block_ordering,
           program);
     } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) {
       MaybeReorderSchurComplementColumnsUsingEigen(
           size_of_first_elimination_group,
           parameter_map,
           program);
     }
   }

   // Schur type solvers also require that their residual blocks be
   // lexicographically ordered.
   if (!LexicographicallyOrderResidualBlocks(size_of_first_elimination_group,
                                             program,
                                             error)) {
     return false;
   }

   return true;
 }

 bool ReorderProgramForSparseNormalCholesky(
     const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
     const ParameterBlockOrdering& parameter_block_ordering,
     Program* program,
     string* error) {
   // Compute a block sparse presentation of J'.
   scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
       program->CreateJacobianBlockSparsityTranspose());

   vector<int> ordering(program->NumParameterBlocks(), 0);
   vector<ParameterBlock*>& parameter_blocks =
       *(program->mutable_parameter_blocks());

   if (sparse_linear_algebra_library_type == SUITE_SPARSE) {
     OrderingForSparseNormalCholeskyUsingSuiteSparse(
         *tsm_block_jacobian_transpose,
         parameter_blocks,
         parameter_block_ordering,
         &ordering[0]);
   } else if (sparse_linear_algebra_library_type == CX_SPARSE) {
     OrderingForSparseNormalCholeskyUsingCXSparse(
         *tsm_block_jacobian_transpose,
         &ordering[0]);
   } else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) {
 #if EIGEN_VERSION_AT_LEAST(3, 2, 2)
        OrderingForSparseNormalCholeskyUsingEigenSparse(
         *tsm_block_jacobian_transpose,
         &ordering[0]);
 #else
     // For Eigen versions less than 3.2.2, there is nothing to do as
     // older versions of Eigen do not expose a method for doing
     // symbolic analysis on pre-ordered matrices, so a block
     // pre-ordering is a bit pointless.

     return true;
 #endif
   }

   // Apply ordering.
   const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
   for (int i = 0; i < program->NumParameterBlocks(); ++i) {
     parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
   }

   program->SetParameterOffsetsAndIndex();
   return true;
 }

 }  // namespace internal
 }  // namespace ceres
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2014 Google Inc. All rights reserved.
	// http://code.google.com/p/ceres-solver/
	//
	// Redistribution and use in source and binary forms, with or without
	// modification, are permitted provided that the following conditions are met:
	//
	// * Redistributions of source code must retain the above copyright notice,
	// this list of conditions and the following disclaimer.
	// * Redistributions in binary form must reproduce the above copyright notice,
	// this list of conditions and the following disclaimer in the documentation
	// and/or other materials provided with the distribution.
	// * Neither the name of Google Inc. nor the names of its contributors may be
	// used to endorse or promote products derived from this software without
	// specific prior written permission.
	//
	// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
	// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
	// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
	// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
	// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
	// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
	// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
	// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
	// POSSIBILITY OF SUCH DAMAGE.
	//
	// Author: sameeragarwal@google.com (Sameer Agarwal)

	#include "ceres/reorder_program.h"

	#include <algorithm>
	#include <numeric>
	#include <vector>

	#include "ceres/cxsparse.h"
	#include "ceres/internal/port.h"
	#include "ceres/ordered_groups.h"
	#include "ceres/parameter_block.h"
	#include "ceres/parameter_block_ordering.h"
	#include "ceres/problem_impl.h"
	#include "ceres/program.h"
	#include "ceres/residual_block.h"
	#include "ceres/solver.h"
	#include "ceres/suitesparse.h"
	#include "ceres/triplet_sparse_matrix.h"
	#include "ceres/types.h"
	#include "Eigen/SparseCore"

	#ifdef CERES_USE_EIGEN_SPARSE
	#include "Eigen/OrderingMethods"
	#endif

	#include "glog/logging.h"

	namespace ceres {
	namespace internal {

	using std::map;
	using std::set;
	using std::string;
	using std::vector;

	namespace {

	// Find the minimum index of any parameter block to the given
	// residual. Parameter blocks that have indices greater than
	// size_of_first_elimination_group are considered to have an index
	// equal to size_of_first_elimination_group.
	static int MinParameterBlock(const ResidualBlock* residual_block,
	int size_of_first_elimination_group) {
	int min_parameter_block_position = size_of_first_elimination_group;
	for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) {
	ParameterBlock* parameter_block = residual_block->parameter_blocks()[i];
	if (!parameter_block->IsConstant()) {
	CHECK_NE(parameter_block->index(), -1)
	<< "Did you forget to call Program::SetParameterOffsetsAndIndex()? "
	<< "This is a Ceres bug; please contact the developers!";
	min_parameter_block_position = std::min(parameter_block->index(),
	min_parameter_block_position);
	}
	}
	return min_parameter_block_position;
	}

	#if EIGEN_VERSION_AT_LEAST(3, 2, 2) && defined(CERES_USE_EIGEN_SPARSE)
	Eigen::SparseMatrix<int> CreateBlockJacobian(
	const TripletSparseMatrix& block_jacobian_transpose) {
	typedef Eigen::SparseMatrix<int> SparseMatrix;
	typedef Eigen::Triplet<int> Triplet;

	const int* rows = block_jacobian_transpose.rows();
	const int* cols = block_jacobian_transpose.cols();
	int num_nonzeros = block_jacobian_transpose.num_nonzeros();
	vector<Triplet> triplets;
	triplets.reserve(num_nonzeros);
	for (int i = 0; i < num_nonzeros; ++i) {
	triplets.push_back(Triplet(cols[i], rows[i], 1));
	}

	SparseMatrix block_jacobian(block_jacobian_transpose.num_cols(),
	block_jacobian_transpose.num_rows());
	block_jacobian.setFromTriplets(triplets.begin(), triplets.end());
	return block_jacobian;
	}
	#endif

	void OrderingForSparseNormalCholeskyUsingSuiteSparse(
	const TripletSparseMatrix& tsm_block_jacobian_transpose,
	const vector<ParameterBlock*>& parameter_blocks,
	const ParameterBlockOrdering& parameter_block_ordering,
	int* ordering) {
	#ifdef CERES_NO_SUITESPARSE
	LOG(FATAL) << "Congratulations, you found a Ceres bug! "
	<< "Please report this error to the developers.";
	#else
	SuiteSparse ss;
	cholmod_sparse* block_jacobian_transpose =
	ss.CreateSparseMatrix(
	const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose));

	// No CAMD or the user did not supply a useful ordering, then just
	// use regular AMD.
	if (parameter_block_ordering.NumGroups() <= 1 \|\|
	!SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) {
	ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]);
	} else {
	vector<int> constraints;
	for (int i = 0; i < parameter_blocks.size(); ++i) {
	constraints.push_back(
	parameter_block_ordering.GroupId(
	parameter_blocks[i]->mutable_user_state()));
	}
	ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose,
	&constraints[0],
	ordering);
	}

	ss.Free(block_jacobian_transpose);
	#endif // CERES_NO_SUITESPARSE
	}

	void OrderingForSparseNormalCholeskyUsingCXSparse(
	const TripletSparseMatrix& tsm_block_jacobian_transpose,
	int* ordering) {
	#ifdef CERES_NO_CXSPARSE
	LOG(FATAL) << "Congratulations, you found a Ceres bug! "
	<< "Please report this error to the developers.";
	#else // CERES_NO_CXSPARSE
	// CXSparse works with J'J instead of J'. So compute the block
	// sparsity for J'J and compute an approximate minimum degree
	// ordering.
	CXSparse cxsparse;
	cs_di* block_jacobian_transpose;
	block_jacobian_transpose =
	cxsparse.CreateSparseMatrix(
	const_cast<TripletSparseMatrix*>(&tsm_block_jacobian_transpose));
	cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose);
	cs_di* block_hessian =
	cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian);
	cxsparse.Free(block_jacobian);
	cxsparse.Free(block_jacobian_transpose);

	cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, ordering);
	cxsparse.Free(block_hessian);
	#endif // CERES_NO_CXSPARSE
	}


	#if EIGEN_VERSION_AT_LEAST(3, 2, 2)
	void OrderingForSparseNormalCholeskyUsingEigenSparse(
	const TripletSparseMatrix& tsm_block_jacobian_transpose,
	int* ordering) {
	#ifndef CERES_USE_EIGEN_SPARSE
	LOG(FATAL) <<
	"SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE "
	"because Ceres was not built with support for "
	"Eigen's SimplicialLDLT decomposition. "
	"This requires enabling building with -DEIGENSPARSE=ON.";
	#else

	// This conversion from a TripletSparseMatrix to a Eigen::Triplet
	// matrix is unfortunate, but unavoidable for now. It is not a
	// significant performance penalty in the grand scheme of
	// things. The right thing to do here would be to get a compressed
	// row sparse matrix representation of the jacobian and go from
	// there. But that is a project for another day.
	typedef Eigen::SparseMatrix<int> SparseMatrix;

	const SparseMatrix block_jacobian =
	CreateBlockJacobian(tsm_block_jacobian_transpose);
	const SparseMatrix block_hessian =
	block_jacobian.transpose() * block_jacobian;

	Eigen::AMDOrdering<int> amd_ordering;
	Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm;
	amd_ordering(block_hessian, perm);
	for (int i = 0; i < block_hessian.rows(); ++i) {
	ordering[i] = perm.indices()[i];
	}
	#endif // CERES_USE_EIGEN_SPARSE
	}
	#endif

	} // namespace

	bool ApplyOrdering(const ProblemImpl::ParameterMap& parameter_map,
	const ParameterBlockOrdering& ordering,
	Program* program,
	string* error) {
	const int num_parameter_blocks = program->NumParameterBlocks();
	if (ordering.NumElements() != num_parameter_blocks) {
	*error = StringPrintf("User specified ordering does not have the same "
	"number of parameters as the problem. The problem"
	"has %d blocks while the ordering has %d blocks.",
	num_parameter_blocks,
	ordering.NumElements());
	return false;
	}

	vector<ParameterBlock> parameter_blocks =
	program->mutable_parameter_blocks();
	parameter_blocks->clear();

	const map<int, set<double*> >& groups = ordering.group_to_elements();
	for (map<int, set<double*> >::const_iterator group_it = groups.begin();
	group_it != groups.end();
	++group_it) {
	const set<double*>& group = group_it->second;
	for (set<double*>::const_iterator parameter_block_ptr_it = group.begin();
	parameter_block_ptr_it != group.end();
	++parameter_block_ptr_it) {
	ProblemImpl::ParameterMap::const_iterator parameter_block_it =
	parameter_map.find(*parameter_block_ptr_it);
	if (parameter_block_it == parameter_map.end()) {
	*error = StringPrintf("User specified ordering contains a pointer "
	"to a double that is not a parameter block in "
	"the problem. The invalid double is in group: %d",
	group_it->first);
	return false;
	}
	parameter_blocks->push_back(parameter_block_it->second);
	}
	}
	return true;
	}

	bool LexicographicallyOrderResidualBlocks(
	const int size_of_first_elimination_group,
	Program* program,
	string* error) {
	CHECK_GE(size_of_first_elimination_group, 1)
	<< "Congratulations, you found a Ceres bug! Please report this error "
	<< "to the developers.";

	// Create a histogram of the number of residuals for each E block. There is an
	// extra bucket at the end to catch all non-eliminated F blocks.
	vector<int> residual_blocks_per_e_block(size_of_first_elimination_group + 1);
	vector<ResidualBlock> residual_blocks = program->mutable_residual_blocks();
	vector<int> min_position_per_residual(residual_blocks->size());
	for (int i = 0; i < residual_blocks->size(); ++i) {
	ResidualBlock* residual_block = (*residual_blocks)[i];
	int position = MinParameterBlock(residual_block,
	size_of_first_elimination_group);
	min_position_per_residual[i] = position;
	DCHECK_LE(position, size_of_first_elimination_group);
	residual_blocks_per_e_block[position]++;
	}

	// Run a cumulative sum on the histogram, to obtain offsets to the start of
	// each histogram bucket (where each bucket is for the residuals for that
	// E-block).
	vector<int> offsets(size_of_first_elimination_group + 1);
	std::partial_sum(residual_blocks_per_e_block.begin(),
	residual_blocks_per_e_block.end(),
	offsets.begin());
	CHECK_EQ(offsets.back(), residual_blocks->size())
	<< "Congratulations, you found a Ceres bug! Please report this error "
	<< "to the developers.";

	CHECK(find(residual_blocks_per_e_block.begin(),
	residual_blocks_per_e_block.end() - 1, 0) !=
	residual_blocks_per_e_block.end())
	<< "Congratulations, you found a Ceres bug! Please report this error "
	<< "to the developers.";

	// Fill in each bucket with the residual blocks for its corresponding E block.
	// Each bucket is individually filled from the back of the bucket to the front
	// of the bucket. The filling order among the buckets is dictated by the
	// residual blocks. This loop uses the offsets as counters; subtracting one
	// from each offset as a residual block is placed in the bucket. When the
	// filling is finished, the offset pointerts should have shifted down one
	// entry (this is verified below).
	vector<ResidualBlock*> reordered_residual_blocks(
	(residual_blocks).size(), static_cast<ResidualBlock>(NULL));
	for (int i = 0; i < residual_blocks->size(); ++i) {
	int bucket = min_position_per_residual[i];

	// Decrement the cursor, which should now point at the next empty position.
	offsets[bucket]--;

	// Sanity.
	CHECK(reordered_residual_blocks[offsets[bucket]] == NULL)
	<< "Congratulations, you found a Ceres bug! Please report this error "
	<< "to the developers.";

	reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i];
	}

	// Sanity check #1: The difference in bucket offsets should match the
	// histogram sizes.
	for (int i = 0; i < size_of_first_elimination_group; ++i) {
	CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i])
	<< "Congratulations, you found a Ceres bug! Please report this error "
	<< "to the developers.";
	}
	// Sanity check #2: No NULL's left behind.
	for (int i = 0; i < reordered_residual_blocks.size(); ++i) {
	CHECK(reordered_residual_blocks[i] != NULL)
	<< "Congratulations, you found a Ceres bug! Please report this error "
	<< "to the developers.";
	}

	// Now that the residuals are collected by E block, swap them in place.
	swap(*program->mutable_residual_blocks(), reordered_residual_blocks);
	return true;
	}

	// Pre-order the columns corresponding to the schur complement if
	// possible.
	void MaybeReorderSchurComplementColumnsUsingSuiteSparse(
	const ParameterBlockOrdering& parameter_block_ordering,
	Program* program) {
	#ifndef CERES_NO_SUITESPARSE
	SuiteSparse ss;
	if (!SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) {
	return;
	}

	vector<int> constraints;
	vector<ParameterBlock*>& parameter_blocks =
	*(program->mutable_parameter_blocks());

	for (int i = 0; i < parameter_blocks.size(); ++i) {
	constraints.push_back(
	parameter_block_ordering.GroupId(
	parameter_blocks[i]->mutable_user_state()));
	}

	// Renumber the entries of constraints to be contiguous integers
	// as camd requires that the group ids be in the range [0,
	// parameter_blocks.size() - 1].
	MapValuesToContiguousRange(constraints.size(), &constraints[0]);

	// Compute a block sparse presentation of J'.
	scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
	program->CreateJacobianBlockSparsityTranspose());

	cholmod_sparse* block_jacobian_transpose =
	ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get());

	vector<int> ordering(parameter_blocks.size(), 0);
	ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose,
	&constraints[0],
	&ordering[0]);
	ss.Free(block_jacobian_transpose);

	const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
	for (int i = 0; i < program->NumParameterBlocks(); ++i) {
	parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
	}

	program->SetParameterOffsetsAndIndex();
	#endif
	}

	void MaybeReorderSchurComplementColumnsUsingEigen(
	const int size_of_first_elimination_group,
	const ProblemImpl::ParameterMap& parameter_map,
	Program* program) {
	#if !EIGEN_VERSION_AT_LEAST(3, 2, 2) \|\| !defined(CERES_USE_EIGEN_SPARSE)
	return;
	#else

	scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
	program->CreateJacobianBlockSparsityTranspose());

	typedef Eigen::SparseMatrix<int> SparseMatrix;
	const SparseMatrix block_jacobian =
	CreateBlockJacobian(*tsm_block_jacobian_transpose);
	const int num_rows = block_jacobian.rows();
	const int num_cols = block_jacobian.cols();

	// Vertically partition the jacobian in parameter blocks of type E
	// and F.
	const SparseMatrix E =
	block_jacobian.block(0,
	0,
	num_rows,
	size_of_first_elimination_group);
	const SparseMatrix F =
	block_jacobian.block(0,
	size_of_first_elimination_group,
	num_rows,
	num_cols - size_of_first_elimination_group);

	// Block sparsity pattern of the schur complement.
	const SparseMatrix block_schur_complement =
	F.transpose() * F - F.transpose() * E * E.transpose() * F;

	Eigen::AMDOrdering<int> amd_ordering;
	Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> perm;
	amd_ordering(block_schur_complement, perm);

	const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
	vector<ParameterBlock*> ordering(num_cols);

	// The ordering of the first size_of_first_elimination_group does
	// not matter, so we preserve the existing ordering.
	for (int i = 0; i < size_of_first_elimination_group; ++i) {
	ordering[i] = parameter_blocks[i];
	}

	// For the rest of the blocks, use the ordering computed using AMD.
	for (int i = 0; i < block_schur_complement.cols(); ++i) {
	ordering[size_of_first_elimination_group + i] =
	parameter_blocks[size_of_first_elimination_group + perm.indices()[i]];
	}

	swap(*program->mutable_parameter_blocks(), ordering);
	program->SetParameterOffsetsAndIndex();
	#endif
	}

	bool ReorderProgramForSchurTypeLinearSolver(
	const LinearSolverType linear_solver_type,
	const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
	const ProblemImpl::ParameterMap& parameter_map,
	ParameterBlockOrdering* parameter_block_ordering,
	Program* program,
	string* error) {
	if (parameter_block_ordering->NumElements() !=
	program->NumParameterBlocks()) {
	*error = StringPrintf(
	"The program has %d parameter blocks, but the parameter block "
	"ordering has %d parameter blocks.",
	program->NumParameterBlocks(),
	parameter_block_ordering->NumElements());
	return false;
	}

	if (parameter_block_ordering->NumGroups() == 1) {
	// If the user supplied an parameter_block_ordering with just one
	// group, it is equivalent to the user supplying NULL as an
	// parameter_block_ordering. Ceres is completely free to choose the
	// parameter block ordering as it sees fit. For Schur type solvers,
	// this means that the user wishes for Ceres to identify the
	// e_blocks, which we do by computing a maximal independent set.
	vector<ParameterBlock*> schur_ordering;
	const int size_of_first_elimination_group =
	ComputeStableSchurOrdering(*program, &schur_ordering);

	CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks())
	<< "Congratulations, you found a Ceres bug! Please report this error "
	<< "to the developers.";

	// Update the parameter_block_ordering object.
	for (int i = 0; i < schur_ordering.size(); ++i) {
	double* parameter_block = schur_ordering[i]->mutable_user_state();
	const int group_id = (i < size_of_first_elimination_group) ? 0 : 1;
	parameter_block_ordering->AddElementToGroup(parameter_block, group_id);
	}

	// We could call ApplyOrdering but this is cheaper and
	// simpler.
	swap(*program->mutable_parameter_blocks(), schur_ordering);
	} else {
	// The user provided an ordering with more than one elimination
	// group.

	// Verify that the first elimination group is an independent set.
	const set<double*>& first_elimination_group =
	parameter_block_ordering
	->group_to_elements()
	.begin()
	->second;
	if (!program->IsParameterBlockSetIndependent(first_elimination_group)) {
	*error =
	StringPrintf("The first elimination group in the parameter block "
	"ordering of size %zd is not an independent set",
	first_elimination_group.size());
	return false;
	}

	if (!ApplyOrdering(parameter_map,
	*parameter_block_ordering,
	program,
	error)) {
	return false;
	}
	}

	program->SetParameterOffsetsAndIndex();

	const int size_of_first_elimination_group =
	parameter_block_ordering->group_to_elements().begin()->second.size();

	if (linear_solver_type == SPARSE_SCHUR) {
	if (sparse_linear_algebra_library_type == SUITE_SPARSE) {
	MaybeReorderSchurComplementColumnsUsingSuiteSparse(
	*parameter_block_ordering,
	program);
	} else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) {
	MaybeReorderSchurComplementColumnsUsingEigen(
	size_of_first_elimination_group,
	parameter_map,
	program);
	}
	}

	// Schur type solvers also require that their residual blocks be
	// lexicographically ordered.
	if (!LexicographicallyOrderResidualBlocks(size_of_first_elimination_group,
	program,
	error)) {
	return false;
	}

	return true;
	}

	bool ReorderProgramForSparseNormalCholesky(
	const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
	const ParameterBlockOrdering& parameter_block_ordering,
	Program* program,
	string* error) {
	// Compute a block sparse presentation of J'.
	scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
	program->CreateJacobianBlockSparsityTranspose());

	vector<int> ordering(program->NumParameterBlocks(), 0);
	vector<ParameterBlock*>& parameter_blocks =
	*(program->mutable_parameter_blocks());

	if (sparse_linear_algebra_library_type == SUITE_SPARSE) {
	OrderingForSparseNormalCholeskyUsingSuiteSparse(
	*tsm_block_jacobian_transpose,
	parameter_blocks,
	parameter_block_ordering,
	&ordering[0]);
	} else if (sparse_linear_algebra_library_type == CX_SPARSE) {
	OrderingForSparseNormalCholeskyUsingCXSparse(
	*tsm_block_jacobian_transpose,
	&ordering[0]);
	} else if (sparse_linear_algebra_library_type == EIGEN_SPARSE) {
	#if EIGEN_VERSION_AT_LEAST(3, 2, 2)
	OrderingForSparseNormalCholeskyUsingEigenSparse(
	*tsm_block_jacobian_transpose,
	&ordering[0]);
	#else
	// For Eigen versions less than 3.2.2, there is nothing to do as
	// older versions of Eigen do not expose a method for doing
	// symbolic analysis on pre-ordered matrices, so a block
	// pre-ordering is a bit pointless.

	return true;
	#endif
	}

	// Apply ordering.
	const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks);
	for (int i = 0; i < program->NumParameterBlocks(); ++i) {
	parameter_blocks[i] = parameter_blocks_copy[ordering[i]];
	}

	program->SetParameterOffsetsAndIndex();
	return true;
	}

	} // namespace internal
	} // namespace ceres