10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: kushalav@google.com (Avanish Kushal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// This include must come before any #ifndef check on Ceres compile options. 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/internal/port.h" 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#ifndef CERES_NO_SUITESPARSE 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/visibility.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cmath> 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <ctime> 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <set> 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <utility> 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_structure.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/collections_port.h" 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/graph.h" 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ComputeVisibility(const CompressedRowBlockStructure& block_structure, 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_eliminate_blocks, 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector< set<int> >* visibility) { 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(visibility); 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Clear the visibility vector and resize it to hold a 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // vector for each camera. 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong visibility->resize(0); 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong visibility->resize(block_structure.cols.size() - num_eliminate_blocks); 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < block_structure.rows.size(); ++i) { 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<Cell>& cells = block_structure.rows[i].cells; 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int block_id = cells[0].block_id; 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the first block is not an e_block, then skip this row block. 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (block_id >= num_eliminate_blocks) { 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong continue; 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 1; j < cells.size(); ++j) { 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int camera_block_id = cells[j].block_id - num_eliminate_blocks; 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK_GE(camera_block_id, 0); 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK_LT(camera_block_id, visibility->size()); 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (*visibility)[camera_block_id].insert(block_id); 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongGraph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const time_t start_time = time(NULL); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute the number of e_blocks/point blocks. Since the visibility 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // set for each e_block/camera contains the set of e_blocks/points 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // visible to it, we find the maximum across all visibility sets. 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_points = 0; 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < visibility.size(); i++) { 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (visibility[i].size() > 0) { 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_points = max(num_points, (*visibility[i].rbegin()) + 1); 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Invert the visibility. The input is a camera->point mapping, 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // which tells us which points are visible in which 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // cameras. However, to compute the sparsity structure of the Schur 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Complement efficiently, its better to have the point->camera 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // mapping. 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<set<int> > inverse_visibility(num_points); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < visibility.size(); i++) { 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const set<int>& visibility_set = visibility[i]; 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (set<int>::const_iterator it = visibility_set.begin(); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != visibility_set.end(); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inverse_visibility[*it].insert(i); 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Map from camera pairs to number of points visible to both cameras 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // in the pair. 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong HashMap<pair<int, int>, int > camera_pairs; 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Count the number of points visible to each camera/f_block pair. 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (vector<set<int> >::const_iterator it = inverse_visibility.begin(); 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != inverse_visibility.end(); 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const set<int>& inverse_visibility_set = *it; 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (set<int>::const_iterator camera1 = inverse_visibility_set.begin(); 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong camera1 != inverse_visibility_set.end(); 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++camera1) { 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set<int>::const_iterator camera2 = camera1; 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) { 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++(camera_pairs[make_pair(*camera1, *camera2)]); 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Graph<int>* graph = new Graph<int>(); 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add vertices and initialize the pairs for self edges so that self 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // edges are guaranteed. This is needed for the Canonical views 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // algorithm to work correctly. 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const double kSelfEdgeWeight = 1.0; 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < visibility.size(); ++i) { 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong graph->AddVertex(i); 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong graph->AddEdge(i, i, kSelfEdgeWeight); 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add an edge for each camera pair. 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (HashMap<pair<int, int>, int>::const_iterator it = camera_pairs.begin(); 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong it != camera_pairs.end(); 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++it) { 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int camera1 = it->first.first; 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int camera2 = it->first.second; 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NE(camera1, camera2); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int count = it->second; 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Static cast necessary for Windows. 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double weight = static_cast<double>(count) / 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (sqrt(static_cast<double>( 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling visibility[camera1].size() * visibility[camera2].size()))); 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong graph->AddEdge(camera1, camera2, weight); 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "Schur complement graph time: " << (time(NULL) - start_time); 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return graph; 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 159399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#endif // CERES_NO_SUITESPARSE 160