I need help with the following problem:
Task script: read in the message sensor_msgs/PointCloud2, display Bird Eye View image and save (png or jpg).
Desired new function: Send out the displayed images directly as an image message.
Problem: cv::Mat *bgr is the matrix that contains the image and gives it to a map (for visualisation only).
Solutions by others/so far: opencv read jpeg image from buffer // How to use cv::imdecode, if the contents of an image file are in a char array?
Using different member functions, but unsuccessful.
Code reduced to necessary snippets (complete version here: https://drive.google.com/file/d/1HI3E4nM9mQ--oNh1Q7zfwRFGJB5JRiGD/view?usp=sharing)
// Global Publishers/Subscribers
ros::Subscriber subPointCloud;
ros::Publisher pubPointCloud;
image_transport::Publisher pubImage;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_grid (new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 output;
// create Matrix to store pointcloud data
cv::Mat *heightmap, *hsv, *bgr;
std::vector<int> compression_params;
std::vector<String> fn; //filename
cv::Mat image;
// main generation function
void DEM(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
{
ROS_DEBUG("Point Cloud Received");
// clear cloud and height map array
lowest = FLT_MAX;
for(int i = 0; i < IMAGE_HEIGHT; ++i){
for(int j = 0; j < IMAGE_WIDTH; ++j){
heightArray[i][j] = (double)(-FLT_MAX);
}
}
// Convert from ROS message to PCL point cloud
pcl::fromROSMsg(*pointCloudMsg, *cloud);
// Populate the DEM grid by looping through every point
int row, column;
for(size_t j = 0; j < cloud->points.size(); ++j){
// If the point is within the image size bounds
if(map_pc2rc(cloud->points[j].x, cloud->points[j].y, &row, &column) == 1 && row >= 0 && row < IMAGE_HEIGHT && column >=0 && column < IMAGE_WIDTH){
if(cloud->points[j].z > heightArray[row][column]){
heightArray[row][column] = cloud->points[j].z;
}
// Keep track of lowest point in cloud for flood fill
else if(cloud->points[j].z < lowest){
lowest = cloud->points[j].z;
}
}
}
// Create "point cloud" and opencv image to be published for visualization
int index = 0;
double x, y;
for(int i = 0; i < IMAGE_HEIGHT; ++i){
for(int j = 0; j < IMAGE_WIDTH; ++j){
// Add point to cloud
(void)map_rc2pc(&x, &y, i, j);
cloud_grid->points[index].x = x;
cloud_grid->points[index].y = y;
cloud_grid->points[index].z = heightArray[i][j];
++index;
// Add point to image
cv::Vec3b &pixel_hsv = hsv->at<cv::Vec3b>(i,j); // access pixels vector HSV
cv::Vec3b &pixel_bgr = heightmap->at<cv::Vec3b>(i,j); // access pixels vector BGR
if(heightArray[i][j] > -FLT_MAX){
//Coloured Pixel Pointcloud
pixel_hsv[0] = map_m2i(heightArray[i][j]); // H - color value (hue)
pixel_hsv[1] = 255; // S -color saturation
pixel_hsv[2] = 255; // V - brightness
// White Pixel PointCloud
pixel_bgr[0] = map_m2i(heightArray[i][j]); // B
pixel_bgr[1] = map_m2i(heightArray[i][j]); // G
pixel_bgr[2] = map_m2i(heightArray[i][j]); // R
}
else{
// Coloured Pixel Pointcloud
pixel_hsv[0] = 0;
pixel_hsv[1] = 0;
pixel_hsv[2] = 0;
// White Pixel Pointcloud
pixel_bgr[0] = 0;
pixel_bgr[1] = 0;
pixel_bgr[2] = 0; //map_m2i(lowest);
}
}
}
// Display image
cv::cvtColor(*hsv, *bgr, cv::COLOR_HSV2BGR); // HSV matrix (src) to BGR matrix (dst)
// Image denoising (filter strength, pixel size template patch, pixel size window)
//cv::fastNlMeansDenoising(*hsv,*bgr,30 , 7, 11);
// Image denoising (filter strength luminance, same colored, pixel size template patch, pixel size window)
//cv::fastNlMeansDenoisingColored(*hsv,*bgr,30 ,1, 7, 11);
// Plot HSV(colored) and BGR (b/w)
cv::imshow(WIN_NAME, *bgr); // show new HSV matrix
cv::imshow(WIN_NAME2, *heightmap); // show old BGR matrix
// Save image to disk
char filename[100];
// FLAG enable/disable saving function
if (save_to_disk == true)
{
// save JPG format
snprintf(filename, 100, "/home/pkatsoulakos/catkin_ws/images/image_%d.jpg", fnameCounter);
std::cout << filename << std::endl;
// JPG image writing
cv::imwrite(filename, *bgr, compression_params);
/* // generate pathnames matching a pattern
glob("/home/pkatsoulakos/catkin_ws/images/*.jpg",fn); // directory, filter pattern
// range based for loop
for (auto f:fn) // range declaration:range_expression
{
image = cv::imread(f, IMREAD_COLOR);
if (image.empty())
{
std::cout << "!!! Failed imread(): image not found" << std::endl;
}
}*/
// Approach 2
//cv::Mat rawdata(1, bgr,CV_8UC1,(void*)bgr);
image = cv::imdecode(cv::Mat(*bgr, CV_8UC3, CV_AUTO_STEP), IMREAD_COLOR);
//image = cv::imdecode(cv::Mat(*bgr, CV_8UC1), IMREAD_UNCHANGED);
if (image.data == NULL)
{
std::cout << "!!! Failed imread(): image not found" << std::endl;
}
/* // save PNG format
snprintf(filename, 100, "/home/pkatsoulakos/catkin_ws/images/image_%d.png", fnameCounter);
std::cout << filename << std::endl;
// PNG image writing
// cv::imwrite(filename, *heightmap, compression_params);*/
}
++fnameCounter;
// Output height map to point cloud for python node to parse to PNG
pcl::toROSMsg(*cloud_grid, output);
output.header.stamp = ros::Time::now();
output.header.frame_id = "yrl_cloud_id"; // fixed frame (oblique alignment) from LiDAR
pubPointCloud.publish(output);
// Publish bird_view img
cv_bridge::CvImage cv_bird_view;
cv_bird_view.header.stamp = ros::Time::now();
cv_bird_view.header.frame_id = "out_bev_image";
cv_bird_view.encoding = "bgr8";
cv_bird_view.image = image;
pubImage.publish(cv_bird_view.toImageMsg());
// Output Image
//sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
//pubImage.publish(msg);pubPoin
}
int main(int argc, char** argv)
{
ROS_INFO("Starting LIDAR Node");
ros::init(argc, argv, "lidar_node");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
// Setup image
cv::Mat map(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC3, cv::Scalar(0, 0, 0));
cv::Mat map2(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC3, cv::Scalar(0, 0, 0));
cv::Mat map3(IMAGE_HEIGHT, IMAGE_WIDTH, CV_8UC3, cv::Scalar(0, 0, 0));
// H S V
// image container
heightmap = ↦ // default source code (mcshiggings)
hsv = &map2; // added for hSV visualization
bgr = &map3; // for displaying colored Pc
cv::namedWindow(WIN_NAME, WINDOW_AUTOSIZE);
cv::namedWindow(WIN_NAME2, WINDOW_AUTOSIZE);
cv::startWindowThread();
cv::imshow(WIN_NAME, *bgr); // BGR visualization of HSV
cv::imshow(WIN_NAME2, *heightmap); // default visualization
// Setup Image Output Parameters
fnameCounter = 0;
lowest = FLT_MAX;
/* PNG compression param
compression_params.push_back(IMWRITE_PNG_COMPRESSION);
A higher value means a smaller size and longer compression time. Default value is 3.
compression_params.push_back(9); */
// JPG compression param
compression_params.push_back(IMWRITE_JPEG_QUALITY);
// from 0 to 100 (the higher is the better). Default value is 95.
compression_params.push_back(95);
// Setup indicies in point clouds
/*
int index = 0;
double x, y;
for(int i = 0; i < IMAGE_HEIGHT; ++i){
for(int j = 0; j < IMAGE_WIDTH; ++j){
index = i * j;
(void)map_rc2pc(&x, &y, i, j);
cloud_grid->points[index].x = x;
cloud_grid->points[index].y = y;
cloud_grid->points[index].z = (-FLT_MAX)master.log
}
*/
// subscriber and publisher
subPointCloud = nh.subscribe<sensor_msgs::PointCloud2>("/pointcloud", 2, DEM);
pubPointCloud = nh.advertise<sensor_msgs::PointCloud2> ("/heightmap/pointcloud", 1);
pubImage = it.advertise("/out_bev_image",1);
ros::spin();
return 0;
}
Thank you for any advice and suggested solutions.
