12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixes1

release/4.3a0
akrishnan86 2020-09-12 14:22:00 -07:00
parent 43af7c4ae4
commit 0d41941cda
1 changed files with 32 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
gtsam/sfm/MFAS.cpp
View File

@ -11,20 +11,21 @@
#include <map>
#include <unordered_map>
#include <vector>
#include <unordered_set>
using namespace gtsam;
using MFAS::KeyPair;
using std::map;
using std::pair;
using std::unordered_map;
using std::vector;
using std::unordered_set;
// A node in the graph.
struct GraphNode {
double inWeightSum; // Sum of absolute weights of incoming edges.
double outWeightSum; // Sum of absolute weights of outgoing edges.
vector<Key> inNeighbors; // Nodes from which there is an incoming edge.
vector<Key> outNeighbors; // Nodes to which there is an outgoing edge.
double inWeightSum; // Sum of absolute weights of incoming edges
double outWeightSum; // Sum of absolute weights of outgoing edges
unordered_set<Key> inNeighbors; // Nodes from which there is an incoming edge
unordered_set<Key> outNeighbors; // Nodes to which there is an outgoing edge
// Heuristic for the node that is to select nodes in MFAS.
double heuristic() { return (outWeightSum + 1) / (inWeightSum + 1); }
@ -33,24 +34,27 @@ struct GraphNode {
// A graph is a map from key to GraphNode. This function returns the graph from
// the edgeWeights between keys.
unordered_map<Key, GraphNode> graphFromEdges(
const map<KeyPair, double>& edgeWeights) {
const map<MFAS::KeyPair, double>& edgeWeights) {
unordered_map<Key, GraphNode> graph;
for (const auto& [edge, weight] : edgeWeights) {
for (const auto& edgeWeight : edgeWeights) {
// The weights can be either negative or positive. The direction of the edge
// is the direction of positive weight. This means that the edges is from
// edge.first -> edge.second if weight is positive and edge.second ->
// edge.first if weight is negative.
MFAS::KeyPair& edge = edgeWeight.first;
double& weight = edgeWeight.second;
Key edgeSource = weight >= 0 ? edge.first : edge.second;
Key edgeDest = weight >= 0 ? edge.second : edge.first;
// Update the in weight and neighbors for the destination.
graph[edgeDest].inWeightSum += std::abs(weight);
graph[edgeDest].inNeighbors.push_back(edgeSource);
graph[edgeDest].inNeighbors.insert(edgeSource);
// Update the out weight and neighbors for the source.
graph[edgeSource].outWeightSum += std::abs(weight);
graph[edgeSource].outNeighbors.push_back(edgeDest);
graph[edgeSource].outNeighbors.insert(edgeDest);
}
return graph;
}
@ -58,36 +62,37 @@ unordered_map<Key, GraphNode> graphFromEdges(
// Selects the next node in the ordering from the graph.
Key selectNextNodeInOrdering(const unordered_map<Key, GraphNode>& graph) {
// Find the root nodes in the graph.
for (const auto& [key, node] : graph) {
for (const auto& keyNode : graph) {
// It is a root node if the inWeightSum is close to zero.
if (node.inWeightSum < 1e-8) {
if (keyNode.second.inWeightSum < 1e-8) {
// TODO(akshay-krishnan) if there are multiple roots, it is better to
// choose the one with highest heuristic. This is missing in the 1dsfm
// solution.
return key;
return keyNode.first;
}
}
// If there are no root nodes, return the node with the highest heuristic.
return std::max_element(graph.begin(), graph.end(),
[](const std::pair<Key, GraphNode>& node1,
const std::pair<Key, GraphNode>& node2) {
return node1.second.heuristic() <
node2.second.heuristic();
[](const std::pair<Key, GraphNode>& keyNode1,
const std::pair<Key, GraphNode>& keyNode2) {
return keyNode1.second.heuristic() <
keyNode2.second.heuristic();
})
->first;
}
// Returns the absolute weight of the edge between node1 and node2.
double absWeightOfEdge(const Key node1, const Key node2,
const map<KeyPair, double>& edgeWeights) {
const map<MFAS::KeyPair, double>& edgeWeights) {
// Check the direction of the edge before returning.
return edgeWeights_.find(KeyPair(node1, node2)) != edgeWeights_.end()
? std::abs(edgeWeights_.at(KeyPair(node1, node2)))
: std::abs(edgeWeights_.at(KeyPair(node2, node1)));
return edgeWeights_.find(MFAS::KeyPair(node1, node2)) != edgeWeights_.end()
? std::abs(edgeWeights_.at(MFAS::KeyPair(node1, node2)))
: std::abs(edgeWeights_.at(MFAS::KeyPair(node2, node1)));
}
// Removes a node from the graph and updates edge weights of its neighbors.
void removeNodeFromGraph(const Key node, const map<KeyPair, double> edgeWeights,
void removeNodeFromGraph(const Key node,
const map<MFAS::KeyPair, double> edgeWeights,
unordered_map<Key, GraphNode>& graph) {
// Update the outweights and outNeighbors of node's inNeighbors
for (const Key neighbor : graph[node].inNeighbors) {
@ -95,12 +100,12 @@ void removeNodeFromGraph(const Key node, const map<KeyPair, double> edgeWeights,
// (choice, *it) with a negative weight
graph[neighbor].outWeightSum -=
absWeightOfEdge(node, neighbor, edgeWeights);
graph[neighbor].outNeighbors.erase(graph[neighbor].outNeighbors.find(node));
graph[neighbor].outNeighbors.erase(node);
}
// Update the inWeights and inNeighbors of node's outNeighbors
for (const Key neighbor : graph[node].outNeighbors) {
graph[neighbor].inWeightSum -= absWeightOfEdge(node, neighbor, edgeWeights);
graph[neighbor].inNeighbors.erase(graph[neighbor].inNeighbors.find(node));
graph[neighbor].inNeighbors.erase(node);
}
// Erase node.
graph.erase(node);
@ -148,10 +153,10 @@ std::map<KeyPair, double> MFAS::computeOutlierWeights() const {
map<KeyPair, double> outlierWeights;
// Check if the direction of each edge is consistent with the ordering.
for (const auto& [edge, weight] : edgeWeights_) {
for (const auto& edgeWeight : edgeWeights_) {
// Find edge source and destination.
Key source = edge.first;
Key dest = edge.second;
Key source = edgeWeight.first.first;
Key dest = edgeWeight.first.second;
if (weight < 0) {
std::swap(source, dest);
}
@ -159,7 +164,7 @@ std::map<KeyPair, double> MFAS::computeOutlierWeights() const {
// If the direction is not consistent with the ordering (i.e dest occurs
// before src), it is an outlier edge, and has non-zero outlier weight.
if (orderingPositions.at(dest) < orderingPositions.at(source)) {
outlierWeights[edge] = std::abs(weight);
outlierWeights[edge] = std::abs(edgeWeight.second);
} else {
outlierWeights[edge] = 0;
}