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