Commit 118b8a1e authored by James Ward's avatar James Ward
Browse files

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
parent 7146922e
......@@ -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;
......
......@@ -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);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment