MFAS.cpp
Go to the documentation of this file.
1 
8 #include <gtsam/sfm/MFAS.h>
9 
10 #include <algorithm>
11 #include <map>
12 #include <unordered_map>
13 #include <vector>
14 #include <unordered_set>
15 
16 using namespace gtsam;
17 using std::map;
18 using std::pair;
19 using std::unordered_map;
20 using std::vector;
21 using std::unordered_set;
22 
23 // A node in the graph.
24 struct GraphNode {
25  double inWeightSum; // Sum of absolute weights of incoming edges
26  double outWeightSum; // Sum of absolute weights of outgoing edges
27  unordered_set<Key> inNeighbors; // Nodes from which there is an incoming edge
28  unordered_set<Key> outNeighbors; // Nodes to which there is an outgoing edge
29 
30  // Heuristic for the node that is to select nodes in MFAS.
31  double heuristic() const { return (outWeightSum + 1) / (inWeightSum + 1); }
32 };
33 
34 // A graph is a map from key to GraphNode. This function returns the graph from
35 // the edgeWeights between keys.
36 unordered_map<Key, GraphNode> graphFromEdges(
37  const map<MFAS::KeyPair, double>& edgeWeights) {
38  unordered_map<Key, GraphNode> graph;
39 
40  for (const auto& edgeWeight : edgeWeights) {
41  // The weights can be either negative or positive. The direction of the edge
42  // is the direction of positive weight. This means that the edges is from
43  // edge.first -> edge.second if weight is positive and edge.second ->
44  // edge.first if weight is negative.
45  const MFAS::KeyPair& edge = edgeWeight.first;
46  const double& weight = edgeWeight.second;
47 
48  Key edgeSource = weight >= 0 ? edge.first : edge.second;
49  Key edgeDest = weight >= 0 ? edge.second : edge.first;
50 
51  // Update the in weight and neighbors for the destination.
52  graph[edgeDest].inWeightSum += std::abs(weight);
53  graph[edgeDest].inNeighbors.insert(edgeSource);
54 
55  // Update the out weight and neighbors for the source.
56  graph[edgeSource].outWeightSum += std::abs(weight);
57  graph[edgeSource].outNeighbors.insert(edgeDest);
58  }
59  return graph;
60 }
61 
62 // Selects the next node in the ordering from the graph.
63 Key selectNextNodeInOrdering(const unordered_map<Key, GraphNode>& graph) {
64  // Find the root nodes in the graph.
65  for (const auto& keyNode : graph) {
66  // It is a root node if the inWeightSum is close to zero.
67  if (keyNode.second.inWeightSum < 1e-8) {
68  // TODO(akshay-krishnan) if there are multiple roots, it is better to
69  // choose the one with highest heuristic. This is missing in the 1dsfm
70  // solution.
71  return keyNode.first;
72  }
73  }
74  // If there are no root nodes, return the node with the highest heuristic.
75  return std::max_element(graph.begin(), graph.end(),
76  [](const std::pair<Key, GraphNode>& keyNode1,
77  const std::pair<Key, GraphNode>& keyNode2) {
78  return keyNode1.second.heuristic() <
79  keyNode2.second.heuristic();
80  })
81  ->first;
82 }
83 
84 // Returns the absolute weight of the edge between node1 and node2.
85 double absWeightOfEdge(const Key key1, const Key key2,
86  const map<MFAS::KeyPair, double>& edgeWeights) {
87  // Check the direction of the edge before returning.
88  return edgeWeights.find(MFAS::KeyPair(key1, key2)) != edgeWeights.end()
89  ? std::abs(edgeWeights.at(MFAS::KeyPair(key1, key2)))
90  : std::abs(edgeWeights.at(MFAS::KeyPair(key2, key1)));
91 }
92 
93 // Removes a node from the graph and updates edge weights of its neighbors.
94 void removeNodeFromGraph(const Key node,
95  const map<MFAS::KeyPair, double> edgeWeights,
96  unordered_map<Key, GraphNode>& graph) {
97  // Update the outweights and outNeighbors of node's inNeighbors
98  for (const Key neighbor : graph[node].inNeighbors) {
99  // the edge could be either (*it, choice) with a positive weight or
100  // (choice, *it) with a negative weight
101  graph[neighbor].outWeightSum -=
102  absWeightOfEdge(node, neighbor, edgeWeights);
103  graph[neighbor].outNeighbors.erase(node);
104  }
105  // Update the inWeights and inNeighbors of node's outNeighbors
106  for (const Key neighbor : graph[node].outNeighbors) {
107  graph[neighbor].inWeightSum -= absWeightOfEdge(node, neighbor, edgeWeights);
108  graph[neighbor].inNeighbors.erase(node);
109  }
110  // Erase node.
111  graph.erase(node);
112 }
113 
114 MFAS::MFAS(const TranslationEdges& relativeTranslations,
115  const Unit3& projectionDirection) {
116  // Iterate over edges, obtain weights by projecting
117  // their relativeTranslations along the projection direction
118  for (const auto& measurement : relativeTranslations) {
119  edgeWeights_[{measurement.key1(), measurement.key2()}] =
120  measurement.measured().dot(projectionDirection);
121  }
122 }
123 
125  KeyVector ordering; // Nodes in MFAS order (result).
126 
127  // A graph is an unordered map from keys to nodes. Each node contains a list
128  // of its adjacent nodes. Create the graph from the edgeWeights.
129  unordered_map<Key, GraphNode> graph = graphFromEdges(edgeWeights_);
130 
131  // In each iteration, one node is removed from the graph and appended to the
132  // ordering.
133  while (!graph.empty()) {
134  Key selection = selectNextNodeInOrdering(graph);
135  removeNodeFromGraph(selection, edgeWeights_, graph);
136  ordering.push_back(selection);
137  }
138  return ordering;
139 }
140 
141 map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
142  // Find the ordering.
143  KeyVector ordering = computeOrdering();
144 
145  // Create a map from the node key to its position in the ordering. This makes
146  // it easier to lookup positions of different nodes.
147  unordered_map<Key, int> orderingPositions;
148  for (size_t i = 0; i < ordering.size(); i++) {
149  orderingPositions[ordering[i]] = i;
150  }
151 
152  map<KeyPair, double> outlierWeights;
153  // Check if the direction of each edge is consistent with the ordering.
154  for (const auto& edgeWeight : edgeWeights_) {
155  // Find edge source and destination.
156  Key source = edgeWeight.first.first;
157  Key dest = edgeWeight.first.second;
158  if (edgeWeight.second < 0) {
159  std::swap(source, dest);
160  }
161 
162  // If the direction is not consistent with the ordering (i.e dest occurs
163  // before src), it is an outlier edge, and has non-zero outlier weight.
164  if (orderingPositions.at(dest) < orderingPositions.at(source)) {
165  outlierWeights[edgeWeight.first] = std::abs(edgeWeight.second);
166  } else {
167  outlierWeights[edgeWeight.first] = 0;
168  }
169  }
170  return outlierWeights;
171 }
void removeNodeFromGraph(const Key node, const map< MFAS::KeyPair, double > edgeWeights, unordered_map< Key, GraphNode > &graph)
Definition: MFAS.cpp:94
MFAS class to solve Minimum Feedback Arc Set graph problem.
KeyVector computeOrdering() const
Computes the 1D MFAS ordering of nodes in the graph.
Definition: MFAS.cpp:124
unordered_set< Key > outNeighbors
Definition: MFAS.cpp:28
NonlinearFactorGraph graph
static enum @1107 ordering
MFAS(const std::map< KeyPair, double > &edgeWeights)
Construct from the weighted directed edges between the nodes. Each node is identified by a Key...
Definition: MFAS.h:69
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
static Point2 measurement(323.0, 240.0)
const Symbol key1('v', 1)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
double outWeightSum
Definition: MFAS.cpp:26
double absWeightOfEdge(const Key key1, const Key key2, const map< MFAS::KeyPair, double > &edgeWeights)
Definition: MFAS.cpp:85
unordered_set< Key > inNeighbors
Definition: MFAS.cpp:27
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
traits
Definition: chartTesting.h:28
double heuristic() const
Definition: MFAS.cpp:31
std::map< KeyPair, double > computeOutlierWeights() const
Computes the outlier weights of the graph. We define the outlier weight of a edge to be zero if the e...
Definition: MFAS.cpp:141
double inWeightSum
Definition: MFAS.cpp:25
std::vector< BinaryMeasurement< Unit3 > > TranslationEdges
Definition: MFAS.h:56
std::pair< Key, Key > KeyPair
Definition: MFAS.h:55
Key selectNextNodeInOrdering(const unordered_map< Key, GraphNode > &graph)
Definition: MFAS.cpp:63
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
unordered_map< Key, GraphNode > graphFromEdges(const map< MFAS::KeyPair, double > &edgeWeights)
Definition: MFAS.cpp:36
#define abs(x)
Definition: datatypes.h:17
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
const Symbol key2('v', 2)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:55