diff --git a/CMakeLists.txt b/CMakeLists.txt index c750467..9cbf5f2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(pose_graph_tools) -find_package(catkin_simple REQUIRED) +find_package(catkin_simple REQUIRED interactive_markers) add_compile_options(-std=c++11) @@ -9,6 +9,8 @@ catkin_simple() cs_add_library(${PROJECT_NAME} src/visualizer.cpp + src/utils.cpp + src/types.cpp ) cs_add_executable(visualizer_node diff --git a/include/pose_graph_tools/types.h b/include/pose_graph_tools/types.h new file mode 100644 index 0000000..262cfc2 --- /dev/null +++ b/include/pose_graph_tools/types.h @@ -0,0 +1,28 @@ +/* + * Copyright Notes + * + * Authors: Yun Chang (yunchang@mit.edu) + */ + +#pragma once + +#include + +#include "pose_graph_tools/PoseGraph.h" + +namespace pose_graph_tools { + +// Buffers +class PoseGraphStampCompare { +public: + bool operator()(pose_graph_tools::PoseGraphConstPtr x, + pose_graph_tools::PoseGraphConstPtr y) { + return x->header.stamp > y->header.stamp; + } +}; + +typedef std::priority_queue, + PoseGraphStampCompare> + StampedQueue; +} // namespace pose_graph_tools \ No newline at end of file diff --git a/include/pose_graph_tools/utils.h b/include/pose_graph_tools/utils.h new file mode 100644 index 0000000..49c9c14 --- /dev/null +++ b/include/pose_graph_tools/utils.h @@ -0,0 +1,20 @@ +/* + * Copyright Notes + * + * Authors: Yulun Tian (yulun@mit.edu) + */ + +#pragma once + +#include + +namespace pose_graph_tools { +bool savePoseGraphEdgesToFile(const PoseGraph &graph, + const std::string &filename); + +// Filter duplicate edges in the input pose graph +// Two edges are considered duplicate if they share the common key_from, key_to, +// robot_from, robot_to +PoseGraph filterDuplicateEdges(const PoseGraph &graph_in); + +} // namespace pose_graph_tools diff --git a/include/pose_graph_tools/visualizer.h b/include/pose_graph_tools/visualizer.h index d11d668..3319033 100644 --- a/include/pose_graph_tools/visualizer.h +++ b/include/pose_graph_tools/visualizer.h @@ -21,7 +21,8 @@ class Visualizer { private: void PoseGraphCallback(const pose_graph_tools::PoseGraph::ConstPtr& msg); - geometry_msgs::Point getPositionFromKey(long unsigned int key) const; + geometry_msgs::Point getPositionFromKey(int robot_id, + long unsigned int key) const; void MakeMenuMarker(const tf::Pose& position, const std::string& id_number); @@ -38,11 +39,12 @@ class Visualizer { ros::Publisher graph_node_pub_; ros::Publisher graph_node_id_pub_; - typedef std::pair Edge; + typedef std::pair Node; // robot id, key + typedef std::pair Edge; std::vector odometry_edges_; std::vector loop_edges_; std::vector rejected_loop_edges_; - std::unordered_map keyed_poses_; + std::map > keyed_poses_; std::shared_ptr interactive_mrkr_srvr_; diff --git a/msg/BowQueries.msg b/msg/BowQueries.msg new file mode 100644 index 0000000..df9889c --- /dev/null +++ b/msg/BowQueries.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint32 destination_robot_id +pose_graph_tools/BowQuery[] queries \ No newline at end of file diff --git a/msg/BowQuery.msg b/msg/BowQuery.msg new file mode 100644 index 0000000..b561069 --- /dev/null +++ b/msg/BowQuery.msg @@ -0,0 +1,3 @@ +uint32 robot_id +uint32 pose_id +BowVector bow_vector \ No newline at end of file diff --git a/msg/BowRequests.msg b/msg/BowRequests.msg new file mode 100644 index 0000000..d3cf1a1 --- /dev/null +++ b/msg/BowRequests.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint32 source_robot_id # Robot that sends this request +uint32 destination_robot_id # Robot that receives this request +uint32[] pose_ids # Pose IDs of requested BoW of the destination robot \ No newline at end of file diff --git a/msg/BowVector.msg b/msg/BowVector.msg new file mode 100644 index 0000000..2d95e8c --- /dev/null +++ b/msg/BowVector.msg @@ -0,0 +1,2 @@ +uint32[] word_ids +float32[] word_values \ No newline at end of file diff --git a/msg/LoopClosures.msg b/msg/LoopClosures.msg new file mode 100644 index 0000000..e7f4623 --- /dev/null +++ b/msg/LoopClosures.msg @@ -0,0 +1,3 @@ +uint16 publishing_robot_id +uint16 destination_robot_id +pose_graph_tools/PoseGraphEdge[] edges \ No newline at end of file diff --git a/msg/LoopClosuresAck.msg b/msg/LoopClosuresAck.msg new file mode 100644 index 0000000..b07d34f --- /dev/null +++ b/msg/LoopClosuresAck.msg @@ -0,0 +1,6 @@ +uint16 publishing_robot_id +uint16 destination_robot_id +uint32[] robot_src +uint32[] frame_src +uint32[] robot_dst +uint32[] frame_dst \ No newline at end of file diff --git a/msg/PoseGraphEdge.msg b/msg/PoseGraphEdge.msg index 110bb3a..3c856ba 100644 --- a/msg/PoseGraphEdge.msg +++ b/msg/PoseGraphEdge.msg @@ -3,6 +3,9 @@ Header header uint64 key_from uint64 key_to +int32 robot_from +int32 robot_to + int32 type # Type enums @@ -10,6 +13,9 @@ int32 ODOM = 0 int32 LOOPCLOSE = 1 int32 LANDMARK = 2 int32 REJECTED_LOOPCLOSE = 3 +int32 MESH = 4 +int32 POSE_MESH = 5 +int32 MESH_POSE = 6 # Transforms in ede geometry_msgs/Pose pose diff --git a/msg/PoseGraphNode.msg b/msg/PoseGraphNode.msg index 08f18b1..76f9ac0 100644 --- a/msg/PoseGraphNode.msg +++ b/msg/PoseGraphNode.msg @@ -1,5 +1,7 @@ Header header +int32 robot_id + uint64 key geometry_msgs/Pose pose \ No newline at end of file diff --git a/msg/TimeRangeQuery.msg b/msg/TimeRangeQuery.msg new file mode 100644 index 0000000..6bca3ce --- /dev/null +++ b/msg/TimeRangeQuery.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +uint32 source_robot_id # Robot that sends this request +uint32 destination_robot_id # Robot that receives this request +time start # Start of time range +time end # End of time range diff --git a/msg/VLCFrameMsg.msg b/msg/VLCFrameMsg.msg new file mode 100644 index 0000000..e3abb32 --- /dev/null +++ b/msg/VLCFrameMsg.msg @@ -0,0 +1,7 @@ +uint32 robot_id +uint32 pose_id +uint32 submap_id +sensor_msgs/Image descriptors_mat # Descriptor of all keypoints stored as a matrix. +sensor_msgs/PointCloud2 versors # Bearing vector of each keypoint expressed as a 3D vector. +float32[] depths # Depth of each keypoint. The 3D keypoint v can be recovered from the depth d and bearing u by v = d * u / u[2] +geometry_msgs/Pose T_submap_pose \ No newline at end of file diff --git a/msg/VLCFrames.msg b/msg/VLCFrames.msg new file mode 100644 index 0000000..f458da7 --- /dev/null +++ b/msg/VLCFrames.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint32 destination_robot_id +pose_graph_tools/VLCFrameMsg[] frames \ No newline at end of file diff --git a/msg/VLCRequests.msg b/msg/VLCRequests.msg new file mode 100644 index 0000000..3f88ebe --- /dev/null +++ b/msg/VLCRequests.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint32 source_robot_id # Robot that sends this request +uint32 destination_robot_id # Robot that receives this request +uint32[] pose_ids # Pose IDs of requested VLC frames of the destination robot \ No newline at end of file diff --git a/package.xml b/package.xml index ce3fd2f..85d0b33 100644 --- a/package.xml +++ b/package.xml @@ -16,6 +16,7 @@ message_runtime std_msgs geometry_msgs + sensor_msgs visualization_msgs interactive_markers diff --git a/src/types.cpp b/src/types.cpp new file mode 100644 index 0000000..af09cbf --- /dev/null +++ b/src/types.cpp @@ -0,0 +1,9 @@ +/* + * Copyright Notes + * + * Authors: Yun Chang (yunchang@mit.edu) + */ + +#include + +namespace pose_graph_tools {} // namespace pose_graph_tools \ No newline at end of file diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..6b10273 --- /dev/null +++ b/src/utils.cpp @@ -0,0 +1,74 @@ +/* + * Copyright Notes + * + * Authors: Yulun Tian (yulun@mit.edu) + */ + +#include +#include + +#include + +namespace pose_graph_tools { + +bool savePoseGraphEdgesToFile(const PoseGraph &graph, + const std::string &filename) { + std::ofstream file; + file.open(filename); + if (!file.is_open()) { + ROS_ERROR_STREAM("Error opening log file: " << filename); + return false; + } + + file << "robot_from,key_from,robot_to,key_to,qx,qy,qz,qw,tx,ty,tz\n"; + for (size_t i = 0; i < graph.edges.size(); ++i) { + PoseGraphEdge edge = graph.edges[i]; + geometry_msgs::Point position = edge.pose.position; + geometry_msgs::Quaternion orientation = edge.pose.orientation; + file << edge.robot_from << ","; + file << edge.key_from << ","; + file << edge.robot_to << ","; + file << edge.key_to << ","; + file << orientation.x << ","; + file << orientation.y << ","; + file << orientation.z << ","; + file << orientation.w << ","; + file << position.x << ","; + file << position.y << ","; + file << position.z << "\n"; + } + + return true; +} + +PoseGraph filterDuplicateEdges(const PoseGraph &graph_in) { + PoseGraph graph_out; + + graph_out.nodes = graph_in.nodes; + + for (size_t i = 0; i < graph_in.edges.size(); ++i) { + PoseGraphEdge edge_in = graph_in.edges[i]; + bool skip = false; + for (size_t j = 0; j < graph_out.edges.size(); ++j) { + PoseGraphEdge edge = graph_out.edges[j]; + if (edge.robot_from == edge_in.robot_from && + edge.robot_to == edge_in.robot_to && + edge.key_from == edge_in.key_from && edge.key_to == edge_in.key_to) { + skip = true; + break; + } + } + if (!skip) { + graph_out.edges.push_back(edge_in); + } + } + + unsigned int num_edges_in = graph_in.edges.size(); + unsigned int num_edges_out = graph_out.edges.size(); + printf("Detected and removed %u duplicate edges from pose graph.\n", + num_edges_in - num_edges_out); + + return graph_out; +} + +} // namespace pose_graph_tools \ No newline at end of file diff --git a/src/visualizer.cpp b/src/visualizer.cpp index a879401..f047354 100644 --- a/src/visualizer.cpp +++ b/src/visualizer.cpp @@ -7,9 +7,6 @@ Visualizer::Visualizer(const ros::NodeHandle& nh) { ROS_INFO("Initializing pose graph visualizer"); - // get parameters - assert(nh.getParam("frame_id", frame_id_)); - // start subscribers ros::NodeHandle nl(nh); pose_graph_sub_ = nl.subscribe( @@ -43,21 +40,24 @@ void Visualizer::PoseGraphCallback( tf::poseMsgToTF(msg_node.pose, pose); // Fill pose nodes (representing the robot position) - keyed_poses_[msg_node.key] = pose; + keyed_poses_[msg_node.robot_id][msg_node.key] = pose; } + // update frame id + frame_id_ = msg->header.frame_id; + // iterate through edges in pose graph for (const pose_graph_tools::PoseGraphEdge& msg_edge : msg->edges) { + Node from = std::make_pair(msg_edge.robot_from, msg_edge.key_from); + Node to = std::make_pair(msg_edge.robot_to, msg_edge.key_to); if (msg_edge.type == pose_graph_tools::PoseGraphEdge::ODOM) { - odometry_edges_.emplace_back( - std::make_pair(msg_edge.key_from, msg_edge.key_to)); + // initialize first seen robot id + odometry_edges_.emplace_back(std::make_pair(from, to)); } else if (msg_edge.type == pose_graph_tools::PoseGraphEdge::LOOPCLOSE) { - loop_edges_.emplace_back( - std::make_pair(msg_edge.key_from, msg_edge.key_to)); + loop_edges_.emplace_back(std::make_pair(from, to)); } else if (msg_edge.type == pose_graph_tools::PoseGraphEdge::REJECTED_LOOPCLOSE) { - rejected_loop_edges_.emplace_back( - std::make_pair(msg_edge.key_from, msg_edge.key_to)); + rejected_loop_edges_.emplace_back(std::make_pair(from, to)); } } @@ -65,8 +65,9 @@ void Visualizer::PoseGraphCallback( } geometry_msgs::Point Visualizer::getPositionFromKey( + int robot_id, long unsigned int key) const { - tf::Vector3 v = keyed_poses_.at(key).getOrigin(); + tf::Vector3 v = keyed_poses_.at(robot_id).at(key).getOrigin(); geometry_msgs::Point p; p.x = v.x(); p.y = v.y(); @@ -117,19 +118,25 @@ void Visualizer::visualize() { m.id = 0; m.action = visualization_msgs::Marker::ADD; m.type = visualization_msgs::Marker::LINE_LIST; - m.color.r = 1.0; - m.color.g = 0.0; - m.color.b = 0.0; - m.color.a = 0.8; - m.scale.x = 0.02; - m.pose.orientation.w = 1.0; - for (size_t ii = 0; ii < odometry_edges_.size(); ++ii) { - const auto key1 = odometry_edges_[ii].first; - const auto key2 = odometry_edges_[ii].second; - - m.points.push_back(getPositionFromKey(key1)); - m.points.push_back(getPositionFromKey(key2)); + int robot_id = odometry_edges_[ii].first.first; + const auto key1 = odometry_edges_[ii].first.second; + const auto key2 = odometry_edges_[ii].second.second; + + // TODO(Yun) currently the below color formula + // means that only support up to 5 robots + std_msgs::ColorRGBA color; + color.r = static_cast(robot_id) / 5; + color.g = 1 - static_cast(robot_id) / 5; + color.b = 0.0; + color.a = 0.8; + m.scale.x = 0.02; + m.pose.orientation.w = 1.0; + + m.points.push_back(getPositionFromKey(robot_id, key1)); + m.points.push_back(getPositionFromKey(robot_id, key2)); + m.colors.push_back(color); + m.colors.push_back(color); } odometry_edge_pub_.publish(m); } @@ -150,11 +157,13 @@ void Visualizer::visualize() { m.pose.orientation.w = 1.0; for (size_t ii = 0; ii < loop_edges_.size(); ++ii) { - const auto key1 = loop_edges_[ii].first; - const auto key2 = loop_edges_[ii].second; + const auto robot1 = loop_edges_[ii].first.first; + const auto robot2 = loop_edges_[ii].second.first; + const auto key1 = loop_edges_[ii].first.second; + const auto key2 = loop_edges_[ii].second.second; - m.points.push_back(getPositionFromKey(key1)); - m.points.push_back(getPositionFromKey(key2)); + m.points.push_back(getPositionFromKey(robot1, key1)); + m.points.push_back(getPositionFromKey(robot2, key2)); } loop_edge_pub_.publish(m); } @@ -175,11 +184,13 @@ void Visualizer::visualize() { m.pose.orientation.w = 1.0; for (size_t ii = 0; ii < rejected_loop_edges_.size(); ++ii) { - const auto key1 = rejected_loop_edges_[ii].first; - const auto key2 = rejected_loop_edges_[ii].second; + const auto robot1 = rejected_loop_edges_[ii].first.first; + const auto robot2 = rejected_loop_edges_[ii].second.first; + const auto key1 = rejected_loop_edges_[ii].first.second; + const auto key2 = rejected_loop_edges_[ii].second.second; - m.points.push_back(getPositionFromKey(key1)); - m.points.push_back(getPositionFromKey(key2)); + m.points.push_back(getPositionFromKey(robot1, key1)); + m.points.push_back(getPositionFromKey(robot2, key2)); } rejected_loop_edge_pub_.publish(m); } @@ -201,19 +212,18 @@ void Visualizer::visualize() { int id_base = 100; int counter = 0; - for (const auto& keyedPose : keyed_poses_) { - tf::poseTFToMsg(keyedPose.second, m.pose); - // Display text for the node - m.text = std::to_string(keyedPose.first); - m.id = id_base + keyedPose.first; - graph_node_id_pub_.publish(m); + for (const auto& robot : keyed_poses_) { + for (const auto& keyedPose : robot.second) { + tf::poseTFToMsg(keyedPose.second, m.pose); + // Display text for the node + std::string robot_id = std::to_string(keyedPose.first); + MakeMenuMarker(keyedPose.second, robot_id); + m.text = robot_id; + m.id = id_base + keyedPose.first; + graph_node_id_pub_.publish(m); + } } - // publish the interactive click-and-see key markers - for (const auto& keyedPose : keyed_poses_) { - std::string robot_id = std::to_string(keyedPose.first); - MakeMenuMarker(keyedPose.second, robot_id); - } if (interactive_mrkr_srvr_ != nullptr) { interactive_mrkr_srvr_->applyChanges(); } @@ -236,8 +246,10 @@ void Visualizer::visualize() { m.scale.z = 0.05; m.pose.orientation.w = 1.0; - for (const auto& keyedPose : keyed_poses_) { - m.points.push_back(getPositionFromKey(keyedPose.first)); + for (const auto& robot : keyed_poses_) { + for (const auto& keyedPose : robot.second) { + m.points.push_back(getPositionFromKey(robot.first, keyedPose.first)); + } } graph_node_pub_.publish(m); } diff --git a/srv/LcdFrameRegistration.srv b/srv/LcdFrameRegistration.srv new file mode 100644 index 0000000..c85c13f --- /dev/null +++ b/srv/LcdFrameRegistration.srv @@ -0,0 +1,7 @@ +uint64 query_robot +uint64 match_robot +uint64 query +uint64 match +--- +bool valid +geometry_msgs/Pose match_T_query diff --git a/srv/PoseGraphQuery.srv b/srv/PoseGraphQuery.srv new file mode 100644 index 0000000..402877a --- /dev/null +++ b/srv/PoseGraphQuery.srv @@ -0,0 +1,3 @@ +uint16 robot_id +--- +pose_graph_tools/PoseGraph pose_graph \ No newline at end of file diff --git a/srv/VLCFrameQuery.srv b/srv/VLCFrameQuery.srv new file mode 100644 index 0000000..4d9fccb --- /dev/null +++ b/srv/VLCFrameQuery.srv @@ -0,0 +1,4 @@ +uint32 robot_id +uint32[] pose_ids +--- +pose_graph_tools/VLCFrameMsg[] frames \ No newline at end of file