NuitrackPointCloudProvider.cpp

Go to the documentation of this file.

#include "NuitrackPointCloudProvider.h"
 
#include <algorithm>
#include <cmath>
#include <cstdio>
#include <numbers>
#include <optional>
#include <string>
#include <vector>
#include <filesystem>
#include <functional>
#include <mutex>
#include <utility>
 
#include <Ice/LocalException.h>
#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/armem_human/types.h>
#include <VisionX/libraries/imrec/helper.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include "nuitrack/types/NuitrackDeviceCommon.h"
#include <Calibration/Calibration.h>
#include <Image/ImageProcessor.h>
 
namespace
{
    // Mapping from Nuitrack JointType to k4abt joint index (matching k4a_bt_body_32 model)
    std::optional<int>
    getNuitrackToK4ABTMapping(tdv::nuitrack::JointType nuitrackJoint)
    {
        using namespace tdv::nuitrack;
        using K4AJoint = armarx::human::pose::model::k4a_bt_body_32::Joints;
 
        static const std::map<JointType, K4AJoint> mapping = {
            {JOINT_HEAD, K4AJoint::Head},
            {JOINT_NECK, K4AJoint::Neck},
            // {JOINT_TORSO, K4AJoint::SpineChest},
            {JOINT_TORSO, K4AJoint::SpineNaval},
            {JOINT_WAIST, K4AJoint::Pelvis},
 
            {JOINT_LEFT_COLLAR, K4AJoint::ClavicleLeft},
            {JOINT_LEFT_SHOULDER, K4AJoint::ShoulderLeft},
            {JOINT_LEFT_ELBOW, K4AJoint::ElbowLeft},
            {JOINT_LEFT_WRIST, K4AJoint::WristLeft},
            {JOINT_LEFT_HAND, K4AJoint::HandLeft},
            // {JOINT_LEFT_FINGERTIP, K4AJoint::HandtipLeft},
 
            {JOINT_RIGHT_COLLAR, K4AJoint::ClavicleRight},
            {JOINT_RIGHT_SHOULDER, K4AJoint::ShoulderRight},
            {JOINT_RIGHT_ELBOW, K4AJoint::ElbowRight},
            {JOINT_RIGHT_WRIST, K4AJoint::WristRight},
            {JOINT_RIGHT_HAND, K4AJoint::HandRight},
            // {JOINT_RIGHT_FINGERTIP, K4AJoint::HandtipRight},
 
            {JOINT_LEFT_HIP, K4AJoint::HipLeft},
            {JOINT_LEFT_KNEE, K4AJoint::KneeLeft},
            {JOINT_LEFT_ANKLE, K4AJoint::AnkleLeft},
            // {JOINT_LEFT_FOOT, K4AJoint::FootLeft},
 
            {JOINT_RIGHT_HIP, K4AJoint::HipRight},
            {JOINT_RIGHT_KNEE, K4AJoint::KneeRight},
            {JOINT_RIGHT_ANKLE, K4AJoint::AnkleRight},
            // {JOINT_RIGHT_FOOT, K4AJoint::FootRight},
 
        };
 
        auto it = mapping.find(nuitrackJoint);
        if (it != mapping.end())
        {
            return static_cast<int>(it->second);
        }
        return std::nullopt;
    }
 
    void
    printBodyInformation(const tdv::nuitrack::Skeleton& skeleton)
    {
        ARMARX_VERBOSE << "Skeleton ID: " << skeleton.id;
        for (const auto& joint : skeleton.joints)
        {
            ARMARX_VERBOSE << "Joint " << joint.type << ": "
                           << "Position[mm] (" << joint.real.x << "," << joint.real.y << ","
                           << joint.real.z << "); "
                           << "Confidence " << joint.confidence;
        }
    }
 
    // Minimum spine vector length (mm) required to accept a joint pair.
    constexpr float minSpineLengthMm = 100.0F;
 
} // namespace
 
namespace visionx
{
    std::optional<Eigen::Vector3f>
    NuitrackPointCloudProvider::computeBodyUpVector(const armarx::armem::human::HumanPose& pose)
    {
        // Ordered list of (bottom, top) keypoint pairs for estimating the body's
        // upward direction. Arm joints are intentionally excluded. The first pair
        // for which both keypoints are present and sufficiently far apart wins.
        static const std::vector<std::pair<std::string, std::string>> pairs = {
            {"Pelvis",     "Head"},
            {"Pelvis",     "Neck"},
            {"Pelvis",     "SpineNaval"},
            // {"SpineNaval", "Head"},
            // {"SpineNaval", "Neck"},
            {"HipLeft",    "Neck"},
            {"HipRight",   "Neck"},
        };
 
        auto success = robotReader.synchronizeRobot(*robot, armarx::Clock::Now());
 
        for (const auto& [bottomName, topName] : pairs)
        {
            auto bottomIt = pose.keypoints.find(bottomName);
            auto topIt    = pose.keypoints.find(topName);
            if (bottomIt == pose.keypoints.end() || topIt == pose.keypoints.end())
            {
                continue;
            }
            Eigen::Vector3f vec = topIt->second.positionCamera.toEigen()
                                      - bottomIt->second.positionCamera.toEigen();
 
            armarx::FramedPosition vecCamera(vec, bodyCameraFrameName, "Armar7");
            vec = vecCamera.toGlobalEigen(robot);
            const float norm = vec.norm();
            if (norm < minSpineLengthMm)
            {
                continue;
            }
            return vec / norm;
        }
        return std::nullopt;
    }
 
    void
    NuitrackPointCloudProvider::filterPosesByUpright(std::vector<armarx::armem::human::HumanPose>& poses,
                         const Eigen::Vector3f& worldUp,
                         float maxAngleDeg)
    {
        // Convert to radians and check against pi; cos(pi) = -1 means no pose
        // would ever be filtered, so skip the work entirely.
        const float maxAngleRad = maxAngleDeg * std::numbers::pi_v<float> / 180.F;
        if (maxAngleRad >= std::numbers::pi_v<float>)
        {
            return;
        }
 
        const float cosThreshold = std::cos(maxAngleRad);
        const float radToDeg     = 180.F / std::numbers::pi_v<float>;
 
        std::erase_if(poses,
                      [&](const armarx::armem::human::HumanPose& pose) -> bool
                      {
                          const auto bodyUp = computeBodyUpVector(pose);
                          if (!bodyUp.has_value())
                          {
                              ARMARX_INFO << "Dropping pose (id "
                                           << pose.humanTrackingId.value_or("?")
                                           << "): Could not compute valid body pose up vector.";
                              return false; // keep if orientation cannot be determined
                          }
                          const float cosAngle = bodyUp->dot(worldUp);
                          if (cosAngle < cosThreshold)
                          {
                              const float angleDeg =
                                  std::acos(std::clamp(cosAngle, -1.F, 1.F)) * radToDeg;
                              ARMARX_INFO << "Dropping pose (id "
                                           << pose.humanTrackingId.value_or("?")
                                           << "): body-up angle " << angleDeg << "° > max "
                                           << maxAngleDeg << "°";
                              return true;
                          }
                          return false;
                      });
    }
 
    std::function<void(armarx::Duration)>
    NuitrackPointCloudProvider::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());
            }
        };
    }
 
    NuitrackPointCloudProviderPropertyDefinitions::NuitrackPointCloudProviderPropertyDefinitions(
        std::string prefix) :
        visionx::CapturingPointCloudProviderPropertyDefinitions(std::move(prefix))
    {
        defineOptionalProperty<int>("ColorWidth", 640, "Width of the RGB camera image.");
 
        defineOptionalProperty<int>("ColorHeight", 480, "Height of the RGB camera image.");
 
        defineOptionalProperty<int>("DepthWidth", 640, "Width of the depth camera image.");
 
        defineOptionalProperty<int>("DepthHeight", 576, "Height of the depth camera image.");
 
        defineOptionalProperty<float>("MaxDepth",
                                      6000.0,
                                      "Max. allowed depth value in mm. Depth values above this "
                                      "threshold will be set to nan.");
    }
 
    armarx::PropertyDefinitionsPtr
    NuitrackPointCloudProvider::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new NuitrackPointCloudProviderPropertyDefinitions(getConfigIdentifier()));
 
        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);
 
        defs->optional(bodyTrackingEnabled,
                       "bodyTrackingEnabled",
                       "Whether the body tracking should be enabled or not.");
        defs->optional(
            bodyTrackingRunAtStart,
            "bodyTrackingRunAtStart",
            "Whether the body tracking should directly run when the component is startet."
            "Otherwise it has to be activated by the ice interface.");
        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(confidenceThreshold, "confidenceThreshold");
        defs->optional(minimumSegmentsWithConfidenceLargerThanThreshold,
                       "minimumSegmentsWithConfidenceLargerThanThreshold");
        defs->optional(uprightFilterMaxAngleDeg,
                       "uprightFilterMaxAngleDeg",
                       "Maximum angle in degrees between the estimated body up vector "
                       "(spine direction, arm joints excluded) and the world up vector (0, 0, 1). "
                       "Poses exceeding this threshold are dropped. Set >= 180 to disable.")
            .setMin(0.)
            .setMax(180.);
 
        humanPoseWriter.registerPropertyDefinitions(defs);
 
        defs->optional(enableHeartbeat, "enableHeartbeat");
 
        return defs;
    }
 
    void
    NuitrackPointCloudProvider::onInitImageProvider()
    {
        // Validate framerate
        if (framerate.value != 5 && framerate.value != 15 && framerate.value != 30)
        {
            throw armarx::LocalException("Invalid image framerate (property 'framerate.images'): ")
                << framerate.value << ".  Only framerates 5, 15 and 30 are supported.";
        }
 
        framerate.pointCloud.value =
            getProperty<float>("framerate"); // point cloud provider framerate
        framerate.pointCloud.update(framerate.value);
 
        framerate.bodyTracking.update(framerate.value);
 
        // Get image dimensions from properties
        int colorWidth = getProperty<int>("ColorWidth").getValue();
        int colorHeight = getProperty<int>("ColorHeight").getValue();
        int depthWidth = getProperty<int>("DepthWidth").getValue();
        int depthHeight = getProperty<int>("DepthHeight").getValue();
 
        ARMARX_INFO << "Depth image size: " << depthWidth << "x" << depthHeight;
        ARMARX_INFO << "Color image size: " << colorWidth << "x" << colorHeight;
 
        // Set number of images per frame.
        setNumberImages(2);
 
        // Initialize OpenCV matrices for image storage
        alignedDepthImage = cv::Mat(colorHeight, colorWidth, CV_16UC1);
        colorImageRGB = cv::Mat(colorHeight, colorWidth, CV_8UC3);
 
        setImageFormat(visionx::ImageDimension(colorWidth, colorHeight),
                       visionx::eRgb,
                       visionx::eBayerPatternGr);
 
        resultDepthImage.reset(visionx::tools::createByteImage(getImageFormat(), visionx::eRgb));
        resultColorImage =
            std::make_unique<CByteImage>(colorWidth, colorHeight, CByteImage::eRGB24);
 
        pointcloud = std::make_unique<pcl::PointCloud<CloudPointType>>();
    }
 
    void
    NuitrackPointCloudProvider::onConnectImageProvider()
    {
        ARMARX_DEBUG << "Connecting " << getName();
 
        setDebugObserverBatchModeEnabled(true);
 
        pointcloudTask = armarx::RunningTask<NuitrackPointCloudProvider>::pointer_type(
            new armarx::RunningTask<NuitrackPointCloudProvider>(
                this, &NuitrackPointCloudProvider::runPointcloudPublishing));
        pointcloudTask->start();
 
        // TODO: Remove hardcoding
        bodyTrackingEnabled = true;
        bodyTrackingRunAtStart = true;
        robotName = "Armar7";
 
 
        if (bodyTrackingEnabled)
        {
            ARMARX_INFO << "Connecting to human memory ...";
            humanPoseWriter.connect(memoryNameSystem());
            ARMARX_INFO << "Connected to human memory.";
 
            bodyTrackingPublishTask = new armarx::RunningTask<NuitrackPointCloudProvider>(
                this, &NuitrackPointCloudProvider::runPublishBodyTrackingResults);
        }
 
        if (enableHeartbeat)
        {
            ARMARX_CHECK_NOT_NULL(heartbeatPlugin);
            heartbeatPlugin->signUp(armarx::Duration::MilliSeconds(500),
                                    armarx::Duration::MilliSeconds(1000),
                                    {"Vision", "Camera"},
                                    "NuitrackPointCloudProvider");
        }
 
        ARMARX_VERBOSE << "Connected " << getName();
    }
 
    void
    NuitrackPointCloudProvider::onDisconnectImageProvider()
    {
        ARMARX_DEBUG << "Disconnecting " << getName();
 
        // Stop task
        {
            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";
        }
 
        if (bodyTrackingIsRunning)
        {
            bodyTrackingIsRunning = false;
            bodyTrackingPublishTask->stop();
        }
 
        ARMARX_DEBUG << "Disconnected " << getName();
    }
 
    void
    NuitrackPointCloudProvider::onInitCapturingPointCloudProvider()
    {
        this->setPointCloudSyncMode(eCaptureSynchronization);
        ARMARX_TRACE;
    }
 
    void
    NuitrackPointCloudProvider::onExitCapturingPointCloudProvider()
    {
    }
 
    void
    NuitrackPointCloudProvider::onStartCapture(float /* framesPerSecond */)
    {
        ARMARX_TRACE;
 
        cloudFormat = getPointCloudFormat();
 
        try
        {
            ARMARX_INFO << "Initializing Nuitrack...";
 
            // Initialize Nuitrack
            tdv::nuitrack::Nuitrack::init();
 
            ARMARX_INFO << "Init done";
            ARMARX_INFO << "Querying devices...";
 
            // Get the list of devices
            auto devices = tdv::nuitrack::Nuitrack::getDeviceList();
 
            if (devices.empty())
            {
                ARMARX_INFO << "No devices found!";
                throw armarx::LocalException("No Nuitrack-compatible devices detected!");
            }
 
            ARMARX_INFO << "Found " << devices.size() << " Nuitrack-compatible devices";
 
            // TODO: Handle case where we have multiple devices
            // We currently just use the first device
 
            // Set VideoMode for color
            // TODO: Does not seem to take effect
            // Maybe: https://community.nuitrack.com/t/azurekinect-dk-nuitrack-config-isnt-applying/3049/4
            tdv::nuitrack::device::VideoMode colorMode{.width = resultColorImage->width,
                                                       .height = resultColorImage->height,
                                                       .fps = framerate.value};
            devices[0]->setVideoMode(tdv::nuitrack::device::StreamType::COLOR, colorMode);
 
 
            // Set VideoMode for depth
            // TODO: Does not seem to take effect
            tdv::nuitrack::device::VideoMode depthMode{.width = resultDepthImage->width,
                                                       .height = resultDepthImage->height,
                                                       .fps = framerate.value};
            devices[0]->setVideoMode(tdv::nuitrack::device::StreamType::DEPTH, depthMode);
 
            tdv::nuitrack::Nuitrack::setDevice(devices[0]);
 
            ARMARX_INFO << "Using device: "
                        << devices[0]->getInfo(tdv::nuitrack::device::DeviceInfoType::DEVICE_NAME);
 
            // Create depth and color sensors
            depthSensor = tdv::nuitrack::DepthSensor::create();
            colorSensor = tdv::nuitrack::ColorSensor::create();
 
            // Connect frame callbacks
            depthSensor->connectOnNewFrame([this](auto&& PH1)
                                           { onNewDepthFrame(std::forward<decltype(PH1)>(PH1)); });
            colorSensor->connectOnNewFrame([this](auto&& PH1)
                                           { onNewColorFrame(std::forward<decltype(PH1)>(PH1)); });
 
            if (bodyTrackingEnabled)
            {
                ARMARX_INFO << "Initializing Nuitrack skeleton tracking...";
                skeletonTracker = tdv::nuitrack::SkeletonTracker::create();
                skeletonTracker->connectOnUpdate(
                    [this](auto&& PH1) { onNewSkeletonData(std::forward<decltype(PH1)>(PH1)); });
 
                if (bodyTrackingRunAtStart and not bodyTrackingIsRunning)
                {
                    bodyTrackingIsRunning = true;
                    bodyTrackingPublishTask->start();
                }
            }
 
            // Start Nuitrack
            tdv::nuitrack::Nuitrack::run();
 
            ARMARX_INFO << "Nuitrack initialized and running";
        }
        catch (const tdv::nuitrack::Exception& e)
        {
            ARMARX_ERROR << "Failed to initialize Nuitrack: " << e.what();
            getArmarXManager()->asyncShutdown();
            throw armarx::LocalException("Nuitrack initialization failed: ") << e.what();
        }
    }
 
    void
    NuitrackPointCloudProvider::onStopCapture()
    {
        ARMARX_INFO << "Releasing Nuitrack";
        try
        {
            tdv::nuitrack::Nuitrack::release();
        }
        catch (const tdv::nuitrack::Exception& e)
        {
            ARMARX_WARNING << "Error releasing Nuitrack: " << e.what();
        }
        ARMARX_INFO << "Released Nuitrack";
    }
 
    void
    NuitrackPointCloudProvider::onNewDepthFrame(tdv::nuitrack::DepthFrame::Ptr frame)
    {
        if (!frame)
        {
            return;
        }
 
        std::lock_guard<std::mutex> lock(depthFrameMutex);
        latestDepthFrame = frame;
    }
 
    void
    NuitrackPointCloudProvider::onNewColorFrame(tdv::nuitrack::RGBFrame::Ptr frame)
    {
        if (!frame)
        {
            return;
        }
        // We don't get a proper timestap from Nuitrack and just take the RGB receive time
        latestTimestamp = armarx::Clock::Now().toMicroSecondsSinceEpoch();
        std::lock_guard<std::mutex> lock(colorFrameMutex);
        latestColorFrame = frame;
    }
 
    void
    NuitrackPointCloudProvider::onNewSkeletonData(tdv::nuitrack::SkeletonData::Ptr skeletonData)
    {
        if (!skeletonData)
        {
            return;
        }
        std::lock_guard<std::mutex> lock(skeletonMutex);
        latestSkeletonData = skeletonData;
    }
 
    bool
    NuitrackPointCloudProvider::doCapture()
    {
        using armarx::core::time::ScopedStopWatch;
        using armarx::core::time::StopWatch;
 
        ScopedStopWatch sw_total{createSwCallback("doCapture")};
 
        try
        {
            tdv::nuitrack::Nuitrack::waitUpdate(depthSensor);
        }
        catch (const tdv::nuitrack::Exception& e)
        {
            ARMARX_WARNING << "Failed to get frame from Nuitrack: " << e.what();
            return false;
        }
 
        tdv::nuitrack::DepthFrame::Ptr depthFrame;
        tdv::nuitrack::RGBFrame::Ptr colorFrame;
 
        {
            std::lock_guard<std::mutex> lock(colorFrameMutex);
            std::lock_guard<std::mutex> lock_(depthFrameMutex);
            depthFrame = latestDepthFrame;
            colorFrame = latestColorFrame;
        }
 
        if (depthFrame && colorFrame)
        {
            ARMARX_DEBUG << "Processing Nuitrack frames";
 
            imagesTime = IceUtil::Time::microSeconds(latestTimestamp);
 
            // Convert Nuitrack RGB frame to IVT image
            {
                ScopedStopWatch sw{createSwCallback("convert Nuitrack image to IVT")};
 
                const int width = colorFrame->getCols();
                const int height = colorFrame->getRows();
                const tdv::nuitrack::Color3* colorData = colorFrame->getData();
 
                ARMARX_CHECK_EQUAL(resultColorImage->width, width);
                ARMARX_CHECK_EQUAL(resultColorImage->height, height);
 
                auto rgb_buffer_ivt = resultColorImage->pixels;
                int index_ivt = 0;
 
                for (int i = 0; i < width * height; ++i)
                {
                    rgb_buffer_ivt[index_ivt] = colorData[i].red;
                    rgb_buffer_ivt[index_ivt + 1] = colorData[i].green;
                    rgb_buffer_ivt[index_ivt + 2] = colorData[i].blue;
                    index_ivt += 3;
                }
            }
 
            // Process depth frame
            {
                ScopedStopWatch sw{createSwCallback("process depth frame")};
 
                const int width = depthFrame->getCols();
                const int height = depthFrame->getRows();
                const uint16_t* depthData = depthFrame->getData();
 
                // Copy depth data to OpenCV mat for pointcloud processing
                if (not framerate.pointCloud.skip())
                {
                    std::lock_guard lock{pointcloudProcMutex};
 
                    // Copy depth data to alignedDepthImage
                    std::memcpy(
                        alignedDepthImage.data, depthData, width * height * sizeof(uint16_t));
 
                    // Signal that point cloud processing may proceed
                    depthImageReady = true;
                    ARMARX_DEBUG << "Notifying pointcloud thread.";
                    pointcloudProcSignal.notify_one();
                }
            }
 
            // Prepare result depth image for visualization
            {
                ScopedStopWatch sw{createSwCallback("prepare result depth image")};
 
                const int width = depthFrame->getCols();
                const int height = depthFrame->getRows();
                const uint16_t* depthData = depthFrame->getData();
 
                ARMARX_CHECK_EXPRESSION(resultDepthImage);
                ARMARX_CHECK_EQUAL(resultDepthImage->width, width);
                ARMARX_CHECK_EQUAL(resultDepthImage->height, height);
 
                auto result_depth_buffer = resultDepthImage->pixels;
 
                int index = 0;
                for (int i = 0; i < width * height; ++i)
                {
                    uint16_t depth_value = depthData[i];
                    visionx::tools::depthValueToRGB(depth_value,
                                                    result_depth_buffer[index],
                                                    result_depth_buffer[index + 1],
                                                    result_depth_buffer[index + 2]);
                    index += 3;
                }
            }
 
            // Broadcast RGB-D image
            {
                ScopedStopWatch sw{createSwCallback("broadcast RGB-D image")};
                CByteImage* images[2] = {resultColorImage.get(), resultDepthImage.get()};
                provideImages(images, imagesTime);
            }
 
            // Wait until depth image was processed
            {
                std::unique_lock signal_lock{pointcloudProcMutex};
                ARMARX_DEBUG << "Capturing thread waiting for signal...";
                pointcloudProcSignal.wait(signal_lock, [&] { return depthImageProcessed; });
                ARMARX_DEBUG << "Capturing thread received signal.";
            }
 
            {
                std::lock_guard g{debugObserverMtx};
                sendDebugObserverBatch();
            }
 
            if (enableHeartbeat)
            {
                heartbeatPlugin->heartbeat();
            }
 
            return true;
        }
 
        ARMARX_INFO << deactivateSpam(30) << "Did not get frames from Nuitrack";
 
        {
            std::lock_guard g{debugObserverMtx};
            sendDebugObserverBatch();
        }
 
        return false;
    }
 
    void
    NuitrackPointCloudProvider::runPublishBodyTrackingResults()
    {
        armarx::Metronome metronome(armarx::Frequency::Hertz(100));
        while (bodyTrackingIsRunning)
        {
            // Get latest skeleton data
            tdv::nuitrack::SkeletonData::Ptr skeletonData;
            {
                std::lock_guard<std::mutex> lock(skeletonMutex);
                skeletonData = latestSkeletonData;
            }
 
            if (skeletonData)
            {
                armarx::core::time::ScopedStopWatch sw{
                    createSwCallback("publish body tracking result")};
 
                armarx::DateTime timestamp;
                {
                    std::lock_guard<std::mutex> lock(colorFrameMutex);
                    timestamp = armarx::DateTime(armarx::Duration::MicroSeconds(latestTimestamp));
                }
                const auto& skeletons = skeletonData->getSkeletons();
                std::uint32_t num_bodies = skeletons.size();
 
                {
                    std::lock_guard g{debugObserverMtx};
                    setDebugObserverDatafield("n_bodies_detected", num_bodies);
                }
 
                std::vector<armarx::armem::human::HumanPose> humanPoses;
                humanPoses.reserve(num_bodies);
 
                for (const auto& skeleton : skeletons)
                {
                    printBodyInformation(skeleton);
 
                    armarx::armem::human::HumanPose humanPose;
                    humanPose.humanTrackingId = std::to_string(skeleton.id);
                    humanPose.cameraFrameName = bodyCameraFrameName;
                    humanPose.poseModelId = armarx::human::pose::model::k4a_bt_body_32::ModelId;
 
                    int segmentsWithConfidenceLargerThanThreshold = 0;
 
                    // Iterate through all Nuitrack joints and map them to k4abt joints
                    for (const auto& nuitrackJoint : skeleton.joints)
                    {
                        auto k4aJointIdx = getNuitrackToK4ABTMapping(nuitrackJoint.type);
                        if (!k4aJointIdx.has_value())
                        {
                            // Skip joints that don't have a mapping
                            continue;
                        }
 
                        const auto joint =
                            static_cast<armarx::human::pose::model::k4a_bt_body_32::Joints>(
                                k4aJointIdx.value());
                        const auto name =
                            armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(joint);
 
                        // Nuitrack provides position in mm already
                        // In contrast to AzureKinect framework (positive y -> downwards)
                        // Nuitrack uses (positive y -> upward)
                        // Therefore, we take -1 * y
                        Eigen::Vector3f position(
                            nuitrackJoint.real.x, -nuitrackJoint.real.y, nuitrackJoint.real.z);
 
                        // Convert Nuitrack orientation (rotation matrix) to quaternion
                        Eigen::Matrix3f rotMatrix;
                        rotMatrix << nuitrackJoint.orient.matrix[0], nuitrackJoint.orient.matrix[1],
                            nuitrackJoint.orient.matrix[2], nuitrackJoint.orient.matrix[3],
                            nuitrackJoint.orient.matrix[4], nuitrackJoint.orient.matrix[5],
                            nuitrackJoint.orient.matrix[6], nuitrackJoint.orient.matrix[7],
                            nuitrackJoint.orient.matrix[8];
 
                        humanPose.keypoints[name] = armarx::armem::human::PoseKeypoint{
                            .label = name,
                            .confidence = nuitrackJoint.confidence,
                            .positionCamera =
                                armarx::FramedPosition(position, bodyCameraFrameName, robotName),
                            .orientationCamera = armarx::FramedOrientation(
                                rotMatrix, bodyCameraFrameName, robotName)};
 
                        segmentsWithConfidenceLargerThanThreshold +=
                            static_cast<int>(nuitrackJoint.confidence > confidenceThreshold);
                    }
 
                    if (segmentsWithConfidenceLargerThanThreshold >=
                        minimumSegmentsWithConfidenceLargerThanThreshold)
                    {
                        humanPoses.push_back(humanPose);
                    }
                    else
                    {
                        ARMARX_INFO << deactivateSpam(10.)
                                    << "Ignoring human due to minimum confident segments="
                                    << segmentsWithConfidenceLargerThanThreshold << "<"
                                    << minimumSegmentsWithConfidenceLargerThanThreshold;
                    }
                }
 
                {
                    std::lock_guard g{debugObserverMtx};
                    setDebugObserverDatafield("bodyTrackingCaptureUntilPublish [ms]",
                                              (armarx::Clock::Now() - timestamp).toMilliSeconds());
                }
 
                // In the K4ABT camera frame (positive y = down), world up is -y.
                filterPosesByUpright(humanPoses,
                                     Eigen::Vector3f::UnitZ(),
                                     uprightFilterMaxAngleDeg);
 
                if (not humanPoses.empty())
                {
                    ARMARX_TRACE;
                    humanPoseWriter.commitHumanPosesInCameraFrame(humanPoses, getName(), timestamp);
                }
            }
            else
            {
                ARMARX_VERBOSE << deactivateSpam(10) << "No skeleton data available";
            }
 
            metronome.waitForNextTick();
        }
    }
 
    void
    NuitrackPointCloudProvider::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.";
 
            // Assess timings, reset flags.
            const IceUtil::Time TIMESTAMP = imagesTime;
            depthImageReady = false;
            depthImageProcessed = false;
 
            // Construct PCL pointcloud from depth data
            {
                time = armarx::TimeUtil::GetTime(armarx::TimeMode::SystemTime);
 
                ARMARX_CHECK_EXPRESSION(pointcloud);
 
                const int width = alignedDepthImage.cols;
                const int height = alignedDepthImage.rows;
 
                pointcloud->width = static_cast<uint32_t>(width);
                pointcloud->height = static_cast<uint32_t>(height);
                pointcloud->is_dense = false;
                pointcloud->points.resize(pointcloud->width * pointcloud->height);
 
                const uint16_t* depth_buffer =
                    reinterpret_cast<const uint16_t*>(alignedDepthImage.data);
                unsigned char* color_buffer = resultColorImage->pixels;
 
                ARMARX_CHECK_NOT_NULL(depth_buffer);
                ARMARX_CHECK_NOT_NULL(color_buffer);
                ARMARX_CHECK_EQUAL(width, resultColorImage->width);
                ARMARX_CHECK_EQUAL(height, resultColorImage->height);
 
                // TODO: Seems to be bugged...
                // auto intrinsics = colorSensor->getOutputMode().intrinsics;
                // const float fx = intrinsics.fx;
                // const float fy = intrinsics.fy;
                // const float cx = intrinsics.cx;
                // const float cy = intrinsics.cy;
 
                const float fx = 525.0f; // Focal length x (approximate for common RGB-D sensors)
                const float fy = 525.0f; // Focal length y
                const float cx = width / 2.0f; // Principal point x
                const float cy = height / 2.0f; // Principal point y
 
                float max_depth = getProperty<float>("MaxDepth").getValue();
 
                size_t point_index = 0;
                int color_index = 0;
 
                for (int y = 0; y < height; ++y)
                {
                    for (int x = 0; x < width; ++x, ++point_index)
                    {
                        auto& p = pointcloud->points[point_index];
 
                        // Get color
                        p.r = color_buffer[color_index++];
                        p.g = color_buffer[color_index++];
                        p.b = color_buffer[color_index++];
 
                        // Get depth and convert to 3D coordinates
                        uint16_t depth_value = depth_buffer[point_index];
 
                        if (depth_value > 0 && depth_value <= max_depth)
                        {
                            float depth_meters =
                                depth_value; // Nuitrack already provides depth in mm
                            p.z = depth_meters;
                            p.x = (x - cx) * depth_meters / fx;
                            p.y = (y - cy) * depth_meters / fy;
                        }
                        else
                        {
                            p.x = std::numeric_limits<float>::quiet_NaN();
                            p.y = std::numeric_limits<float>::quiet_NaN();
                            p.z = std::numeric_limits<float>::quiet_NaN();
                        }
                    }
                }
 
                ARMARX_DEBUG << "Constructing point cloud took "
                             << armarx::TimeUtil::GetTimeSince(time, armarx::TimeMode::SystemTime);
            }
 
            // Notify capturing thread that data was processed and may be overridden.
            depthImageProcessed = true;
            signal_lock.unlock();
            ARMARX_DEBUG << "Notifying capturing thread...";
            pointcloudProcSignal.notify_all();
 
            ARMARX_TRACE;
 
            // Broadcast PCL pointcloud.
            {
                time = armarx::TimeUtil::GetTime(armarx::TimeMode::SystemTime);
                pointcloud->header.stamp = static_cast<unsigned long>(TIMESTAMP.toMicroSeconds());
                providePointCloud(pointcloud);
                ARMARX_DEBUG << "Broadcasting pointcloud took "
                             << armarx::TimeUtil::GetTimeSince(time, armarx::TimeMode::SystemTime);
            }
        }
 
        ARMARX_DEBUG << "Stopped pointcloud processing task.";
    }
 
    std::string
    NuitrackPointCloudProvider::getDefaultName() const
    {
        // TODO(@rietsch): Change to different name at some point
        return "AzureKinectPointCloudProvider";
    }
 
    void
    NuitrackPointCloudProvider::onInitComponent()
    {
        usingProxy("HumanMemory");
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
    }
 
    void
    NuitrackPointCloudProvider::onConnectComponent()
    {
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
        robotReader.connect(memoryNameSystem());
        robot = robotReader.getRobot("Armar7");
    }
 
    void
    NuitrackPointCloudProvider::onDisconnectComponent()
    {
        captureEnabled = false;
 
        if (bodyTrackingIsRunning)
        {
            bodyTrackingIsRunning = false;
            bodyTrackingPublishTask->stop();
        }
 
        // Release Nuitrack so it can be re-initialized on reconnect.
        stopCapture();
 
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void
    NuitrackPointCloudProvider::onExitComponent()
    {
        ImageProvider::onExitComponent();
        CapturingPointCloudProvider::onExitComponent();
    }
 
    StereoCalibration
    NuitrackPointCloudProvider::getStereoCalibration(const Ice::Current&)
    {
        using namespace Eigen;
        // using Matrix3FRowMajor = Matrix<float, 3, 3, StorageOptions::RowMajor>;
        using visionx::tools::convertEigenMatToVisionX;
 
        StereoCalibration stereo_calibration;
 
        // Get image dimensions from properties
        int colorWidth = getProperty<int>("ColorWidth").getValue();
        int colorHeight = getProperty<int>("ColorHeight").getValue();
 
        // Create calibration for color camera (left)
        MonocularCalibration colorCalib;
        colorCalib.cameraParam.width = colorWidth;
        colorCalib.cameraParam.height = colorHeight;
 
        auto intrinsics = colorSensor->getOutputMode().intrinsics;
        colorCalib.cameraParam.focalLength = {intrinsics.fx, intrinsics.fy};
 
        colorCalib.cameraParam.principalPoint = {intrinsics.cx, intrinsics.cy};
 
        // Minimal distortion (assuming Nuitrack provides undistorted images or handles distortion)
        colorCalib.cameraParam.distortion = {0.0f, 0.0f, 0.0f, 0.0f};
 
        // Identity rotation and zero translation (color camera as reference)
        colorCalib.cameraParam.rotation = convertEigenMatToVisionX(Matrix3f::Identity());
        colorCalib.cameraParam.translation = {0.0f, 0.0f, 0.0f};
 
        stereo_calibration.calibrationLeft = colorCalib;
 
        // Depth camera calibration (right) - same as color for simplicity
        // since Nuitrack aligns depth to color frame
        MonocularCalibration depthCalib = colorCalib;
        depthCalib.cameraParam.distortion = {0.0f, 0.0f, 0.0f, 0.0f}; // Depth is rectified
        stereo_calibration.calibrationRight = depthCalib;
 
        stereo_calibration.rectificationHomographyLeft =
            convertEigenMatToVisionX(Matrix3f::Identity());
        stereo_calibration.rectificationHomographyRight =
            convertEigenMatToVisionX(Matrix3f::Identity());
 
        return stereo_calibration;
    }
 
    bool
    NuitrackPointCloudProvider::getImagesAreUndistorted(const Ice::Current&)
    {
        return true;
    }
 
    std::string
    NuitrackPointCloudProvider::getReferenceFrame(const Ice::Current&)
    {
        return getProperty<std::string>("bodyCameraFrameName");
    }
 
    std::vector<imrec::ChannelPreferences>
    NuitrackPointCloudProvider::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
    NuitrackPointCloudProvider::enableHumanPoseEstimation(
        const armarx::EnableHumanPoseEstimationInput& input,
        const Ice::Current&)
    {
        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";
        }
    }
 
    void
    NuitrackPointCloudProvider::setMaxDepthBodyTracking(int maxDepthInMM, const Ice::Current&)
    {
        std::scoped_lock lock(bodyTrackingParameterMutex);
        bodyTrackingDepthMaskMaxZ = maxDepthInMM;
    }
 
    void
    NuitrackPointCloudProvider::setWidthBodyTracking(int minXinPixel,
                                                     int maxXinPixel,
                                                     const Ice::Current&)
    {
        std::scoped_lock lock(bodyTrackingParameterMutex);
        bodyTrackingDepthMaskMinX = minXinPixel;
        bodyTrackingDepthMaskMaxX = maxXinPixel;
    }
 
    NuitrackPointCloudProvider::NuitrackPointCloudProvider()
    {
        addPlugin(heartbeatPlugin);
    }
 
    void
    NuitrackPointCloudProvider::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
    NuitrackPointCloudProvider::Framerate::Subordinate::skip()
    {
        bool skip = false;
        if (skipFrames > 0)
        {
            skip = skipFramesCount;
            skipFramesCount++;
            skipFramesCount %= (skipFrames + 1);
        }
        return skip;
    }
 
} // namespace visionx