visibility_based_preconditioner_test.cc revision 0ae28bd5885b5daa526898fcf7c323dc2c3e1963
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 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::AssertionResult; 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::AssertionSuccess; 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongusing testing::AssertionFailure; 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstatic const double kTolerance = 1e-12; 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass VisibilityBasedPreconditionerTest : public ::testing::Test { 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const int kCameraSize = 9; 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong protected: 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void SetUp() { 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong string input_file = TestFileAbsolutePath("problem-6-1384-000.lsqp"); 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<LinearLeastSquaresProblem> problem( 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file))); 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release())); 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong b_.reset(problem->b.release()); 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong D_.reset(problem->D.release()); 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(A_->block_structure()); 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_col_blocks = bs->cols.size(); 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = A_->num_cols(); 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = A_->num_rows(); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_eliminate_blocks_ = problem->num_eliminate_blocks; 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_; 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.elimination_groups.push_back(num_eliminate_blocks_); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.elimination_groups.push_back( 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->block_structure()->cols.size() - num_eliminate_blocks_); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0); 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong blocks[i - num_eliminate_blocks_] = bs->cols[i].size; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The input matrix is a real jacobian and fairly poorly 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // conditioned. Setting D to a large constant makes the normal 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // equations better conditioned and makes the tests below better 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // conditioned. 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(D_.get(), num_cols_).setConstant(10.0); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks)); 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector rhs(schur_complement_->num_rows()); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<SchurEliminatorBase> eliminator; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator.reset(SchurEliminatorBase::Create(options_)); 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator->Init(num_eliminate_blocks_, bs); 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator->Eliminate(A_.get(), b_.get(), D_.get(), 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong schur_complement_.get(), rhs.data()); 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AssertionResult IsSparsityStructureValid() { 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong preconditioner_->InitStorage(*A_->block_structure()); 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs(); 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& cluster_membership = get_cluster_membership(); 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_camera_blocks_; ++i) { 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = i; j < num_camera_blocks_; ++j) { 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cluster_pairs.count(make_pair(cluster_membership[i], 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_membership[j]))) { 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsBlockPairInPreconditioner(i, j)) { 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AssertionFailure() 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "block pair (" << i << "," << j << "missing"; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsBlockPairInPreconditioner(i, j)) { 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AssertionFailure() 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "block pair (" << i << "," << j << "should not be present"; 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AssertionSuccess(); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AssertionResult PreconditionerValuesMatch() { 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong preconditioner_->Update(*A_, D_.get()); 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs(); 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const BlockRandomAccessSparseMatrix* m = get_m(); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Matrix preconditioner_matrix; 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->matrix()->ToDenseMatrix(&preconditioner_matrix); 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ConstMatrixRef full_schur_complement(schur_complement_->values(), 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->num_rows(), 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m->num_rows()); 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_clusters = get_num_clusters(); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int kDiagonalBlockSize = 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kCameraSize * num_camera_blocks_ / num_clusters; 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_clusters; ++i) { 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = i; j < num_clusters; ++j) { 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double diff = 0.0; 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cluster_pairs.count(make_pair(i, j))) { 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diff = 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (preconditioner_matrix.block(kDiagonalBlockSize * i, 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize * j, 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize, 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize) - 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong full_schur_complement.block(kDiagonalBlockSize * i, 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize * j, 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize, 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize)).norm(); 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diff = preconditioner_matrix.block(kDiagonalBlockSize * i, 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize * j, 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize, 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kDiagonalBlockSize).norm(); 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (diff > kTolerance) { 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AssertionFailure() 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Preconditioner block " << i << " " << j << " differs " 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "from expected value by " << diff; 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AssertionSuccess(); 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Accessors 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int get_num_blocks() { return preconditioner_->num_blocks_; } 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int get_num_clusters() { return preconditioner_->num_clusters_; } 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; } 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& get_block_size() { 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return preconditioner_->block_size_; } 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>* get_mutable_block_size() { 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return &preconditioner_->block_size_; } 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& get_cluster_membership() { 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return preconditioner_->cluster_membership_; 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>* get_mutable_cluster_membership() { 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return &preconditioner_->cluster_membership_; 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const set<pair<int, int> >& get_block_pairs() { 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return preconditioner_->block_pairs_; 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set<pair<int, int> >* get_mutable_block_pairs() { 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return &preconditioner_->block_pairs_; 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<pair<int, int> >& get_cluster_pairs() { 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return preconditioner_->cluster_pairs_; 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashSet<pair<int, int> >* get_mutable_cluster_pairs() { 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return &preconditioner_->cluster_pairs_; 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool IsBlockPairInPreconditioner(const int block1, const int block2) { 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return preconditioner_->IsBlockPairInPreconditioner(block1, block2); 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool IsBlockPairOffDiagonal(const int block1, const int block2) { 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return preconditioner_->IsBlockPairOffDiagonal(block1, block2); 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const BlockRandomAccessSparseMatrix* get_m() { 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return preconditioner_->m_.get(); 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows_; 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols_; 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_eliminate_blocks_; 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_camera_blocks_; 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<BlockSparseMatrix> A_; 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_array<double> b_; 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_array<double> D_; 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearSolver::Options options_; 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<VisibilityBasedPreconditioner> preconditioner_; 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_; 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_NO_PROTOCOL_BUFFERS 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(VisibilityBasedPreconditionerTest, SchurJacobiStructure) { 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.preconditioner_type = SCHUR_JACOBI; 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong preconditioner_.reset( 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new VisibilityBasedPreconditioner(*A_->block_structure(), options_)); 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(get_num_blocks(), num_camera_blocks_); 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_EQ(get_num_clusters(), num_camera_blocks_); 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_camera_blocks_; ++i) { 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < num_camera_blocks_; ++j) { 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const string msg = StringPrintf("Camera pair: %d %d", i, j); 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong SCOPED_TRACE(msg); 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (i == j) { 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(IsBlockPairInPreconditioner(i, j)); 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_FALSE(IsBlockPairOffDiagonal(i, j)); 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_FALSE(IsBlockPairInPreconditioner(i, j)); 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(IsBlockPairOffDiagonal(i, j)); 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) { 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.preconditioner_type = CLUSTER_JACOBI; 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong preconditioner_.reset( 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new VisibilityBasedPreconditioner(*A_->block_structure(), options_)); 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Override the clustering to be a single clustering containing all 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the cameras. 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>& cluster_membership = *get_mutable_cluster_membership(); 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_camera_blocks_; ++i) { 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_membership[i] = 0; 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *get_mutable_num_clusters() = 1; 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs(); 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs.clear(); 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs.insert(make_pair(0, 0)); 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(IsSparsityStructureValid()); 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(PreconditionerValuesMatch()); 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Multiplication by the inverse of the preconditioner. 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_rows = schur_complement_->num_rows(); 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ConstMatrixRef full_schur_complement(schur_complement_->values(), 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows, 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows); 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector x(num_rows); 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector y(num_rows); 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector z(num_rows); 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows; ++i) { 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x.setZero(); 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y.setZero(); 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong z.setZero(); 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x[i] = 1.0; 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong preconditioner_->RightMultiply(x.data(), y.data()); 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong z = full_schur_complement 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong .selfadjointView<Eigen::Upper>() 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong .ldlt().solve(x); 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double max_relative_difference = 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>(); 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_NEAR(max_relative_difference, 0.0, kTolerance); 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) { 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.preconditioner_type = CLUSTER_JACOBI; 3080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong preconditioner_.reset( 3090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new VisibilityBasedPreconditioner(*A_->block_structure(), options_)); 3100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Override the clustering to be equal number of cameras. 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>& cluster_membership = *get_mutable_cluster_membership(); 3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_membership.resize(num_camera_blocks_); 3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const int kNumClusters = 3; 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_camera_blocks_; ++i) { 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_; 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *get_mutable_num_clusters() = kNumClusters; 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs(); 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs.clear(); 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < kNumClusters; ++i) { 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs.insert(make_pair(i, i)); 3250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(IsSparsityStructureValid()); 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(PreconditionerValuesMatch()); 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongTEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) { 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.preconditioner_type = CLUSTER_TRIDIAGONAL; 3340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong preconditioner_.reset( 3350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new VisibilityBasedPreconditioner(*A_->block_structure(), options_)); 3360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const int kNumClusters = 3; 3370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Override the clustering to be 3 clusters. 3390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>& cluster_membership = *get_mutable_cluster_membership(); 3400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_membership.resize(num_camera_blocks_); 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_camera_blocks_; ++i) { 3420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_; 3430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *get_mutable_num_clusters() = kNumClusters; 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Spanning forest has structure 0-1 2 3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs(); 3480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs.clear(); 3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < kNumClusters; ++i) { 3500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs.insert(make_pair(i, i)); 3510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs.insert(make_pair(0, 1)); 3530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(IsSparsityStructureValid()); 3550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong EXPECT_TRUE(PreconditionerValuesMatch()); 3560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_NO_PROTOCOL_BUFFERS 3580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 3600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_NO_SUITESPARSE 363