internal/ceres/visibility.cc

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