Merge pull request #92 from helenol/feature/pointcloud_speedup

Speed up pointcloud output by colorizing depth images instead of depthifying color images

docs/usage.md

@@ -20,6 +20,7 @@ The node accepts the following parameters:
 - `fps` (int) : Defaults to `5`. This parameter controls the FPS of the color and depth cameras. The cameras cannot operate at different frame rates. Valid options are `5`, `15`, `30`. Note that some FPS values are not compatible with high color camera resolutions or depth camera resolutions. For more information, see the [Azure Kinect Sensor SDK documentation](https://docs.microsoft.com/en-us/azure/Kinect-dk/hardware-specification#depth-camera-supported-operating-modes).
 - `point_cloud` (bool) : Defaults to `true`. If this parameter is set to `true`, the node will generate a sensor_msgs::PointCloud2 message from the depth camera data. This requires that the `depth_enabled` parameter be `true`.
 - `rgb_point_cloud` (bool) : Defaults to `false`. If this parameter is set to `true`, the node will generate a sensor_msgs::PointCloud2 message from the depth camera data and colorize it using the color camera data. This requires that the `point_cloud` parameter be `true`, and the `color_enabled` parameter be `true`.
+- `point_cloud_in_depth_frame` (bool) : Defaults to `true`. Whether the RGB pointcloud is rendered in the depth frame (true) or RGB frame (false). Will either match the resolution of the depth camera (true) or the RGB camera (false).
 - `recording_file` (string) : No default value. If this parameter contains a valid absolute path to a k4arecording file, the node will use the playback api with this file instead of opening a device.
 - `recording_loop_enabled` (bool) : Defaults to `false`. If this parameter is set to `true`, the node will rewind the recording file to the beginning after reaching the last frame. Otherwise the node will stop working after reaching the end of the recording file.
 - `body_tracking_enabled` (bool) : Defaults to `false`. If this parameter is set to `true`, the node will generate visualization_msgs::MarkerArray messages for the body tracking data. This requires that the `depth_enabled` parameter is set to `true` and an installed azure kinect body tracking sdk.

include/azure_kinect_ros_driver/k4a_ros_device.h

@@ -56,7 +56,8 @@ class K4AROSDevice
 
     k4a_result_t getPointCloud(const k4a::capture &capture, sensor_msgs::PointCloud2Ptr& point_cloud);
 
-    k4a_result_t getRgbPointCloud(const k4a::capture &capture, sensor_msgs::PointCloud2Ptr& point_cloud);
+    k4a_result_t getRgbPointCloudInRgbFrame(const k4a::capture &capture, sensor_msgs::PointCloud2Ptr& point_cloud);
+    k4a_result_t getRgbPointCloudInDepthFrame(const k4a::capture &capture, sensor_msgs::PointCloud2Ptr& point_cloud);
 
     k4a_result_t getImuFrame(const k4a_imu_sample_t &capture, sensor_msgs::ImuPtr& imu_frame);
 
@@ -78,6 +79,10 @@ class K4AROSDevice
     k4a_result_t renderDepthToROS(sensor_msgs::ImagePtr& depth_image, k4a::image& k4a_depth_frame);
     k4a_result_t renderIrToROS(sensor_msgs::ImagePtr& ir_image, k4a::image& k4a_ir_frame);
 
+    k4a_result_t fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::PointCloud2Ptr& point_cloud);
+    k4a_result_t fillColorPointCloud(const k4a::image& pointcloud_image, const k4a::image& color_image,
+                                     sensor_msgs::PointCloud2Ptr& point_cloud);
+
     void framePublisherThread();
     void imuPublisherThread();
 

include/azure_kinect_ros_driver/k4a_ros_device_params.h

@@ -37,6 +37,7 @@
     LIST_ENTRY(fps, "The FPS of the RGB and Depth cameras. Options are: 5, 15, 30", int, 5) \
     LIST_ENTRY(point_cloud, "A PointCloud2 based on depth data. Requires depth_enabled=true, and cannot be used with depth_mode=PASSIVE_IR", bool, true) \
     LIST_ENTRY(rgb_point_cloud, "Add RGB camera data to the point cloud. Requires point_cloud=true and color_enabled=true", bool, false) \
+    LIST_ENTRY(point_cloud_in_depth_frame, "Whether the RGB pointcloud is rendered in the depth frame (true) or RGB frame (false). Will either match the resolution of the depth camera (true) or the RGB camera (false)", bool, true) \
     LIST_ENTRY(tf_prefix, "The prefix prepended to tf frame ID's", std::string, std::string()) \
     LIST_ENTRY(recording_file, "Path to a recording file to open instead of opening a device", std::string, std::string("")) \
     LIST_ENTRY(recording_loop_enabled, "True if the recording should be rewound at EOF", bool, false) \

launch/driver.launch

@@ -20,6 +20,7 @@ Licensed under the MIT License.
   <arg name="fps"                     default="5" />              <!-- FPS to run both cameras at. Valid options are 5, 15, and 30 -->
   <arg name="point_cloud"             default="true" />           <!-- Generate a point cloud from depth data. Requires depth_enabled -->
   <arg name="rgb_point_cloud"         default="true" />           <!-- Colorize the point cloud using the RBG camera. Requires color_enabled and depth_enabled -->
+  <arg name="point_cloud_in_depth_frame" default="false" />        <!-- Whether the RGB pointcloud is rendered in the depth frame (true) or RGB frame (false). Will either match the resolution of the depth camera (true) or the RGB camera (false). -->
   <arg name="required"                default="false" />          <!-- Argument which specified if the entire launch file should terminate if the node dies -->
   <arg name="sensor_sn"               default="" />               <!-- Sensor serial number. If none provided, the first sensor will be selected -->
   <arg name="recording_file"          default="" />               <!-- Absolute path to a mkv recording file which will be used with the playback api instead of opening a device -->
@@ -38,6 +39,7 @@ Licensed under the MIT License.
     <param name="fps"               type="int"    value="$(arg fps)" />
     <param name="point_cloud"       type="bool"   value="$(arg point_cloud)" />
     <param name="rgb_point_cloud"   type="bool"   value="$(arg rgb_point_cloud)" />
+    <param name="point_cloud_in_depth_frame"   type="bool"   value="$(arg point_cloud_in_depth_frame)" />
     <param name="sensor_sn"         type="string" value="$(arg sensor_sn)" />
     <param name="tf_prefix"         type="string" value="$(arg tf_prefix)" />
     <param name="recording_file"          type="string" value="$(arg recording_file)" />

src/k4a_calibration_transform_data.cpp

@@ -48,7 +48,7 @@ void K4ACalibrationTransformData::initialize(const K4AROSDeviceParams params)
     bool colorEnabled = (getColorWidth() * getColorHeight() > 0);
 
     // Create a buffer to store the point cloud
-    if (params.point_cloud && !params.rgb_point_cloud)
+    if (params.point_cloud && (!params.rgb_point_cloud || params.point_cloud_in_depth_frame))
     {
         point_cloud_image_ = k4a::image::create(
             K4A_IMAGE_FORMAT_DEPTH16,