AzureKinectPointCloudProvider.cpp

Go to the documentation of this file.

#include "AzureKinectPointCloudProvider.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <cstdio>
#include <filesystem>
#include <functional>
#include <mutex>
#include <utility>
 
#include <IceUtil/Time.h>
 
#include <opencv2/core/hal/interface.h>
#include <opencv2/imgproc/imgproc.hpp>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/forward_declarations.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "VisionX/libraries/armem_human/client/HumanPoseWriter.h"
#include "VisionX/libraries/human/pose/model/k4a_bt_body_32.h"
#include <VisionX/libraries/imrec/helper.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Image/ImageProcessor.h>
 
#ifdef INCLUDE_BODY_TRACKING
#include <VisionX/libraries/armem_human/types.h>
#endif
 
 
namespace
{
    /**
     * @brief Converts a k4a image to an IVT image.
     * @param color_image Reference to a k4a image containing the color data that should be converted.
     * @param result IVT image the converted image should be stored in. The size of this image and the color_image need to match.
     */
    [[maybe_unused]] void
    k4aToIvtImage(const k4a::image& color_image, ::CByteImage& result)
    {
        if (color_image.get_format() == K4A_IMAGE_FORMAT_COLOR_YUY2)
        {
            // Convert YUY2 to RGB using OpenCV
            cv::Mat yuy2_image(color_image.get_height_pixels(),
                               color_image.get_width_pixels(),
                               CV_8UC2,
                               const_cast<uint8_t*>(color_image.get_buffer()));
            cv::Mat rgb_image;
            cv::cvtColor(yuy2_image, rgb_image, cv::COLOR_YUV2RGB_YUY2);
 
            // Convert the OpenCV Mat to the IVT CByteImage format
            visionx::imrec::convert(rgb_image, result);
        }
        else if (color_image.get_format() == K4A_IMAGE_FORMAT_COLOR_BGRA32)
        {
            auto cw = static_cast<unsigned int>(color_image.get_width_pixels());
            auto ch = static_cast<unsigned int>(color_image.get_height_pixels());
            ARMARX_CHECK_EQUAL(static_cast<unsigned int>(result.width), cw);
            ARMARX_CHECK_EQUAL(static_cast<unsigned int>(result.height), ch);
 
            auto color_buffer = color_image.get_buffer();
            auto rgb_buffer_ivt = result.pixels;
 
            // Index in the IVT image. This value will be increased by 3 per pixel.
            int index_ivt = 0;
 
            // Index in the k4a image. This value increases by 4 per pixel.
            int index_k4a = 0;
 
            for (unsigned int y = 0; y < ch; ++y)
            {
                for (unsigned int x = 0; x < cw; ++x)
                {
                    rgb_buffer_ivt[index_ivt] = color_buffer[index_k4a + 2];
                    rgb_buffer_ivt[index_ivt + 1] = color_buffer[index_k4a + 1];
                    rgb_buffer_ivt[index_ivt + 2] = color_buffer[index_k4a + 0];
                    index_ivt += 3;
                    index_k4a += 4;
                }
            }
        }
        else
        {
            throw std::runtime_error("Unsupported color format in k4a image");
        }
    }
 
 
#ifdef INCLUDE_BODY_TRACKING
    void
    printBodyInformation(k4abt_body_t body)
    {
        ARMARX_VERBOSE << "Body ID: " << body.id;
        for (int i = 0; i < static_cast<int>(K4ABT_JOINT_COUNT); i++)
        {
            const k4a_float3_t position = body.skeleton.joints[i].position;
            const k4a_quaternion_t orientation = body.skeleton.joints[i].orientation;
            const k4abt_joint_confidence_level_t confidence_level =
                body.skeleton.joints[i].confidence_level;
 
            ARMARX_VERBOSE << "Joint" << i << ": "
                           << "Position[mm] (" << position.v[0] << "," << position.v[1] << ","
                           << position.v[2] << "); "
                           << "Orientation (" << orientation.v[0] << "," << orientation.v[1] << ","
                           << orientation.v[2] << "," << orientation.v[3] << "); "
                           << "Confidence Level " << confidence_level;
        }
    }
#endif
 
    const char*
    k4aImageFormatToString(k4a_image_format_t format)
    {
        switch (format)
        {
            case K4A_IMAGE_FORMAT_COLOR_MJPG:
                return "COLOR_MJPG";
            case K4A_IMAGE_FORMAT_COLOR_NV12:
                return "COLOR_NV12";
            case K4A_IMAGE_FORMAT_COLOR_YUY2:
                return "COLOR_YUY2";
            case K4A_IMAGE_FORMAT_COLOR_BGRA32:
                return "COLOR_BGRA32";
            case K4A_IMAGE_FORMAT_DEPTH16:
                return "DEPTH16";
            case K4A_IMAGE_FORMAT_IR16:
                return "IR16";
            case K4A_IMAGE_FORMAT_CUSTOM:
                return "CUSTOM";
            default:
                return "UNKNOWN";
        }
    }
 
} // namespace
 
namespace visionx
{
 
    std::function<void(armarx::Duration)>
    AzureKinectPointCloudProvider::createSwCallback(const std::string& description)
    {
        return [description, this](armarx::Duration duration)
        {
            std::string name = "duration " + description + " in [ms]";
            {
                std::lock_guard g{metaInfoMtx};
                setMetaInfo(name, new armarx::Variant{duration.toMilliSecondsDouble()});
            }
 
            {
                std::lock_guard g{debugObserverMtx};
                setDebugObserverDatafield(name, duration.toMilliSecondsDouble());
            }
        };
    }
 
    AzureKinectPointCloudProviderPropertyDefinitions::
        AzureKinectPointCloudProviderPropertyDefinitions(std::string prefix) :
        visionx::CapturingPointCloudProviderPropertyDefinitions(std::move(prefix))
    {
        defineOptionalProperty<k4a_color_resolution_t>("ColorResolution",
                                                       K4A_COLOR_RESOLUTION_720P,
                                                       "Resolution of the RGB camera image.")
            .map("0", K4A_COLOR_RESOLUTION_OFF) /** Color camera will be turned off */
            .map("720", K4A_COLOR_RESOLUTION_720P) /** 1280x720  16:9 */
            .map("1080", K4A_COLOR_RESOLUTION_1080P) /** 1920x1080 16:9 */
            .map("1440", K4A_COLOR_RESOLUTION_1440P) /** 2560x1440 16:9 */
            .map("1536", K4A_COLOR_RESOLUTION_1536P) /** 2048x1536 4:3  */
            .map("2160", K4A_COLOR_RESOLUTION_2160P) /** 3840x2160 16:9 */
            .map("3072", K4A_COLOR_RESOLUTION_3072P); /** 4096x3072 4:3  */
        defineOptionalProperty<k4a_depth_mode_t>("DepthMode",
                                                 K4A_DEPTH_MODE_NFOV_UNBINNED,
                                                 "Resolution/mode of the depth camera image.")
            .setCaseInsensitive(true)
            .map("OFF", K4A_DEPTH_MODE_OFF)
            .map("NFOV_2X2BINNED", K4A_DEPTH_MODE_NFOV_2X2BINNED)
            .map("NFOV_UNBINNED", K4A_DEPTH_MODE_NFOV_UNBINNED)
            .map("WFOV_2X2BINNED", K4A_DEPTH_MODE_WFOV_2X2BINNED)
            .map("WFOV_UNBINNED", K4A_DEPTH_MODE_WFOV_UNBINNED)
            .map("PASSIVE_IR", K4A_DEPTH_MODE_PASSIVE_IR);
 
        defineOptionalProperty<float>("MaxDepth",
                                      6000.0,
                                      "Max. allowed depth value in mm. Depth values above this "
                                      "threshold will be set to nan.");
 
        defineOptionalProperty<float>("CaptureTimeOffset",
                                      16.0f,
                                      "In Milliseconds. Time offset between capturing the image on "
                                      "the hardware and receiving the image in this process.",
                                      armarx::PropertyDefinitionBase::eModifiable);
 
        defineOptionalProperty<k4a_image_format_t>("ColorFormat",
                                                   K4A_IMAGE_FORMAT_COLOR_BGRA32,
                                                   "Color format of the RGB camera image.")
            .map("BGRA", K4A_IMAGE_FORMAT_COLOR_BGRA32)
            .map("YUY2", K4A_IMAGE_FORMAT_COLOR_YUY2);
    }
 
    armarx::PropertyDefinitionsPtr
    AzureKinectPointCloudProvider::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new AzureKinectPointCloudProviderPropertyDefinitions(getConfigIdentifier()));
 
        defs->optional(
            enableColorUndistortion,
            "EnableColorUndistortion",
            "Undistort the color images using the full 8 radial and tangential distortion "
            "parameters provided by the Azure Kinect.\n"
            "This can help for processing tasks which cannot handle radial parameters k3-k6.\n"
            "Note that this drastically reduces the FPS (to something like 3).");
        defs->optional(externalCalibrationFilePath,
                       "ExternalCalibrationFilePath",
                       "Path to an optional external"
                       " calibration file, which has a"
                       " camera matrix and distortion"
                       " parameters.");
        defs->optional(mDeviceId, "device_id", "ID of the device.");
 
        defs->optional(robotName, "robotName");
        defs->optional(bodyCameraFrameName, "bodyCameraFrameName");
        defs->optional(framerate.value,
                       "framerate.images",
                       "The framerate of RGB-D images [frames per second]."
                       "\nNote that the point cloud and body tracking frame rates are controlled by"
                       " the properties 'framerate' (point cloud) and 'framerate.bodyTracking'"
                       " (body tracking), respectively.")
            .setMin(5)
            .setMax(30);
 
#ifdef INCLUDE_BODY_TRACKING
        defs->optional(bodyTrackingEnabled,
                       "bodyTrackingEnabled",
                       "Whether the Azure Kinect Body Tracking SDK should be enabled or not.");
        defs->optional(bodyTrackingRunAtStart,
                       "bodyTrackingRunAtStart",
                       "Whether the Azure Kinect Body Tracking SDK should directly run when the "
                       "component is startet."
                       "Otherwise it has to be activated by the ice interface.");
        defs->optional(bodyTrackingModelFilename,
                       "bodyTrackingModelPath",
                       "Path where the .onnx DNN files can be found");
        defs->optional(bodyTrackingGPUDeviceID, "bodyTrackingGPUDeviceID", "GPU Device ID.");
        defs->optional(bodyTrackingTemporalSmoothingFactor,
                       "bodyTrackingTemporalSmoothingFactor",
                       "Temporal smoothing factor for Azure Kinect body tracking.");
        defs->optional(useCPU, "useCPU", "Whether to use cpu");
 
        defs->optional(bodyTrackingDepthMaskMinX, "bodyTrackingDepthMaskMinX");
        defs->optional(bodyTrackingDepthMaskMaxX, "bodyTrackingDepthMaskMaxX");
        defs->optional(bodyTrackingDepthMaskMaxZ, "bodyTrackingDepthMaskMaxZ");
        defs->optional(framerate.bodyTracking.value,
                       "framerate.bodyTracking",
                       "The framerate with with the body tracking is run [frames per second]."
                       "\nNote that the RGB-D image and point cloud frame rates are controlled by"
                       " the properties 'framerate.image' (RGB-D images) and 'framerate'"
                       " (point cloud), respectively.")
            .setMin(1.)
            .setMax(30.);
 
        defs->optional(startIMU, "startIMU");
 
        humanPoseWriter.registerPropertyDefinitions(defs);
#endif
 
        defs->optional(enableHeartbeat, "enableHeartbeat");
 
        return defs;
    }
 
    void
    AzureKinectPointCloudProvider::onInitImageProvider()
    {
        config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
        if (framerate.value == 5)
        {
            config.camera_fps = K4A_FRAMES_PER_SECOND_5;
        }
        else if (framerate.value == 15)
        {
            config.camera_fps = K4A_FRAMES_PER_SECOND_15;
        }
        else if (framerate.value == 30)
        {
            config.camera_fps = K4A_FRAMES_PER_SECOND_30;
        }
        else
        {
            throw armarx::LocalException("Invalid image framerate (property 'framerate.images'): ")
                << framerate.value << ".  Only framerates 5, 15 and 30 are "
                << "supported by Azure Kinect.";
        }
 
        framerate.pointCloud.value =
            getProperty<float>("framerate"); // point cloud provider framerate
        framerate.pointCloud.update(framerate.value);
 
#ifdef INCLUDE_BODY_TRACKING
        framerate.bodyTracking.update(framerate.value);
#endif
 
        config.depth_mode = getProperty<k4a_depth_mode_t>("DepthMode");
        config.color_format = getProperty<k4a_image_format_t>("ColorFormat");
        config.color_resolution = getProperty<k4a_color_resolution_t>("ColorResolution");
 
        // This means that we'll only get captures that have both color and depth images, so we don't
        // need to check if the capture contains a particular type of image.
        config.synchronized_images_only = true;
 
        // Set number of images per frame.
        setNumberImages(2);
 
        auto depth_dim = GetDepthDimensions(config.depth_mode);
        auto color_dim = GetColorDimensions(config.color_resolution);
 
        ARMARX_INFO << "Depth image size: " << depth_dim.first << "x" << depth_dim.second;
        ARMARX_INFO << "Color image size: " << color_dim.first << "x" << color_dim.second;
 
        alignedDepthImage =
            k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
                               color_dim.first,
                               color_dim.second,
                               color_dim.first * 2 * static_cast<int32_t>(sizeof(uint8_t)));
 
        setImageFormat(visionx::ImageDimension(color_dim.first, color_dim.second),
                       visionx::eRgb,
                       visionx::eBayerPatternGr);
 
        resultDepthImage.reset(visionx::tools::createByteImage(getImageFormat(), visionx::eRgb));
        resultColorImage =
            std::make_unique<CByteImage>(color_dim.first, color_dim.second, CByteImage::eRGB24);
 
        xyzImage = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
                                      color_dim.first,
                                      color_dim.second,
                                      color_dim.first * 3 * static_cast<int32_t>(sizeof(int16_t)));
 
        pointcloud = std::make_unique<pcl::PointCloud<CloudPointType>>();
    }
 
    void
    AzureKinectPointCloudProvider::onConnectImageProvider()
    {
        ARMARX_DEBUG << "Connecting " << getName();
 
        setDebugObserverBatchModeEnabled(true);
 
        // Note: The point cloud processing task is *not* started here.  It is started at the end
        // of `onStartCapture()`, because it uses `transformation`, which only exists from that
        // point on.
 
#ifdef INCLUDE_BODY_TRACKING
        if (bodyTrackingEnabled)
        {
            ARMARX_INFO << "Connecting to human memory ...";
            humanPoseWriter.connect(memoryNameSystem());
            ARMARX_INFO << "Connected to human memory.";
 
            bodyTrackingPublishTask = new armarx::RunningTask<AzureKinectPointCloudProvider>(
                this,
                &AzureKinectPointCloudProvider::runPublishBodyTrackingResults);
        }
#endif
 
        if (enableHeartbeat)
        {
            ARMARX_CHECK_NOT_NULL(heartbeatPlugin);
            heartbeatPlugin->signUp(armarx::Duration::MilliSeconds(500),
                                    armarx::Duration::MilliSeconds(1000),
                                    {"Vision", "Camera"},
                                    "AzureKinectPointCloudProvider");
        }
 
        ARMARX_VERBOSE << "Connected " << getName();
    }
 
    void
    AzureKinectPointCloudProvider::onDisconnectImageProvider()
    {
        ARMARX_DEBUG << "Disconnecting " << getName();
 
        // Stop task.  It is only present if capturing was started (see `onStartCapture()`).
        if (pointcloudTask)
        {
            std::lock_guard<std::mutex> lock(pointcloudProcMutex);
            ARMARX_DEBUG << "Stopping pointcloud processing thread...";
            const bool WAIT_FOR_JOIN = false;
            pointcloudTask->stop(WAIT_FOR_JOIN);
            pointcloudProcSignal.notify_all();
            ARMARX_DEBUG << "Waiting for pointcloud processing thread to stop...";
            pointcloudTask->waitForStop();
            ARMARX_DEBUG << "Pointcloud processing thread stopped";
        }
 
#ifdef INCLUDE_BODY_TRACKING
        if (bodyTrackingIsRunning)
        {
            bodyTrackingPublishTask->stop();
        }
#endif
 
        ARMARX_DEBUG << "Disconnected " << getName();
    }
 
    void
    AzureKinectPointCloudProvider::onInitCapturingPointCloudProvider()
    {
        this->setPointCloudSyncMode(eCaptureSynchronization);
        ARMARX_TRACE;
    }
 
    void
    AzureKinectPointCloudProvider::onExitCapturingPointCloudProvider()
    {
        ARMARX_INFO << "Closing Azure Kinect device";
        device.close();
        ARMARX_INFO << "Closed Azure Kinect device";
    }
 
    void
    AzureKinectPointCloudProvider::onStartCapture(float /* framesPerSecond */)
    {
        ARMARX_TRACE;
 
        cloudFormat = getPointCloudFormat();
 
        // Check for devices.
        const uint32_t DEVICE_COUNT = k4a::device::get_installed_count();
        if (DEVICE_COUNT == 0)
        {
            getArmarXManager()->asyncShutdown();
            throw armarx::LocalException("No Azure Kinect devices detected!");
        }
 
        ARMARX_TRACE;
 
        device = k4a::device::open(static_cast<uint32_t>(mDeviceId));
        ARMARX_DEBUG << "Opened device id #" << mDeviceId << " with serial number "
                     << device.get_serialnum() << ".";
 
        ARMARX_TRACE;
 
        auto depthDim = GetDepthDimensions(config.depth_mode);
        auto colorDim = GetColorDimensions(config.color_resolution);
        (void)depthDim;
        (void)colorDim;
 
        k4aCalibration = device.get_calibration(config.depth_mode, config.color_resolution);
        ARMARX_INFO
            << "Color camera calibration:"
            << "\n"
            << "cx: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.cx
            << "\n"
            << "cy: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.cy
            << "\n"
            << "fx: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.fx
            << "\n"
            << "fy: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.fy
            << "\n"
            << "k1: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.k1
            << "\n"
            << "k2: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.k2
            << "\n"
            << "p1: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.p1
            << "\n"
            << "p2: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.p2
            << "\n"
            << "k3: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.k3
            << "\n"
            << "k4: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.k4
            << "\n"
            << "k5: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.k5
            << "\n"
            << "k6: " << k4aCalibration.color_camera_calibration.intrinsics.parameters.param.k6;
 
 
        auto c = getStereoCalibration(Ice::Current());
        CCalibration* c_left = tools::convert(c.calibrationLeft);
        c_left->PrintCameraParameters();
        delete c_left;
        CCalibration* c_right = tools::convert(c.calibrationRight);
        c_right->PrintCameraParameters();
        delete c_right;
 
        // `k4a::transformation`'s calibration constructor is `noexcept`: on failure it silently
        // yields a null handle, and the problem only surfaces later as a per-frame
        // "Failed to transform depth image to point cloud!".  Create the handle explicitly so
        // that the actual cause is reported here, where it happens.
        k4a_transformation_t transformationHandle = k4a_transformation_create(&k4aCalibration);
        ARMARX_CHECK_NOT_NULL(transformationHandle)
            << "Failed to create the Azure Kinect transformation.  The most common cause is that "
               "the depth engine plugin 'libdepthengine.so.2.0' could not be loaded.  Make sure "
               "it is on LD_LIBRARY_PATH (axii module 'deps/azure-kinect/depth-engine').";
        transformation = k4a::transformation(transformationHandle);
 
#ifdef INCLUDE_BODY_TRACKING
        if (bodyTrackingEnabled)
        {
            // eventually, resolve environment variable
            armarx::ArmarXDataPath::ReplaceEnvVars(bodyTrackingModelFilename);
 
            const bool found = armarx::ArmarXDataPath::getAbsolutePath(bodyTrackingModelFilename,
                                                                       bodyTrackingModelFilename);
            ARMARX_CHECK(found) << "Body tracking DNN model could not be found/resolved at `"
                                << bodyTrackingModelFilename << "`.";
 
            ARMARX_INFO << "Using body tracking DNN model from directory `"
                        << bodyTrackingModelFilename << "`.";
 
            ARMARX_CHECK(std::filesystem::exists(bodyTrackingModelFilename))
                << "The path `" << bodyTrackingModelFilename << "` does not exist!";
 
            k4abt_tracker_configuration_t const bodyTrackingConfig{
                .sensor_orientation = K4ABT_SENSOR_ORIENTATION_DEFAULT,
                .processing_mode = useCPU ? K4ABT_TRACKER_PROCESSING_MODE_CPU
                                          : K4ABT_TRACKER_PROCESSING_MODE_GPU_CUDA,
                .gpu_device_id = bodyTrackingGPUDeviceID,
                .model_path = bodyTrackingModelFilename.c_str()};
 
            bodyTracker = k4abt::tracker::create(k4aCalibration, bodyTrackingConfig);
            bodyTracker.set_temporal_smoothing(bodyTrackingTemporalSmoothingFactor);
 
            if (bodyTrackingRunAtStart and not bodyTrackingIsRunning)
            {
                bodyTrackingIsRunning = true;
                bodyTrackingPublishTask->start();
            }
        }
#endif
 
        ARMARX_TRACE;
        device.set_color_control(K4A_COLOR_CONTROL_BRIGHTNESS, K4A_COLOR_CONTROL_MODE_MANUAL, 128);
 
        device.start_cameras(&config);
 
        ARMARX_TRACE;
 
        if (startIMU)
        {
            device.start_imu();
            ARMARX_INFO << "IMU is active" << std::endl;
 
            ARMARX_TRACE;
        }
 
        setMetaInfo("serialNumber", new armarx::Variant(device.get_serialnum()));
        setMetaInfo("rgbVersion", new armarx::Variant(VersionToString(device.get_version().rgb)));
        setMetaInfo("depthVersion",
                    new armarx::Variant(VersionToString(device.get_version().depth)));
        setMetaInfo("depthSensorVersion",
                    new armarx::Variant(VersionToString(device.get_version().depth_sensor)));
        setMetaInfo("audioVersion",
                    new armarx::Variant(VersionToString(device.get_version().audio)));
 
        ARMARX_TRACE;
 
 
        // Color image calibration
        {
            // Load intrinsics from camera
            const k4a_calibration_camera_t calibration{k4aCalibration.color_camera_calibration};
            const k4a_calibration_intrinsic_parameters_t::_param param =
                calibration.intrinsics.parameters.param;
 
            // // Scale intrinsics according to image scale
            // param.fx *= image_scale_;
            // param.fy *= image_scale_;
 
            // Calculate distortion map
            cv::Mat1f camera_matrix(3, 3);
            camera_matrix << param.fx, 0, param.cx, 0, param.fy, param.cy, 0, 0, 1;
            cv::Mat1f new_camera_matrix(3, 3);
            new_camera_matrix << param.fx, 0, param.cx, 0, param.fx, param.cy, 0, 0, 1;
            cv::Mat1f distortion_coeff(1, 8);
            distortion_coeff << param.k1, param.k2, param.p1, param.p2, param.k3, param.k4,
                param.k5, param.k6;
            cv::Mat map1, map2, map3;
            cv::initUndistortRectifyMap(
                camera_matrix,
                distortion_coeff,
                cv::Mat{},
                new_camera_matrix,
                cv::Size{calibration.resolution_width, calibration.resolution_height},
                CV_32FC1,
                map1,
                map2);
            cv::convertMaps(map1, map2, colorDistortionMap, map3, CV_16SC2, true);
        }
 
        // Start the point cloud processing task only now: everything it touches (in particular
        // `transformation`, `alignedDepthImage` and `xyzImage`) is set up at this point.
        if (pointcloudTask)
        {
            // Capturing was started before without an intermediate disconnect.
            std::lock_guard<std::mutex> lock(pointcloudProcMutex);
            pointcloudTask->stop(false);
            pointcloudProcSignal.notify_all();
        }
        if (pointcloudTask)
        {
            pointcloudTask->waitForStop();
        }
        {
            std::lock_guard<std::mutex> lock(pointcloudProcMutex);
            depthImageReady = false;
            depthImageProcessed = false;
        }
        pointcloudTask = armarx::RunningTask<AzureKinectPointCloudProvider>::pointer_type(
            new armarx::RunningTask<AzureKinectPointCloudProvider>(
                this,
                &AzureKinectPointCloudProvider::runPointcloudPublishing));
        pointcloudTask->start();
    }
 
    void
    AzureKinectPointCloudProvider::onStopCapture()
    {
        if (startIMU)
        {
            device.stop_imu();
        }
    }
 
    bool
    AzureKinectPointCloudProvider::doCapture()
    {
        using armarx::core::time::ScopedStopWatch;
        using armarx::core::time::StopWatch;
 
        ScopedStopWatch sw_total{createSwCallback("doCapture")};
 
        k4a::capture capture;
        const std::chrono::milliseconds TIMEOUT{1000};
 
        StopWatch sw_get_capture;
        bool status = false;
        try
        {
            ARMARX_DEBUG << "Try capture.";
            status = device.get_capture(&capture, TIMEOUT);
            ARMARX_DEBUG << "Got capture.";
        }
        catch (const std::exception&)
        {
            ARMARX_WARNING << "Failed to get capture from device (#" << ++mDiagnostics.num_crashes
                           << ").  Restarting camera.";
            StopWatch sw;
            device.stop_cameras();
            device.start_cameras(&config);
            ARMARX_INFO << "Restarting took " << sw.stop() << ".";
            return false;
        }
 
        if (status)
        {
            createSwCallback("waiting for get_capture")(sw_get_capture.stop());
 
            {
                std::lock_guard g{metaInfoMtx};
                setMetaInfo("temperature", new armarx::Variant(capture.get_temperature_c()));
            }
 
            // see ROS: This will give INCORRECT timestamps until the first image.
            const bool mustInitializeTimestampOffset = [&]()
            {
                std::lock_guard g{deviceToRealtimeOffsetMtx};
                return device_to_realtime_offset_.count() == 0;
            }();
            if (mustInitializeTimestampOffset)
            {
                initializeTimestampOffset(capture.get_depth_image().get_device_timestamp());
            }
 
            // next, we update the timestamp offset continuously
            updateTimestampOffset(capture.get_ir_image().get_device_timestamp(),
                                  capture.get_ir_image().get_system_timestamp());
 
            const k4a::image DEPTH_IMAGE = capture.get_depth_image();
 
            //  This function assumes that the image is made of depth pixels (i.e. uint16_t's),
            //  which is only true for IR/depth images.
            const k4a_image_format_t IMAGE_FORMAT = DEPTH_IMAGE.get_format();
            if (IMAGE_FORMAT != K4A_IMAGE_FORMAT_DEPTH16 && IMAGE_FORMAT != K4A_IMAGE_FORMAT_IR16)
            {
                const char* format_str = ::k4aImageFormatToString(IMAGE_FORMAT);
                std::stringstream error_msg;
                error_msg << "Attempted to colorize a non-depth image with format: " << format_str;
                throw std::logic_error(error_msg.str());
            }
 
            // Provide data for pointcloud processing thread and signal to start processing.
            if (not framerate.pointCloud.skip())
            {
                ScopedStopWatch sw{createSwCallback("transform depth image to camera")};
                // Acquire lock and write data needed by pointcloud thread (i.e.,
                // alignedDepthImageScaled and depthImageReady).
                {
                    std::lock_guard lock{pointcloudProcMutex};
                    transformation.depth_image_to_color_camera(DEPTH_IMAGE, &alignedDepthImage);
 
                    // Signal that point cloud processing may proceed and reset processed flag.
                    depthImageReady = true;
                }
 
                ARMARX_DEBUG << "Notifying pointcloud thread.";
                pointcloudProcSignal.notify_one();
            }
 
#ifdef INCLUDE_BODY_TRACKING
            if (bodyTrackingIsRunning)
            {
                ARMARX_DEBUG << "body Tracking Is Running.";
                std::scoped_lock lock(bodyTrackingParameterMutex);
 
                if (not framerate.bodyTracking.skip())
                {
                    k4a::image ir_image = capture.get_ir_image();
                    std::uint8_t* ir_image_buffer = ir_image.get_buffer();
 
                    k4a::image depth_image = capture.get_depth_image();
                    std::uint8_t* depth_image_buffer = depth_image.get_buffer();
 
                    ARMARX_CHECK_EQUAL(ir_image.get_width_pixels(), depth_image.get_width_pixels());
                    ARMARX_CHECK_EQUAL(ir_image.get_height_pixels(),
                                       depth_image.get_height_pixels());
 
                    const int stride = ir_image.get_stride_bytes() / ir_image.get_width_pixels();
 
                    for (int x = 0; x < ir_image.get_width_pixels(); x++)
                    {
                        for (int y = 0; y < ir_image.get_height_pixels(); y++)
                        {
                            const int i = (y * ir_image.get_width_pixels() * stride) + (x * stride);
                            const int z = (static_cast<int>(depth_image_buffer[i])) +
                                          (static_cast<int>(depth_image_buffer[i + 1]) << 8);
 
                            if ((bodyTrackingDepthMaskMinX > 0 and x < bodyTrackingDepthMaskMinX) or
                                (bodyTrackingDepthMaskMaxX > 0 and x > bodyTrackingDepthMaskMaxX) or
                                (bodyTrackingDepthMaskMaxZ > 0 and z > bodyTrackingDepthMaskMaxZ))
                            {
                                ir_image_buffer[i] = std::numeric_limits<std::uint8_t>::max();
                                ir_image_buffer[i + 1] = std::numeric_limits<std::uint8_t>::max();
                                depth_image_buffer[i] = std::numeric_limits<std::uint8_t>::max();
                                depth_image_buffer[i + 1] =
                                    std::numeric_limits<std::uint8_t>::max();
                            }
                        }
                    }
 
                    if (not bodyTracker.enqueue_capture(capture))
                    {
                        ARMARX_WARNING << "Add capture to tracker process queue timeout";
                    }
                }
            }
#endif
 
            const k4a::image COLOR_IMAGE = capture.get_color_image();
            ARMARX_DEBUG << "Got COLOR image.";
 
            auto real_time = IceUtil::Time::now();
            auto monotonic_time = IceUtil::Time::now(IceUtil::Time::Monotonic);
            auto clock_diff = real_time - monotonic_time;
 
            auto image_monotonic_time =
                IceUtil::Time::microSeconds(std::chrono::duration_cast<std::chrono::microseconds>(
                                                DEPTH_IMAGE.get_system_timestamp())
                                                .count());
            long offset = long(getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f);
 
            imagesTime = image_monotonic_time + clock_diff - IceUtil::Time::microSeconds(offset);
 
            {
                std::lock_guard g{metaInfoMtx};
                setMetaInfo("image age in [ms]",
                            new armarx::Variant{(real_time - imagesTime).toMilliSecondsDouble()});
            }
 
            {
                std::lock_guard g{debugObserverMtx};
                setDebugObserverDatafield("image age in [ms]",
                                          (real_time - imagesTime).toMilliSecondsDouble());
            }
 
            if (enableColorUndistortion)
            {
                // only the color image needs to be rectified
                cv::Mat tmp_rgb_image;
 
                if (COLOR_IMAGE.get_format() == K4A_IMAGE_FORMAT_COLOR_YUY2)
                {
                    ARMARX_DEBUG << "Converting YUY2 image.";
                    cv::Mat yuy2_image(COLOR_IMAGE.get_height_pixels(),
                                       COLOR_IMAGE.get_width_pixels(),
                                       CV_8UC2,
                                       const_cast<uint8_t*>(COLOR_IMAGE.get_buffer()));
 
                    cv::cvtColor(yuy2_image, tmp_rgb_image, cv::COLOR_YUV2RGB_YUY2);
                }
                else if (COLOR_IMAGE.get_format() == K4A_IMAGE_FORMAT_COLOR_BGRA32)
                {
                    cv::cvtColor(cv::Mat{cv::Size{COLOR_IMAGE.get_width_pixels(),
                                                  COLOR_IMAGE.get_height_pixels()},
                                         CV_8UC4,
                                         (void*)COLOR_IMAGE.get_buffer(),
                                         cv::Mat::AUTO_STEP},
                                 tmp_rgb_image,
                                 cv::COLOR_BGRA2RGB);
                }
                else
                {
                    throw std::runtime_error("Unsupported color format in k4a image");
                }
 
                cv::Mat cv_color_image_undistorted(COLOR_IMAGE.get_width_pixels(),
                                                   COLOR_IMAGE.get_height_pixels(),
                                                   CV_8UC3);
 
                cv::remap(tmp_rgb_image,
                          cv_color_image_undistorted,
                          colorDistortionMap,
                          cv::Mat{},
                          cv::INTER_NEAREST,
                          cv::BORDER_CONSTANT);
 
                visionx::imrec::convert_rgb2cbi(cv_color_image_undistorted, *resultColorImage);
            }
            else
            {
                // Convert the k4a image to an IVT image.
                ScopedStopWatch sw{createSwCallback("convert k4a image to IVT")};
                ::k4aToIvtImage(COLOR_IMAGE, *resultColorImage);
            }
 
 
            // Prepare result depth image.
            {
                ScopedStopWatch sw{createSwCallback("prepare result depth image")};
 
                const int DW = alignedDepthImage.get_width_pixels();
                const int DH = alignedDepthImage.get_height_pixels();
 
                ARMARX_CHECK_EXPRESSION(resultDepthImage);
                ARMARX_CHECK_EQUAL(resultDepthImage->width, DW);
                ARMARX_CHECK_EQUAL(resultDepthImage->height, DH);
 
                auto result_depth_buffer = resultDepthImage->pixels;
                const auto* depth_buffer =
                    reinterpret_cast<const uint16_t*>(alignedDepthImage.get_buffer());
 
                int index = 0;
                int index_2 = 0;
                for (int y = 0; y < DH; ++y)
                {
                    for (int x = 0; x < DW; ++x)
                    {
                        uint16_t depth_value = depth_buffer[index_2];
                        visionx::tools::depthValueToRGB(depth_value,
                                                        result_depth_buffer[index],
                                                        result_depth_buffer[index + 1],
                                                        result_depth_buffer[index + 2]);
                        index += 3;
                        index_2 += 1;
                    }
                }
            }
 
            // Broadcast RGB-D image.
            {
                ScopedStopWatch sw{createSwCallback("broadcast RGB-D image")};
                CByteImage* images[2] = {resultColorImage.get(), resultDepthImage.get()};
                provideImages(images, imagesTime);
            }
 
            // Wait until `alignedDepthImage` was processed and can be overridden again.
            {
                std::unique_lock signal_lock{pointcloudProcMutex};
                ARMARX_DEBUG << "Capturing thread waiting for signal...";
                // Never wait unconditionally here: if the point cloud thread is not running (or
                // is stuck), an unbounded wait silently wedges the entire component -- no point
                // clouds, no RGB-D images and no further log output.
                const std::chrono::milliseconds POINTCLOUD_TIMEOUT{1000};
                if (not pointcloudProcSignal.wait_for(
                        signal_lock, POINTCLOUD_TIMEOUT, [&] { return depthImageProcessed; }))
                {
                    ARMARX_WARNING << deactivateSpam(10)
                                   << "Point cloud processing did not signal completion within "
                                   << POINTCLOUD_TIMEOUT.count()
                                   << " ms.  Continuing capture without it.";
                }
                ARMARX_DEBUG << "Capturing thread received signal.";
            }
 
            {
                std::lock_guard g{debugObserverMtx};
                sendDebugObserverBatch();
            }
 
            if (enableHeartbeat)
            {
                heartbeatPlugin->heartbeat();
            }
 
            // return true;
        }
        else
        {
            ARMARX_INFO << deactivateSpam(30) << "Did not get frame until timeout of " << TIMEOUT;
 
            {
                std::lock_guard g{debugObserverMtx};
                sendDebugObserverBatch();
            }
 
            return false;
        }
 
        if (startIMU)
        {
            try
            {
                Eigen::Vector3f accMean = Eigen::Vector3f::Zero();
                std::size_t cnt = 0;
 
                k4a_imu_sample_t imu_sample;
                while (device.get_imu_sample(&imu_sample, std::chrono::milliseconds(0)))
                {
                    const Eigen::Vector3f acceleration{imu_sample.acc_sample.xyz.x,
                                                       imu_sample.acc_sample.xyz.y,
                                                       imu_sample.acc_sample.xyz.z};
 
                    accMean += acceleration;
                    cnt++;
                }
 
                accMean /= static_cast<float>(cnt);
 
                ARMARX_INFO << "Got IMU sample";
 
                {
                    std::lock_guard g{debugObserverMtx};
 
                    setDebugObserverDatafield("acceleration.x", accMean.x());
                    setDebugObserverDatafield("acceleration.y", accMean.y());
                    setDebugObserverDatafield("acceleration.z", accMean.z());
 
                    sendDebugObserverBatch();
                }
            }
            catch (const std::exception&)
            {
                ARMARX_WARNING << "Failed to get imu samples from device (#"
                               << ++mDiagnostics.num_crashes << ").";
            }
        }
 
        return true;
    }
 
#ifdef INCLUDE_BODY_TRACKING
    void
    AzureKinectPointCloudProvider::runPublishBodyTrackingResults()
    {
        armarx::Metronome metronome(armarx::Frequency::Hertz(100));
        while (bodyTrackingIsRunning)
        {
 
            // body frames might just not be available.
            const k4abt::frame body_frame = [&]() -> k4abt::frame
            {
                try
                {
                    auto result = bodyTracker.pop_result();
                    return result;
                }
                catch (...)
                {
                    ARMARX_VERBOSE << deactivateSpam(1) << "Exception in body tracking publishing: "
                                   << armarx::GetHandledExceptionString();
                    return {};
                }
            }();
 
            if (body_frame != nullptr)
            {
                armarx::core::time::ScopedStopWatch sw{
                    createSwCallback("publish body tracking result")};
                // see https://github.com/microsoft/Azure_Kinect_ROS_Driver/blob/melodic/src/k4a_ros_device.cpp
                const armarx::DateTime timestamp =
                    timestampToArmarX(body_frame.get_device_timestamp());
 
                {
                    auto real_time = IceUtil::Time::now();
                    auto monotonic_time = IceUtil::Time::now(IceUtil::Time::Monotonic);
                    auto clock_diff = real_time - monotonic_time;
 
                    auto image_monotonic_time = IceUtil::Time::microSeconds(
                        std::chrono::duration_cast<std::chrono::microseconds>(
                            body_frame.get_system_timestamp())
                            .count());
                    // long offset = long(getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f);
 
                    // IceUtil::Time imageTime = image_monotonic_time + clock_diff - IceUtil::Time::microSeconds(offset);
                    IceUtil::Time imageTime = image_monotonic_time + clock_diff;
 
                    {
                        std::lock_guard g{debugObserverMtx};
                        setDebugObserverDatafield("ros_vs_ice_timestamp [µs]",
                                                  imageTime.toMicroSeconds());
                    }
                }
 
                {
                    const armarx::Clock realtimeClock = armarx::Clock(armarx::ClockType::Realtime);
                    const armarx::Clock monotonicClock =
                        armarx::Clock(armarx::ClockType::Monotonic);
 
                    auto real_time = realtimeClock.now();
                    auto monotonic_time = monotonicClock.now();
                    auto clock_diff = real_time - monotonic_time;
 
                    auto image_monotonic_time = armarx::Duration::MicroSeconds(
                        std::chrono::duration_cast<std::chrono::microseconds>(
                            body_frame.get_system_timestamp())
                            .count());
                    // long offset = long(getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f);
 
                    // IceUtil::Time imageTime = image_monotonic_time + clock_diff - IceUtil::Time::microSeconds(offset);
                    auto imageTime = image_monotonic_time + clock_diff;
 
                    armarx::DateTime imageTimestamp = armarx::DateTime(imageTime);
 
                    {
                        std::lock_guard g{debugObserverMtx};
                        setDebugObserverDatafield("ros_vs_armarx_timestamp [µs]",
                                                  imageTime.toMicroSeconds());
                    }
                }
 
 
                std::uint32_t num_bodies = body_frame.get_num_bodies();
                {
                    std::lock_guard g{debugObserverMtx};
                    setDebugObserverDatafield("n_bodies_detected", num_bodies);
                }
 
                std::vector<armarx::armem::human::HumanPose> humanPoses;
                humanPoses.reserve(num_bodies);
 
                for (std::uint32_t i = 0; i < num_bodies; i++)
                {
                    k4abt_body_t body = body_frame.get_body(i);
                    printBodyInformation(body);
 
                    armarx::armem::human::HumanPose humanPose;
                    humanPose.humanTrackingId = std::to_string(body.id);
                    humanPose.cameraFrameName = bodyCameraFrameName;
                    humanPose.poseModelId = armarx::human::pose::model::k4a_bt_body_32::ModelId;
 
                    for (int i = 0; i < static_cast<int>(K4ABT_JOINT_COUNT); i++)
                    {
                        const auto joint =
                            static_cast<armarx::human::pose::model::k4a_bt_body_32::Joints>(i);
                        const auto name =
                            armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(joint);
 
                        k4a_float3_t position = body.skeleton.joints[i].position;
                        k4a_quaternion_t orientation = body.skeleton.joints[i].orientation;
                        k4abt_joint_confidence_level_t confidence_level =
                            body.skeleton.joints[i].confidence_level;
 
                        humanPose.keypoints[name] = armarx::armem::human::PoseKeypoint{
                            .label = name,
                            .confidence = static_cast<float>(static_cast<int>(confidence_level)),
                            .positionCamera = armarx::FramedPosition(
                                Eigen::Vector3f{position.v[0], position.v[1], position.v[2]},
                                bodyCameraFrameName,
                                robotName),
                            .orientationCamera =
                                armarx::FramedOrientation(Eigen::Quaternionf(orientation.wxyz.w,
                                                                             orientation.wxyz.x,
                                                                             orientation.wxyz.y,
                                                                             orientation.wxyz.z)
                                                              .toRotationMatrix(),
                                                          bodyCameraFrameName,
                                                          robotName)};
                    }
 
                    humanPoses.push_back(humanPose);
                }
 
                {
                    std::lock_guard g{debugObserverMtx};
                    setDebugObserverDatafield("bodyTrackingCaptureUntilPublish [ms]",
                                              (armarx::Clock::Now() - timestamp).toMilliSeconds());
                }
 
 
                {
                    ARMARX_TRACE;
                    //ARMARX_INFO << deactivateSpam(1) << "committing human poses";
                    humanPoseWriter.commitHumanPosesInCameraFrame(humanPoses, getName(), timestamp);
                }
 
 
                // k4a::image body_index_map = body_frame.get_body_index_map();
                // if (body_index_map != nullptr)
                // {
                //     //print_body_index_map_middle_line(body_index_map);
                // }
                // else
                // {
                //     ARMARX_WARNING << "Error: Failed to generate bodyindex map!";
                // }
            }
            else
            {
                //  It should never hit timeout when K4A_WAIT_INFINITE is set.
                ARMARX_VERBOSE << "Error! Pop body frame result time out!";
            }
 
            metronome.waitForNextTick();
        }
    }
#endif
 
    void
    AzureKinectPointCloudProvider::runPointcloudPublishing()
    {
        ARMARX_DEBUG << "Started pointcloud processing task.";
 
        IceUtil::Time time;
 
        // Main pointcloud processing loop.
        while (not pointcloudTask->isStopped())
        {
            // Wait for data.
            std::unique_lock signal_lock{pointcloudProcMutex};
            ARMARX_DEBUG << "Pointcloud thread waiting for signal...";
            pointcloudProcSignal.wait(signal_lock,
                                      [&]
                                      { return pointcloudTask->isStopped() or depthImageReady; });
            ARMARX_DEBUG << "Pointcloud thread received signal.";
 
            if (pointcloudTask->isStopped())
            {
                break;
            }
 
            // Assess timings, reset flags.
            const IceUtil::Time TIMESTAMP = imagesTime;
            depthImageReady = false;
            depthImageProcessed = false;
 
            // Everything from here up to `depthImageProcessed = true` must not escape this loop:
            // an exception would terminate this thread, and the capturing thread would then never
            // be released again (see the wait in `doCapture()`).
            bool pointcloudValid = true;
            try
            {
 
                // Transform depth image to pointcloud.
                {
                    time = armarx::TimeUtil::GetTime(armarx::TimeMode::SystemTime);
                    transformation.depth_image_to_point_cloud(
                        alignedDepthImage, K4A_CALIBRATION_TYPE_COLOR, &xyzImage);
                    ARMARX_DEBUG << "Transforming depth image to point cloud took "
                                 << armarx::TimeUtil::GetTimeSince(time,
                                                                   armarx::TimeMode::SystemTime);
                }
 
                // Construct PCL pointcloud.
                {
                    time = armarx::TimeUtil::GetTime(armarx::TimeMode::SystemTime);
 
                    ARMARX_CHECK_EXPRESSION(pointcloud);
 
                    pointcloud->width = static_cast<uint32_t>(xyzImage.get_width_pixels());
                    pointcloud->height = static_cast<uint32_t>(xyzImage.get_height_pixels());
 
                    ARMARX_TRACE;
 
                    pointcloud->is_dense = false;
                    pointcloud->points.resize(pointcloud->width * pointcloud->height);
 
                    auto k4a_cloud_buffer = reinterpret_cast<const int16_t*>(xyzImage.get_buffer());
 
                    ARMARX_CHECK_EXPRESSION(k4a_cloud_buffer);
 
                    unsigned char* color_buffer = resultColorImage->pixels;
                    while (color_buffer == nullptr)
                    {
                        ARMARX_INFO
                            << "color_buffer is null. This should never happen. There is "
                               "probably a race condition somewhere that needs to be fixed. "
                               "Temporarily we ignore this and continue.\n Timestamp: "
                            << armarx::Clock::Now().toMilliSecondsSinceEpoch() << "\n";
                        color_buffer = resultColorImage->pixels;
                    }
                    ARMARX_CHECK_NOT_NULL(color_buffer);
 
                    ARMARX_CHECK_EQUAL(xyzImage.get_width_pixels(), resultColorImage->width);
                    ARMARX_CHECK_EQUAL(xyzImage.get_height_pixels(), resultColorImage->height);
                    ARMARX_CHECK_EQUAL(pointcloud->points.size(),
                                       pointcloud->width * pointcloud->height);
 
                    ARMARX_TRACE;
 
                    size_t index = 0;
                    float max_depth = getProperty<float>("MaxDepth").getValue();
                    for (auto& p : pointcloud->points)
                    {
                        p.r = color_buffer[index];
                        p.x = k4a_cloud_buffer[index];
 
                        index++;
 
                        p.g = color_buffer[index];
                        p.y = k4a_cloud_buffer[index];
 
                        index++;
 
                        p.b = color_buffer[index];
                        auto z = k4a_cloud_buffer[index];
 
                        index++;
 
                        if (z <= max_depth and z != 0)
                        {
                            p.z = z;
                        }
                        else
                        {
                            p.z = std::numeric_limits<float>::quiet_NaN();
                        }
                    }
 
                    ARMARX_DEBUG << "Constructing point cloud took "
                                 << armarx::TimeUtil::GetTimeSince(time,
                                                                   armarx::TimeMode::SystemTime);
                }
            }
            catch (const std::exception& e)
            {
                pointcloudValid = false;
                ARMARX_WARNING << deactivateSpam(10)
                               << "Failed to process depth image into a point cloud: " << e.what()
                               << "\nSkipping this frame.";
            }
 
            // Notify capturing thread that data was processed and may be overridden.  This must
            // happen even if processing failed, otherwise the capturing thread stalls.
            depthImageProcessed = true;
            signal_lock.unlock();
            ARMARX_DEBUG << "Notifying capturing thread...";
            pointcloudProcSignal.notify_all();
 
            ARMARX_TRACE;
 
            // Broadcast PCL pointcloud.
            if (pointcloudValid)
            {
                time = armarx::TimeUtil::GetTime(armarx::TimeMode::SystemTime);
                pointcloud->header.stamp = static_cast<unsigned long>(TIMESTAMP.toMicroSeconds());
                try
                {
                    providePointCloud(pointcloud);
                }
                catch (const std::exception& e)
                {
                    ARMARX_WARNING << deactivateSpam(10)
                                   << "Failed to broadcast point cloud: " << e.what();
                }
                ARMARX_DEBUG << "Broadcasting pointcloud took "
                             << armarx::TimeUtil::GetTimeSince(time, armarx::TimeMode::SystemTime);
            }
        }
 
        ARMARX_DEBUG << "Stopped pointcloud processing task.";
    }
 
    std::string
    AzureKinectPointCloudProvider::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    AzureKinectPointCloudProvider::GetDefaultName()
    {
        return "AzureKinectPointCloudProvider";
    }
 
    void
    AzureKinectPointCloudProvider::onInitComponent()
    {
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
    }
 
    void
    AzureKinectPointCloudProvider::onConnectComponent()
    {
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
    }
 
    void
    AzureKinectPointCloudProvider::onDisconnectComponent()
    {
        captureEnabled = false;
 
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void
    AzureKinectPointCloudProvider::onExitComponent()
    {
        ImageProvider::onExitComponent();
        CapturingPointCloudProvider::onExitComponent();
    }
 
    StereoCalibration
    AzureKinectPointCloudProvider::getStereoCalibration(const Ice::Current&)
    {
        using namespace Eigen;
        using Matrix3FRowMajor = Matrix<float, 3, 3, StorageOptions::RowMajor>;
        using visionx::tools::convertEigenMatToVisionX;
 
        // TODO: use the externally used cameraMatrix and distCoeffs
        const auto convert_calibration =
            [](const k4a_calibration_camera_t& k4a_calib, float scale = 1.f)
        {
            MonocularCalibration monocular_calibration;
 
            const k4a_calibration_intrinsic_parameters_t& params = k4a_calib.intrinsics.parameters;
            monocular_calibration.cameraParam.principalPoint = {params.param.cx * scale,
                                                                params.param.cy * scale};
            monocular_calibration.cameraParam.focalLength = {params.param.fx * scale,
                                                             params.param.fy * scale};
            // TODO: Figure out convertions.  IVT (Calibration.h) expects 4 parameters:
            //  - The first radial lens distortion parameter.
            //  - The second radial lens distortion parameter.
            //  - The first tangential lens distortion parameter.
            //  - The second tangential lens distortion parameter.
            // However, the Kinect offers k1-k6 radial distortion coefficients and 2 p1-p2
            // tangential distortion parameters, which means that k3-k6 are unused.
            // It is even unclear whether this is correct now, as previously it was a vector of all
            // 6 k-params, which resulted in failed assertions in TypeMapping.cpp in the function
            // CStereoCalibration* visionx::tools::convert(const visionx::StereoCalibration& stereoCalibration)
            // lin 314 at time of this commit.
            // See: https://microsoft.github.io/Azure-Kinect-Sensor-SDK/master/structk4a__calibration__intrinsic__parameters__t_1_1__param.html
            monocular_calibration.cameraParam.distortion = {
                params.param.k1,
                params.param.k2,
                params.param.p1,
                params.param.p2,
            };
 
            // TODO this needs to be scaled! Or why shouldn't it ?
            monocular_calibration.cameraParam.width =
                std::floor(float(k4a_calib.resolution_width) * scale);
            monocular_calibration.cameraParam.height =
                std::floor(float(k4a_calib.resolution_height) * scale);
 
            const Matrix3FRowMajor rotation =
                Map<const Matrix3FRowMajor>{k4a_calib.extrinsics.rotation};
 
            monocular_calibration.cameraParam.rotation = convertEigenMatToVisionX(rotation);
            monocular_calibration.cameraParam.translation = {k4a_calib.extrinsics.translation,
                                                             k4a_calib.extrinsics.translation + 3};
 
            return monocular_calibration;
        };
 
        StereoCalibration stereo_calibration;
 
        stereo_calibration.calibrationLeft =
            convert_calibration(k4aCalibration.color_camera_calibration);
 
        // if the camera images are rectified, the distortion params are 0
        if (enableColorUndistortion)
        {
            auto& colorDistortionParams = stereo_calibration.calibrationLeft.cameraParam.distortion;
            std::fill(colorDistortionParams.begin(), colorDistortionParams.end(), 0);
        }
 
        // the depth image is rectified by default
        {
            // The depth image has been warped to to color camera frame. See depth_image_to_color_camera() above.
            // Therefore we use the calibration of the color camera (focal length etc) and set the distortion parameters to 0.
            stereo_calibration.calibrationRight =
                convert_calibration(k4aCalibration.color_camera_calibration);
 
            auto& depthDistortionParams =
                stereo_calibration.calibrationRight.cameraParam.distortion;
            std::fill(depthDistortionParams.begin(), depthDistortionParams.end(), 0);
        }
 
        stereo_calibration.rectificationHomographyLeft =
            convertEigenMatToVisionX(Matrix3f::Identity());
        stereo_calibration.rectificationHomographyRight =
            convertEigenMatToVisionX(Matrix3f::Identity());
 
        return stereo_calibration;
    }
 
    bool
    AzureKinectPointCloudProvider::getImagesAreUndistorted(const Ice::Current&)
    {
        return enableColorUndistortion;
    }
 
    std::string
    AzureKinectPointCloudProvider::getReferenceFrame(const Ice::Current&)
    {
        return getProperty<std::string>("frameName");
    }
 
    std::vector<imrec::ChannelPreferences>
    AzureKinectPointCloudProvider::getImageRecordingChannelPreferences(const Ice::Current&)
    {
        ARMARX_TRACE;
 
        imrec::ChannelPreferences rgb;
        rgb.requiresLossless = false;
        rgb.name = "rgb";
 
        imrec::ChannelPreferences depth;
        depth.requiresLossless = true;
        depth.name = "depth";
 
        return {rgb, depth};
    }
 
    void
    AzureKinectPointCloudProvider::enableHumanPoseEstimation(
        const armarx::EnableHumanPoseEstimationInput& input,
        const Ice::Current&)
    {
#ifndef INCLUDE_BODY_TRACKING
        {
            ARMARX_ERROR << "INCLUDE_BODY_TRACKING is not defined.";
            return;
        }
#endif
 
#ifdef INCLUDE_BODY_TRACKING
        if (bodyTrackingEnabled)
        {
            // should not require a mutex
            if (not bodyTrackingIsRunning and input.enable3d)
            {
                bodyTrackingIsRunning = true;
                bodyTrackingPublishTask->start();
            }
            else if (bodyTrackingIsRunning and not input.enable3d)
            {
                bodyTrackingIsRunning = false;
                bodyTrackingPublishTask->stop();
            }
        }
        else
        {
            ARMARX_ERROR << "Azure Kinect Body Tracking is not enabled";
        }
#endif
    }
 
    void
    AzureKinectPointCloudProvider::setMaxDepthBodyTracking(int maxDepthInMM, const Ice::Current&)
    {
        std::scoped_lock lock(bodyTrackingParameterMutex);
        bodyTrackingDepthMaskMaxZ = maxDepthInMM;
    }
 
    void
    AzureKinectPointCloudProvider::setWidthBodyTracking(int minXinPixel,
                                                        int maxXinPixel,
                                                        const Ice::Current&)
    {
        std::scoped_lock lock(bodyTrackingParameterMutex);
        bodyTrackingDepthMaskMinX = minXinPixel;
        bodyTrackingDepthMaskMaxX = maxXinPixel;
    }
 
    // the code below is taken from https://github.com/microsoft/Azure_Kinect_ROS_Driver
    // -> MIT license
 
    void
    AzureKinectPointCloudProvider::updateTimestampOffset(
        const std::chrono::microseconds& k4a_device_timestamp_us,
        const std::chrono::nanoseconds& k4a_system_timestamp_ns)
    {
        // System timestamp is on monotonic system clock.
        // Device time is on AKDK hardware clock.
        // We want to continuously estimate diff between realtime and AKDK hardware clock as low-pass offset.
        // This consists of two parts: device to monotonic, and monotonic to realtime.
 
        // First figure out realtime to monotonic offset. This will change to keep updating it.
        std::chrono::nanoseconds realtime_clock =
            std::chrono::system_clock::now().time_since_epoch();
        std::chrono::nanoseconds monotonic_clock =
            std::chrono::steady_clock::now().time_since_epoch();
 
        std::chrono::nanoseconds monotonic_to_realtime = realtime_clock - monotonic_clock;
 
        // Next figure out the other part (combined).
        std::chrono::nanoseconds device_to_realtime =
            k4a_system_timestamp_ns - k4a_device_timestamp_us + monotonic_to_realtime;
 
        {
            std::lock_guard g{deviceToRealtimeOffsetMtx};
 
            {
                std::lock_guard g{debugObserverMtx};
                setDebugObserverDatafield("device_to_realtime_offset [ms]",
                                          device_to_realtime_offset_.count() /
                                              1'000'000.f); //  [ns] -> [ms]
                setDebugObserverDatafield(
                    "clock_error",
                    std::abs<float>((device_to_realtime_offset_ - device_to_realtime).count()) /
                        1'000.f); //  [ns] -> [µs]
            }
 
            const std::int64_t timeOffsetThreshold = 1e7; // 10 ms
 
            // If we're over a certain time off, just snap into place.
            if (device_to_realtime_offset_.count() == 0 ||
                std::abs((device_to_realtime_offset_ - device_to_realtime).count()) >
                    timeOffsetThreshold)
            {
                ARMARX_VERBOSE << deactivateSpam()
                               << "Initializing or re-initializing the device to realtime offset: "
                               << device_to_realtime.count() << " ns";
                device_to_realtime_offset_ = device_to_realtime;
            }
            else
            {
                // Low-pass filter!
                constexpr double alpha = 0.10;
 
                const std::chrono::nanoseconds timeCorrection(static_cast<int64_t>(
                    std::floor(alpha * (device_to_realtime - device_to_realtime_offset_).count())));
                device_to_realtime_offset_ = device_to_realtime_offset_ + timeCorrection;
 
                {
                    std::lock_guard g{debugObserverMtx};
                    setDebugObserverDatafield("timeCorrection [µs]",
                                              timeCorrection.count() / 1'000); // [ns] -> [µs]
                }
            }
        }
    }
 
    armarx::DateTime
    AzureKinectPointCloudProvider::timestampToArmarX(
        const std::chrono::microseconds& k4a_timestamp_us)
    {
        std::lock_guard g{deviceToRealtimeOffsetMtx};
 
        // must be initialized beforehand
        ARMARX_CHECK(device_to_realtime_offset_.count() != 0);
 
        std::chrono::nanoseconds timestamp_in_realtime =
            k4a_timestamp_us + device_to_realtime_offset_;
 
        return armarx::Duration::MicroSeconds(timestamp_in_realtime.count() /
                                              1'000); // [ns] -> [µs]
    }
 
    void
    AzureKinectPointCloudProvider::initializeTimestampOffset(
        const std::chrono::microseconds& k4a_device_timestamp_us)
    {
        std::lock_guard g{deviceToRealtimeOffsetMtx};
 
        // We have no better guess than "now".
        std::chrono::nanoseconds realtime_clock =
            std::chrono::system_clock::now().time_since_epoch();
 
        device_to_realtime_offset_ = realtime_clock - k4a_device_timestamp_us;
 
        ARMARX_INFO << "Initializing the device to realtime offset based on wall clock: "
                    << device_to_realtime_offset_.count() << " ns";
    }
 
    AzureKinectPointCloudProvider::AzureKinectPointCloudProvider()
    {
        addPlugin(heartbeatPlugin);
    }
 
    void
    AzureKinectPointCloudProvider::Framerate::Subordinate::update(int higherFramerate)
    {
        if (std::abs(std::fmod(higherFramerate, value)) > 1e-6)
        {
            std::stringstream ss;
            ss << "Invalid value (" << value << " fps) for framerate of '" << name << "'."
               << "  Reason: The framerate has to be a divider of the property framerate.image ("
               << higherFramerate << " fps).";
            throw armarx::LocalException() << ss.str();
        }
        skipFrames = std::round(higherFramerate / value) - 1;
        if (skipFrames != 0)
        {
            ARMARX_INFO_S << "Only publishing every " << (skipFrames + 1) << "'th " << name
                          << " result!";
        }
    }
 
    bool
    AzureKinectPointCloudProvider::Framerate::Subordinate::skip()
    {
        bool skip = false;
        if (skipFrames > 0)
        {
            skip = skipFramesCount;
            skipFramesCount++;
            skipFramesCount %= (skipFrames + 1);
        }
        return skip;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::visionx::AzureKinectPointCloudProvider,
                                         ::visionx::AzureKinectPointCloudProvider::GetDefaultName());
 
} // namespace visionx