Feature/hydra

CMakeLists.txt

@@ -9,6 +9,7 @@ catkin_simple()
 
 cs_add_library(${PROJECT_NAME}
   src/visualizer.cpp
+  src/utils.cpp
 )
 
 cs_add_executable(visualizer_node

include/pose_graph_tools/utils.h

@@ -0,0 +1,19 @@
+/*
+ * Copyright Notes
+ *
+ * Authors: Yulun Tian ([email protected])
+ */
+
+#pragma once
+
+#include <pose_graph_tools/PoseGraph.h>
+
+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

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<long unsigned int, long unsigned int> Edge;
+  typedef std::pair<int, long unsigned int> Node;  // robot id, key
+  typedef std::pair<Node, Node> Edge;
   std::vector<Edge> odometry_edges_;
   std::vector<Edge> loop_edges_;
   std::vector<Edge> rejected_loop_edges_;
-  std::unordered_map<long unsigned int, tf::Pose> keyed_poses_;
+  std::map<int, std::map<long unsigned int, tf::Pose> > keyed_poses_;
 
   std::shared_ptr<interactive_markers::InteractiveMarkerServer>
       interactive_mrkr_srvr_;

msg/BowQuery.msg

@@ -0,0 +1,3 @@
+uint32 robot_id
+uint32 pose_id
+BowVector bow_vector

msg/BowVector.msg

@@ -0,0 +1,2 @@
+uint32[] word_ids
+float32[] word_values

msg/PoseGraphEdge.msg

@@ -3,13 +3,19 @@ Header header
 uint64 key_from
 uint64 key_to
 
+int32 robot_from
+int32 robot_to
+
 int32 type
 
 # Type enums
 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

msg/PoseGraphNode.msg

@@ -1,5 +1,7 @@
 Header header
 
+int32 robot_id
+
 uint64 key
 
 geometry_msgs/Pose pose

msg/VLCFrameMsg.msg

@@ -0,0 +1,6 @@
+uint32 robot_id
+uint32 pose_id
+uint32 submap_id
+sensor_msgs/Image descriptors_mat
+sensor_msgs/PointCloud2 keypoints
+geometry_msgs/Pose T_submap_pose

msg/VLCFrames.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+pose_graph_tools/VLCFrameMsg[] frames

msg/VLCRequests.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+uint32 robot_id
+uint32[] pose_ids

package.xml

@@ -16,6 +16,7 @@
   <depend>message_runtime</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>interactive_markers</depend>
 </package>

src/utils.cpp

@@ -0,0 +1,71 @@
+/*
+ * Copyright Notes
+ *
+ * Authors: Yulun Tian ([email protected])
+ */
+
+#include <pose_graph_tools/utils.h>
+#include <ros/console.h>
+
+#include <fstream>
+
+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

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<pose_graph_tools::PoseGraph>(
@@ -43,30 +40,34 @@ 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));
     }
   }
 
   visualize();
 }
 
 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<float>(robot_id) / 5;
+      color.g = 1 - static_cast<float>(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);
   }

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

srv/PoseGraphQuery.srv

@@ -0,0 +1,3 @@
+uint16 robot_id
+---
+pose_graph_tools/PoseGraph pose_graph

srv/VLCFrameQuery.srv

@@ -0,0 +1,4 @@
+uint32 robot_id
+uint32[] pose_ids
+---
+pose_graph_tools/VLCFrameMsg[] frames