Skip to content

Commit

Permalink
Improve efficiency when looking up edges in pose graph
Browse files Browse the repository at this point in the history
  • Loading branch information
@yuluntian
yuluntian committed Jan 3, 2023
1 parent 1644b8d commit da43cec
Show file tree
Hide file tree
Showing 3 changed files with 112 additions and 16 deletions.
68 changes: 68 additions & 0 deletions include/DPGO/DPGO_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <map>
#include <memory>
#include <tuple>
#include <boost/functional/hash.hpp>

namespace DPGO {

Expand Down Expand Up @@ -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 {
Expand All @@ -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
12 changes: 12 additions & 0 deletions include/DPGO/PoseGraph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@
#include "RelativeSEMeasurement.h"
#include <glog/logging.h>
#include <set>
#include <map>
#include <unordered_set>
#include <unordered_map>
#include <memory>

namespace DPGO {
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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<EdgeID, size_t, HashEdgeID> edge_id_to_index;
};

}
Expand Down
48 changes: 32 additions & 16 deletions src/PoseGraph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand All @@ -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_);
Expand All @@ -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_);
Expand All @@ -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<RelativeSEMeasurement> PoseGraph::sharedLoopClosuresWithRobot(unsigned int neighbor_id) const {
Expand Down Expand Up @@ -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<RelativeSEMeasurement *> PoseGraph::writableLoopClosures() {
Expand Down

0 comments on commit da43cec

Please sign in to comment.