Improve rviz visualisation of captured samples

src/feature_extractor.cpp

@@ -136,16 +136,16 @@ void FeatureExtractor::visualiseSamples()
   visualization_msgs::MarkerArray vis_array;
 
   int id = 1;
+  visualization_msgs::Marker clear;
+  clear.action = visualization_msgs::Marker::DELETEALL;
+  vis_array.markers.push_back(clear);
   for (auto& sample : optimiser_.samples_)
   {
-    visualization_msgs::Marker clear, lidar_board, lidar_normal;
-
-    clear.action = visualization_msgs::Marker::DELETEALL;
-    vis_array.markers.push_back(clear);
+    visualization_msgs::Marker lidar_board, lidar_normal;
 
     lidar_board.header.frame_id = lidar_normal.header.frame_id = lidar_frame_;
     lidar_board.action = lidar_normal.action = visualization_msgs::Marker::ADD;
-    lidar_board.type = visualization_msgs::Marker::SPHERE;
+    lidar_board.type = visualization_msgs::Marker::LINE_STRIP;
     lidar_normal.type = visualization_msgs::Marker::ARROW;
 
     lidar_normal.scale.x = 0.5;
@@ -173,178 +173,17 @@ void FeatureExtractor::visualiseSamples()
     lidar_board.pose.orientation.w = 1.0;
     for (auto& c : sample.lidar_corners)
     {
-      lidar_board.pose.position.x = c.x;
-      lidar_board.pose.position.y = c.y;
-      lidar_board.pose.position.z = c.z;
-      lidar_board.id = id++;
-      vis_array.markers.push_back(lidar_board);
+      geometry_msgs::Point p;
+      p.x = c.x;
+      p.y = c.y;
+      p.z = c.z;
+      lidar_board.points.push_back(p);
     }
+    lidar_board.points.push_back(lidar_board.points[0]);
+    lidar_board.id = id++;
+    vis_array.markers.push_back(lidar_board);
   }
   samples_pub_.publish(vis_array);
-
-  // Visualize 4 corner points of velodyne board, the board edge lines and the centre point
-  /*
-  marker1.header.frame_id = line_strip.header.frame_id = corners_board.header.frame_id = "/velodyne_front_link";
-  marker1.header.stamp = line_strip.header.stamp = corners_board.header.stamp = ros::Time();
-  marker1.ns = line_strip.ns = corners_board.ns = "my_sphere";
-  line_strip.id = 10;
-  marker1.id = 11;
-  marker1.type = visualization_msgs::Marker::POINTS;
-  line_strip.type = visualization_msgs::Marker::LINE_STRIP;
-  corners_board.type = visualization_msgs::Marker::SPHERE;
-  marker1.action = line_strip.action = corners_board.action = visualization_msgs::Marker::ADD;
-  marker1.pose.orientation.w = line_strip.pose.orientation.w = corners_board.pose.orientation.w = 1.0;
-  marker1.scale.x = 0.02;
-  marker1.scale.y = 0.02;
-  corners_board.scale.x = 0.04;
-  corners_board.scale.y = 0.04;
-  corners_board.scale.z = 0.04;
-  line_strip.scale.x = 0.009;
-  marker1.color.a = line_strip.color.a = corners_board.color.a = 1.0;
-  line_strip.color.b = 1.0;
-  marker1.color.b = marker1.color.g = marker1.color.r = 1.0;
-
-  for (int i = 0; i < 5; i++)
-  {
-    if (i < 4)
-    {
-      corners_board.pose.position.x = basic_cloud_ptr->points[i].x;
-      corners_board.pose.position.y = basic_cloud_ptr->points[i].y;
-      corners_board.pose.position.z = basic_cloud_ptr->points[i].z;
-    }
-    else
-    {
-      corners_board.pose.position.x = sample.lidar_centre.x / 1000;
-      corners_board.pose.position.y = sample.lidar_centre.y / 1000;
-      corners_board.pose.position.z = sample.lidar_centre.z / 1000;
-    }
-
-    corners_board.id = i;
-    if (corners_board.id == 0)
-      corners_board.color.b = 1.0;
-    else if (corners_board.id == 1)
-    {
-      corners_board.color.b = 0.0;
-      corners_board.color.g = 1.0;
-    }
-    else if (corners_board.id == 2)
-    {
-      corners_board.color.b = 0.0;
-      corners_board.color.g = 0.0;
-      corners_board.color.r = 1.0;
-    }
-    else if (corners_board.id == 3)
-    {
-      corners_board.color.b = 0.0;
-      corners_board.color.r = 1.0;
-      corners_board.color.g = 1.0;
-    }
-    else if (corners_board.id == 4)
-    {
-      corners_board.color.b = 1.0;
-      corners_board.color.r = 1.0;
-      corners_board.color.g = 1.0;
-    }
-    visPub.publish(corners_board);
-  }
-
-  // Visualize minimum and maximum points
-  visualization_msgs::Marker minmax;
-  minmax.header.frame_id = "/velodyne_front_link";
-  minmax.header.stamp = ros::Time();
-  minmax.ns = "my_sphere";
-  minmax.type = visualization_msgs::Marker::SPHERE;
-  minmax.action = visualization_msgs::Marker::ADD;
-  minmax.pose.orientation.w = 1.0;
-  minmax.scale.x = 0.02;
-  minmax.scale.y = 0.02;
-  minmax.scale.z = 0.02;
-  minmax.color.a = 1.0;  // Don't forget to set the alpha!
-  size_t y_min_pts = 0;
-  for (y_min_pts = 0; y_min_pts < min_points->points.size(); y_min_pts++)
-  {
-    minmax.id = y_min_pts + 13;
-    minmax.pose.position.x = min_points->points[y_min_pts].x;
-    minmax.pose.position.y = min_points->points[y_min_pts].y;
-    minmax.pose.position.z = min_points->points[y_min_pts].z;
-    minmax.color.b = 1.0;
-    minmax.color.r = 1.0;
-    minmax.color.g = 0.0;
-    visPub.publish(minmax);
-  }
-  for (size_t y_max_pts = 0; y_max_pts < max_points->points.size(); y_max_pts++)
-  {
-    minmax.id = y_min_pts + 13 + y_max_pts;
-    minmax.pose.position.x = max_points->points[y_max_pts].x;
-    minmax.pose.position.y = max_points->points[y_max_pts].y;
-    minmax.pose.position.z = max_points->points[y_max_pts].z;
-    minmax.color.r = 0.0;
-    minmax.color.g = 1.0;
-    minmax.color.b = 1.0;
-    visPub.publish(minmax);
-  }
-  // Draw board edge lines
-  for (int i = 0; i < 2; i++)
-  {
-    geometry_msgs::Point p;
-    p.x = basic_cloud_ptr->points[1 - i].x;
-    p.y = basic_cloud_ptr->points[1 - i].y;
-    p.z = basic_cloud_ptr->points[1 - i].z;
-    marker1.points.push_back(p);
-    line_strip.points.push_back(p);
-    p.x = basic_cloud_ptr->points[3 - i].x;
-    p.y = basic_cloud_ptr->points[3 - i].y;
-    p.z = basic_cloud_ptr->points[3 - i].z;
-    marker1.points.push_back(p);
-    line_strip.points.push_back(p);
-  }
-
-  geometry_msgs::Point p;
-  p.x = basic_cloud_ptr->points[1].x;
-  p.y = basic_cloud_ptr->points[1].y;
-  p.z = basic_cloud_ptr->points[1].z;
-  marker1.points.push_back(p);
-  line_strip.points.push_back(p);
-  p.x = basic_cloud_ptr->points[0].x;
-  p.y = basic_cloud_ptr->points[0].y;
-  p.z = basic_cloud_ptr->points[0].z;
-  marker1.points.push_back(p);
-  line_strip.points.push_back(p);
-
-  // Publish board edges
-  visPub.publish(line_strip);
-
-  // Visualize board normal vector
-  marker.header.frame_id = "/velodyne_front_link";
-  marker.header.stamp = ros::Time();
-  marker.ns = "my_namespace";
-  marker.id = 12;
-  marker.type = visualization_msgs::Marker::ARROW;
-  marker.action = visualization_msgs::Marker::ADD;
-  marker.scale.x = 0.02;
-  marker.scale.y = 0.04;
-  marker.scale.z = 0.06;
-  marker.color.a = 1.0;
-  marker.color.r = 0.0;
-  marker.color.g = 0.0;
-  marker.color.b = 1.0;
-  geometry_msgs::Point start, end;
-  start.x = sample.lidar_centre.x / 1000;
-  start.y = sample.lidar_centre.y / 1000;
-  start.z = sample.lidar_centre.z / 1000;
-  end.x = start.x + sample.lidar_normal.x / 2;
-  end.y = start.y + sample.lidar_normal.y / 2;
-  end.z = start.z + sample.lidar_normal.z / 2;
-  marker.points.resize(2);
-  marker.points[0].x = start.x;
-  marker.points[0].y = start.y;
-  marker.points[0].z = start.z;
-  marker.points[1].x = end.x;
-  marker.points[1].y = end.y;
-  marker.points[1].z = end.z;
-  // Publish Board normal
-  samples_pub_.publish(marker);
-  */
 }
 
 void FeatureExtractor::passthrough(const PointCloud::ConstPtr& input_pc, PointCloud::Ptr& output_pc)