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
3179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// This include must come before any #ifndef check on Ceres compile options.
3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/internal/port.h"
3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez
340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_NO_SUITESPARSE
350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/visibility_based_preconditioner.h"
370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Dense"
390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_dense_matrix.h"
400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_sparse_matrix.h"
410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h"
420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h"
430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/collections_port.h"
440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/file.h"
450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h"
460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h"
470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_least_squares_problems.h"
480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/schur_eliminator.h"
490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stringprintf.h"
500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h"
510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/test_util.h"
520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h"
530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "gtest/gtest.h"
540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres {
560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal {
570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TODO(sameeragarwal): Re-enable this test once serialization is
591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// working again.
601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// using testing::AssertionResult;
621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// using testing::AssertionSuccess;
631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// using testing::AssertionFailure;
641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// static const double kTolerance = 1e-12;
661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// class VisibilityBasedPreconditionerTest : public ::testing::Test {
681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//  public:
691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   static const int kCameraSize = 9;
701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//  protected:
721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   void SetUp() {
731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     string input_file = TestFileAbsolutePath("problem-6-1384-000.lsqp");
741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     scoped_ptr<LinearLeastSquaresProblem> problem(
761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file)));
771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     b_.reset(problem->b.release());
791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     D_.reset(problem->D.release());
801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const CompressedRowBlockStructure* bs =
821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         CHECK_NOTNULL(A_->block_structure());
831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const int num_col_blocks = bs->cols.size();
841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     num_cols_ = A_->num_cols();
861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     num_rows_ = A_->num_rows();
871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     num_eliminate_blocks_ = problem->num_eliminate_blocks;
881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_;
891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     options_.elimination_groups.push_back(num_eliminate_blocks_);
901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     options_.elimination_groups.push_back(
911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         A_->block_structure()->cols.size() - num_eliminate_blocks_);
921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     }
971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     // The input matrix is a real jacobian and fairly poorly
991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     // conditioned. Setting D to a large constant makes the normal
1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     // equations better conditioned and makes the tests below better
1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     // conditioned.
1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     VectorRef(D_.get(), num_cols_).setConstant(10.0);
1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks));
1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     Vector rhs(schur_complement_->num_rows());
1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     scoped_ptr<SchurEliminatorBase> eliminator;
1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     LinearSolver::Options eliminator_options;
1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     eliminator_options.elimination_groups = options_.elimination_groups;
1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     eliminator_options.num_threads = options_.num_threads;
1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     eliminator.reset(SchurEliminatorBase::Create(eliminator_options));
1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     eliminator->Init(num_eliminate_blocks_, bs);
1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     eliminator->Eliminate(A_.get(), b_.get(), D_.get(),
1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                           schur_complement_.get(), rhs.data());
1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   AssertionResult IsSparsityStructureValid() {
1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     preconditioner_->InitStorage(*A_->block_structure());
1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const vector<int>& cluster_membership = get_cluster_membership();
1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     for (int i = 0; i < num_camera_blocks_; ++i) {
1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       for (int j = i; j < num_camera_blocks_; ++j) {
1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         if (cluster_pairs.count(make_pair(cluster_membership[i],
1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                           cluster_membership[j]))) {
1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           if (!IsBlockPairInPreconditioner(i, j)) {
1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//             return AssertionFailure()
1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                 << "block pair (" << i << "," << j << "missing";
1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           }
1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         } else {
1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           if (IsBlockPairInPreconditioner(i, j)) {
1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//             return AssertionFailure()
1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                << "block pair (" << i << "," << j << "should not be present";
1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           }
1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         }
1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       }
1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     }
1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return AssertionSuccess();
1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   AssertionResult PreconditionerValuesMatch() {
1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     preconditioner_->Update(*A_, D_.get());
1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const BlockRandomAccessSparseMatrix* m = get_m();
1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     Matrix preconditioner_matrix;
1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     m->matrix()->ToDenseMatrix(&preconditioner_matrix);
1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     ConstMatrixRef full_schur_complement(schur_complement_->values(),
1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                          m->num_rows(),
1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                          m->num_rows());
1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const int num_clusters = get_num_clusters();
1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     const int kDiagonalBlockSize =
1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         kCameraSize * num_camera_blocks_ / num_clusters;
1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     for (int i = 0; i < num_clusters; ++i) {
1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       for (int j = i; j < num_clusters; ++j) {
1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         double diff = 0.0;
1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         if (cluster_pairs.count(make_pair(i, j))) {
1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           diff =
1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//               (preconditioner_matrix.block(kDiagonalBlockSize * i,
1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                            kDiagonalBlockSize * j,
1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                            kDiagonalBlockSize,
1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                            kDiagonalBlockSize) -
1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                full_schur_complement.block(kDiagonalBlockSize * i,
1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                            kDiagonalBlockSize * j,
1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                            kDiagonalBlockSize,
1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                            kDiagonalBlockSize)).norm();
1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         } else {
1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           diff = preconditioner_matrix.block(kDiagonalBlockSize * i,
1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                              kDiagonalBlockSize * j,
1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                              kDiagonalBlockSize,
1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                              kDiagonalBlockSize).norm();
1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         }
1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         if (diff > kTolerance) {
1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//           return AssertionFailure()
1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//               << "Preconditioner block " << i << " " << j << " differs "
1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//               << "from expected value by " << diff;
1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         }
1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       }
1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     }
1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return AssertionSuccess();
1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   // Accessors
1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   int get_num_blocks() { return preconditioner_->num_blocks_; }
1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   int get_num_clusters() { return preconditioner_->num_clusters_; }
1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; }
1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   const vector<int>& get_block_size() {
1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return preconditioner_->block_size_; }
1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   vector<int>* get_mutable_block_size() {
1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return &preconditioner_->block_size_; }
1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   const vector<int>& get_cluster_membership() {
1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return preconditioner_->cluster_membership_;
1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   vector<int>* get_mutable_cluster_membership() {
2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return &preconditioner_->cluster_membership_;
2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   const set<pair<int, int> >& get_block_pairs() {
2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return preconditioner_->block_pairs_;
2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   set<pair<int, int> >* get_mutable_block_pairs() {
2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return &preconditioner_->block_pairs_;
2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   const HashSet<pair<int, int> >& get_cluster_pairs() {
2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return preconditioner_->cluster_pairs_;
2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   HashSet<pair<int, int> >* get_mutable_cluster_pairs() {
2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return &preconditioner_->cluster_pairs_;
2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   bool IsBlockPairInPreconditioner(const int block1, const int block2) {
2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return preconditioner_->IsBlockPairInPreconditioner(block1, block2);
2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   bool IsBlockPairOffDiagonal(const int block1, const int block2) {
2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return preconditioner_->IsBlockPairOffDiagonal(block1, block2);
2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   const BlockRandomAccessSparseMatrix* get_m() {
2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     return preconditioner_->m_.get();
2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   int num_rows_;
2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   int num_cols_;
2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   int num_eliminate_blocks_;
2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   int num_camera_blocks_;
2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   scoped_ptr<BlockSparseMatrix> A_;
2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   scoped_array<double> b_;
2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   scoped_array<double> D_;
2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   Preconditioner::Options options_;
2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   scoped_ptr<VisibilityBasedPreconditioner> preconditioner_;
2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_;
2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// };
2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   options_.type = CLUSTER_JACOBI;
2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   preconditioner_.reset(
2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   // Override the clustering to be a single clustering containing all
2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   // the cameras.
2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   for (int i = 0; i < num_camera_blocks_; ++i) {
2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     cluster_membership[i] = 0;
2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   *get_mutable_num_clusters() = 1;
2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   cluster_pairs.clear();
2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   cluster_pairs.insert(make_pair(0, 0));
2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   EXPECT_TRUE(IsSparsityStructureValid());
2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   EXPECT_TRUE(PreconditionerValuesMatch());
2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   // Multiplication by the inverse of the preconditioner.
2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   const int num_rows = schur_complement_->num_rows();
2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   ConstMatrixRef full_schur_complement(schur_complement_->values(),
2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                        num_rows,
2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//                                        num_rows);
2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   Vector x(num_rows);
2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   Vector y(num_rows);
2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   Vector z(num_rows);
2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   for (int i = 0; i < num_rows; ++i) {
2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     x.setZero();
2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     y.setZero();
2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     z.setZero();
2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     x[i] = 1.0;
2821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     preconditioner_->RightMultiply(x.data(), y.data());
2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     z = full_schur_complement
2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         .selfadjointView<Eigen::Upper>()
285399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger//         .llt().solve(x);
2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     double max_relative_difference =
2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//         ((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>();
2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     EXPECT_NEAR(max_relative_difference, 0.0, kTolerance);
2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// }
2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) {
2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   options_.type = CLUSTER_JACOBI;
2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   preconditioner_.reset(
2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   // Override the clustering to be equal number of cameras.
3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   cluster_membership.resize(num_camera_blocks_);
3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   static const int kNumClusters = 3;
3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   for (int i = 0; i < num_camera_blocks_; ++i) {
3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   *get_mutable_num_clusters() = kNumClusters;
3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   cluster_pairs.clear();
3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   for (int i = 0; i < kNumClusters; ++i) {
3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     cluster_pairs.insert(make_pair(i, i));
3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   EXPECT_TRUE(IsSparsityStructureValid());
3161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   EXPECT_TRUE(PreconditionerValuesMatch());
3171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// }
3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) {
3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   options_.type = CLUSTER_TRIDIAGONAL;
3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   preconditioner_.reset(
3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   static const int kNumClusters = 3;
3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   // Override the clustering to be 3 clusters.
3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
3281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   cluster_membership.resize(num_camera_blocks_);
3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   for (int i = 0; i < num_camera_blocks_; ++i) {
3301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   *get_mutable_num_clusters() = kNumClusters;
3331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   // Spanning forest has structure 0-1 2
3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   cluster_pairs.clear();
3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   for (int i = 0; i < kNumClusters; ++i) {
3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//     cluster_pairs.insert(make_pair(i, i));
3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   }
3401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   cluster_pairs.insert(make_pair(0, 1));
3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling
3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   EXPECT_TRUE(IsSparsityStructureValid());
3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling//   EXPECT_TRUE(PreconditionerValuesMatch());
3441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// }
3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}  // namespace internal
3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}  // namespace ceres
3480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong
3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif  // CERES_NO_SUITESPARSE
350