Add parameter body_tracking_smoothing_factor to control the smoothing factor between frames. (opencv#80)

Author: RoseFlunder

docs/usage.md

@@ -23,6 +23,7 @@ The node accepts the following parameters:
 - `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.
+- `body_tracking_smoothing_factor` (float) : Defaults to `0.0`. Controls the temporal smoothing across frames. Set between `0` for no smoothing and `1` for full smoothing. Less smoothing will increase the responsiveness of the detected skeletons but will cause more positional and oriantational jitters.
 
 #### Parameter Restrictions
 

include/azure_kinect_ros_driver/k4a_ros_device_params.h

@@ -40,6 +40,7 @@
     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) \
     LIST_ENTRY(body_tracking_enabled, "True if body joints should be published as a marker array message", bool, false) \
+    LIST_ENTRY(body_tracking_smoothing_factor, "Controls the temporal smoothing of joints across frames. Set between 0 for no smoothing and 1 for full smoothing.", float, 0.0f) \
 
 class K4AROSDeviceParams
 {

launch/driver.launch

@@ -16,14 +16,15 @@ Licensed under the MIT License.
   <arg name="depth_mode"              default="WFOV_UNBINNED" />  <!-- Set the depth camera mode, which affects FOV, depth range, and camera resolution. See Azure Kinect documentation for full details. Valid options: NFOV_UNBINNED, NFOV_2X2BINNED, WFOV_UNBINNED, WFOV_2X2BINNED -->
   <arg name="color_enabled"           default="true" />           <!-- Enable or disable the color camera -->
   <arg name="color_resolution"        default="1536P" />          <!-- Resolution at which to run the color camera. Valid options: 720P, 1080P, 1440P, 1536P, 2160P, 3072P -->
-  <arg name="fps"                     default="5" />             <!-- FPS to run both cameras at. Valid options are 5, 15, and 30 -->
+  <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="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 -->
   <arg name="recording_loop_enabled"  default="false" />          <!-- If set to true the recording file will rewind the beginning once end of file is reached -->
-  <arg name="body_tracking_enabled"   default="false" />          <!-- If set to true the joint positions will be published as marker arrays -->
+  <arg name="body_tracking_enabled"           default="false" />  <!-- If set to true the joint positions will be published as marker arrays -->
+  <arg name="body_tracking_smoothing_factor"  default="0.0" />    <!-- Set between 0 for no smoothing and 1 for full smoothing -->
 
   <node pkg="azure_kinect_ros_driver" type="node" name="node" output="screen" required="$(arg required)">
     <param name="depth_enabled"     type="bool"   value="$(arg depth_enabled)" /> 
@@ -37,6 +38,7 @@ Licensed under the MIT License.
     <param name="tf_prefix"         type="string" value="$(arg tf_prefix)" />
     <param name="recording_file"          type="string" value="$(arg recording_file)" />
     <param name="recording_loop_enabled"  type="bool"   value="$(arg recording_loop_enabled)" />
-    <param name="body_tracking_enabled"   type="bool"   value="$(arg body_tracking_enabled)" />
+    <param name="body_tracking_enabled"           type="bool"   value="$(arg body_tracking_enabled)" />
+    <param name="body_tracking_smoothing_factor"  type="double" value="$(arg body_tracking_smoothing_factor)" />
   </node>
 </launch>

src/k4a_ros_device.cpp

@@ -291,6 +291,7 @@ k4a_result_t K4AROSDevice::startCameras()
     if (params_.body_tracking_enabled)
     {
         k4abt_tracker_ = k4abt::tracker::create(calibration_data_.k4a_calibration_);
+        k4abt_tracker_.set_temporal_smoothing(params_.body_tracking_smoothing_factor);
     }
 #endif