visibility_based_preconditioner_test.cc revision 399f7d09e0c45af54b77b4ab9508d6f23759b927
10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_NO_SUITESPARSE 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/visibility_based_preconditioner.h" 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Dense" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_dense_matrix.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_sparse_matrix.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/collections_port.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/file.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_least_squares_problems.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/schur_eliminator.h" 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stringprintf.h" 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/test_util.h" 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h" 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TODO(sameeragarwal): Re-enable this test once serialization is 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// working again. 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// using testing::AssertionResult; 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// using testing::AssertionSuccess; 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// using testing::AssertionFailure; 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// static const double kTolerance = 1e-12; 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// class VisibilityBasedPreconditionerTest : public ::testing::Test { 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// public: 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// static const int kCameraSize = 9; 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// protected: 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// void SetUp() { 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// string input_file = TestFileAbsolutePath("problem-6-1384-000.lsqp"); 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// scoped_ptr<LinearLeastSquaresProblem> problem( 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file))); 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release())); 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// b_.reset(problem->b.release()); 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// D_.reset(problem->D.release()); 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const CompressedRowBlockStructure* bs = 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CHECK_NOTNULL(A_->block_structure()); 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const int num_col_blocks = bs->cols.size(); 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// num_cols_ = A_->num_cols(); 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// num_rows_ = A_->num_rows(); 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// num_eliminate_blocks_ = problem->num_eliminate_blocks; 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_; 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// options_.elimination_groups.push_back(num_eliminate_blocks_); 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// options_.elimination_groups.push_back( 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A_->block_structure()->cols.size() - num_eliminate_blocks_); 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0); 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// blocks[i - num_eliminate_blocks_] = bs->cols[i].size; 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // The input matrix is a real jacobian and fairly poorly 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // conditioned. Setting D to a large constant makes the normal 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // equations better conditioned and makes the tests below better 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // conditioned. 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// VectorRef(D_.get(), num_cols_).setConstant(10.0); 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks)); 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Vector rhs(schur_complement_->num_rows()); 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// scoped_ptr<SchurEliminatorBase> eliminator; 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LinearSolver::Options eliminator_options; 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// eliminator_options.elimination_groups = options_.elimination_groups; 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// eliminator_options.num_threads = options_.num_threads; 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// eliminator.reset(SchurEliminatorBase::Create(eliminator_options)); 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// eliminator->Init(num_eliminate_blocks_, bs); 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// eliminator->Eliminate(A_.get(), b_.get(), D_.get(), 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// schur_complement_.get(), rhs.data()); 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AssertionResult IsSparsityStructureValid() { 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// preconditioner_->InitStorage(*A_->block_structure()); 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs(); 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const vector<int>& cluster_membership = get_cluster_membership(); 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < num_camera_blocks_; ++i) { 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int j = i; j < num_camera_blocks_; ++j) { 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// if (cluster_pairs.count(make_pair(cluster_membership[i], 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_membership[j]))) { 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// if (!IsBlockPairInPreconditioner(i, j)) { 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return AssertionFailure() 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// << "block pair (" << i << "," << j << "missing"; 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } else { 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// if (IsBlockPairInPreconditioner(i, j)) { 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return AssertionFailure() 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// << "block pair (" << i << "," << j << "should not be present"; 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return AssertionSuccess(); 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AssertionResult PreconditionerValuesMatch() { 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// preconditioner_->Update(*A_, D_.get()); 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs(); 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const BlockRandomAccessSparseMatrix* m = get_m(); 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Matrix preconditioner_matrix; 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// m->matrix()->ToDenseMatrix(&preconditioner_matrix); 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ConstMatrixRef full_schur_complement(schur_complement_->values(), 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// m->num_rows(), 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// m->num_rows()); 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const int num_clusters = get_num_clusters(); 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const int kDiagonalBlockSize = 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kCameraSize * num_camera_blocks_ / num_clusters; 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < num_clusters; ++i) { 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int j = i; j < num_clusters; ++j) { 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// double diff = 0.0; 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// if (cluster_pairs.count(make_pair(i, j))) { 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// diff = 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// (preconditioner_matrix.block(kDiagonalBlockSize * i, 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize * j, 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize, 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize) - 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// full_schur_complement.block(kDiagonalBlockSize * i, 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize * j, 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize, 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize)).norm(); 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } else { 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// diff = preconditioner_matrix.block(kDiagonalBlockSize * i, 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize * j, 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize, 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// kDiagonalBlockSize).norm(); 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// if (diff > kTolerance) { 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return AssertionFailure() 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// << "Preconditioner block " << i << " " << j << " differs " 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// << "from expected value by " << diff; 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return AssertionSuccess(); 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // Accessors 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// int get_num_blocks() { return preconditioner_->num_blocks_; } 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// int get_num_clusters() { return preconditioner_->num_clusters_; } 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; } 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const vector<int>& get_block_size() { 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return preconditioner_->block_size_; } 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector<int>* get_mutable_block_size() { 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return &preconditioner_->block_size_; } 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const vector<int>& get_cluster_membership() { 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return preconditioner_->cluster_membership_; 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector<int>* get_mutable_cluster_membership() { 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return &preconditioner_->cluster_membership_; 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const set<pair<int, int> >& get_block_pairs() { 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return preconditioner_->block_pairs_; 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// set<pair<int, int> >* get_mutable_block_pairs() { 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return &preconditioner_->block_pairs_; 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const HashSet<pair<int, int> >& get_cluster_pairs() { 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return preconditioner_->cluster_pairs_; 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// HashSet<pair<int, int> >* get_mutable_cluster_pairs() { 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return &preconditioner_->cluster_pairs_; 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// bool IsBlockPairInPreconditioner(const int block1, const int block2) { 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return preconditioner_->IsBlockPairInPreconditioner(block1, block2); 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// bool IsBlockPairOffDiagonal(const int block1, const int block2) { 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return preconditioner_->IsBlockPairOffDiagonal(block1, block2); 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const BlockRandomAccessSparseMatrix* get_m() { 2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return preconditioner_->m_.get(); 2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// int num_rows_; 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// int num_cols_; 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// int num_eliminate_blocks_; 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// int num_camera_blocks_; 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// scoped_ptr<BlockSparseMatrix> A_; 2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// scoped_array<double> b_; 2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// scoped_array<double> D_; 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Preconditioner::Options options_; 2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// scoped_ptr<VisibilityBasedPreconditioner> preconditioner_; 2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_; 2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// }; 2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) { 2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// options_.type = CLUSTER_JACOBI; 2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// preconditioner_.reset( 2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// new VisibilityBasedPreconditioner(*A_->block_structure(), options_)); 2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // Override the clustering to be a single clustering containing all 2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // the cameras. 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector<int>& cluster_membership = *get_mutable_cluster_membership(); 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < num_camera_blocks_; ++i) { 2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_membership[i] = 0; 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// *get_mutable_num_clusters() = 1; 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs(); 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_pairs.clear(); 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_pairs.insert(make_pair(0, 0)); 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// EXPECT_TRUE(IsSparsityStructureValid()); 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// EXPECT_TRUE(PreconditionerValuesMatch()); 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // Multiplication by the inverse of the preconditioner. 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const int num_rows = schur_complement_->num_rows(); 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ConstMatrixRef full_schur_complement(schur_complement_->values(), 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// num_rows, 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// num_rows); 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Vector x(num_rows); 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Vector y(num_rows); 2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Vector z(num_rows); 2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < num_rows; ++i) { 2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// x.setZero(); 2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// y.setZero(); 2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// z.setZero(); 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// x[i] = 1.0; 2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// preconditioner_->RightMultiply(x.data(), y.data()); 2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// z = full_schur_complement 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// .selfadjointView<Eigen::Upper>() 282399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// .llt().solve(x); 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// double max_relative_difference = 2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>(); 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// EXPECT_NEAR(max_relative_difference, 0.0, kTolerance); 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) { 2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// options_.type = CLUSTER_JACOBI; 2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// preconditioner_.reset( 2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// new VisibilityBasedPreconditioner(*A_->block_structure(), options_)); 2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // Override the clustering to be equal number of cameras. 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector<int>& cluster_membership = *get_mutable_cluster_membership(); 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_membership.resize(num_camera_blocks_); 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// static const int kNumClusters = 3; 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < num_camera_blocks_; ++i) { 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_; 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// *get_mutable_num_clusters() = kNumClusters; 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs(); 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_pairs.clear(); 3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < kNumClusters; ++i) { 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_pairs.insert(make_pair(i, i)); 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// EXPECT_TRUE(IsSparsityStructureValid()); 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// EXPECT_TRUE(PreconditionerValuesMatch()); 3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 3151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) { 3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// options_.type = CLUSTER_TRIDIAGONAL; 3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// preconditioner_.reset( 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// new VisibilityBasedPreconditioner(*A_->block_structure(), options_)); 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// static const int kNumClusters = 3; 3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // Override the clustering to be 3 clusters. 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector<int>& cluster_membership = *get_mutable_cluster_membership(); 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_membership.resize(num_camera_blocks_); 3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < num_camera_blocks_; ++i) { 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_; 3281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// *get_mutable_num_clusters() = kNumClusters; 3301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // Spanning forest has structure 0-1 2 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs(); 3331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_pairs.clear(); 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// for (int i = 0; i < kNumClusters; ++i) { 3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_pairs.insert(make_pair(i, i)); 3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cluster_pairs.insert(make_pair(0, 1)); 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// EXPECT_TRUE(IsSparsityStructureValid()); 3401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// EXPECT_TRUE(PreconditionerValuesMatch()); 3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 3420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_NO_SUITESPARSE 347