350 lines
13 KiB
C++
350 lines
13 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)
|
||
|
|
|
||
|
|
// This include must come before any #ifndef check on Ceres compile options.
|
||
|
|
#include "ceres/internal/port.h"
|
||
|
|
|
||
|
|
#ifndef CERES_NO_SUITESPARSE
|
||
|
|
|
||
|
|
#include "ceres/visibility_based_preconditioner.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/file.h"
|
||
|
|
#include "ceres/internal/eigen.h"
|
||
|
|
#include "ceres/internal/scoped_ptr.h"
|
||
|
|
#include "ceres/linear_least_squares_problems.h"
|
||
|
|
#include "ceres/schur_eliminator.h"
|
||
|
|
#include "ceres/stringprintf.h"
|
||
|
|
#include "ceres/types.h"
|
||
|
|
#include "ceres/test_util.h"
|
||
|
|
#include "glog/logging.h"
|
||
|
|
#include "gtest/gtest.h"
|
||
|
|
|
||
|
|
namespace ceres {
|
||
|
|
namespace internal {
|
||
|
|
|
||
|
|
// TODO(sameeragarwal): Re-enable this test once serialization is
|
||
|
|
// working again.
|
||
|
|
|
||
|
|
// 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 = TestFileAbsolutePath("problem-6-1384-000.lsqp");
|
||
|
|
|
||
|
|
// 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_.elimination_groups.push_back(num_eliminate_blocks_);
|
||
|
|
// options_.elimination_groups.push_back(
|
||
|
|
// A_->block_structure()->cols.size() - 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;
|
||
|
|
// LinearSolver::Options eliminator_options;
|
||
|
|
// eliminator_options.elimination_groups = options_.elimination_groups;
|
||
|
|
// eliminator_options.num_threads = options_.num_threads;
|
||
|
|
|
||
|
|
// eliminator.reset(SchurEliminatorBase::Create(eliminator_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_->Update(*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_;
|
||
|
|
|
||
|
|
// Preconditioner::Options options_;
|
||
|
|
// scoped_ptr<VisibilityBasedPreconditioner> preconditioner_;
|
||
|
|
// scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_;
|
||
|
|
// };
|
||
|
|
|
||
|
|
// TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
|
||
|
|
// options_.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>()
|
||
|
|
// .llt().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_.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_.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());
|
||
|
|
// }
|
||
|
|
|
||
|
|
} // namespace internal
|
||
|
|
} // namespace ceres
|
||
|
|
|
||
|
|
#endif // CERES_NO_SUITESPARSE
|