k4a_result_t K4AROSDevice::renderBodyIndexMapToROS()

in src/k4a_ros_device.cpp [837:873]

• 26 lines of code
• 3 McCabe index (conditional complexity)

k4a_result_t K4AROSDevice::renderBodyIndexMapToROS(sensor_msgs::ImagePtr body_index_map_image,
                                                   k4a::image& k4a_body_index_map, const k4abt::frame& body_frame)
{
  // Access the body index map as an array of uint8 pixels
  BodyIndexMapPixel* body_index_map_frame_buffer = k4a_body_index_map.get_buffer();
  auto body_index_map_pixel_count = k4a_body_index_map.get_size() / sizeof(BodyIndexMapPixel);

  // Build the ROS message
  body_index_map_image->height = k4a_body_index_map.get_height_pixels();
  body_index_map_image->width = k4a_body_index_map.get_width_pixels();
  body_index_map_image->encoding = sensor_msgs::image_encodings::MONO8;
  body_index_map_image->is_bigendian = false;
  body_index_map_image->step = k4a_body_index_map.get_width_pixels() * sizeof(BodyIndexMapPixel);

  // Enlarge the data buffer in the ROS message to hold the frame
  body_index_map_image->data.resize(body_index_map_image->height * body_index_map_image->step);

  // If the pixel doesn't belong to a detected body the pixels value will be 255 (K4ABT_BODY_INDEX_MAP_BACKGROUND).
  // If the pixel belongs to a detected body the value is calculated by body id mod 255.
  // This means that up to body id 254 the value is equals the body id.
  // Afterwards it will lose the relation to the body id and is only a information for the segmentation of the image.
  for (size_t i = 0; i < body_index_map_pixel_count; ++i)
  {
    BodyIndexMapPixel val = body_index_map_frame_buffer[i];
    if (val == K4ABT_BODY_INDEX_MAP_BACKGROUND)
    {
      body_index_map_image->data[i] = K4ABT_BODY_INDEX_MAP_BACKGROUND;
    }
    else
    {
      auto body_id = k4abt_frame_get_body_id(body_frame.handle(), val);
      body_index_map_image->data[i] = body_id % K4ABT_BODY_INDEX_MAP_BACKGROUND;
    }
  }

  return K4A_RESULT_SUCCEEDED;
}