NuitrackPointCloudProvider.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::ArmarXObjects::NuitrackPointCloudProvider
 * @author     Mirko Wächter
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
// STD
#include <chrono>
#include <condition_variable>
#include <mutex>
 
// Eigen3
#include <Eigen/Core>
 
// OpenCV
#include <opencv2/opencv.hpp>
#include <VirtualRobot/VirtualRobot.h>
 
// ArmarXCore
#include "ArmarXCore/core/time/forward_declarations.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
// RobotAPI
#include "RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
// VisionX
#include <VisionX/components/pointcloud_core/CapturingPointCloudProvider.h>
#include <VisionX/components/pointcloud_provider/ImageToPointCloud/DepthImageUtils.h>
#include <VisionX/core/CapturingImageProvider.h>
//#include <VisionX/interface/components/RGBDImageProvider.h>
#include <VisionX/interface/components/AzureKinectPointCloudProviderInterface.h>
#include <VisionX/libraries/armem_human/server/HumanMemoryServerInterface.h>
 
// Nuitrack
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
 
#include "VisionX/libraries/armem_human/client/HumanPoseWriter.h"
 
// Nuitrack SDK (suppress warnings from third-party headers)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wswitch"
#pragma GCC diagnostic ignored "-Wreorder"
#include <nuitrack/Nuitrack.h>
#pragma GCC diagnostic pop
 
namespace visionx
{
    /// @class NuitrackPointCloudProviderPropertyDefinitions
    class NuitrackPointCloudProviderPropertyDefinitions :
        public visionx::CapturingPointCloudProviderPropertyDefinitions
    {
    public:
        explicit NuitrackPointCloudProviderPropertyDefinitions(std::string prefix);
    };
 
    // TODO: Update the comments for this component.
    /**
     * @defgroup Component-NuitrackPointCloudProvider NuitrackPointCloudProvider
     * @ingroup VisionX-Components
     * Provides support for the Azure Kinect cameras for ArmarX.
     *
     * @class NuitrackPointCloudProvider
     * @ingroup Component-NuitrackPointCloudProvider
     * @brief Brief description of class NuitrackPointCloudProvider.
     */
    class NuitrackPointCloudProvider :
        //        virtual public visionx::RGBDPointCloudProviderInterface,
        virtual public armarx::AzureKinectPointCloudProviderInterface,
        virtual public visionx::CapturingPointCloudProvider,
        virtual public visionx::ImageProvider,
        virtual public armarx::DebugObserverComponentPluginUser,
        virtual public armarx::armem::ClientPluginUser
    {
    public:
        NuitrackPointCloudProvider();
 
        using CloudPointType = pcl::PointXYZRGBA;
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override;
 
    protected:
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        // ManagedIceObject interface
    protected:
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
 
        void onExitComponent() override;
 
        // StereoCalibrationInterface interface
    public:
        visionx::StereoCalibration getStereoCalibration(const Ice::Current& c) override;
        bool getImagesAreUndistorted(const ::Ice::Current& c) override;
 
        std::string getReferenceFrame(const Ice::Current& c) override;
 
        std::vector<imrec::ChannelPreferences>
        getImageRecordingChannelPreferences(const Ice::Current&) override;
 
        // NuitrackBodyTrackingInterface interface
    public:
        void enableHumanPoseEstimation(const armarx::EnableHumanPoseEstimationInput& input,
                                       const Ice::Current& = Ice::emptyCurrent) override;
 
        void setMaxDepthBodyTracking(int maxDepthInMM,
                                     const Ice::Current& = Ice::emptyCurrent) override;
 
        void setWidthBodyTracking(int minXinPixel,
                                  int maxXinPixel,
                                  const Ice::Current& = Ice::emptyCurrent) override;
 
    protected:
        void onStartCapture(float frames_per_second) override;
        void onStopCapture() override;
        void onInitImageProvider() override;
        void onConnectImageProvider() override;
        void onDisconnectImageProvider() override;
 
        void
        onExitImageProvider() override
        {
        }
 
        void onInitCapturingPointCloudProvider() override;
        void onExitCapturingPointCloudProvider() override;
        bool doCapture() override;
 
        bool
        hasSharedMemorySupport(const Ice::Current& c) override
        {
            return true;
        }
 
        MetaPointCloudFormatPtr
        getDefaultPointCloudFormat() override
        {
            MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
            //info->frameId = getProperty<std::string>("frameId").getValue();
            info->type = PointContentType::eColoredPoints;
 
            ARMARX_CHECK_EXPRESSION(resultColorImage);
 
            ARMARX_INFO << "default pointcloud format: " << resultColorImage->width << ", "
                        << resultColorImage->height;
 
            info->capacity =
                resultColorImage->width * resultColorImage->height * sizeof(ColoredPoint3D);
            info->size = info->capacity;
            return info;
        }
 
        void runPointcloudPublishing();
 
 
        std::function<void(armarx::Duration)> createSwCallback(const std::string& description);
 
        void onNewDepthFrame(tdv::nuitrack::DepthFrame::Ptr frame);
        void onNewColorFrame(tdv::nuitrack::RGBFrame::Ptr frame);
        void onNewSkeletonData(tdv::nuitrack::SkeletonData::Ptr skeletonData);
 
        std::optional<Eigen::Vector3f>
        computeBodyUpVector(const armarx::armem::human::HumanPose& pose);
 
        void
        filterPosesByUpright(std::vector<armarx::armem::human::HumanPose>& poses,
                            const Eigen::Vector3f& worldUp,
                            float maxAngleDeg);
 
    private:
        // Time the last image was recorded.
        IceUtil::Time imagesTime;
 
        // Mutex to protect the point cloud provider.
        std::mutex pointcloudProcMutex;
        std::condition_variable pointcloudProcSignal;
 
        bool depthImageReady;
        bool depthImageProcessed;
 
        // Task for the point cloud provider.
        armarx::RunningTask<visionx::NuitrackPointCloudProvider>::pointer_type pointcloudTask;
 
        // IVT result images scaled to the size of the color image.
        visionx::CByteImageUPtr resultDepthImage, resultColorImage;
 
        // Result point cloud.
        pcl::PointCloud<CloudPointType>::Ptr pointcloud;
        MetaPointCloudFormatPtr cloudFormat;
 
        // Stereo calibration used.
        visionx::StereoCalibration calibration;
 
        bool enableHeartbeat = true;
 
        // Nuitrack handles.
        tdv::nuitrack::DepthSensor::Ptr depthSensor;
        tdv::nuitrack::ColorSensor::Ptr colorSensor;
 
        // Latest frames from Nuitrack
        tdv::nuitrack::DepthFrame::Ptr latestDepthFrame;
        tdv::nuitrack::RGBFrame::Ptr latestColorFrame;
        std::int64_t latestTimestamp;
        std::mutex colorFrameMutex;
        std::mutex depthFrameMutex;
 
        // Processed images
        cv::Mat alignedDepthImage;
        cv::Mat colorImageRGB;
 
        tdv::nuitrack::SkeletonTracker::Ptr skeletonTracker;
        tdv::nuitrack::SkeletonData::Ptr latestSkeletonData;
        std::mutex skeletonMutex;
        //        armarx::armem::human::HumanMemoryServerInterfacePrx humanMemoryServer;
 
        armarx::armem::human::client::Writer humanPoseWriter;
 
        armarx::RunningTask<NuitrackPointCloudProvider>::pointer_type bodyTrackingPublishTask;
 
        void runPublishBodyTrackingResults();
 
        bool bodyTrackingEnabled = true;
        bool bodyTrackingRunAtStart = true;
        std::atomic<bool> bodyTrackingIsRunning = false;
        std::mutex bodyTrackingParameterMutex;
        int bodyTrackingDepthMaskMinX = -1;
        int bodyTrackingDepthMaskMaxX = -1;
        int bodyTrackingDepthMaskMaxZ = -1;
        double confidenceThreshold = 0.;
        int minimumSegmentsWithConfidenceLargerThanThreshold = 0;
        float uprightFilterMaxAngleDeg = 45.F;
 
        std::string bodyCameraFrameName = "AzureKinect_Depth";
        std::string robotName = "Armar6";
 
        struct Framerate
        {
            int value = 30;
 
            struct Subordinate
            {
                std::string name;
                float value = 30;
                unsigned int skipFrames = 0;
                unsigned int skipFramesCount = 0;
 
                void update(int higherFramerate);
                bool skip();
            };
 
            Subordinate bodyTracking{.name = "Body Tracking"};
            Subordinate pointCloud{.name = "Point Cloud"};
        } framerate;
 
        std::mutex deviceToRealtimeOffsetMtx;
        std::chrono::nanoseconds device_to_realtime_offset_{0};
 
        std::mutex debugObserverMtx;
        std::mutex metaInfoMtx;
 
        armarx::plugins::HeartbeatComponentPlugin* heartbeatPlugin = nullptr;
 
        armarx::armem::robot_state::VirtualRobotReader robotReader;
        VirtualRobot::RobotPtr robot;
 
    };
} // namespace visionx