diff --git a/modules/3d/src/mst.cpp b/modules/3d/src/mst.cpp index e9e67ee146..989db09ab5 100644 --- a/modules/3d/src/mst.cpp +++ b/modules/3d/src/mst.cpp @@ -109,7 +109,10 @@ bool buildMSTPrim(int numNodes, while (!pq.empty()) { - auto [w, u, v] = pq.top(); + HeapElem he = pq.top(); + double w = std::get<0>(he); + int u = std::get<1>(he); + int v = std::get<2>(he); pq.pop(); if (inMST[v]) diff --git a/modules/3d/src/rgbd/pose_graph.cpp b/modules/3d/src/rgbd/pose_graph.cpp index ccbc4b74d0..57932a5c1f 100644 --- a/modules/3d/src/rgbd/pose_graph.cpp +++ b/modules/3d/src/rgbd/pose_graph.cpp @@ -533,8 +533,11 @@ void PoseGraphImpl::applyMST(const std::vector& resultingEdges, con if (it == adj.end()) continue; - for (const auto& [neighbor, relativePose] : it->second) + for (const auto& pr : it->second) { + auto neighbor = pr.first; + auto relativePose = pr.second; + if (visited.count(neighbor)) continue; newPoses[neighbor] = currentPose * relativePose; @@ -543,8 +546,11 @@ void PoseGraphImpl::applyMST(const std::vector& resultingEdges, con } // Apply the new poses - for (const auto& [nodeId, pose] : newPoses) + for (const auto& np : newPoses) { + auto nodeId = np.first; + auto pose = np.second; + if (!nodes.at(nodeId).isFixed) nodes.at(nodeId).setPose(pose.getAffine()); }