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