add interactive_markers dependency

CMakeLists.txt

@@ -1,14 +1,16 @@
 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)
 
 catkin_simple()
 
 cs_add_library(${PROJECT_NAME}
   src/visualizer.cpp
+  src/utils.cpp
+  src/types.cpp
 )
 
 cs_add_executable(visualizer_node

include/pose_graph_tools/types.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright Notes
+ *
+ * Authors: Yun Chang ([email protected])
+ */
+
+#pragma once
+
+#include <queue>
+
+#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<pose_graph_tools::PoseGraphConstPtr,
+                            std::vector<pose_graph_tools::PoseGraphConstPtr>,
+                            PoseGraphStampCompare>
+    StampedQueue;
+} // namespace pose_graph_tools

include/pose_graph_tools/utils.h

@@ -0,0 +1,20 @@
+/*
+ * 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/BowQueries.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+uint32 destination_robot_id
+pose_graph_tools/BowQuery[] queries

msg/BowQuery.msg

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

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 

msg/BowVector.msg

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

msg/LoopClosures.msg

@@ -0,0 +1,3 @@
+uint16 publishing_robot_id
+uint16 destination_robot_id
+pose_graph_tools/PoseGraphEdge[] edges

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

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/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

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

msg/VLCFrames.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+uint32 destination_robot_id
+pose_graph_tools/VLCFrameMsg[] frames

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 

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/types.cpp

@@ -0,0 +1,9 @@
+/*
+ * Copyright Notes
+ *
+ * Authors: Yun Chang ([email protected])
+ */
+
+#include <pose_graph_tools/types.h>
+
+namespace pose_graph_tools {} // namespace pose_graph_tools

src/utils.cpp

@@ -0,0 +1,74 @@
+/*
+ * 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