1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: sameeragarwal (at) google.com (Sameer Agarwal) 30 31 #include "ceres/visibility_based_preconditioner.h" 32 33 #include <algorithm> 34 #include <functional> 35 #include <iterator> 36 #include <set> 37 #include <utility> 38 #include <vector> 39 #include "Eigen/Dense" 40 #include "ceres/block_random_access_sparse_matrix.h" 41 #include "ceres/block_sparse_matrix.h" 42 #include "ceres/canonical_views_clustering.h" 43 #include "ceres/collections_port.h" 44 #include "ceres/detect_structure.h" 45 #include "ceres/graph.h" 46 #include "ceres/graph_algorithms.h" 47 #include "ceres/internal/scoped_ptr.h" 48 #include "ceres/linear_solver.h" 49 #include "ceres/schur_eliminator.h" 50 #include "ceres/visibility.h" 51 #include "glog/logging.h" 52 53 namespace ceres { 54 namespace internal { 55 56 // TODO(sameeragarwal): Currently these are magic weights for the 57 // preconditioner construction. Move these higher up into the Options 58 // struct and provide some guidelines for choosing them. 59 // 60 // This will require some more work on the clustering algorithm and 61 // possibly some more refactoring of the code. 62 static const double kSizePenaltyWeight = 3.0; 63 static const double kSimilarityPenaltyWeight = 0.0; 64 65 #ifndef CERES_NO_SUITESPARSE 66 VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( 67 const CompressedRowBlockStructure& bs, 68 const LinearSolver::Options& options) 69 : options_(options), 70 num_blocks_(0), 71 num_clusters_(0), 72 factor_(NULL) { 73 CHECK_GT(options_.elimination_groups.size(), 1); 74 CHECK_GT(options_.elimination_groups[0], 0); 75 CHECK(options_.preconditioner_type == SCHUR_JACOBI || 76 options_.preconditioner_type == CLUSTER_JACOBI || 77 options_.preconditioner_type == CLUSTER_TRIDIAGONAL) 78 << "Unknown preconditioner type: " << options_.preconditioner_type; 79 num_blocks_ = bs.cols.size() - options_.elimination_groups[0]; 80 CHECK_GT(num_blocks_, 0) 81 << "Jacobian should have atleast 1 f_block for " 82 << "visibility based preconditioning."; 83 84 // Vector of camera block sizes 85 block_size_.resize(num_blocks_); 86 for (int i = 0; i < num_blocks_; ++i) { 87 block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size; 88 } 89 90 const time_t start_time = time(NULL); 91 switch (options_.preconditioner_type) { 92 case SCHUR_JACOBI: 93 ComputeSchurJacobiSparsity(bs); 94 break; 95 case CLUSTER_JACOBI: 96 ComputeClusterJacobiSparsity(bs); 97 break; 98 case CLUSTER_TRIDIAGONAL: 99 ComputeClusterTridiagonalSparsity(bs); 100 break; 101 default: 102 LOG(FATAL) << "Unknown preconditioner type"; 103 } 104 const time_t structure_time = time(NULL); 105 InitStorage(bs); 106 const time_t storage_time = time(NULL); 107 InitEliminator(bs); 108 const time_t eliminator_time = time(NULL); 109 110 // Allocate temporary storage for a vector used during 111 // RightMultiply. 112 tmp_rhs_ = CHECK_NOTNULL(ss_.CreateDenseVector(NULL, 113 m_->num_rows(), 114 m_->num_rows())); 115 const time_t init_time = time(NULL); 116 VLOG(2) << "init time: " 117 << init_time - start_time 118 << " structure time: " << structure_time - start_time 119 << " storage time:" << storage_time - structure_time 120 << " eliminator time: " << eliminator_time - storage_time; 121 } 122 123 VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() { 124 if (factor_ != NULL) { 125 ss_.Free(factor_); 126 factor_ = NULL; 127 } 128 if (tmp_rhs_ != NULL) { 129 ss_.Free(tmp_rhs_); 130 tmp_rhs_ = NULL; 131 } 132 } 133 134 // Determine the sparsity structure of the SCHUR_JACOBI 135 // preconditioner. SCHUR_JACOBI is an extreme case of a visibility 136 // based preconditioner where each camera block corresponds to a 137 // cluster and there is no interaction between clusters. 138 void VisibilityBasedPreconditioner::ComputeSchurJacobiSparsity( 139 const CompressedRowBlockStructure& bs) { 140 num_clusters_ = num_blocks_; 141 cluster_membership_.resize(num_blocks_); 142 cluster_pairs_.clear(); 143 144 // Each camea block is a member of its own cluster and the only 145 // cluster pairs are the self edges (i,i). 146 for (int i = 0; i < num_clusters_; ++i) { 147 cluster_membership_[i] = i; 148 cluster_pairs_.insert(make_pair(i, i)); 149 } 150 } 151 152 // Determine the sparsity structure of the CLUSTER_JACOBI 153 // preconditioner. It clusters cameras using their scene 154 // visibility. The clusters form the diagonal blocks of the 155 // preconditioner matrix. 156 void VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity( 157 const CompressedRowBlockStructure& bs) { 158 vector<set<int> > visibility; 159 ComputeVisibility(bs, options_.elimination_groups[0], &visibility); 160 CHECK_EQ(num_blocks_, visibility.size()); 161 ClusterCameras(visibility); 162 cluster_pairs_.clear(); 163 for (int i = 0; i < num_clusters_; ++i) { 164 cluster_pairs_.insert(make_pair(i, i)); 165 } 166 } 167 168 // Determine the sparsity structure of the CLUSTER_TRIDIAGONAL 169 // preconditioner. It clusters cameras using using the scene 170 // visibility and then finds the strongly interacting pairs of 171 // clusters by constructing another graph with the clusters as 172 // vertices and approximating it with a degree-2 maximum spanning 173 // forest. The set of edges in this forest are the cluster pairs. 174 void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( 175 const CompressedRowBlockStructure& bs) { 176 vector<set<int> > visibility; 177 ComputeVisibility(bs, options_.elimination_groups[0], &visibility); 178 CHECK_EQ(num_blocks_, visibility.size()); 179 ClusterCameras(visibility); 180 181 // Construct a weighted graph on the set of clusters, where the 182 // edges are the number of 3D points/e_blocks visible in both the 183 // clusters at the ends of the edge. Return an approximate degree-2 184 // maximum spanning forest of this graph. 185 vector<set<int> > cluster_visibility; 186 ComputeClusterVisibility(visibility, &cluster_visibility); 187 scoped_ptr<Graph<int> > cluster_graph( 188 CHECK_NOTNULL(CreateClusterGraph(cluster_visibility))); 189 scoped_ptr<Graph<int> > forest( 190 CHECK_NOTNULL(Degree2MaximumSpanningForest(*cluster_graph))); 191 ForestToClusterPairs(*forest, &cluster_pairs_); 192 } 193 194 // Allocate storage for the preconditioner matrix. 195 void VisibilityBasedPreconditioner::InitStorage( 196 const CompressedRowBlockStructure& bs) { 197 ComputeBlockPairsInPreconditioner(bs); 198 m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_)); 199 } 200 201 // Call the canonical views algorithm and cluster the cameras based on 202 // their visibility sets. The visibility set of a camera is the set of 203 // e_blocks/3D points in the scene that are seen by it. 204 // 205 // The cluster_membership_ vector is updated to indicate cluster 206 // memberships for each camera block. 207 void VisibilityBasedPreconditioner::ClusterCameras( 208 const vector<set<int> >& visibility) { 209 scoped_ptr<Graph<int> > schur_complement_graph( 210 CHECK_NOTNULL(CreateSchurComplementGraph(visibility))); 211 212 CanonicalViewsClusteringOptions options; 213 options.size_penalty_weight = kSizePenaltyWeight; 214 options.similarity_penalty_weight = kSimilarityPenaltyWeight; 215 216 vector<int> centers; 217 HashMap<int, int> membership; 218 ComputeCanonicalViewsClustering(*schur_complement_graph, 219 options, 220 ¢ers, 221 &membership); 222 num_clusters_ = centers.size(); 223 CHECK_GT(num_clusters_, 0); 224 VLOG(2) << "num_clusters: " << num_clusters_; 225 FlattenMembershipMap(membership, &cluster_membership_); 226 } 227 228 // Compute the block sparsity structure of the Schur complement 229 // matrix. For each pair of cameras contributing a non-zero cell to 230 // the schur complement, determine if that cell is present in the 231 // preconditioner or not. 232 // 233 // A pair of cameras contribute a cell to the preconditioner if they 234 // are part of the same cluster or if the the two clusters that they 235 // belong have an edge connecting them in the degree-2 maximum 236 // spanning forest. 237 // 238 // For example, a camera pair (i,j) where i belonges to cluster1 and 239 // j belongs to cluster2 (assume that cluster1 < cluster2). 240 // 241 // The cell corresponding to (i,j) is present in the preconditioner 242 // if cluster1 == cluster2 or the pair (cluster1, cluster2) were 243 // connected by an edge in the degree-2 maximum spanning forest. 244 // 245 // Since we have already expanded the forest into a set of camera 246 // pairs/edges, including self edges, the check can be reduced to 247 // checking membership of (cluster1, cluster2) in cluster_pairs_. 248 void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( 249 const CompressedRowBlockStructure& bs) { 250 block_pairs_.clear(); 251 for (int i = 0; i < num_blocks_; ++i) { 252 block_pairs_.insert(make_pair(i, i)); 253 } 254 255 int r = 0; 256 const int num_row_blocks = bs.rows.size(); 257 const int num_eliminate_blocks = options_.elimination_groups[0]; 258 259 // Iterate over each row of the matrix. The block structure of the 260 // matrix is assumed to be sorted in order of the e_blocks/point 261 // blocks. Thus all row blocks containing an e_block/point occur 262 // contiguously. Further, if present, an e_block is always the first 263 // parameter block in each row block. These structural assumptions 264 // are common to all Schur complement based solvers in Ceres. 265 // 266 // For each e_block/point block we identify the set of cameras 267 // seeing it. The cross product of this set with itself is the set 268 // of non-zero cells contibuted by this e_block. 269 // 270 // The time complexity of this is O(nm^2) where, n is the number of 271 // 3d points and m is the maximum number of cameras seeing any 272 // point, which for most scenes is a fairly small number. 273 while (r < num_row_blocks) { 274 int e_block_id = bs.rows[r].cells.front().block_id; 275 if (e_block_id >= num_eliminate_blocks) { 276 // Skip the rows whose first block is an f_block. 277 break; 278 } 279 280 set<int> f_blocks; 281 for (; r < num_row_blocks; ++r) { 282 const CompressedRow& row = bs.rows[r]; 283 if (row.cells.front().block_id != e_block_id) { 284 break; 285 } 286 287 // Iterate over the blocks in the row, ignoring the first block 288 // since it is the one to be eliminated and adding the rest to 289 // the list of f_blocks associated with this e_block. 290 for (int c = 1; c < row.cells.size(); ++c) { 291 const Cell& cell = row.cells[c]; 292 const int f_block_id = cell.block_id - num_eliminate_blocks; 293 CHECK_GE(f_block_id, 0); 294 f_blocks.insert(f_block_id); 295 } 296 } 297 298 for (set<int>::const_iterator block1 = f_blocks.begin(); 299 block1 != f_blocks.end(); 300 ++block1) { 301 set<int>::const_iterator block2 = block1; 302 ++block2; 303 for (; block2 != f_blocks.end(); ++block2) { 304 if (IsBlockPairInPreconditioner(*block1, *block2)) { 305 block_pairs_.insert(make_pair(*block1, *block2)); 306 } 307 } 308 } 309 } 310 311 // The remaining rows which do not contain any e_blocks. 312 for (; r < num_row_blocks; ++r) { 313 const CompressedRow& row = bs.rows[r]; 314 CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); 315 for (int i = 0; i < row.cells.size(); ++i) { 316 const int block1 = row.cells[i].block_id - num_eliminate_blocks; 317 for (int j = 0; j < row.cells.size(); ++j) { 318 const int block2 = row.cells[j].block_id - num_eliminate_blocks; 319 if (block1 <= block2) { 320 if (IsBlockPairInPreconditioner(block1, block2)) { 321 block_pairs_.insert(make_pair(block1, block2)); 322 } 323 } 324 } 325 } 326 } 327 328 VLOG(1) << "Block pair stats: " << block_pairs_.size(); 329 } 330 331 // Initialize the SchurEliminator. 332 void VisibilityBasedPreconditioner::InitEliminator( 333 const CompressedRowBlockStructure& bs) { 334 LinearSolver::Options eliminator_options; 335 336 eliminator_options.elimination_groups = options_.elimination_groups; 337 eliminator_options.num_threads = options_.num_threads; 338 339 DetectStructure(bs, options_.elimination_groups[0], 340 &eliminator_options.row_block_size, 341 &eliminator_options.e_block_size, 342 &eliminator_options.f_block_size); 343 344 eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); 345 eliminator_->Init(options_.elimination_groups[0], &bs); 346 } 347 348 // Update the values of the preconditioner matrix and factorize it. 349 bool VisibilityBasedPreconditioner::Update(const BlockSparseMatrixBase& A, 350 const double* D) { 351 const time_t start_time = time(NULL); 352 const int num_rows = m_->num_rows(); 353 CHECK_GT(num_rows, 0); 354 355 // We need a dummy rhs vector and a dummy b vector since the Schur 356 // eliminator combines the computation of the reduced camera matrix 357 // with the computation of the right hand side of that linear 358 // system. 359 // 360 // TODO(sameeragarwal): Perhaps its worth refactoring the 361 // SchurEliminator::Eliminate function to allow NULL for the rhs. As 362 // of now it does not seem to be worth the effort. 363 Vector rhs = Vector::Zero(m_->num_rows()); 364 Vector b = Vector::Zero(A.num_rows()); 365 366 // Compute a subset of the entries of the Schur complement. 367 eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); 368 369 // Try factorizing the matrix. For SCHUR_JACOBI and CLUSTER_JACOBI, 370 // this should always succeed modulo some numerical/conditioning 371 // problems. For CLUSTER_TRIDIAGONAL, in general the preconditioner 372 // matrix as constructed is not positive definite. However, we will 373 // go ahead and try factorizing it. If it works, great, otherwise we 374 // scale all the cells in the preconditioner corresponding to the 375 // edges in the degree-2 forest and that guarantees positive 376 // definiteness. The proof of this fact can be found in Lemma 1 in 377 // "Visibility Based Preconditioning for Bundle Adjustment". 378 // 379 // Doing the factorization like this saves us matrix mass when 380 // scaling is not needed, which is quite often in our experience. 381 bool status = Factorize(); 382 383 // The scaling only affects the tri-diagonal case, since 384 // ScaleOffDiagonalBlocks only pays attenion to the cells that 385 // belong to the edges of the degree-2 forest. In the SCHUR_JACOBI 386 // and the CLUSTER_JACOBI cases, the preconditioner is guaranteed to 387 // be positive semidefinite. 388 if (!status && options_.preconditioner_type == CLUSTER_TRIDIAGONAL) { 389 VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal " 390 << "scaling"; 391 ScaleOffDiagonalCells(); 392 status = Factorize(); 393 } 394 395 VLOG(2) << "Compute time: " << time(NULL) - start_time; 396 return status; 397 } 398 399 // Consider the preconditioner matrix as meta-block matrix, whose 400 // blocks correspond to the clusters. Then cluster pairs corresponding 401 // to edges in the degree-2 forest are off diagonal entries of this 402 // matrix. Scaling these off-diagonal entries by 1/2 forces this 403 // matrix to be positive definite. 404 void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() { 405 for (set< pair<int, int> >::const_iterator it = block_pairs_.begin(); 406 it != block_pairs_.end(); 407 ++it) { 408 const int block1 = it->first; 409 const int block2 = it->second; 410 if (!IsBlockPairOffDiagonal(block1, block2)) { 411 continue; 412 } 413 414 int r, c, row_stride, col_stride; 415 CellInfo* cell_info = m_->GetCell(block1, block2, 416 &r, &c, 417 &row_stride, &col_stride); 418 CHECK(cell_info != NULL) 419 << "Cell missing for block pair (" << block1 << "," << block2 << ")" 420 << " cluster pair (" << cluster_membership_[block1] 421 << " " << cluster_membership_[block2] << ")"; 422 423 // Ah the magic of tri-diagonal matrices and diagonal 424 // dominance. See Lemma 1 in "Visibility Based Preconditioning 425 // For Bundle Adjustment". 426 MatrixRef m(cell_info->values, row_stride, col_stride); 427 m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5; 428 } 429 } 430 431 // Compute the sparse Cholesky factorization of the preconditioner 432 // matrix. 433 bool VisibilityBasedPreconditioner::Factorize() { 434 // Extract the TripletSparseMatrix that is used for actually storing 435 // S and convert it into a cholmod_sparse object. 436 cholmod_sparse* lhs = ss_.CreateSparseMatrix( 437 down_cast<BlockRandomAccessSparseMatrix*>( 438 m_.get())->mutable_matrix()); 439 440 // The matrix is symmetric, and the upper triangular part of the 441 // matrix contains the values. 442 lhs->stype = 1; 443 444 // Symbolic factorization is computed if we don't already have one handy. 445 if (factor_ == NULL) { 446 if (options_.use_block_amd) { 447 factor_ = ss_.BlockAnalyzeCholesky(lhs, block_size_, block_size_); 448 } else { 449 factor_ = ss_.AnalyzeCholesky(lhs); 450 } 451 452 if (VLOG_IS_ON(2)) { 453 cholmod_print_common("Symbolic Analysis", ss_.mutable_cc()); 454 } 455 } 456 457 CHECK_NOTNULL(factor_); 458 459 bool status = ss_.Cholesky(lhs, factor_); 460 ss_.Free(lhs); 461 return status; 462 } 463 464 void VisibilityBasedPreconditioner::RightMultiply(const double* x, 465 double* y) const { 466 CHECK_NOTNULL(x); 467 CHECK_NOTNULL(y); 468 SuiteSparse* ss = const_cast<SuiteSparse*>(&ss_); 469 470 const int num_rows = m_->num_rows(); 471 memcpy(CHECK_NOTNULL(tmp_rhs_)->x, x, m_->num_rows() * sizeof(*x)); 472 cholmod_dense* solution = CHECK_NOTNULL(ss->Solve(factor_, tmp_rhs_)); 473 memcpy(y, solution->x, sizeof(*y) * num_rows); 474 ss->Free(solution); 475 } 476 477 int VisibilityBasedPreconditioner::num_rows() const { 478 return m_->num_rows(); 479 } 480 481 // Classify camera/f_block pairs as in and out of the preconditioner, 482 // based on whether the cluster pair that they belong to is in the 483 // preconditioner or not. 484 bool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner( 485 const int block1, 486 const int block2) const { 487 int cluster1 = cluster_membership_[block1]; 488 int cluster2 = cluster_membership_[block2]; 489 if (cluster1 > cluster2) { 490 std::swap(cluster1, cluster2); 491 } 492 return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0); 493 } 494 495 bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal( 496 const int block1, 497 const int block2) const { 498 return (cluster_membership_[block1] != cluster_membership_[block2]); 499 } 500 501 // Convert a graph into a list of edges that includes self edges for 502 // each vertex. 503 void VisibilityBasedPreconditioner::ForestToClusterPairs( 504 const Graph<int>& forest, 505 HashSet<pair<int, int> >* cluster_pairs) const { 506 CHECK_NOTNULL(cluster_pairs)->clear(); 507 const HashSet<int>& vertices = forest.vertices(); 508 CHECK_EQ(vertices.size(), num_clusters_); 509 510 // Add all the cluster pairs corresponding to the edges in the 511 // forest. 512 for (HashSet<int>::const_iterator it1 = vertices.begin(); 513 it1 != vertices.end(); 514 ++it1) { 515 const int cluster1 = *it1; 516 cluster_pairs->insert(make_pair(cluster1, cluster1)); 517 const HashSet<int>& neighbors = forest.Neighbors(cluster1); 518 for (HashSet<int>::const_iterator it2 = neighbors.begin(); 519 it2 != neighbors.end(); 520 ++it2) { 521 const int cluster2 = *it2; 522 if (cluster1 < cluster2) { 523 cluster_pairs->insert(make_pair(cluster1, cluster2)); 524 } 525 } 526 } 527 } 528 529 // The visibilty set of a cluster is the union of the visibilty sets 530 // of all its cameras. In other words, the set of points visible to 531 // any camera in the cluster. 532 void VisibilityBasedPreconditioner::ComputeClusterVisibility( 533 const vector<set<int> >& visibility, 534 vector<set<int> >* cluster_visibility) const { 535 CHECK_NOTNULL(cluster_visibility)->resize(0); 536 cluster_visibility->resize(num_clusters_); 537 for (int i = 0; i < num_blocks_; ++i) { 538 const int cluster_id = cluster_membership_[i]; 539 (*cluster_visibility)[cluster_id].insert(visibility[i].begin(), 540 visibility[i].end()); 541 } 542 } 543 544 // Construct a graph whose vertices are the clusters, and the edge 545 // weights are the number of 3D points visible to cameras in both the 546 // vertices. 547 Graph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( 548 const vector<set<int> >& cluster_visibility) const { 549 Graph<int>* cluster_graph = new Graph<int>; 550 551 for (int i = 0; i < num_clusters_; ++i) { 552 cluster_graph->AddVertex(i); 553 } 554 555 for (int i = 0; i < num_clusters_; ++i) { 556 const set<int>& cluster_i = cluster_visibility[i]; 557 for (int j = i+1; j < num_clusters_; ++j) { 558 vector<int> intersection; 559 const set<int>& cluster_j = cluster_visibility[j]; 560 set_intersection(cluster_i.begin(), cluster_i.end(), 561 cluster_j.begin(), cluster_j.end(), 562 back_inserter(intersection)); 563 564 if (intersection.size() > 0) { 565 // Clusters interact strongly when they share a large number 566 // of 3D points. The degree-2 maximum spanning forest 567 // alorithm, iterates on the edges in decreasing order of 568 // their weight, which is the number of points shared by the 569 // two cameras that it connects. 570 cluster_graph->AddEdge(i, j, intersection.size()); 571 } 572 } 573 } 574 return cluster_graph; 575 } 576 577 // Canonical views clustering returns a HashMap from vertices to 578 // cluster ids. Convert this into a flat array for quick lookup. It is 579 // possible that some of the vertices may not be associated with any 580 // cluster. In that case, randomly assign them to one of the clusters. 581 void VisibilityBasedPreconditioner::FlattenMembershipMap( 582 const HashMap<int, int>& membership_map, 583 vector<int>* membership_vector) const { 584 CHECK_NOTNULL(membership_vector)->resize(0); 585 membership_vector->resize(num_blocks_, -1); 586 // Iterate over the cluster membership map and update the 587 // cluster_membership_ vector assigning arbitrary cluster ids to 588 // the few cameras that have not been clustered. 589 for (HashMap<int, int>::const_iterator it = membership_map.begin(); 590 it != membership_map.end(); 591 ++it) { 592 const int camera_id = it->first; 593 int cluster_id = it->second; 594 595 // If the view was not clustered, randomly assign it to one of the 596 // clusters. This preserves the mathematical correctness of the 597 // preconditioner. If there are too many views which are not 598 // clustered, it may lead to some quality degradation though. 599 // 600 // TODO(sameeragarwal): Check if a large number of views have not 601 // been clustered and deal with it? 602 if (cluster_id == -1) { 603 cluster_id = camera_id % num_clusters_; 604 } 605 606 membership_vector->at(camera_id) = cluster_id; 607 } 608 } 609 610 #endif // CERES_NO_SUITESPARSE 611 612 } // namespace internal 613 } // namespace ceres 614