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