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

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.