1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
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.
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.
29 // Author: sameeragarwal@google.com (Sameer Agarwal)
31 #include "ceres/visibility_based_preconditioner.h"
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/graph.h"
45 #include "ceres/graph_algorithms.h"
46 #include "ceres/internal/scoped_ptr.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"
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.
66 // This will require some more work on the clustering algorithm and
67 // possibly some more refactoring of the code.
68 static const double kCanonicalViewsSizePenaltyWeight = 3.0;
69 static const double kCanonicalViewsSimilarityPenaltyWeight = 0.0;
70 static const double kSingleLinkageMinSimilarity = 0.9;
72 VisibilityBasedPreconditioner::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 atleast 1 f_block for "
82 << "visibility based preconditioning.";
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;
90 const time_t start_time = time(NULL);
91 switch (options_.type) {
93 ComputeClusterJacobiSparsity(bs);
95 case CLUSTER_TRIDIAGONAL:
96 ComputeClusterTridiagonalSparsity(bs);
99 LOG(FATAL) << "Unknown preconditioner type";
101 const time_t structure_time = time(NULL);
103 const time_t storage_time = time(NULL);
105 const time_t eliminator_time = time(NULL);
107 sparse_cholesky_.reset(
108 SparseCholesky::Create(options_.sparse_linear_algebra_library_type, AMD));
110 const time_t init_time = time(NULL);
111 VLOG(2) << "init time: " << init_time - start_time
112 << " structure time: " << structure_time - start_time
113 << " storage time:" << storage_time - structure_time
114 << " eliminator time: " << eliminator_time - storage_time;
117 VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() {}
119 // Determine the sparsity structure of the CLUSTER_JACOBI
120 // preconditioner. It clusters cameras using their scene
121 // visibility. The clusters form the diagonal blocks of the
122 // preconditioner matrix.
123 void VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity(
124 const CompressedRowBlockStructure& bs) {
125 vector<set<int> > visibility;
126 ComputeVisibility(bs, options_.elimination_groups[0], &visibility);
127 CHECK_EQ(num_blocks_, visibility.size());
128 ClusterCameras(visibility);
129 cluster_pairs_.clear();
130 for (int i = 0; i < num_clusters_; ++i) {
131 cluster_pairs_.insert(make_pair(i, i));
135 // Determine the sparsity structure of the CLUSTER_TRIDIAGONAL
136 // preconditioner. It clusters cameras using using the scene
137 // visibility and then finds the strongly interacting pairs of
138 // clusters by constructing another graph with the clusters as
139 // vertices and approximating it with a degree-2 maximum spanning
140 // forest. The set of edges in this forest are the cluster pairs.
141 void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity(
142 const CompressedRowBlockStructure& bs) {
143 vector<set<int> > visibility;
144 ComputeVisibility(bs, options_.elimination_groups[0], &visibility);
145 CHECK_EQ(num_blocks_, visibility.size());
146 ClusterCameras(visibility);
148 // Construct a weighted graph on the set of clusters, where the
149 // edges are the number of 3D points/e_blocks visible in both the
150 // clusters at the ends of the edge. Return an approximate degree-2
151 // maximum spanning forest of this graph.
152 vector<set<int> > cluster_visibility;
153 ComputeClusterVisibility(visibility, &cluster_visibility);
154 scoped_ptr<WeightedGraph<int> > cluster_graph(
155 CHECK_NOTNULL(CreateClusterGraph(cluster_visibility)));
156 scoped_ptr<WeightedGraph<int> > forest(
157 CHECK_NOTNULL(Degree2MaximumSpanningForest(*cluster_graph)));
158 ForestToClusterPairs(*forest, &cluster_pairs_);
161 // Allocate storage for the preconditioner matrix.
162 void VisibilityBasedPreconditioner::InitStorage(
163 const CompressedRowBlockStructure& bs) {
164 ComputeBlockPairsInPreconditioner(bs);
165 m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_));
168 // Call the canonical views algorithm and cluster the cameras based on
169 // their visibility sets. The visibility set of a camera is the set of
170 // e_blocks/3D points in the scene that are seen by it.
172 // The cluster_membership_ vector is updated to indicate cluster
173 // memberships for each camera block.
174 void VisibilityBasedPreconditioner::ClusterCameras(
175 const vector<set<int> >& visibility) {
176 scoped_ptr<WeightedGraph<int> > schur_complement_graph(
177 CHECK_NOTNULL(CreateSchurComplementGraph(visibility)));
179 HashMap<int, int> membership;
181 if (options_.visibility_clustering_type == CANONICAL_VIEWS) {
183 CanonicalViewsClusteringOptions clustering_options;
184 clustering_options.size_penalty_weight = kCanonicalViewsSizePenaltyWeight;
185 clustering_options.similarity_penalty_weight =
186 kCanonicalViewsSimilarityPenaltyWeight;
187 ComputeCanonicalViewsClustering(
188 clustering_options, *schur_complement_graph, ¢ers, &membership);
189 num_clusters_ = centers.size();
190 } else if (options_.visibility_clustering_type == SINGLE_LINKAGE) {
191 SingleLinkageClusteringOptions clustering_options;
192 clustering_options.min_similarity = kSingleLinkageMinSimilarity;
193 num_clusters_ = ComputeSingleLinkageClustering(
194 clustering_options, *schur_complement_graph, &membership);
196 LOG(FATAL) << "Unknown visibility clustering algorithm.";
199 CHECK_GT(num_clusters_, 0);
200 VLOG(2) << "num_clusters: " << num_clusters_;
201 FlattenMembershipMap(membership, &cluster_membership_);
204 // Compute the block sparsity structure of the Schur complement
205 // matrix. For each pair of cameras contributing a non-zero cell to
206 // the schur complement, determine if that cell is present in the
207 // preconditioner or not.
209 // A pair of cameras contribute a cell to the preconditioner if they
210 // are part of the same cluster or if the the two clusters that they
211 // belong have an edge connecting them in the degree-2 maximum
214 // For example, a camera pair (i,j) where i belonges to cluster1 and
215 // j belongs to cluster2 (assume that cluster1 < cluster2).
217 // The cell corresponding to (i,j) is present in the preconditioner
218 // if cluster1 == cluster2 or the pair (cluster1, cluster2) were
219 // connected by an edge in the degree-2 maximum spanning forest.
221 // Since we have already expanded the forest into a set of camera
222 // pairs/edges, including self edges, the check can be reduced to
223 // checking membership of (cluster1, cluster2) in cluster_pairs_.
224 void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner(
225 const CompressedRowBlockStructure& bs) {
226 block_pairs_.clear();
227 for (int i = 0; i < num_blocks_; ++i) {
228 block_pairs_.insert(make_pair(i, i));
232 const int num_row_blocks = bs.rows.size();
233 const int num_eliminate_blocks = options_.elimination_groups[0];
235 // Iterate over each row of the matrix. The block structure of the
236 // matrix is assumed to be sorted in order of the e_blocks/point
237 // blocks. Thus all row blocks containing an e_block/point occur
238 // contiguously. Further, if present, an e_block is always the first
239 // parameter block in each row block. These structural assumptions
240 // are common to all Schur complement based solvers in Ceres.
242 // For each e_block/point block we identify the set of cameras
243 // seeing it. The cross product of this set with itself is the set
244 // of non-zero cells contibuted by this e_block.
246 // The time complexity of this is O(nm^2) where, n is the number of
247 // 3d points and m is the maximum number of cameras seeing any
248 // point, which for most scenes is a fairly small number.
249 while (r < num_row_blocks) {
250 int e_block_id = bs.rows[r].cells.front().block_id;
251 if (e_block_id >= num_eliminate_blocks) {
252 // Skip the rows whose first block is an f_block.
257 for (; r < num_row_blocks; ++r) {
258 const CompressedRow& row = bs.rows[r];
259 if (row.cells.front().block_id != e_block_id) {
263 // Iterate over the blocks in the row, ignoring the first block
264 // since it is the one to be eliminated and adding the rest to
265 // the list of f_blocks associated with this e_block.
266 for (int c = 1; c < row.cells.size(); ++c) {
267 const Cell& cell = row.cells[c];
268 const int f_block_id = cell.block_id - num_eliminate_blocks;
269 CHECK_GE(f_block_id, 0);
270 f_blocks.insert(f_block_id);
274 for (set<int>::const_iterator block1 = f_blocks.begin();
275 block1 != f_blocks.end();
277 set<int>::const_iterator block2 = block1;
279 for (; block2 != f_blocks.end(); ++block2) {
280 if (IsBlockPairInPreconditioner(*block1, *block2)) {
281 block_pairs_.insert(make_pair(*block1, *block2));
287 // The remaining rows which do not contain any e_blocks.
288 for (; r < num_row_blocks; ++r) {
289 const CompressedRow& row = bs.rows[r];
290 CHECK_GE(row.cells.front().block_id, num_eliminate_blocks);
291 for (int i = 0; i < row.cells.size(); ++i) {
292 const int block1 = row.cells[i].block_id - num_eliminate_blocks;
293 for (int j = 0; j < row.cells.size(); ++j) {
294 const int block2 = row.cells[j].block_id - num_eliminate_blocks;
295 if (block1 <= block2) {
296 if (IsBlockPairInPreconditioner(block1, block2)) {
297 block_pairs_.insert(make_pair(block1, block2));
304 VLOG(1) << "Block pair stats: " << block_pairs_.size();
307 // Initialize the SchurEliminator.
308 void VisibilityBasedPreconditioner::InitEliminator(
309 const CompressedRowBlockStructure& bs) {
310 LinearSolver::Options eliminator_options;
311 eliminator_options.elimination_groups = options_.elimination_groups;
312 eliminator_options.num_threads = options_.num_threads;
313 eliminator_options.e_block_size = options_.e_block_size;
314 eliminator_options.f_block_size = options_.f_block_size;
315 eliminator_options.row_block_size = options_.row_block_size;
316 eliminator_.reset(SchurEliminatorBase::Create(eliminator_options));
317 const bool kFullRankETE = true;
319 eliminator_options.elimination_groups[0], kFullRankETE, &bs);
322 // Update the values of the preconditioner matrix and factorize it.
323 bool VisibilityBasedPreconditioner::UpdateImpl(const BlockSparseMatrix& A,
325 const time_t start_time = time(NULL);
326 const int num_rows = m_->num_rows();
327 CHECK_GT(num_rows, 0);
329 // We need a dummy rhs vector and a dummy b vector since the Schur
330 // eliminator combines the computation of the reduced camera matrix
331 // with the computation of the right hand side of that linear
334 // TODO(sameeragarwal): Perhaps its worth refactoring the
335 // SchurEliminator::Eliminate function to allow NULL for the rhs. As
336 // of now it does not seem to be worth the effort.
337 Vector rhs = Vector::Zero(m_->num_rows());
338 Vector b = Vector::Zero(A.num_rows());
340 // Compute a subset of the entries of the Schur complement.
341 eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data());
343 // Try factorizing the matrix. For CLUSTER_JACOBI, this should
344 // always succeed modulo some numerical/conditioning problems. For
345 // CLUSTER_TRIDIAGONAL, in general the preconditioner matrix as
346 // constructed is not positive definite. However, we will go ahead
347 // and try factorizing it. If it works, great, otherwise we scale
348 // all the cells in the preconditioner corresponding to the edges in
349 // the degree-2 forest and that guarantees positive
350 // definiteness. The proof of this fact can be found in Lemma 1 in
351 // "Visibility Based Preconditioning for Bundle Adjustment".
353 // Doing the factorization like this saves us matrix mass when
354 // scaling is not needed, which is quite often in our experience.
355 LinearSolverTerminationType status = Factorize();
357 if (status == LINEAR_SOLVER_FATAL_ERROR) {
361 // The scaling only affects the tri-diagonal case, since
362 // ScaleOffDiagonalBlocks only pays attenion to the cells that
363 // belong to the edges of the degree-2 forest. In the CLUSTER_JACOBI
364 // case, the preconditioner is guaranteed to be positive
366 if (status == LINEAR_SOLVER_FAILURE && options_.type == CLUSTER_TRIDIAGONAL) {
367 VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal "
369 ScaleOffDiagonalCells();
370 status = Factorize();
373 VLOG(2) << "Compute time: " << time(NULL) - start_time;
374 return (status == LINEAR_SOLVER_SUCCESS);
377 // Consider the preconditioner matrix as meta-block matrix, whose
378 // blocks correspond to the clusters. Then cluster pairs corresponding
379 // to edges in the degree-2 forest are off diagonal entries of this
380 // matrix. Scaling these off-diagonal entries by 1/2 forces this
381 // matrix to be positive definite.
382 void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() {
383 for (set<pair<int, int> >::const_iterator it = block_pairs_.begin();
384 it != block_pairs_.end();
386 const int block1 = it->first;
387 const int block2 = it->second;
388 if (!IsBlockPairOffDiagonal(block1, block2)) {
392 int r, c, row_stride, col_stride;
393 CellInfo* cell_info =
394 m_->GetCell(block1, block2, &r, &c, &row_stride, &col_stride);
395 CHECK(cell_info != NULL)
396 << "Cell missing for block pair (" << block1 << "," << block2 << ")"
397 << " cluster pair (" << cluster_membership_[block1] << " "
398 << cluster_membership_[block2] << ")";
400 // Ah the magic of tri-diagonal matrices and diagonal
401 // dominance. See Lemma 1 in "Visibility Based Preconditioning
402 // For Bundle Adjustment".
403 MatrixRef m(cell_info->values, row_stride, col_stride);
404 m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5;
408 // Compute the sparse Cholesky factorization of the preconditioner
410 LinearSolverTerminationType VisibilityBasedPreconditioner::Factorize() {
411 // Extract the TripletSparseMatrix that is used for actually storing
412 // S and convert it into a CompressedRowSparseMatrix.
413 const TripletSparseMatrix* tsm =
414 down_cast<BlockRandomAccessSparseMatrix*>(m_.get())->mutable_matrix();
416 scoped_ptr<CompressedRowSparseMatrix> lhs;
417 const CompressedRowSparseMatrix::StorageType storage_type =
418 sparse_cholesky_->StorageType();
419 if (storage_type == CompressedRowSparseMatrix::UPPER_TRIANGULAR) {
420 lhs.reset(CompressedRowSparseMatrix::FromTripletSparseMatrix(*tsm));
421 lhs->set_storage_type(CompressedRowSparseMatrix::UPPER_TRIANGULAR);
424 CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(*tsm));
425 lhs->set_storage_type(CompressedRowSparseMatrix::LOWER_TRIANGULAR);
429 return sparse_cholesky_->Factorize(lhs.get(), &message);
432 void VisibilityBasedPreconditioner::RightMultiply(const double* x,
436 CHECK_NOTNULL(sparse_cholesky_.get());
438 sparse_cholesky_->Solve(x, y, &message);
441 int VisibilityBasedPreconditioner::num_rows() const { return m_->num_rows(); }
443 // Classify camera/f_block pairs as in and out of the preconditioner,
444 // based on whether the cluster pair that they belong to is in the
445 // preconditioner or not.
446 bool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner(
447 const int block1, const int block2) const {
448 int cluster1 = cluster_membership_[block1];
449 int cluster2 = cluster_membership_[block2];
450 if (cluster1 > cluster2) {
451 swap(cluster1, cluster2);
453 return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0);
456 bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal(
457 const int block1, const int block2) const {
458 return (cluster_membership_[block1] != cluster_membership_[block2]);
461 // Convert a graph into a list of edges that includes self edges for
463 void VisibilityBasedPreconditioner::ForestToClusterPairs(
464 const WeightedGraph<int>& forest,
465 HashSet<pair<int, int> >* cluster_pairs) const {
466 CHECK_NOTNULL(cluster_pairs)->clear();
467 const HashSet<int>& vertices = forest.vertices();
468 CHECK_EQ(vertices.size(), num_clusters_);
470 // Add all the cluster pairs corresponding to the edges in the
472 for (HashSet<int>::const_iterator it1 = vertices.begin();
473 it1 != vertices.end();
475 const int cluster1 = *it1;
476 cluster_pairs->insert(make_pair(cluster1, cluster1));
477 const HashSet<int>& neighbors = forest.Neighbors(cluster1);
478 for (HashSet<int>::const_iterator it2 = neighbors.begin();
479 it2 != neighbors.end();
481 const int cluster2 = *it2;
482 if (cluster1 < cluster2) {
483 cluster_pairs->insert(make_pair(cluster1, cluster2));
489 // The visibilty set of a cluster is the union of the visibilty sets
490 // of all its cameras. In other words, the set of points visible to
491 // any camera in the cluster.
492 void VisibilityBasedPreconditioner::ComputeClusterVisibility(
493 const vector<set<int> >& visibility,
494 vector<set<int> >* cluster_visibility) const {
495 CHECK_NOTNULL(cluster_visibility)->resize(0);
496 cluster_visibility->resize(num_clusters_);
497 for (int i = 0; i < num_blocks_; ++i) {
498 const int cluster_id = cluster_membership_[i];
499 (*cluster_visibility)[cluster_id].insert(visibility[i].begin(),
500 visibility[i].end());
504 // Construct a graph whose vertices are the clusters, and the edge
505 // weights are the number of 3D points visible to cameras in both the
507 WeightedGraph<int>* VisibilityBasedPreconditioner::CreateClusterGraph(
508 const vector<set<int> >& cluster_visibility) const {
509 WeightedGraph<int>* cluster_graph = new WeightedGraph<int>;
511 for (int i = 0; i < num_clusters_; ++i) {
512 cluster_graph->AddVertex(i);
515 for (int i = 0; i < num_clusters_; ++i) {
516 const set<int>& cluster_i = cluster_visibility[i];
517 for (int j = i + 1; j < num_clusters_; ++j) {
518 vector<int> intersection;
519 const set<int>& cluster_j = cluster_visibility[j];
520 set_intersection(cluster_i.begin(),
524 back_inserter(intersection));
526 if (intersection.size() > 0) {
527 // Clusters interact strongly when they share a large number
528 // of 3D points. The degree-2 maximum spanning forest
529 // alorithm, iterates on the edges in decreasing order of
530 // their weight, which is the number of points shared by the
531 // two cameras that it connects.
532 cluster_graph->AddEdge(i, j, intersection.size());
536 return cluster_graph;
539 // Canonical views clustering returns a HashMap from vertices to
540 // cluster ids. Convert this into a flat array for quick lookup. It is
541 // possible that some of the vertices may not be associated with any
542 // cluster. In that case, randomly assign them to one of the clusters.
544 // The cluster ids can be non-contiguous integers. So as we flatten
545 // the membership_map, we also map the cluster ids to a contiguous set
546 // of integers so that the cluster ids are in [0, num_clusters_).
547 void VisibilityBasedPreconditioner::FlattenMembershipMap(
548 const HashMap<int, int>& membership_map,
549 vector<int>* membership_vector) const {
550 CHECK_NOTNULL(membership_vector)->resize(0);
551 membership_vector->resize(num_blocks_, -1);
553 HashMap<int, int> cluster_id_to_index;
554 // Iterate over the cluster membership map and update the
555 // cluster_membership_ vector assigning arbitrary cluster ids to
556 // the few cameras that have not been clustered.
557 for (HashMap<int, int>::const_iterator it = membership_map.begin();
558 it != membership_map.end();
560 const int camera_id = it->first;
561 int cluster_id = it->second;
563 // If the view was not clustered, randomly assign it to one of the
564 // clusters. This preserves the mathematical correctness of the
565 // preconditioner. If there are too many views which are not
566 // clustered, it may lead to some quality degradation though.
568 // TODO(sameeragarwal): Check if a large number of views have not
569 // been clustered and deal with it?
570 if (cluster_id == -1) {
571 cluster_id = camera_id % num_clusters_;
574 const int index = FindWithDefault(
575 cluster_id_to_index, cluster_id, cluster_id_to_index.size());
577 if (index == cluster_id_to_index.size()) {
578 cluster_id_to_index[cluster_id] = index;
581 CHECK_LT(index, num_clusters_);
582 membership_vector->at(camera_id) = index;
586 } // namespace internal