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

fixes3

release/4.3a0
Akshay Krishnan 2020-09-12 21:40:11 +00:00
parent 539ebb032a
commit 0ab7f4c7c8
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/sfm/MFAS.cpp
View File

@ -82,12 +82,12 @@ Key selectNextNodeInOrdering(const unordered_map<Key, GraphNode>& graph) {
} }
// 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 key1, const Key key2,
const map<MFAS::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(MFAS::KeyPair(node1, node2)) != edgeWeights_.end() return edgeWeights.find(MFAS::KeyPair(key1, key2)) != edgeWeights.end()
? std::abs(edgeWeights_.at(MFAS::KeyPair(node1, node2))) ? std::abs(edgeWeights.at(MFAS::KeyPair(key1, key2)))
: std::abs(edgeWeights_.at(MFAS::KeyPair(node2, node1))); : std::abs(edgeWeights.at(MFAS::KeyPair(key2, key1)));
} }
// 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.
@ -157,17 +157,17 @@ map<MFAS::KeyPair, double> MFAS::computeOutlierWeights() const {
// Find edge source and destination. // Find edge source and destination.
Key source = edgeWeight.first.first; Key source = edgeWeight.first.first;
Key dest = edgeWeight.first.second; Key dest = edgeWeight.first.second;
if (weight < 0) { if (edgeWeight.second < 0) {
std::swap(source, dest); std::swap(source, dest);
} }
// 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(edgeWeight.second); outlierWeights[edgeWeight.first] = std::abs(edgeWeight.second);
} else { } else {
outlierWeights[edge] = 0; outlierWeights[edgeWeight.first] = 0;
} }
} }
return outlierWeights; return outlierWeights;
} }