diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index dff8b6fa5..a48482768 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -11,20 +11,21 @@ #include #include #include +#include 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 inNeighbors; // Nodes from which there is an incoming edge. - vector 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 inNeighbors; // Nodes from which there is an incoming edge + unordered_set 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 graphFromEdges( - const map& edgeWeights) { + const map& edgeWeights) { unordered_map 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 graphFromEdges( // Selects the next node in the ordering from the graph. Key selectNextNodeInOrdering(const unordered_map& 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& node1, - const std::pair& node2) { - return node1.second.heuristic() < - node2.second.heuristic(); + [](const std::pair& keyNode1, + const std::pair& 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& edgeWeights) { + const map& 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 edgeWeights, +void removeNodeFromGraph(const Key node, + const map edgeWeights, unordered_map& 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 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 MFAS::computeOutlierWeights() const { map 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 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; }