Store chessboard point clouds

All sampled point clouds are displayed.
Clouds are deleted when samples are discarded.
Board perimeter line width reduced to improve clarity.
Closes #5

include/cam_lidar_calibration/feature_extractor.h

@@ -47,6 +47,7 @@ private:
                    pcl::PointCloud<pcl::PointXYZIR>::Ptr& output_pc);
   std::tuple<std::vector<cv::Point3d>, cv::Mat> locateChessboard(const sensor_msgs::Image::ConstPtr& image);
   auto chessboardProjection(const std::vector<cv::Point2d>& corners, const cv_bridge::CvImagePtr& cv_ptr);
+  void publishBoardPointCloud();
 
   std::tuple<pcl::PointCloud<pcl::PointXYZIR>::Ptr, cv::Point3d>
   extractBoard(const pcl::PointCloud<pcl::PointXYZIR>::Ptr& cloud);
@@ -68,6 +69,7 @@ private:
   std::shared_ptr<message_filters::Synchronizer<ImageLidarSyncPolicy>> image_pc_sync_;
   int queue_rate_ = 5;
 
+  std::vector<pcl::PointCloud<pcl::PointXYZIR>::Ptr> pc_samples_;
   ros::Publisher board_cloud_pub_, bounded_cloud_pub_;
   ros::Publisher samples_pub_;
   image_transport::Publisher image_publisher;

src/feature_extractor.cpp

@@ -79,6 +79,7 @@ bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res
       if (!optimiser_->samples_.empty())
       {
         optimiser_->samples_.pop_back();
+        pc_samples_.pop_back();
       }
       break;
     case Optimise::Request::OPTIMISE:
@@ -89,11 +90,23 @@ bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res
       }
       break;
   }
-
+  publishBoardPointCloud();
   flag = req.operation;  // read flag published by rviz calibration panel
   return true;
 }
 
+void FeatureExtractor::publishBoardPointCloud()
+{
+  // Publish collected board clouds
+  PointCloud pc;
+  for (auto board : pc_samples_)
+  {
+    pc += *board;
+    pc.header = board->header;
+  }
+  board_cloud_pub_.publish(pc);
+}
+
 void FeatureExtractor::boundsCB(cam_lidar_calibration::boundsConfig& config, uint32_t level)
 {
   // Read the values corresponding to the motion of slider bars in reconfigure gui
@@ -148,9 +161,9 @@ void FeatureExtractor::visualiseSamples()
 
     vis_array.markers.push_back(lidar_normal);
 
-    lidar_board.scale.x = 0.04;
-    lidar_board.scale.y = 0.04;
-    lidar_board.scale.z = 0.04;
+    lidar_board.scale.x = 0.01;
+    lidar_board.scale.y = 0.01;
+    lidar_board.scale.z = 0.01;
     lidar_board.color.a = 1.0;
     lidar_board.color.b = 0.0;
     lidar_board.color.g = 1.0;
@@ -364,7 +377,8 @@ FeatureExtractor::extractBoard(const PointCloud::Ptr& cloud)
   proj.filter(*cloud_projected);
 
   // Publish the projected inliers
-  board_cloud_pub_.publish(cloud_projected);
+  pc_samples_.push_back(cloud_projected);
+  publishBoardPointCloud();
   return std::make_tuple(cloud_projected, lidar_normal);
 }