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@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
53namespace ceres {
54namespace 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.
62static const double kSizePenaltyWeight = 3.0;
63static const double kSimilarityPenaltyWeight = 0.0;
64
65#ifndef CERES_NO_SUITESPARSE
66VisibilityBasedPreconditioner::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
123VisibilityBasedPreconditioner::~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.
138void 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.
156void 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.
174void 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.
195void 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.
207void 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                                  &centers,
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_.
248void 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.
332void 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.
349bool 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.
404void 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.
433bool 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
464void 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
477int 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.
484bool 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
495bool 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.
503void 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.
532void 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.
547Graph<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.
581void 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