From da43cec13bc730ef384f73cac4c2f1330445e1d6 Mon Sep 17 00:00:00 2001 From: Yulun Tian Date: Tue, 3 Jan 2023 15:13:32 -0500 Subject: [PATCH] Improve efficiency when looking up edges in pose graph --- include/DPGO/DPGO_types.h | 68 +++++++++++++++++++++++++++++++++++++++ include/DPGO/PoseGraph.h | 12 +++++++ src/PoseGraph.cpp | 48 ++++++++++++++++++--------- 3 files changed, 112 insertions(+), 16 deletions(-) diff --git a/include/DPGO/DPGO_types.h b/include/DPGO/DPGO_types.h index 2669094..4d57e28 100644 --- a/include/DPGO/DPGO_types.h +++ b/include/DPGO/DPGO_types.h @@ -16,6 +16,7 @@ #include #include #include +#include namespace DPGO { @@ -108,6 +109,11 @@ class PoseID { unsigned int robot_id; // robot ID unsigned int frame_id; // frame ID explicit PoseID(unsigned int rid = 0, unsigned int fid = 0) : robot_id(rid), frame_id(fid) {} + PoseID(const PoseID &other) : robot_id(other.robot_id), frame_id(other.frame_id) {} + bool operator==(const PoseID &other) const + { return (robot_id == other.robot_id + && frame_id == other.frame_id); + } }; // Comparator for PoseID struct ComparePoseID { @@ -118,6 +124,68 @@ struct ComparePoseID { } }; +// Edge measurement (edge) is uniquely determined by an ordered pair of poses +class EdgeID { + public: + PoseID src_pose_id; + PoseID dst_pose_id; + EdgeID(const PoseID &src_id, const PoseID &dst_id) + : src_pose_id(src_id), dst_pose_id(dst_id) {} + bool operator==(const EdgeID &other) const + { return (src_pose_id == other.src_pose_id + && dst_pose_id == other.dst_pose_id); + } + bool isOdometry() const { + return (src_pose_id.robot_id == dst_pose_id.robot_id && + src_pose_id.frame_id + 1 == dst_pose_id.frame_id); + } + bool isPrivateLoopClosure() const { + return (src_pose_id.robot_id == dst_pose_id.robot_id && + src_pose_id.frame_id + 1 != dst_pose_id.frame_id); + } + bool isSharedLoopClosure() const { + return src_pose_id.robot_id != dst_pose_id.robot_id; + } +}; +// Comparator for EdgeID +struct CompareEdgeID { + bool operator()(const EdgeID &a, const EdgeID &b) const { + // Treat edge ID as an ordered tuple + const auto ta = std::make_tuple(a.src_pose_id.robot_id, + a.dst_pose_id.robot_id, + a.src_pose_id.frame_id, + a.dst_pose_id.frame_id); + const auto tb = std::make_tuple(b.src_pose_id.robot_id, + b.dst_pose_id.robot_id, + b.src_pose_id.frame_id, + b.dst_pose_id.frame_id); + return ta < tb; + } +}; +// Hasher for EdgeID +struct HashEdgeID +{ + std::size_t operator()(const EdgeID& edge_id) const + { + // Reference: + // https://stackoverflow.com/questions/17016175/c-unordered-map-using-a-custom-class-type-as-the-key + using boost::hash_value; + using boost::hash_combine; + + // Start with a hash value of 0 . + std::size_t seed = 0; + + // Modify 'seed' by XORing and bit-shifting in + // one member of 'Key' after the other: + hash_combine(seed, hash_value(edge_id.src_pose_id.robot_id)); + hash_combine(seed, hash_value(edge_id.dst_pose_id.robot_id)); + hash_combine(seed, hash_value(edge_id.src_pose_id.frame_id)); + hash_combine(seed, hash_value(edge_id.dst_pose_id.frame_id)); + + // Return the result. + return seed; + } +}; } // namespace DPGO #endif diff --git a/include/DPGO/PoseGraph.h b/include/DPGO/PoseGraph.h index fabb860..9e5ea1f 100644 --- a/include/DPGO/PoseGraph.h +++ b/include/DPGO/PoseGraph.h @@ -14,6 +14,9 @@ #include "RelativeSEMeasurement.h" #include #include +#include +#include +#include #include namespace DPGO { @@ -222,6 +225,10 @@ class PoseGraph { * @return */ Statistics statistics() const; + /** + * @brief Check if a measurement exists in the pose graph + */ + bool hasMeasurement(const PoseID &srcID, const PoseID &dstID) const; /** * @brief Find and return a writable pointer to the specified measurement within this pose graph * @param measurements @@ -316,6 +323,11 @@ class PoseGraph { * and my neighbors. */ void updatePublicPoseIDs(); + + private: + // Mapping Edge ID to the corresponding index in the vector of measurements + // (either odometry, private loop closures, or public loop closures) + std::unordered_map edge_id_to_index; }; } diff --git a/src/PoseGraph.cpp b/src/PoseGraph.cpp index b11cc07..687c6cc 100644 --- a/src/PoseGraph.cpp +++ b/src/PoseGraph.cpp @@ -24,6 +24,7 @@ PoseGraph::~PoseGraph() { void PoseGraph::empty() { // Reset this pose graph to be empty n_ = 0; + edge_id_to_index.clear(); odometry_.clear(); private_lcs_.clear(); shared_lcs_.clear(); @@ -74,7 +75,7 @@ void PoseGraph::addOdometry(const RelativeSEMeasurement &factor) { // Check for duplicate inter-robot loop closure const PoseID src_id(factor.r1, factor.p1); const PoseID dst_id(factor.r2, factor.p2); - if (findMeasurement(src_id, dst_id)) + if (hasMeasurement(src_id, dst_id)) return; // Check that this is an odometry measurement @@ -85,13 +86,15 @@ void PoseGraph::addOdometry(const RelativeSEMeasurement &factor) { CHECK(factor.t.rows() == d_ && factor.t.cols() == 1); n_ = std::max(n_, (unsigned int) factor.p2 + 1); odometry_.push_back(factor); + const EdgeID edge_id(src_id, dst_id); + edge_id_to_index.emplace(edge_id, odometry_.size() - 1); } void PoseGraph::addPrivateLoopClosure(const RelativeSEMeasurement &factor) { // Check for duplicate inter-robot loop closure const PoseID src_id(factor.r1, factor.p1); const PoseID dst_id(factor.r2, factor.p2); - if (findMeasurement(src_id, dst_id)) + if (hasMeasurement(src_id, dst_id)) return; CHECK(factor.r1 == id_); @@ -101,13 +104,15 @@ void PoseGraph::addPrivateLoopClosure(const RelativeSEMeasurement &factor) { // update number of poses n_ = std::max(n_, (unsigned int) std::max(factor.p1 + 1, factor.p2 + 1)); private_lcs_.push_back(factor); + const EdgeID edge_id(src_id, dst_id); + edge_id_to_index.emplace(edge_id, private_lcs_.size() - 1); } void PoseGraph::addSharedLoopClosure(const RelativeSEMeasurement &factor) { // Check for duplicate inter-robot loop closure const PoseID src_id(factor.r1, factor.p1); const PoseID dst_id(factor.r2, factor.p2); - if (findMeasurement(src_id, dst_id)) + if (hasMeasurement(src_id, dst_id)) return; CHECK(factor.R.rows() == d_ && factor.R.cols() == d_); @@ -127,6 +132,8 @@ void PoseGraph::addSharedLoopClosure(const RelativeSEMeasurement &factor) { } shared_lcs_.push_back(factor); + const EdgeID edge_id(src_id, dst_id); + edge_id_to_index.emplace(edge_id, shared_lcs_.size() - 1); } std::vector PoseGraph::sharedLoopClosuresWithRobot(unsigned int neighbor_id) const { @@ -164,23 +171,32 @@ bool PoseGraph::hasNeighborPose(const PoseID &pose_id) const { return nbr_shared_pose_ids_.find(pose_id) != nbr_shared_pose_ids_.end(); } +bool PoseGraph::hasMeasurement(const PoseID &srcID, const PoseID &dstID) const { + const EdgeID edge_id(srcID, dstID); + return edge_id_to_index.find(edge_id) != edge_id_to_index.end(); +} + RelativeSEMeasurement *PoseGraph::findMeasurement(const PoseID &srcID, const PoseID &dstID) { - for (auto &m : odometry_) { - if (m.r1 == srcID.robot_id && m.p1 == srcID.frame_id && dstID.robot_id == m.r2 && dstID.frame_id == m.p2) { - return &m; - } - } - for (auto &m : private_lcs_) { - if (m.r1 == srcID.robot_id && m.p1 == srcID.frame_id && dstID.robot_id == m.r2 && dstID.frame_id == m.p2) { - return &m; + RelativeSEMeasurement *edge = nullptr; + if (hasMeasurement(srcID, dstID)) { + const EdgeID edge_id(srcID, dstID); + size_t index = edge_id_to_index.at(edge_id); + if (edge_id.isOdometry()) { + edge = &odometry_[index]; + } else if (edge_id.isPrivateLoopClosure()) { + edge = &private_lcs_[index]; + } else { + edge = &shared_lcs_[index]; } } - for (auto &m : shared_lcs_) { - if (m.r1 == srcID.robot_id && m.p1 == srcID.frame_id && dstID.robot_id == m.r2 && dstID.frame_id == m.p2) { - return &m; - } + if (edge) { + // Sanity check + CHECK_EQ(edge->r1, srcID.robot_id); + CHECK_EQ(edge->p1, srcID.frame_id); + CHECK_EQ(edge->r2, dstID.robot_id); + CHECK_EQ(edge->p2, dstID.frame_id); } - return nullptr; + return edge; } std::vector PoseGraph::writableLoopClosures() {