Add body index map image publishing (opencv#81)

* Add body index map image publishing

* Change body index map image to mono8 format

* Fixed small syntax error

* Added topic description in usage.md

* Fixed typos in usage.md

Author: RoseFlunder

docs/usage.md

@@ -52,6 +52,8 @@ The node emits a variety of topics into its namespace.
 - `ir/camera_info` (`sensor_msgs::CameraInfo`) : Calibration information for the infrared camera, converted from the Azure Kinect Sensor SDK format. Since the depth camera and infrared camera are physically the same camera, this `camera_info` is a copy of the camera info published for the depth camera.
 - `imu` (`sensor_msgs::Imu`) : The intrinsics-corrected IMU sensor stream, provided by the Azure Kinect Sensor SDK. The sensor SDK automatically corrects for IMU intrinsics before sensor data is emitted.
 - `body_tracking_data` (`visualization_msgs::MarkerArray`) : Topic for receiving body tracking data. Each message contains all joints for all bodies for the most recent depth image. The markers are grouped by the [body id](https://microsoft.github.io/Azure-Kinect-Body-Tracking/release/0.9.x/structk4abt__body__t.html#a38fed6c7125f92b41165ffe2e1da9dd4) and the joints are in the same order as in the [joint enum of body tracking sdk](https://microsoft.github.io/Azure-Kinect-Body-Tracking/release/0.9.x/group__btenums.html#ga5fe6fa921525a37dec7175c91c473781). The id field of a marker is calculated by: `body_id * 100 + joint_index` where body_id is the corresponding body id from the body tracking sdk and the joint index is analogue to enum value for joints in the body tracking sdk. Subscribers can calculate the body.id by `floor(marker.id / 100)` and the joint_index by `marker.id % 100`.
+- `body_index_map/image_raw` (`sensor_msgs::Image`) : The [body index map](https://docs.microsoft.com/de-de/azure/Kinect-dk/body-index-map) represented as mono8 image with a background value of 255.
+Up until body id 254 the pixel values of detected bodies will be equal to their corresponding body id. Afterwards the pixel value of detected bodies will be calculated as `body_id % 255` and therefore can only be used for segmentation without a relation to the body id.
 
 ## Azure Kinect Developer Kit Calibration
 

include/azure_kinect_ros_driver/k4a_ros_device.h

@@ -65,6 +65,10 @@ class K4AROSDevice
 
 #if defined(K4A_BODY_TRACKING)
     k4a_result_t getBodyMarker(const k4abt_body_t& body, visualization_msgs::MarkerPtr marker_msg, int jointType, ros::Time capture_time);
+
+    k4a_result_t getBodyIndexMap(const k4abt::frame& body_frame, sensor_msgs::ImagePtr body_index_map_image);
+
+    k4a_result_t renderBodyIndexMapToROS(sensor_msgs::ImagePtr body_index_map_image, k4a::image& k4a_body_index_map, const k4abt::frame& body_frame);
 #endif
 
   private:
@@ -111,6 +115,8 @@ class K4AROSDevice
 
 #if defined(K4A_BODY_TRACKING)
     ros::Publisher body_marker_publisher_;
+
+    image_transport::Publisher body_index_map_publisher_;
 #endif
 
     // Parameters

include/azure_kinect_ros_driver/k4a_ros_types.h

@@ -15,4 +15,26 @@ struct BgraPixel
     uint8_t Alpha;
 };
 
+#if defined(K4A_BODY_TRACKING)
+using BodyIndexMapPixel = uint8_t;
+
+struct Color
+{
+    float r, g, b, a;
+};
+
+using ColorPalette = std::array<Color, 8>;
+// a palette of 8 colors to colorize the different body markers and the body index map
+static const ColorPalette BODY_COLOR_PALETTE{{
+    {1.0f, 0.0f, 0.0f, 1.0f},
+    {0.0f, 1.0f, 0.0f, 1.0f},
+    {0.0f, 0.0f, 1.0f, 1.0f},
+    {1.0f, 1.0f, 0.0f, 1.0f},
+    {1.0f, 0.0f, 1.0f, 1.0f},
+    {0.0f, 1.0f, 1.0f, 1.0f},
+    {0.0f, 0.0f, 0.0f, 1.0f},
+    {1.0f, 1.0f, 1.0f, 1.0f}
+}};
+#endif
+
 #endif // K4A_ROS_TYPES_H

src/k4a_ros_device.cpp

@@ -226,6 +226,8 @@ K4AROSDevice::K4AROSDevice(const NodeHandle &n, const NodeHandle &p) : k4a_devic
 
 #if defined(K4A_BODY_TRACKING)
     body_marker_publisher_ = node_.advertise<MarkerArray>("body_tracking_data", 1);
+
+    body_index_map_publisher_ = image_transport_.advertise("body_index_map/image_raw", 1);
 #endif
 }
 
@@ -781,10 +783,12 @@ k4a_result_t K4AROSDevice::getBodyMarker(const k4abt_body_t& body, MarkerPtr mar
     marker_msg->id = body.id * 100 + jointType;
     marker_msg->type = Marker::SPHERE;
 
-    marker_msg->color.a = 1.0;
-    marker_msg->color.r = 255;
-    marker_msg->color.g = 0;
-    marker_msg->color.b = 0;
+    Color color = BODY_COLOR_PALETTE[body.id % BODY_COLOR_PALETTE.size()];
+
+    marker_msg->color.a = color.a;
+    marker_msg->color.r = color.r;
+    marker_msg->color.g = color.g;
+    marker_msg->color.b = color.b;
 
     marker_msg->scale.x = 0.05;
     marker_msg->scale.y = 0.05;
@@ -800,6 +804,56 @@ k4a_result_t K4AROSDevice::getBodyMarker(const k4abt_body_t& body, MarkerPtr mar
 
     return K4A_RESULT_SUCCEEDED;
 }
+
+k4a_result_t K4AROSDevice::getBodyIndexMap(const k4abt::frame& body_frame, sensor_msgs::ImagePtr body_index_map_image)
+{
+    k4a::image k4a_body_index_map = body_frame.get_body_index_map();
+
+    if (!k4a_body_index_map)
+    {
+        ROS_ERROR("Cannot render body index map: no body index map");
+        return K4A_RESULT_FAILED;
+    }
+
+    return renderBodyIndexMapToROS(body_index_map_image, k4a_body_index_map, body_frame);
+}
+
+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;
+}
 #endif
 
 void K4AROSDevice::framePublisherThread()
@@ -962,7 +1016,7 @@ void K4AROSDevice::framePublisherThread()
 
 #if defined(K4A_BODY_TRACKING)
                 // Publish body markers when body tracking is enabled and a depth image is available
-                if (params_.body_tracking_enabled && body_marker_publisher_.getNumSubscribers() > 0)
+                if (params_.body_tracking_enabled && (body_marker_publisher_.getNumSubscribers() > 0 || body_index_map_publisher_.getNumSubscribers() > 0))
                 {
                     capture_time = timestampToROS(capture.get_depth_image().get_device_timestamp());
 
@@ -983,23 +1037,45 @@ void K4AROSDevice::framePublisherThread()
                         }
                         else
                         {
-                            auto num_bodies = body_frame.get_num_bodies();
-                            if (num_bodies > 0)
+                            if (body_marker_publisher_.getNumSubscribers() > 0)
                             {
+                                // Joint marker array
                                 MarkerArrayPtr markerArrayPtr(new MarkerArray);
+                                auto num_bodies = body_frame.get_num_bodies();
                                 for (size_t i = 0; i < num_bodies; ++i)
                                 {
-                                    k4abt_body_t body = body_frame.get_body(i);   
-                                    for (int j = 0; j < (int) K4ABT_JOINT_COUNT; ++j)
+                                    k4abt_body_t body = body_frame.get_body(i);
+                                    for (int j = 0; j < (int)K4ABT_JOINT_COUNT; ++j)
                                     {
                                         MarkerPtr markerPtr(new Marker);
                                         getBodyMarker(body, markerPtr, j, capture_time);
                                         markerArrayPtr->markers.push_back(*markerPtr);
                                     }
                                 }
-
                                 body_marker_publisher_.publish(markerArrayPtr);
                             }
+                            
+                            if (body_index_map_publisher_.getNumSubscribers() > 0)
+                            {
+                                // Body index map
+                                ImagePtr body_index_map_frame(new Image);
+                                result = getBodyIndexMap(body_frame, body_index_map_frame);
+
+                                if (result != K4A_RESULT_SUCCEEDED)
+                                {
+                                    ROS_ERROR_STREAM("Failed to get body index map");
+                                    ros::shutdown();
+                                    return;
+                                }
+                                else if (result == K4A_RESULT_SUCCEEDED)
+                                {
+                                    // Re-sychronize the timestamps with the capture timestamp
+                                    body_index_map_frame->header.stamp = capture_time;
+                                    body_index_map_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_;
+
+                                    body_index_map_publisher_.publish(body_index_map_frame);
+                                }
+                            }
                         }
                     }
                 }