mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-29 16:40:38 +08:00
Initial commit of Ceres Solver.
This commit is contained in:
Keir Mierle
@@ -0,0 +1,363 @@
|
||||
// Ceres Solver - A fast non-linear least squares minimizer
|
||||
// Copyright 2010, 2011, 2012 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_NO_SUITESPARSE
|
||||
|
||||
#include "ceres/visibility_based_preconditioner.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include "ceres/file.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "Eigen/Dense"
|
||||
#include "ceres/block_random_access_dense_matrix.h"
|
||||
#include "ceres/block_random_access_sparse_matrix.h"
|
||||
#include "ceres/block_sparse_matrix.h"
|
||||
#include "ceres/casts.h"
|
||||
#include "ceres/collections_port.h"
|
||||
#include "ceres/linear_least_squares_problems.h"
|
||||
#include "ceres/schur_eliminator.h"
|
||||
#include "ceres/stringprintf.h"
|
||||
#include "ceres/internal/eigen.h"
|
||||
#include "ceres/internal/scoped_ptr.h"
|
||||
#include "ceres/types.h"
|
||||
|
||||
DECLARE_string(test_srcdir);
|
||||
|
||||
namespace ceres {
|
||||
namespace internal {
|
||||
|
||||
using testing::AssertionResult;
|
||||
using testing::AssertionSuccess;
|
||||
using testing::AssertionFailure;
|
||||
|
||||
static const double kTolerance = 1e-12;
|
||||
|
||||
class VisibilityBasedPreconditionerTest : public ::testing::Test {
|
||||
public:
|
||||
static const int kCameraSize = 9;
|
||||
|
||||
protected:
|
||||
void SetUp() {
|
||||
string input_file =
|
||||
JoinPath(FLAGS_test_srcdir,
|
||||
"problem-6-1384-000.lsqp"); // NOLINT
|
||||
|
||||
scoped_ptr<LinearLeastSquaresProblem> problem(
|
||||
CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file)));
|
||||
A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
|
||||
b_.reset(problem->b.release());
|
||||
D_.reset(problem->D.release());
|
||||
|
||||
const CompressedRowBlockStructure* bs =
|
||||
CHECK_NOTNULL(A_->block_structure());
|
||||
const int num_col_blocks = bs->cols.size();
|
||||
|
||||
num_cols_ = A_->num_cols();
|
||||
num_rows_ = A_->num_rows();
|
||||
num_eliminate_blocks_ = problem->num_eliminate_blocks;
|
||||
num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_;
|
||||
options_.num_eliminate_blocks = num_eliminate_blocks_;
|
||||
|
||||
vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
|
||||
for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
|
||||
blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
|
||||
}
|
||||
|
||||
// The input matrix is a real jacobian and fairly poorly
|
||||
// conditioned. Setting D to a large constant makes the normal
|
||||
// equations better conditioned and makes the tests below better
|
||||
// conditioned.
|
||||
VectorRef(D_.get(), num_cols_).setConstant(10.0);
|
||||
|
||||
schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks));
|
||||
Vector rhs(schur_complement_->num_rows());
|
||||
|
||||
scoped_ptr<SchurEliminatorBase> eliminator;
|
||||
eliminator.reset(SchurEliminatorBase::Create(options_));
|
||||
eliminator->Init(num_eliminate_blocks_, bs);
|
||||
eliminator->Eliminate(A_.get(), b_.get(), D_.get(),
|
||||
schur_complement_.get(), rhs.data());
|
||||
}
|
||||
|
||||
|
||||
AssertionResult IsSparsityStructureValid() {
|
||||
preconditioner_->InitStorage(*A_->block_structure());
|
||||
const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
|
||||
const vector<int>& cluster_membership = get_cluster_membership();
|
||||
|
||||
for (int i = 0; i < num_camera_blocks_; ++i) {
|
||||
for (int j = i; j < num_camera_blocks_; ++j) {
|
||||
if (cluster_pairs.count(make_pair(cluster_membership[i],
|
||||
cluster_membership[j]))) {
|
||||
if (!IsBlockPairInPreconditioner(i, j)) {
|
||||
return AssertionFailure()
|
||||
<< "block pair (" << i << "," << j << "missing";
|
||||
}
|
||||
} else {
|
||||
if (IsBlockPairInPreconditioner(i, j)) {
|
||||
return AssertionFailure()
|
||||
<< "block pair (" << i << "," << j << "should not be present";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return AssertionSuccess();
|
||||
}
|
||||
|
||||
AssertionResult PreconditionerValuesMatch() {
|
||||
preconditioner_->Compute(*A_, D_.get());
|
||||
const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
|
||||
const BlockRandomAccessSparseMatrix* m = get_m();
|
||||
Matrix preconditioner_matrix;
|
||||
m->matrix()->ToDenseMatrix(&preconditioner_matrix);
|
||||
ConstMatrixRef full_schur_complement(schur_complement_->values(),
|
||||
m->num_rows(),
|
||||
m->num_rows());
|
||||
const int num_clusters = get_num_clusters();
|
||||
const int kDiagonalBlockSize =
|
||||
kCameraSize * num_camera_blocks_ / num_clusters;
|
||||
|
||||
for (int i = 0; i < num_clusters; ++i) {
|
||||
for (int j = i; j < num_clusters; ++j) {
|
||||
double diff = 0.0;
|
||||
if (cluster_pairs.count(make_pair(i, j))) {
|
||||
diff =
|
||||
(preconditioner_matrix.block(kDiagonalBlockSize * i,
|
||||
kDiagonalBlockSize * j,
|
||||
kDiagonalBlockSize,
|
||||
kDiagonalBlockSize) -
|
||||
full_schur_complement.block(kDiagonalBlockSize * i,
|
||||
kDiagonalBlockSize * j,
|
||||
kDiagonalBlockSize,
|
||||
kDiagonalBlockSize)).norm();
|
||||
} else {
|
||||
diff = preconditioner_matrix.block(kDiagonalBlockSize * i,
|
||||
kDiagonalBlockSize * j,
|
||||
kDiagonalBlockSize,
|
||||
kDiagonalBlockSize).norm();
|
||||
}
|
||||
if (diff > kTolerance) {
|
||||
return AssertionFailure()
|
||||
<< "Preconditioner block " << i << " " << j << " differs "
|
||||
<< "from expected value by " << diff;
|
||||
}
|
||||
}
|
||||
}
|
||||
return AssertionSuccess();
|
||||
}
|
||||
|
||||
// Accessors
|
||||
int get_num_blocks() { return preconditioner_->num_blocks_; }
|
||||
|
||||
int get_num_clusters() { return preconditioner_->num_clusters_; }
|
||||
int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; }
|
||||
|
||||
const vector<int>& get_block_size() {
|
||||
return preconditioner_->block_size_; }
|
||||
|
||||
vector<int>* get_mutable_block_size() {
|
||||
return &preconditioner_->block_size_; }
|
||||
|
||||
const vector<int>& get_cluster_membership() {
|
||||
return preconditioner_->cluster_membership_;
|
||||
}
|
||||
|
||||
vector<int>* get_mutable_cluster_membership() {
|
||||
return &preconditioner_->cluster_membership_;
|
||||
}
|
||||
|
||||
const set<pair<int, int> >& get_block_pairs() {
|
||||
return preconditioner_->block_pairs_;
|
||||
}
|
||||
|
||||
set<pair<int, int> >* get_mutable_block_pairs() {
|
||||
return &preconditioner_->block_pairs_;
|
||||
}
|
||||
|
||||
const HashSet<pair<int, int> >& get_cluster_pairs() {
|
||||
return preconditioner_->cluster_pairs_;
|
||||
}
|
||||
|
||||
HashSet<pair<int, int> >* get_mutable_cluster_pairs() {
|
||||
return &preconditioner_->cluster_pairs_;
|
||||
}
|
||||
|
||||
bool IsBlockPairInPreconditioner(const int block1, const int block2) {
|
||||
return preconditioner_->IsBlockPairInPreconditioner(block1, block2);
|
||||
}
|
||||
|
||||
bool IsBlockPairOffDiagonal(const int block1, const int block2) {
|
||||
return preconditioner_->IsBlockPairOffDiagonal(block1, block2);
|
||||
}
|
||||
|
||||
const BlockRandomAccessSparseMatrix* get_m() {
|
||||
return preconditioner_->m_.get();
|
||||
}
|
||||
|
||||
int num_rows_;
|
||||
int num_cols_;
|
||||
int num_eliminate_blocks_;
|
||||
int num_camera_blocks_;
|
||||
|
||||
scoped_ptr<BlockSparseMatrix> A_;
|
||||
scoped_array<double> b_;
|
||||
scoped_array<double> D_;
|
||||
|
||||
LinearSolver::Options options_;
|
||||
scoped_ptr<VisibilityBasedPreconditioner> preconditioner_;
|
||||
scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_;
|
||||
};
|
||||
|
||||
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
|
||||
TEST_F(VisibilityBasedPreconditionerTest, SchurJacobiStructure) {
|
||||
options_.preconditioner_type = SCHUR_JACOBI;
|
||||
preconditioner_.reset(
|
||||
new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
|
||||
EXPECT_EQ(get_num_blocks(), num_camera_blocks_);
|
||||
EXPECT_EQ(get_num_clusters(), num_camera_blocks_);
|
||||
for (int i = 0; i < num_camera_blocks_; ++i) {
|
||||
for (int j = 0; j < num_camera_blocks_; ++j) {
|
||||
const string msg = StringPrintf("Camera pair: %d %d", i, j);
|
||||
SCOPED_TRACE(msg);
|
||||
if (i == j) {
|
||||
EXPECT_TRUE(IsBlockPairInPreconditioner(i, j));
|
||||
EXPECT_FALSE(IsBlockPairOffDiagonal(i, j));
|
||||
} else {
|
||||
EXPECT_FALSE(IsBlockPairInPreconditioner(i, j));
|
||||
EXPECT_TRUE(IsBlockPairOffDiagonal(i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
|
||||
options_.preconditioner_type = CLUSTER_JACOBI;
|
||||
preconditioner_.reset(
|
||||
new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
|
||||
|
||||
// Override the clustering to be a single clustering containing all
|
||||
// the cameras.
|
||||
vector<int>& cluster_membership = *get_mutable_cluster_membership();
|
||||
for (int i = 0; i < num_camera_blocks_; ++i) {
|
||||
cluster_membership[i] = 0;
|
||||
}
|
||||
|
||||
*get_mutable_num_clusters() = 1;
|
||||
|
||||
HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
|
||||
cluster_pairs.clear();
|
||||
cluster_pairs.insert(make_pair(0, 0));
|
||||
|
||||
EXPECT_TRUE(IsSparsityStructureValid());
|
||||
EXPECT_TRUE(PreconditionerValuesMatch());
|
||||
|
||||
// Multiplication by the inverse of the preconditioner.
|
||||
const int num_rows = schur_complement_->num_rows();
|
||||
ConstMatrixRef full_schur_complement(schur_complement_->values(),
|
||||
num_rows,
|
||||
num_rows);
|
||||
Vector x(num_rows);
|
||||
Vector y(num_rows);
|
||||
Vector z(num_rows);
|
||||
|
||||
for (int i = 0; i < num_rows; ++i) {
|
||||
x.setZero();
|
||||
y.setZero();
|
||||
z.setZero();
|
||||
x[i] = 1.0;
|
||||
preconditioner_->RightMultiply(x.data(), y.data());
|
||||
z = full_schur_complement
|
||||
.selfadjointView<Eigen::Upper>()
|
||||
.ldlt().solve(x);
|
||||
double max_relative_difference =
|
||||
((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>();
|
||||
EXPECT_NEAR(max_relative_difference, 0.0, kTolerance);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
TEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) {
|
||||
options_.preconditioner_type = CLUSTER_JACOBI;
|
||||
preconditioner_.reset(
|
||||
new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
|
||||
|
||||
// Override the clustering to be equal number of cameras.
|
||||
vector<int>& cluster_membership = *get_mutable_cluster_membership();
|
||||
cluster_membership.resize(num_camera_blocks_);
|
||||
static const int kNumClusters = 3;
|
||||
|
||||
for (int i = 0; i < num_camera_blocks_; ++i) {
|
||||
cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
|
||||
}
|
||||
*get_mutable_num_clusters() = kNumClusters;
|
||||
|
||||
HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
|
||||
cluster_pairs.clear();
|
||||
for (int i = 0; i < kNumClusters; ++i) {
|
||||
cluster_pairs.insert(make_pair(i, i));
|
||||
}
|
||||
|
||||
EXPECT_TRUE(IsSparsityStructureValid());
|
||||
EXPECT_TRUE(PreconditionerValuesMatch());
|
||||
}
|
||||
|
||||
|
||||
TEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) {
|
||||
options_.preconditioner_type = CLUSTER_TRIDIAGONAL;
|
||||
preconditioner_.reset(
|
||||
new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
|
||||
static const int kNumClusters = 3;
|
||||
|
||||
// Override the clustering to be 3 clusters.
|
||||
vector<int>& cluster_membership = *get_mutable_cluster_membership();
|
||||
cluster_membership.resize(num_camera_blocks_);
|
||||
for (int i = 0; i < num_camera_blocks_; ++i) {
|
||||
cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
|
||||
}
|
||||
*get_mutable_num_clusters() = kNumClusters;
|
||||
|
||||
// Spanning forest has structure 0-1 2
|
||||
HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
|
||||
cluster_pairs.clear();
|
||||
for (int i = 0; i < kNumClusters; ++i) {
|
||||
cluster_pairs.insert(make_pair(i, i));
|
||||
}
|
||||
cluster_pairs.insert(make_pair(0, 1));
|
||||
|
||||
EXPECT_TRUE(IsSparsityStructureValid());
|
||||
EXPECT_TRUE(PreconditionerValuesMatch());
|
||||
}
|
||||
#endif // CERES_DONT_HAVE_PROTOCOL_BUFFERS
|
||||
|
||||
} // namespace internal
|
||||
} // namespace ceres
|
||||
|
||||
#endif // CERES_NO_SUITESPARSE
|
||||
Reference in New Issue
Block a user