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