| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2023 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/visibility_based_preconditioner.h" |
| |
| #include <memory> |
| |
| #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/file.h" |
| #include "ceres/internal/eigen.h" |
| #include "ceres/linear_least_squares_problems.h" |
| #include "ceres/schur_eliminator.h" |
| #include "ceres/stringprintf.h" |
| #include "ceres/test_util.h" |
| #include "ceres/types.h" |
| #include "glog/logging.h" |
| #include "gtest/gtest.h" |
| |
| namespace ceres::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"); |
| |
| // std::unique_ptr<LinearLeastSquaresProblem> problem = |
| // 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_ = |
| // std::make_unique<BlockRandomAccessDenseMatrix>(blocks); |
| // Vector rhs(schur_complement_->num_rows()); |
| |
| // std::unique_ptr<SchurEliminatorBase> eliminator; |
| // LinearSolver::Options eliminator_options; |
| // eliminator_options.elimination_groups = options_.elimination_groups; |
| // eliminator_options.num_threads = options_.num_threads; |
| |
| // eliminator = 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 std::unordered_set<pair<int, int>, pair_hash>& 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 std::unordered_set<pair<int, int>, pair_hash>& 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 std::unordered_set<pair<int, int>, pair_hash>& get_cluster_pairs() { |
| // return preconditioner_->cluster_pairs_; |
| // } |
| |
| // std::unordered_set<pair<int, int>, pair_hash>* 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_; |
| |
| // std::unique_ptr<BlockSparseMatrix> A_; |
| // std::unique_ptr<double[]> b_; |
| // std::unique_ptr<double[]> D_; |
| |
| // Preconditioner::Options options_; |
| // std::unique_ptr<VisibilityBasedPreconditioner> preconditioner_; |
| // std::unique_ptr<BlockRandomAccessDenseMatrix> schur_complement_; |
| // }; |
| |
| // TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) { |
| // options_.type = CLUSTER_JACOBI; |
| // preconditioner_ = |
| // std::make_unique<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; |
| |
| // std::unordered_set<pair<int, int>, pair_hash>& 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_->RightMultiplyAndAccumulate(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_ = |
| // std::make_unique<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; |
| |
| // std::unordered_set<pair<int, int>, pair_hash>& 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_ = |
| // std::make_unique<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 |
| // std::unordered_set<pair<int, int>, pair_hash>& 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 ceres::internal |
