blob: ed4afb64cee328cb3f73cfc43010c9751a17a81b [file] [log] [blame]
Austin Schuh70cc9552019-01-21 19:46:48 -08001// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2015 Google Inc. All rights reserved.
3// http://ceres-solver.org/
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 <memory>
37#include <set>
38#include <utility>
39#include <vector>
40
41#include "Eigen/Dense"
42#include "ceres/block_random_access_sparse_matrix.h"
43#include "ceres/block_sparse_matrix.h"
44#include "ceres/canonical_views_clustering.h"
45#include "ceres/graph.h"
46#include "ceres/graph_algorithms.h"
47#include "ceres/linear_solver.h"
48#include "ceres/schur_eliminator.h"
49#include "ceres/single_linkage_clustering.h"
50#include "ceres/visibility.h"
51#include "glog/logging.h"
52
53namespace ceres {
54namespace internal {
55
56using std::make_pair;
57using std::pair;
58using std::set;
59using std::swap;
60using std::vector;
61
62// TODO(sameeragarwal): Currently these are magic weights for the
63// preconditioner construction. Move these higher up into the Options
64// struct and provide some guidelines for choosing them.
65//
66// This will require some more work on the clustering algorithm and
67// possibly some more refactoring of the code.
68static const double kCanonicalViewsSizePenaltyWeight = 3.0;
69static const double kCanonicalViewsSimilarityPenaltyWeight = 0.0;
70static const double kSingleLinkageMinSimilarity = 0.9;
71
72VisibilityBasedPreconditioner::VisibilityBasedPreconditioner(
73 const CompressedRowBlockStructure& bs,
74 const Preconditioner::Options& options)
75 : options_(options), num_blocks_(0), num_clusters_(0) {
76 CHECK_GT(options_.elimination_groups.size(), 1);
77 CHECK_GT(options_.elimination_groups[0], 0);
78 CHECK(options_.type == CLUSTER_JACOBI || options_.type == CLUSTER_TRIDIAGONAL)
79 << "Unknown preconditioner type: " << options_.type;
80 num_blocks_ = bs.cols.size() - options_.elimination_groups[0];
81 CHECK_GT(num_blocks_, 0) << "Jacobian should have at least 1 f_block for "
82 << "visibility based preconditioning.";
83 CHECK(options_.context != NULL);
84
85 // Vector of camera block sizes
86 block_size_.resize(num_blocks_);
87 for (int i = 0; i < num_blocks_; ++i) {
88 block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size;
89 }
90
91 const time_t start_time = time(NULL);
92 switch (options_.type) {
93 case CLUSTER_JACOBI:
94 ComputeClusterJacobiSparsity(bs);
95 break;
96 case CLUSTER_TRIDIAGONAL:
97 ComputeClusterTridiagonalSparsity(bs);
98 break;
99 default:
100 LOG(FATAL) << "Unknown preconditioner type";
101 }
102 const time_t structure_time = time(NULL);
103 InitStorage(bs);
104 const time_t storage_time = time(NULL);
105 InitEliminator(bs);
106 const time_t eliminator_time = time(NULL);
107
108 LinearSolver::Options sparse_cholesky_options;
109 sparse_cholesky_options.sparse_linear_algebra_library_type =
110 options_.sparse_linear_algebra_library_type;
111
112 // The preconditioner's sparsity is not available in the
113 // preprocessor, so the columns of the Jacobian have not been
114 // reordered to minimize fill in when computing its sparse Cholesky
115 // factorization. So we must tell the SparseCholesky object to
116 // perform approximate minimum-degree reordering, which is done by
117 // setting use_postordering to true.
118 sparse_cholesky_options.use_postordering = true;
119 sparse_cholesky_ = SparseCholesky::Create(sparse_cholesky_options);
120
121 const time_t init_time = time(NULL);
122 VLOG(2) << "init time: " << init_time - start_time
123 << " structure time: " << structure_time - start_time
124 << " storage time:" << storage_time - structure_time
125 << " eliminator time: " << eliminator_time - storage_time;
126}
127
128VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() {}
129
130// Determine the sparsity structure of the CLUSTER_JACOBI
131// preconditioner. It clusters cameras using their scene
132// visibility. The clusters form the diagonal blocks of the
133// preconditioner matrix.
134void VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity(
135 const CompressedRowBlockStructure& bs) {
136 vector<set<int>> visibility;
137 ComputeVisibility(bs, options_.elimination_groups[0], &visibility);
138 CHECK_EQ(num_blocks_, visibility.size());
139 ClusterCameras(visibility);
140 cluster_pairs_.clear();
141 for (int i = 0; i < num_clusters_; ++i) {
142 cluster_pairs_.insert(make_pair(i, i));
143 }
144}
145
146// Determine the sparsity structure of the CLUSTER_TRIDIAGONAL
147// preconditioner. It clusters cameras using using the scene
148// visibility and then finds the strongly interacting pairs of
149// clusters by constructing another graph with the clusters as
150// vertices and approximating it with a degree-2 maximum spanning
151// forest. The set of edges in this forest are the cluster pairs.
152void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity(
153 const CompressedRowBlockStructure& bs) {
154 vector<set<int>> visibility;
155 ComputeVisibility(bs, options_.elimination_groups[0], &visibility);
156 CHECK_EQ(num_blocks_, visibility.size());
157 ClusterCameras(visibility);
158
159 // Construct a weighted graph on the set of clusters, where the
160 // edges are the number of 3D points/e_blocks visible in both the
161 // clusters at the ends of the edge. Return an approximate degree-2
162 // maximum spanning forest of this graph.
163 vector<set<int>> cluster_visibility;
164 ComputeClusterVisibility(visibility, &cluster_visibility);
165 std::unique_ptr<WeightedGraph<int>> cluster_graph(
166 CreateClusterGraph(cluster_visibility));
167 CHECK(cluster_graph != nullptr);
168 std::unique_ptr<WeightedGraph<int>> forest(
169 Degree2MaximumSpanningForest(*cluster_graph));
170 CHECK(forest != nullptr);
171 ForestToClusterPairs(*forest, &cluster_pairs_);
172}
173
174// Allocate storage for the preconditioner matrix.
175void VisibilityBasedPreconditioner::InitStorage(
176 const CompressedRowBlockStructure& bs) {
177 ComputeBlockPairsInPreconditioner(bs);
178 m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_));
179}
180
181// Call the canonical views algorithm and cluster the cameras based on
182// their visibility sets. The visibility set of a camera is the set of
183// e_blocks/3D points in the scene that are seen by it.
184//
185// The cluster_membership_ vector is updated to indicate cluster
186// memberships for each camera block.
187void VisibilityBasedPreconditioner::ClusterCameras(
188 const vector<set<int> >& visibility) {
189 std::unique_ptr<WeightedGraph<int>> schur_complement_graph(
190 CreateSchurComplementGraph(visibility));
191 CHECK(schur_complement_graph != nullptr);
192
193 std::unordered_map<int, int> membership;
194
195 if (options_.visibility_clustering_type == CANONICAL_VIEWS) {
196 vector<int> centers;
197 CanonicalViewsClusteringOptions clustering_options;
198 clustering_options.size_penalty_weight = kCanonicalViewsSizePenaltyWeight;
199 clustering_options.similarity_penalty_weight =
200 kCanonicalViewsSimilarityPenaltyWeight;
201 ComputeCanonicalViewsClustering(
202 clustering_options, *schur_complement_graph, &centers, &membership);
203 num_clusters_ = centers.size();
204 } else if (options_.visibility_clustering_type == SINGLE_LINKAGE) {
205 SingleLinkageClusteringOptions clustering_options;
206 clustering_options.min_similarity = kSingleLinkageMinSimilarity;
207 num_clusters_ = ComputeSingleLinkageClustering(
208 clustering_options, *schur_complement_graph, &membership);
209 } else {
210 LOG(FATAL) << "Unknown visibility clustering algorithm.";
211 }
212
213 CHECK_GT(num_clusters_, 0);
214 VLOG(2) << "num_clusters: " << num_clusters_;
215 FlattenMembershipMap(membership, &cluster_membership_);
216}
217
218// Compute the block sparsity structure of the Schur complement
219// matrix. For each pair of cameras contributing a non-zero cell to
220// the schur complement, determine if that cell is present in the
221// preconditioner or not.
222//
223// A pair of cameras contribute a cell to the preconditioner if they
224// are part of the same cluster or if the two clusters that they
225// belong have an edge connecting them in the degree-2 maximum
226// spanning forest.
227//
228// For example, a camera pair (i,j) where i belongs to cluster1 and
229// j belongs to cluster2 (assume that cluster1 < cluster2).
230//
231// The cell corresponding to (i,j) is present in the preconditioner
232// if cluster1 == cluster2 or the pair (cluster1, cluster2) were
233// connected by an edge in the degree-2 maximum spanning forest.
234//
235// Since we have already expanded the forest into a set of camera
236// pairs/edges, including self edges, the check can be reduced to
237// checking membership of (cluster1, cluster2) in cluster_pairs_.
238void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner(
239 const CompressedRowBlockStructure& bs) {
240 block_pairs_.clear();
241 for (int i = 0; i < num_blocks_; ++i) {
242 block_pairs_.insert(make_pair(i, i));
243 }
244
245 int r = 0;
246 const int num_row_blocks = bs.rows.size();
247 const int num_eliminate_blocks = options_.elimination_groups[0];
248
249 // Iterate over each row of the matrix. The block structure of the
250 // matrix is assumed to be sorted in order of the e_blocks/point
251 // blocks. Thus all row blocks containing an e_block/point occur
252 // contiguously. Further, if present, an e_block is always the first
253 // parameter block in each row block. These structural assumptions
254 // are common to all Schur complement based solvers in Ceres.
255 //
256 // For each e_block/point block we identify the set of cameras
257 // seeing it. The cross product of this set with itself is the set
258 // of non-zero cells contributed by this e_block.
259 //
260 // The time complexity of this is O(nm^2) where, n is the number of
261 // 3d points and m is the maximum number of cameras seeing any
262 // point, which for most scenes is a fairly small number.
263 while (r < num_row_blocks) {
264 int e_block_id = bs.rows[r].cells.front().block_id;
265 if (e_block_id >= num_eliminate_blocks) {
266 // Skip the rows whose first block is an f_block.
267 break;
268 }
269
270 set<int> f_blocks;
271 for (; r < num_row_blocks; ++r) {
272 const CompressedRow& row = bs.rows[r];
273 if (row.cells.front().block_id != e_block_id) {
274 break;
275 }
276
277 // Iterate over the blocks in the row, ignoring the first block
278 // since it is the one to be eliminated and adding the rest to
279 // the list of f_blocks associated with this e_block.
280 for (int c = 1; c < row.cells.size(); ++c) {
281 const Cell& cell = row.cells[c];
282 const int f_block_id = cell.block_id - num_eliminate_blocks;
283 CHECK_GE(f_block_id, 0);
284 f_blocks.insert(f_block_id);
285 }
286 }
287
288 for (set<int>::const_iterator block1 = f_blocks.begin();
289 block1 != f_blocks.end();
290 ++block1) {
291 set<int>::const_iterator block2 = block1;
292 ++block2;
293 for (; block2 != f_blocks.end(); ++block2) {
294 if (IsBlockPairInPreconditioner(*block1, *block2)) {
295 block_pairs_.insert(make_pair(*block1, *block2));
296 }
297 }
298 }
299 }
300
301 // The remaining rows which do not contain any e_blocks.
302 for (; r < num_row_blocks; ++r) {
303 const CompressedRow& row = bs.rows[r];
304 CHECK_GE(row.cells.front().block_id, num_eliminate_blocks);
305 for (int i = 0; i < row.cells.size(); ++i) {
306 const int block1 = row.cells[i].block_id - num_eliminate_blocks;
307 for (int j = 0; j < row.cells.size(); ++j) {
308 const int block2 = row.cells[j].block_id - num_eliminate_blocks;
309 if (block1 <= block2) {
310 if (IsBlockPairInPreconditioner(block1, block2)) {
311 block_pairs_.insert(make_pair(block1, block2));
312 }
313 }
314 }
315 }
316 }
317
318 VLOG(1) << "Block pair stats: " << block_pairs_.size();
319}
320
321// Initialize the SchurEliminator.
322void VisibilityBasedPreconditioner::InitEliminator(
323 const CompressedRowBlockStructure& bs) {
324 LinearSolver::Options eliminator_options;
325 eliminator_options.elimination_groups = options_.elimination_groups;
326 eliminator_options.num_threads = options_.num_threads;
327 eliminator_options.e_block_size = options_.e_block_size;
328 eliminator_options.f_block_size = options_.f_block_size;
329 eliminator_options.row_block_size = options_.row_block_size;
330 eliminator_options.context = options_.context;
331 eliminator_.reset(SchurEliminatorBase::Create(eliminator_options));
332 const bool kFullRankETE = true;
333 eliminator_->Init(
334 eliminator_options.elimination_groups[0], kFullRankETE, &bs);
335}
336
337// Update the values of the preconditioner matrix and factorize it.
338bool VisibilityBasedPreconditioner::UpdateImpl(const BlockSparseMatrix& A,
339 const double* D) {
340 const time_t start_time = time(NULL);
341 const int num_rows = m_->num_rows();
342 CHECK_GT(num_rows, 0);
343
344 // Compute a subset of the entries of the Schur complement.
345 eliminator_->Eliminate(&A, nullptr, D, m_.get(), nullptr);
346
347 // Try factorizing the matrix. For CLUSTER_JACOBI, this should
348 // always succeed modulo some numerical/conditioning problems. For
349 // CLUSTER_TRIDIAGONAL, in general the preconditioner matrix as
350 // constructed is not positive definite. However, we will go ahead
351 // and try factorizing it. If it works, great, otherwise we scale
352 // all the cells in the preconditioner corresponding to the edges in
353 // the degree-2 forest and that guarantees positive
354 // definiteness. The proof of this fact can be found in Lemma 1 in
355 // "Visibility Based Preconditioning for Bundle Adjustment".
356 //
357 // Doing the factorization like this saves us matrix mass when
358 // scaling is not needed, which is quite often in our experience.
359 LinearSolverTerminationType status = Factorize();
360
361 if (status == LINEAR_SOLVER_FATAL_ERROR) {
362 return false;
363 }
364
365 // The scaling only affects the tri-diagonal case, since
366 // ScaleOffDiagonalBlocks only pays attention to the cells that
367 // belong to the edges of the degree-2 forest. In the CLUSTER_JACOBI
368 // case, the preconditioner is guaranteed to be positive
369 // semidefinite.
370 if (status == LINEAR_SOLVER_FAILURE && options_.type == CLUSTER_TRIDIAGONAL) {
371 VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal "
372 << "scaling";
373 ScaleOffDiagonalCells();
374 status = Factorize();
375 }
376
377 VLOG(2) << "Compute time: " << time(NULL) - start_time;
378 return (status == LINEAR_SOLVER_SUCCESS);
379}
380
381// Consider the preconditioner matrix as meta-block matrix, whose
382// blocks correspond to the clusters. Then cluster pairs corresponding
383// to edges in the degree-2 forest are off diagonal entries of this
384// matrix. Scaling these off-diagonal entries by 1/2 forces this
385// matrix to be positive definite.
386void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() {
387 for (const auto& block_pair : block_pairs_) {
388 const int block1 = block_pair.first;
389 const int block2 = block_pair.second;
390 if (!IsBlockPairOffDiagonal(block1, block2)) {
391 continue;
392 }
393
394 int r, c, row_stride, col_stride;
395 CellInfo* cell_info =
396 m_->GetCell(block1, block2, &r, &c, &row_stride, &col_stride);
397 CHECK(cell_info != NULL)
398 << "Cell missing for block pair (" << block1 << "," << block2 << ")"
399 << " cluster pair (" << cluster_membership_[block1] << " "
400 << cluster_membership_[block2] << ")";
401
402 // Ah the magic of tri-diagonal matrices and diagonal
403 // dominance. See Lemma 1 in "Visibility Based Preconditioning
404 // For Bundle Adjustment".
405 MatrixRef m(cell_info->values, row_stride, col_stride);
406 m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5;
407 }
408}
409
410// Compute the sparse Cholesky factorization of the preconditioner
411// matrix.
412LinearSolverTerminationType VisibilityBasedPreconditioner::Factorize() {
413 // Extract the TripletSparseMatrix that is used for actually storing
414 // S and convert it into a CompressedRowSparseMatrix.
415 const TripletSparseMatrix* tsm =
416 down_cast<BlockRandomAccessSparseMatrix*>(m_.get())->mutable_matrix();
417
418 std::unique_ptr<CompressedRowSparseMatrix> lhs;
419 const CompressedRowSparseMatrix::StorageType storage_type =
420 sparse_cholesky_->StorageType();
421 if (storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR) {
422 lhs.reset(CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm));
423 lhs->set_storage_type(CompressedRowSparseMatrix::UPPER_TRIANGULAR);
424 } else {
425 lhs.reset(
426 CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm));
427 lhs->set_storage_type(CompressedRowSparseMatrix::LOWER_TRIANGULAR);
428 }
429
430 std::string message;
431 return sparse_cholesky_->Factorize(lhs.get(), &message);
432}
433
434void VisibilityBasedPreconditioner::RightMultiply(const double* x,
435 double* y) const {
436 CHECK(x != nullptr);
437 CHECK(y != nullptr);
438 CHECK(sparse_cholesky_ != nullptr);
439 std::string message;
440 sparse_cholesky_->Solve(x, y, &message);
441}
442
443int VisibilityBasedPreconditioner::num_rows() const { return m_->num_rows(); }
444
445// Classify camera/f_block pairs as in and out of the preconditioner,
446// based on whether the cluster pair that they belong to is in the
447// preconditioner or not.
448bool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner(
449 const int block1, const int block2) const {
450 int cluster1 = cluster_membership_[block1];
451 int cluster2 = cluster_membership_[block2];
452 if (cluster1 > cluster2) {
453 swap(cluster1, cluster2);
454 }
455 return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0);
456}
457
458bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal(
459 const int block1, const int block2) const {
460 return (cluster_membership_[block1] != cluster_membership_[block2]);
461}
462
463// Convert a graph into a list of edges that includes self edges for
464// each vertex.
465void VisibilityBasedPreconditioner::ForestToClusterPairs(
466 const WeightedGraph<int>& forest,
467 std::unordered_set<pair<int, int>, pair_hash >* cluster_pairs) const {
468 CHECK(cluster_pairs != nullptr);
469 cluster_pairs->clear();
470 const std::unordered_set<int>& vertices = forest.vertices();
471 CHECK_EQ(vertices.size(), num_clusters_);
472
473 // Add all the cluster pairs corresponding to the edges in the
474 // forest.
475 for (const int cluster1 : vertices) {
476 cluster_pairs->insert(make_pair(cluster1, cluster1));
477 const std::unordered_set<int>& neighbors = forest.Neighbors(cluster1);
478 for (const int cluster2 : neighbors) {
479 if (cluster1 < cluster2) {
480 cluster_pairs->insert(make_pair(cluster1, cluster2));
481 }
482 }
483 }
484}
485
486// The visibility set of a cluster is the union of the visibility sets
487// of all its cameras. In other words, the set of points visible to
488// any camera in the cluster.
489void VisibilityBasedPreconditioner::ComputeClusterVisibility(
490 const vector<set<int>>& visibility,
491 vector<set<int>>* cluster_visibility) const {
492 CHECK(cluster_visibility != nullptr);
493 cluster_visibility->resize(0);
494 cluster_visibility->resize(num_clusters_);
495 for (int i = 0; i < num_blocks_; ++i) {
496 const int cluster_id = cluster_membership_[i];
497 (*cluster_visibility)[cluster_id].insert(visibility[i].begin(),
498 visibility[i].end());
499 }
500}
501
502// Construct a graph whose vertices are the clusters, and the edge
503// weights are the number of 3D points visible to cameras in both the
504// vertices.
505WeightedGraph<int>* VisibilityBasedPreconditioner::CreateClusterGraph(
506 const vector<set<int>>& cluster_visibility) const {
507 WeightedGraph<int>* cluster_graph = new WeightedGraph<int>;
508
509 for (int i = 0; i < num_clusters_; ++i) {
510 cluster_graph->AddVertex(i);
511 }
512
513 for (int i = 0; i < num_clusters_; ++i) {
514 const set<int>& cluster_i = cluster_visibility[i];
515 for (int j = i + 1; j < num_clusters_; ++j) {
516 vector<int> intersection;
517 const set<int>& cluster_j = cluster_visibility[j];
518 set_intersection(cluster_i.begin(),
519 cluster_i.end(),
520 cluster_j.begin(),
521 cluster_j.end(),
522 back_inserter(intersection));
523
524 if (intersection.size() > 0) {
525 // Clusters interact strongly when they share a large number
526 // of 3D points. The degree-2 maximum spanning forest
527 // algorithm, iterates on the edges in decreasing order of
528 // their weight, which is the number of points shared by the
529 // two cameras that it connects.
530 cluster_graph->AddEdge(i, j, intersection.size());
531 }
532 }
533 }
534 return cluster_graph;
535}
536
537// Canonical views clustering returns a std::unordered_map from vertices to
538// cluster ids. Convert this into a flat array for quick lookup. It is
539// possible that some of the vertices may not be associated with any
540// cluster. In that case, randomly assign them to one of the clusters.
541//
542// The cluster ids can be non-contiguous integers. So as we flatten
543// the membership_map, we also map the cluster ids to a contiguous set
544// of integers so that the cluster ids are in [0, num_clusters_).
545void VisibilityBasedPreconditioner::FlattenMembershipMap(
546 const std::unordered_map<int, int>& membership_map,
547 vector<int>* membership_vector) const {
548 CHECK(membership_vector != nullptr);
549 membership_vector->resize(0);
550 membership_vector->resize(num_blocks_, -1);
551
552 std::unordered_map<int, int> cluster_id_to_index;
553 // Iterate over the cluster membership map and update the
554 // cluster_membership_ vector assigning arbitrary cluster ids to
555 // the few cameras that have not been clustered.
556 for (const auto& m : membership_map) {
557 const int camera_id = m.first;
558 int cluster_id = m.second;
559
560 // If the view was not clustered, randomly assign it to one of the
561 // clusters. This preserves the mathematical correctness of the
562 // preconditioner. If there are too many views which are not
563 // clustered, it may lead to some quality degradation though.
564 //
565 // TODO(sameeragarwal): Check if a large number of views have not
566 // been clustered and deal with it?
567 if (cluster_id == -1) {
568 cluster_id = camera_id % num_clusters_;
569 }
570
571 const int index = FindWithDefault(
572 cluster_id_to_index, cluster_id, cluster_id_to_index.size());
573
574 if (index == cluster_id_to_index.size()) {
575 cluster_id_to_index[cluster_id] = index;
576 }
577
578 CHECK_LT(index, num_clusters_);
579 membership_vector->at(camera_id) = index;
580 }
581}
582
583} // namespace internal
584} // namespace ceres