123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147 |
- #include "ceres/visibility.h"
- #include <algorithm>
- #include <cmath>
- #include <ctime>
- #include <memory>
- #include <set>
- #include <unordered_map>
- #include <utility>
- #include <vector>
- #include "ceres/block_structure.h"
- #include "ceres/graph.h"
- #include "ceres/pair_hash.h"
- #include "glog/logging.h"
- namespace ceres::internal {
- void ComputeVisibility(const CompressedRowBlockStructure& block_structure,
- const int num_eliminate_blocks,
- std::vector<std::set<int>>* visibility) {
- CHECK(visibility != nullptr);
-
-
- visibility->resize(0);
- visibility->resize(block_structure.cols.size() - num_eliminate_blocks);
- for (const auto& row : block_structure.rows) {
- const std::vector<Cell>& cells = row.cells;
- int block_id = cells[0].block_id;
-
- if (block_id >= num_eliminate_blocks) {
- continue;
- }
- for (int j = 1; j < cells.size(); ++j) {
- int camera_block_id = cells[j].block_id - num_eliminate_blocks;
- DCHECK_GE(camera_block_id, 0);
- DCHECK_LT(camera_block_id, visibility->size());
- (*visibility)[camera_block_id].insert(block_id);
- }
- }
- }
- std::unique_ptr<WeightedGraph<int>> CreateSchurComplementGraph(
- const std::vector<std::set<int>>& visibility) {
- const time_t start_time = time(nullptr);
-
-
-
- int num_points = 0;
- for (const auto& visible : visibility) {
- if (!visible.empty()) {
- num_points = std::max(num_points, (*visible.rbegin()) + 1);
- }
- }
-
-
-
-
-
- std::vector<std::set<int>> inverse_visibility(num_points);
- for (int i = 0; i < visibility.size(); i++) {
- const std::set<int>& visibility_set = visibility[i];
- for (int v : visibility_set) {
- inverse_visibility[v].insert(i);
- }
- }
-
-
- std::unordered_map<std::pair<int, int>, int, pair_hash> camera_pairs;
-
- for (const auto& inverse_visibility_set : inverse_visibility) {
- for (auto camera1 = inverse_visibility_set.begin();
- camera1 != inverse_visibility_set.end();
- ++camera1) {
- auto camera2 = camera1;
- for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) {
- ++(camera_pairs[std::make_pair(*camera1, *camera2)]);
- }
- }
- }
- auto graph = std::make_unique<WeightedGraph<int>>();
-
-
-
- static constexpr double kSelfEdgeWeight = 1.0;
- for (int i = 0; i < visibility.size(); ++i) {
- graph->AddVertex(i);
- graph->AddEdge(i, i, kSelfEdgeWeight);
- }
-
- for (const auto& camera_pair_count : camera_pairs) {
- const int camera1 = camera_pair_count.first.first;
- const int camera2 = camera_pair_count.first.second;
- const int count = camera_pair_count.second;
- DCHECK_NE(camera1, camera2);
-
- const double weight =
- static_cast<double>(count) /
- (sqrt(static_cast<double>(visibility[camera1].size() *
- visibility[camera2].size())));
- graph->AddEdge(camera1, camera2, weight);
- }
- VLOG(2) << "Schur complement graph time: " << (time(nullptr) - start_time);
- return graph;
- }
- }
|