Visualize inter and intra constraint (like cartographer_ros) localization nodes and edges with different colors in the pose graph marker message.

slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp

@@ -47,6 +47,7 @@ class LoopClosureAssistant
   void clearMovedNodes();
   void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
   void publishGraph();
+  void setMapper(karto::Mapper * mapper);
 
 private:
   bool manualLoopClosureCallback(slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp);

slam_toolbox/include/slam_toolbox/visualization_utils.hpp

@@ -139,6 +139,41 @@ inline void toNavMap(
   return;
 }
 
+inline std_msgs::ColorRGBA getColor(
+  const float& r,
+  const float& g,
+  const float& b)
+{
+  std_msgs::ColorRGBA color;
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  return color;
+}
+
+inline visualization_msgs::Marker getMarker(
+  const std::string& frame,
+  const std::string& name,
+  const int& reserve)
+{
+  visualization_msgs::Marker edges_marker;
+  edges_marker.header.frame_id = frame;
+  edges_marker.header.stamp = ros::Time::now();
+  edges_marker.id = 0;
+  edges_marker.ns = name;
+  edges_marker.action = visualization_msgs::Marker::ADD;
+  edges_marker.type = visualization_msgs::Marker::LINE_LIST;
+  edges_marker.pose.orientation.w = 1;
+  edges_marker.scale.x = 0.01;
+  edges_marker.color.r = 0.0;
+  edges_marker.color.g = 0.0;
+  edges_marker.color.b = 0.0;
+  edges_marker.color.a = 1;
+  edges_marker.lifetime = ros::Duration(0.0);
+  edges_marker.points.reserve(reserve);
+  return edges_marker;
+}
+
 }  // end namespace
 
 #endif //SLAM_TOOLBOX_VISUALIZATION_UTILS_H_

slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -2056,7 +2056,15 @@ namespace karto
     {
       return m_pMapperSensorManager;
     }
-
+
+    /**
+     * Gets the localization vertices
+     * @return localization vertices
+     */
+    const LocalizationScanVertices &GetLocalizationVertices()
+    {
+      return m_LocalizationScanVertices;
+    }
     /**
      * Tries to close a loop using the given scan with the scans from the given sensor
      * @param pScan

slam_toolbox/src/loop_closure_assistant.cpp

@@ -119,6 +119,12 @@ void LoopClosureAssistant::processInteractiveFeedback(const
     scan_publisher_.publish(scan);
   }
 }
+/*****************************************************************************/
+void LoopClosureAssistant::setMapper(karto::Mapper *mapper)
+/*****************************************************************************/
+{
+  mapper_ = mapper;
+}
 
 /*****************************************************************************/
 void LoopClosureAssistant::publishGraph()
@@ -139,34 +145,156 @@ void LoopClosureAssistant::publishGraph()
     interactive_mode = interactive_mode_;
   }
 
-  visualization_msgs::MarkerArray marray;
-  visualization_msgs::Marker m = vis_utils::toMarker(map_frame_,
-    "slam_toolbox", 0.1);
+    if (!mapper_->GetGraph())
+      ROS_ERROR_STREAM("Invalid pointer");
 
-  for (ConstGraphIterator it = graph->begin(); it != graph->end(); ++it)
-  {
-    m.id = it->first + 1;
-    m.pose.position.x = it->second(0);
-    m.pose.position.y = it->second(1);
+    const auto &vertices = mapper_->GetGraph()->GetVertices();
 
-    if (interactive_mode && enable_interactive_mode_)
+    const auto &edges = mapper_->GetGraph()->GetEdges();
+
+    const auto &localization_vertices = mapper_->GetLocalizationVertices();
+
+    int first_localization_id = std::numeric_limits<int>::max();
+    if (!localization_vertices.empty())
     {
-      visualization_msgs::InteractiveMarker int_marker =
-        vis_utils::toInteractiveMarker(m, 0.3);
-      interactive_server_->insert(int_marker,
+      first_localization_id = localization_vertices.front().vertex->GetObject()->GetUniqueId();
+    }
+
+    visualization_msgs::MarkerArray marray;
+
+    // clear existing markers to account for any removed nodes
+    visualization_msgs::Marker clear;
+    clear.header.stamp = ros::Time::now();
+    clear.action = visualization_msgs::Marker::DELETEALL;
+    marray.markers.push_back(clear);
+
+    visualization_msgs::Marker slam_node = vis_utils::toMarker(map_frame_, "slam_nodes", 0.1);
+    visualization_msgs::Marker loc_node = vis_utils::toMarker(map_frame_, "loc_nodes", 0.1);
+
+    // add map nodes
+    for (const auto &sensor_name : vertices)
+    {
+      for (const auto &vertex : sensor_name.second)
+      {
+
+        visualization_msgs::Marker m;
+        if (vertex.first < first_localization_id)
+          m = slam_node;
+        else
+        {
+          m = loc_node;
+          m.color.r = 0.0;
+          m.color.b = 0.0;
+        }
+
+        const auto &pose = vertex.second->GetObject()->GetCorrectedPose();
+        m.id = vertex.first + 1;
+        m.pose.position.x = pose.GetX();
+        m.pose.position.y = pose.GetY();
+
+        if (interactive_mode && enable_interactive_mode_)
+        {
+          visualization_msgs::InteractiveMarker int_marker =
+              vis_utils::toInteractiveMarker(m, 0.3);
+          interactive_server_->insert(int_marker,
         boost::bind(
         &LoopClosureAssistant::processInteractiveFeedback,
         this, _1));
+        }
+        else
+        {
+          marray.markers.push_back(m);
+        }
+      }
     }
-    else
+
+    // add line markers for graph edges
+    visualization_msgs::Marker edges_marker = vis_utils::getMarker(map_frame_,"intra_slam_edges", edges.size() * 2);
+    visualization_msgs::Marker edges_marker_inter = vis_utils::getMarker(map_frame_,"inter_slam_edges", edges.size() * 2);
+    visualization_msgs::Marker loc_edges_marker = vis_utils::getMarker(map_frame_,"intra_loc_edges", localization_vertices.size() * 3);
+    visualization_msgs::Marker inter_loc_edges_marker = vis_utils::getMarker(map_frame_,"inter_loc_edges", localization_vertices.size() * 3);
+
+    geometry_msgs::Point prevSourcePoint, prevTargetPoint;
+
+    for (const auto &edge : edges)
     {
-      marray.markers.push_back(m);
+      bool isInter = false;
+
+      std_msgs::ColorRGBA color;
+
+      color.a = 0.3;
+
+      int source_id = edge->GetSource()->GetObject()->GetUniqueId();
+      const auto &pose0 = edge->GetSource()->GetObject()->GetCorrectedPose();
+      geometry_msgs::Point sourcePoint;
+      sourcePoint.x = pose0.GetX();
+      sourcePoint.y = pose0.GetY();
+
+      int target_id = edge->GetTarget()->GetObject()->GetUniqueId();
+      const auto &pose1 = edge->GetTarget()->GetObject()->GetCorrectedPose();
+      geometry_msgs::Point targetPoint;
+      targetPoint.x = pose1.GetX();
+      targetPoint.y = pose1.GetY();
+      if (prevTargetPoint == targetPoint) //checking for inter constraints
+      {
+        isInter = true;
+        color = (source_id >= first_localization_id) ? vis_utils::getColor(1, 0, 1) : vis_utils::getColor(0, 1, 1);
+      }
+      else
+      {
+        color = vis_utils::getColor(1, 1, 0); 
+      }
+
+      prevSourcePoint = sourcePoint;
+      prevTargetPoint = targetPoint;
+
+      if (source_id >= first_localization_id || target_id >= first_localization_id)
+      {
+        if (isInter)
+        {
+          inter_loc_edges_marker.colors.push_back(color); 
+          inter_loc_edges_marker.colors.push_back(color);
+          inter_loc_edges_marker.points.push_back(sourcePoint);
+          inter_loc_edges_marker.points.push_back(targetPoint);
+        }
+        else
+        {
+          loc_edges_marker.colors.push_back(color);
+          loc_edges_marker.colors.push_back(color);
+          loc_edges_marker.points.push_back(sourcePoint);
+          loc_edges_marker.points.push_back(targetPoint);
+        }
+      }
+      else
+      {
+        color.g = 0;
+        if (isInter)
+        {
+          edges_marker_inter.colors.push_back(color);
+          edges_marker_inter.colors.push_back(color);
+          edges_marker_inter.points.push_back(sourcePoint);
+          edges_marker_inter.points.push_back(targetPoint);
+        }
+        else
+        {
+          edges_marker.colors.push_back(color);
+          edges_marker.colors.push_back(color);
+          edges_marker.points.push_back(sourcePoint);
+          edges_marker.points.push_back(targetPoint);
+        }
+      }
     }
-  }
-
-  // if disabled, clears out old markers
-  interactive_server_->applyChanges();
-  marker_publisher_.publish(marray);
+
+    marray.markers.push_back(edges_marker_inter);
+    marray.markers.push_back(edges_marker);
+     if(inter_loc_edges_marker.points.size() > 0)  // to avoid error when mapping
+    marray.markers.push_back(inter_loc_edges_marker);
+     if (loc_edges_marker.points.size() > 0)
+    marray.markers.push_back(loc_edges_marker);
+
+    // if disabled, clears out old markers
+    interactive_server_->applyChanges();
+    marker_publisher_.publish(marray);
   return;
 }
 

slam_toolbox/src/slam_toolbox_common.cpp

@@ -638,6 +638,7 @@ void SlamToolbox::loadSerializedPoseGraph(
   smapper_->setMapper(mapper.release());
   smapper_->configure(nh_);
   dataset_.reset(dataset.release());
+  closure_assistant_->setMapper(smapper_->getMapper());
 
   if (!smapper_->getMapper())
   {