How to use cv::decode (access image) correct? - opencv

Question

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 = &map; // 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.

Answer 1

You can't simply pass the char array to opencv functions to create an image because of how the data is formatted. PointCloud2 data fields are strictly containing information about where a point lives in 3d space(think [x,y,z]); this means nothing in terms of an actual image. Instead you have to first convert the pointcloud into something that better represents an image. Luckily, this already exists. Check out the CloudToImage ROS package.