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 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#ifndef CERES_NO_SUITESPARSE 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/visibility_based_preconditioner.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <functional> 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <iterator> 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <set> 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <utility> 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Dense" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_sparse_matrix.h" 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h" 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/canonical_views_clustering.h" 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/collections_port.h" 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/graph.h" 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/graph_algorithms.h" 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_solver.h" 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/schur_eliminator.h" 5479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/single_linkage_clustering.h" 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/visibility.h" 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// TODO(sameeragarwal): Currently these are magic weights for the 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// preconditioner construction. Move these higher up into the Options 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// struct and provide some guidelines for choosing them. 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// This will require some more work on the clustering algorithm and 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// possibly some more refactoring of the code. 6779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstatic const double kCanonicalViewsSizePenaltyWeight = 3.0; 6879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstatic const double kCanonicalViewsSimilarityPenaltyWeight = 0.0; 6979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstatic const double kSingleLinkageMinSimilarity = 0.9; 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongVisibilityBasedPreconditioner::VisibilityBasedPreconditioner( 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure& bs, 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Preconditioner::Options& options) 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : options_(options), 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_blocks_(0), 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_clusters_(0), 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong factor_(NULL) { 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GT(options_.elimination_groups.size(), 1); 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GT(options_.elimination_groups[0], 0); 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK(options_.type == CLUSTER_JACOBI || 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options_.type == CLUSTER_TRIDIAGONAL) 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Unknown preconditioner type: " << options_.type; 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_blocks_ = bs.cols.size() - options_.elimination_groups[0]; 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GT(num_blocks_, 0) 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Jacobian should have atleast 1 f_block for " 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "visibility based preconditioning."; 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Vector of camera block sizes 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_size_.resize(num_blocks_); 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_blocks_; ++i) { 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size; 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const time_t start_time = time(NULL); 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling switch (options_.type) { 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CLUSTER_JACOBI: 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ComputeClusterJacobiSparsity(bs); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CLUSTER_TRIDIAGONAL: 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ComputeClusterTridiagonalSparsity(bs); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong default: 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(FATAL) << "Unknown preconditioner type"; 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const time_t structure_time = time(NULL); 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong InitStorage(bs); 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const time_t storage_time = time(NULL); 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong InitEliminator(bs); 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const time_t eliminator_time = time(NULL); 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Allocate temporary storage for a vector used during 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // RightMultiply. 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rhs_ = CHECK_NOTNULL(ss_.CreateDenseVector(NULL, 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m_->num_rows(), 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m_->num_rows())); 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const time_t init_time = time(NULL); 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "init time: " 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << init_time - start_time 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " structure time: " << structure_time - start_time 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " storage time:" << storage_time - structure_time 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " eliminator time: " << eliminator_time - storage_time; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongVisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() { 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (factor_ != NULL) { 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss_.Free(factor_); 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong factor_ = NULL; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (tmp_rhs_ != NULL) { 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss_.Free(tmp_rhs_); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rhs_ = NULL; 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Determine the sparsity structure of the CLUSTER_JACOBI 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// preconditioner. It clusters cameras using their scene 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// visibility. The clusters form the diagonal blocks of the 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// preconditioner matrix. 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity( 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure& bs) { 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<set<int> > visibility; 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ComputeVisibility(bs, options_.elimination_groups[0], &visibility); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(num_blocks_, visibility.size()); 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ClusterCameras(visibility); 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs_.clear(); 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_clusters_; ++i) { 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs_.insert(make_pair(i, i)); 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Determine the sparsity structure of the CLUSTER_TRIDIAGONAL 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// preconditioner. It clusters cameras using using the scene 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// visibility and then finds the strongly interacting pairs of 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// clusters by constructing another graph with the clusters as 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// vertices and approximating it with a degree-2 maximum spanning 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// forest. The set of edges in this forest are the cluster pairs. 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure& bs) { 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<set<int> > visibility; 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ComputeVisibility(bs, options_.elimination_groups[0], &visibility); 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(num_blocks_, visibility.size()); 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ClusterCameras(visibility); 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Construct a weighted graph on the set of clusters, where the 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // edges are the number of 3D points/e_blocks visible in both the 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // clusters at the ends of the edge. Return an approximate degree-2 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // maximum spanning forest of this graph. 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<set<int> > cluster_visibility; 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ComputeClusterVisibility(visibility, &cluster_visibility); 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<Graph<int> > cluster_graph( 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(CreateClusterGraph(cluster_visibility))); 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<Graph<int> > forest( 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(Degree2MaximumSpanningForest(*cluster_graph))); 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ForestToClusterPairs(*forest, &cluster_pairs_); 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Allocate storage for the preconditioner matrix. 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::InitStorage( 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure& bs) { 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ComputeBlockPairsInPreconditioner(bs); 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_)); 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Call the canonical views algorithm and cluster the cameras based on 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// their visibility sets. The visibility set of a camera is the set of 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// e_blocks/3D points in the scene that are seen by it. 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The cluster_membership_ vector is updated to indicate cluster 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// memberships for each camera block. 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::ClusterCameras( 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<set<int> >& visibility) { 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_ptr<Graph<int> > schur_complement_graph( 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(CreateSchurComplementGraph(visibility))); 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<int, int> membership; 19679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.visibility_clustering_type == CANONICAL_VIEWS) { 19879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<int> centers; 19979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CanonicalViewsClusteringOptions clustering_options; 20079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez clustering_options.size_penalty_weight = 20179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez kCanonicalViewsSizePenaltyWeight; 20279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez clustering_options.similarity_penalty_weight = 20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez kCanonicalViewsSimilarityPenaltyWeight; 20479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ComputeCanonicalViewsClustering(clustering_options, 20579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *schur_complement_graph, 20679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ¢ers, 20779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &membership); 20879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez num_clusters_ = centers.size(); 20979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else if (options_.visibility_clustering_type == SINGLE_LINKAGE) { 21079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez SingleLinkageClusteringOptions clustering_options; 21179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez clustering_options.min_similarity = 21279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez kSingleLinkageMinSimilarity; 21379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez num_clusters_ = ComputeSingleLinkageClustering(clustering_options, 21479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *schur_complement_graph, 21579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &membership); 21679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else { 21779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(FATAL) << "Unknown visibility clustering algorithm."; 21879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 21979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GT(num_clusters_, 0); 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "num_clusters: " << num_clusters_; 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FlattenMembershipMap(membership, &cluster_membership_); 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Compute the block sparsity structure of the Schur complement 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// matrix. For each pair of cameras contributing a non-zero cell to 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the schur complement, determine if that cell is present in the 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// preconditioner or not. 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A pair of cameras contribute a cell to the preconditioner if they 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// are part of the same cluster or if the the two clusters that they 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// belong have an edge connecting them in the degree-2 maximum 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// spanning forest. 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// For example, a camera pair (i,j) where i belonges to cluster1 and 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// j belongs to cluster2 (assume that cluster1 < cluster2). 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The cell corresponding to (i,j) is present in the preconditioner 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// if cluster1 == cluster2 or the pair (cluster1, cluster2) were 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// connected by an edge in the degree-2 maximum spanning forest. 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Since we have already expanded the forest into a set of camera 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// pairs/edges, including self edges, the check can be reduced to 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// checking membership of (cluster1, cluster2) in cluster_pairs_. 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure& bs) { 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_pairs_.clear(); 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_blocks_; ++i) { 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_pairs_.insert(make_pair(i, i)); 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int r = 0; 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_row_blocks = bs.rows.size(); 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_eliminate_blocks = options_.elimination_groups[0]; 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over each row of the matrix. The block structure of the 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // matrix is assumed to be sorted in order of the e_blocks/point 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // blocks. Thus all row blocks containing an e_block/point occur 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // contiguously. Further, if present, an e_block is always the first 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // parameter block in each row block. These structural assumptions 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // are common to all Schur complement based solvers in Ceres. 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For each e_block/point block we identify the set of cameras 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // seeing it. The cross product of this set with itself is the set 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // of non-zero cells contibuted by this e_block. 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The time complexity of this is O(nm^2) where, n is the number of 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 3d points and m is the maximum number of cameras seeing any 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // point, which for most scenes is a fairly small number. 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong while (r < num_row_blocks) { 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int e_block_id = bs.rows[r].cells.front().block_id; 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (e_block_id >= num_eliminate_blocks) { 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Skip the rows whose first block is an f_block. 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set<int> f_blocks; 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (; r < num_row_blocks; ++r) { 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRow& row = bs.rows[r]; 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (row.cells.front().block_id != e_block_id) { 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the blocks in the row, ignoring the first block 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // since it is the one to be eliminated and adding the rest to 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the list of f_blocks associated with this e_block. 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = 1; c < row.cells.size(); ++c) { 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Cell& cell = row.cells[c]; 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int f_block_id = cell.block_id - num_eliminate_blocks; 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(f_block_id, 0); 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong f_blocks.insert(f_block_id); 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (set<int>::const_iterator block1 = f_blocks.begin(); 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block1 != f_blocks.end(); 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++block1) { 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set<int>::const_iterator block2 = block1; 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++block2; 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (; block2 != f_blocks.end(); ++block2) { 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsBlockPairInPreconditioner(*block1, *block2)) { 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_pairs_.insert(make_pair(*block1, *block2)); 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The remaining rows which do not contain any e_blocks. 3090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (; r < num_row_blocks; ++r) { 3100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRow& row = bs.rows[r]; 3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < row.cells.size(); ++i) { 3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block1 = row.cells[i].block_id - num_eliminate_blocks; 3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < row.cells.size(); ++j) { 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block2 = row.cells[j].block_id - num_eliminate_blocks; 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (block1 <= block2) { 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsBlockPairInPreconditioner(block1, block2)) { 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_pairs_.insert(make_pair(block1, block2)); 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(1) << "Block pair stats: " << block_pairs_.size(); 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Initialize the SchurEliminator. 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::InitEliminator( 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure& bs) { 3310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LinearSolver::Options eliminator_options; 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator_options.elimination_groups = options_.elimination_groups; 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator_options.num_threads = options_.num_threads; 33479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez eliminator_options.e_block_size = options_.e_block_size; 33579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez eliminator_options.f_block_size = options_.f_block_size; 33679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez eliminator_options.row_block_size = options_.row_block_size; 3370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); 33879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez eliminator_->Init(eliminator_options.elimination_groups[0], &bs); 3390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Update the values of the preconditioner matrix and factorize it. 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool VisibilityBasedPreconditioner::UpdateImpl(const BlockSparseMatrix& A, 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* D) { 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const time_t start_time = time(NULL); 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_rows = m_->num_rows(); 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GT(num_rows, 0); 3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We need a dummy rhs vector and a dummy b vector since the Schur 3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // eliminator combines the computation of the reduced camera matrix 3500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // with the computation of the right hand side of that linear 3510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // system. 3520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 3530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // TODO(sameeragarwal): Perhaps its worth refactoring the 3540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // SchurEliminator::Eliminate function to allow NULL for the rhs. As 3550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // of now it does not seem to be worth the effort. 3560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector rhs = Vector::Zero(m_->num_rows()); 3570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector b = Vector::Zero(A.num_rows()); 3580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute a subset of the entries of the Schur complement. 3600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Try factorizing the matrix. For CLUSTER_JACOBI, this should 3631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // always succeed modulo some numerical/conditioning problems. For 3641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // CLUSTER_TRIDIAGONAL, in general the preconditioner matrix as 3651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // constructed is not positive definite. However, we will go ahead 3661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // and try factorizing it. If it works, great, otherwise we scale 3671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // all the cells in the preconditioner corresponding to the edges in 3681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the degree-2 forest and that guarantees positive 3690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // definiteness. The proof of this fact can be found in Lemma 1 in 3700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // "Visibility Based Preconditioning for Bundle Adjustment". 3710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 3720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Doing the factorization like this saves us matrix mass when 3730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // scaling is not needed, which is quite often in our experience. 37479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolverTerminationType status = Factorize(); 37579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 37679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (status == LINEAR_SOLVER_FATAL_ERROR) { 37779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 37879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 3790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The scaling only affects the tri-diagonal case, since 3810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // ScaleOffDiagonalBlocks only pays attenion to the cells that 3821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // belong to the edges of the degree-2 forest. In the CLUSTER_JACOBI 3831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // case, the preconditioner is guaranteed to be positive 3841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // semidefinite. 38579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (status == LINEAR_SOLVER_FAILURE && options_.type == CLUSTER_TRIDIAGONAL) { 3860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal " 3870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "scaling"; 3880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ScaleOffDiagonalCells(); 3890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong status = Factorize(); 3900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "Compute time: " << time(NULL) - start_time; 39379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return (status == LINEAR_SOLVER_SUCCESS); 3940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Consider the preconditioner matrix as meta-block matrix, whose 3970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// blocks correspond to the clusters. Then cluster pairs corresponding 3980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to edges in the degree-2 forest are off diagonal entries of this 3990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// matrix. Scaling these off-diagonal entries by 1/2 forces this 4000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// matrix to be positive definite. 4010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::ScaleOffDiagonalCells() { 4020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (set< pair<int, int> >::const_iterator it = block_pairs_.begin(); 4030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != block_pairs_.end(); 4040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 4050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block1 = it->first; 4060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block2 = it->second; 4070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsBlockPairOffDiagonal(block1, block2)) { 4080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong continue; 4090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int r, c, row_stride, col_stride; 4120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CellInfo* cell_info = m_->GetCell(block1, block2, 4130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong &r, &c, 4140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong &row_stride, &col_stride); 4150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(cell_info != NULL) 4160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Cell missing for block pair (" << block1 << "," << block2 << ")" 4170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " cluster pair (" << cluster_membership_[block1] 4180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << cluster_membership_[block2] << ")"; 4190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Ah the magic of tri-diagonal matrices and diagonal 4210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // dominance. See Lemma 1 in "Visibility Based Preconditioning 4220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For Bundle Adjustment". 4230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MatrixRef m(cell_info->values, row_stride, col_stride); 4240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5; 4250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Compute the sparse Cholesky factorization of the preconditioner 4290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// matrix. 43079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolverTerminationType VisibilityBasedPreconditioner::Factorize() { 4310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Extract the TripletSparseMatrix that is used for actually storing 4320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // S and convert it into a cholmod_sparse object. 4330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_sparse* lhs = ss_.CreateSparseMatrix( 4340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong down_cast<BlockRandomAccessSparseMatrix*>( 4350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m_.get())->mutable_matrix()); 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The matrix is symmetric, and the upper triangular part of the 4380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // matrix contains the values. 4390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong lhs->stype = 1; 4400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 44179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // TODO(sameeragarwal): Refactor to pipe this up and out. 44279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string status; 44379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Symbolic factorization is computed if we don't already have one handy. 4450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (factor_ == NULL) { 44679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez factor_ = ss_.BlockAnalyzeCholesky(lhs, block_size_, block_size_, &status); 4470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 44979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const LinearSolverTerminationType termination_type = 45079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (factor_ != NULL) 45179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ? ss_.Cholesky(lhs, factor_, &status) 45279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : LINEAR_SOLVER_FATAL_ERROR; 45379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss_.Free(lhs); 45579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return termination_type; 4560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::RightMultiply(const double* x, 4590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* y) const { 4600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 4610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(y); 4620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong SuiteSparse* ss = const_cast<SuiteSparse*>(&ss_); 4630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_rows = m_->num_rows(); 4650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong memcpy(CHECK_NOTNULL(tmp_rhs_)->x, x, m_->num_rows() * sizeof(*x)); 46679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // TODO(sameeragarwal): Better error handling. 46779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string status; 46879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cholmod_dense* solution = 46979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(ss->Solve(factor_, tmp_rhs_, &status)); 4700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong memcpy(y, solution->x, sizeof(*y) * num_rows); 4710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss->Free(solution); 4720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint VisibilityBasedPreconditioner::num_rows() const { 4750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return m_->num_rows(); 4760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Classify camera/f_block pairs as in and out of the preconditioner, 4790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// based on whether the cluster pair that they belong to is in the 4800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// preconditioner or not. 4810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner( 4820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block1, 4830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block2) const { 4840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int cluster1 = cluster_membership_[block1]; 4850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int cluster2 = cluster_membership_[block2]; 4860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cluster1 > cluster2) { 4870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::swap(cluster1, cluster2); 4880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0); 4900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal( 4930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block1, 4940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block2) const { 4950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (cluster_membership_[block1] != cluster_membership_[block2]); 4960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Convert a graph into a list of edges that includes self edges for 4990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// each vertex. 5000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::ForestToClusterPairs( 5010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Graph<int>& forest, 5020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashSet<pair<int, int> >* cluster_pairs) const { 5030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(cluster_pairs)->clear(); 5040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<int>& vertices = forest.vertices(); 5050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(vertices.size(), num_clusters_); 5060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add all the cluster pairs corresponding to the edges in the 5080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // forest. 5090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (HashSet<int>::const_iterator it1 = vertices.begin(); 5100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it1 != vertices.end(); 5110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it1) { 5120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int cluster1 = *it1; 5130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs->insert(make_pair(cluster1, cluster1)); 5140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashSet<int>& neighbors = forest.Neighbors(cluster1); 5150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (HashSet<int>::const_iterator it2 = neighbors.begin(); 5160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it2 != neighbors.end(); 5170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it2) { 5180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int cluster2 = *it2; 5190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cluster1 < cluster2) { 5200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_pairs->insert(make_pair(cluster1, cluster2)); 5210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The visibilty set of a cluster is the union of the visibilty sets 5270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// of all its cameras. In other words, the set of points visible to 5280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// any camera in the cluster. 5290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::ComputeClusterVisibility( 5300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<set<int> >& visibility, 5310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<set<int> >* cluster_visibility) const { 5320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(cluster_visibility)->resize(0); 5330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_visibility->resize(num_clusters_); 5340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_blocks_; ++i) { 5350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int cluster_id = cluster_membership_[i]; 5360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (*cluster_visibility)[cluster_id].insert(visibility[i].begin(), 5370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong visibility[i].end()); 5380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Construct a graph whose vertices are the clusters, and the edge 5420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// weights are the number of 3D points visible to cameras in both the 5430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// vertices. 5440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongGraph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( 5450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<set<int> >& cluster_visibility) const { 5460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Graph<int>* cluster_graph = new Graph<int>; 5470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_clusters_; ++i) { 5490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_graph->AddVertex(i); 5500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_clusters_; ++i) { 5530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const set<int>& cluster_i = cluster_visibility[i]; 5540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = i+1; j < num_clusters_; ++j) { 5550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> intersection; 5560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const set<int>& cluster_j = cluster_visibility[j]; 5570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_intersection(cluster_i.begin(), cluster_i.end(), 5580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_j.begin(), cluster_j.end(), 5590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong back_inserter(intersection)); 5600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (intersection.size() > 0) { 5620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Clusters interact strongly when they share a large number 5630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // of 3D points. The degree-2 maximum spanning forest 5640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // alorithm, iterates on the edges in decreasing order of 5650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // their weight, which is the number of points shared by the 5660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // two cameras that it connects. 5670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_graph->AddEdge(i, j, intersection.size()); 5680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return cluster_graph; 5720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Canonical views clustering returns a HashMap from vertices to 5750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// cluster ids. Convert this into a flat array for quick lookup. It is 5760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// possible that some of the vertices may not be associated with any 5770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// cluster. In that case, randomly assign them to one of the clusters. 57879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 57979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// The cluster ids can be non-contiguous integers. So as we flatten 58079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// the membership_map, we also map the cluster ids to a contiguous set 58179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// of integers so that the cluster ids are in [0, num_clusters_). 5820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid VisibilityBasedPreconditioner::FlattenMembershipMap( 5830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const HashMap<int, int>& membership_map, 5840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>* membership_vector) const { 5850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(membership_vector)->resize(0); 5860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong membership_vector->resize(num_blocks_, -1); 58779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 58879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez HashMap<int, int> cluster_id_to_index; 5890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the cluster membership map and update the 5900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // cluster_membership_ vector assigning arbitrary cluster ids to 5910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the few cameras that have not been clustered. 5920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (HashMap<int, int>::const_iterator it = membership_map.begin(); 5930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != membership_map.end(); 5940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 5950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int camera_id = it->first; 5960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int cluster_id = it->second; 5970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the view was not clustered, randomly assign it to one of the 5990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // clusters. This preserves the mathematical correctness of the 6000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // preconditioner. If there are too many views which are not 6010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // clustered, it may lead to some quality degradation though. 6020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 6030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // TODO(sameeragarwal): Check if a large number of views have not 6040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // been clustered and deal with it? 6050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cluster_id == -1) { 6060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cluster_id = camera_id % num_clusters_; 6070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 60979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int index = FindWithDefault(cluster_id_to_index, 61079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cluster_id, 61179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cluster_id_to_index.size()); 61279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 61379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (index == cluster_id_to_index.size()) { 61479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cluster_id_to_index[cluster_id] = index; 61579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 61679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 61779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_LT(index, num_clusters_); 61879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez membership_vector->at(camera_id) = index; 6190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 6230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 6241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#endif // CERES_NO_SUITESPARSE 626