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: kushalav (at) google.com (Avanish Kushal) 30 31 #include <cmath> 32 #include <ctime> 33 #include <algorithm> 34 #include <set> 35 #include <vector> 36 #include <utility> 37 #include "ceres/block_structure.h" 38 #include "ceres/collections_port.h" 39 #include "ceres/graph.h" 40 #include "glog/logging.h" 41 42 namespace ceres { 43 namespace internal { 44 45 void ComputeVisibility(const CompressedRowBlockStructure& block_structure, 46 const int num_eliminate_blocks, 47 vector< set<int> >* visibility) { 48 CHECK_NOTNULL(visibility); 49 50 // Clear the visibility vector and resize it to hold a 51 // vector for each camera. 52 visibility->resize(0); 53 visibility->resize(block_structure.cols.size() - num_eliminate_blocks); 54 55 for (int i = 0; i < block_structure.rows.size(); ++i) { 56 const vector<Cell>& cells = block_structure.rows[i].cells; 57 int block_id = cells[0].block_id; 58 // If the first block is not an e_block, then skip this row block. 59 if (block_id >= num_eliminate_blocks) { 60 continue; 61 } 62 63 for (int j = 1; j < cells.size(); ++j) { 64 int camera_block_id = cells[j].block_id - num_eliminate_blocks; 65 DCHECK_GE(camera_block_id, 0); 66 DCHECK_LT(camera_block_id, visibility->size()); 67 (*visibility)[camera_block_id].insert(block_id); 68 } 69 } 70 } 71 72 Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { 73 const time_t start_time = time(NULL); 74 // Compute the number of e_blocks/point blocks. Since the visibility 75 // set for each e_block/camera contains the set of e_blocks/points 76 // visible to it, we find the maximum across all visibility sets. 77 int num_points = 0; 78 for (int i = 0; i < visibility.size(); i++) { 79 if (visibility[i].size() > 0) { 80 num_points = max(num_points, (*visibility[i].rbegin()) + 1); 81 } 82 } 83 84 // Invert the visibility. The input is a camera->point mapping, 85 // which tells us which points are visible in which 86 // cameras. However, to compute the sparsity structure of the Schur 87 // Complement efficiently, its better to have the point->camera 88 // mapping. 89 vector<set<int> > inverse_visibility(num_points); 90 for (int i = 0; i < visibility.size(); i++) { 91 const set<int>& visibility_set = visibility[i]; 92 for (set<int>::const_iterator it = visibility_set.begin(); 93 it != visibility_set.end(); 94 ++it) { 95 inverse_visibility[*it].insert(i); 96 } 97 } 98 99 // Map from camera pairs to number of points visible to both cameras 100 // in the pair. 101 HashMap<pair<int, int>, int > camera_pairs; 102 103 // Count the number of points visible to each camera/f_block pair. 104 for (vector<set<int> >::const_iterator it = inverse_visibility.begin(); 105 it != inverse_visibility.end(); 106 ++it) { 107 const set<int>& inverse_visibility_set = *it; 108 for (set<int>::const_iterator camera1 = inverse_visibility_set.begin(); 109 camera1 != inverse_visibility_set.end(); 110 ++camera1) { 111 set<int>::const_iterator camera2 = camera1; 112 for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) { 113 ++(camera_pairs[make_pair(*camera1, *camera2)]); 114 } 115 } 116 } 117 118 Graph<int>* graph = new Graph<int>(); 119 120 // Add vertices and initialize the pairs for self edges so that self 121 // edges are guaranteed. This is needed for the Canonical views 122 // algorithm to work correctly. 123 static const double kSelfEdgeWeight = 1.0; 124 for (int i = 0; i < visibility.size(); ++i) { 125 graph->AddVertex(i); 126 graph->AddEdge(i, i, kSelfEdgeWeight); 127 } 128 129 // Add an edge for each camera pair. 130 for (HashMap<pair<int, int>, int>::const_iterator it = camera_pairs.begin(); 131 it != camera_pairs.end(); 132 ++it) { 133 const int camera1 = it->first.first; 134 const int camera2 = it->first.second; 135 CHECK_NE(camera1, camera2); 136 137 const int count = it->second; 138 // Static cast necessary for Windows. 139 const double weight = static_cast<double>(count) / 140 (sqrt(static_cast<double>(visibility[camera1].size() * visibility[camera2].size()))); 141 graph->AddEdge(camera1, camera2, weight); 142 } 143 144 VLOG(2) << "Schur complement graph time: " << (time(NULL) - start_time); 145 return graph; 146 } 147 148 } // namespace internal 149 } // namespace ceres 150
