AzureKinectPointCloudProvider.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::AzureKinectPointCloudProvider
 * @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>
 
// 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/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>
 
// K4A
#include <k4a/k4a.h>
#include <k4a/k4a.hpp>
 
#ifdef INCLUDE_BODY_TRACKING
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
 
#include "VisionX/libraries/armem_human/client/HumanPoseWriter.h"
 
#include <k4abt.hpp>
#endif
 
namespace visionx
{
    /// @class AzureKinectPointCloudProviderPropertyDefinitions
    class AzureKinectPointCloudProviderPropertyDefinitions :
        public visionx::CapturingPointCloudProviderPropertyDefinitions
    {
    public:
        explicit AzureKinectPointCloudProviderPropertyDefinitions(std::string prefix);
    };
 
 
    // TODO: Update the comments for this component.
    /**
     * @defgroup Component-AzureKinectPointCloudProvider AzureKinectPointCloudProvider
     * @ingroup VisionX-Components
     * Provides support for the Azure Kinect cameras for ArmarX.
     *
     * @class AzureKinectPointCloudProvider
     * @ingroup Component-AzureKinectPointCloudProvider
     * @brief Brief description of class AzureKinectPointCloudProvider.
     */
    class AzureKinectPointCloudProvider :
        //        virtual public visionx::RGBDPointCloudProviderInterface,
        virtual public armarx::AzureKinectPointCloudProviderInterface,
        virtual public visionx::CapturingPointCloudProvider,
        virtual public visionx::ImageProvider,
        virtual public armarx::DebugObserverComponentPluginUser
#ifdef INCLUDE_BODY_TRACKING
        ,
        virtual public armarx::armem::ClientPluginUser
#endif
    {
    public:
        AzureKinectPointCloudProvider();
 
        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;
 
        // AzureKinectBodyTrackingInterface 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();
 
        /*!
         * @brief Returns the dimension of the color images that will be produced for a certain resolution.
         * @param resolution The resolution that should be used for the color image.
         * @return Pair of width and height that color images will have.
         */
        static inline std::pair<int, int>
        GetColorDimensions(const k4a_color_resolution_t resolution)
        {
            switch (resolution)
            {
                case K4A_COLOR_RESOLUTION_720P:
                    return {1280, 720};
                case K4A_COLOR_RESOLUTION_2160P:
                    return {3840, 2160};
                case K4A_COLOR_RESOLUTION_1440P:
                    return {2560, 1440};
                case K4A_COLOR_RESOLUTION_1080P:
                    return {1920, 1080};
                case K4A_COLOR_RESOLUTION_3072P:
                    return {4096, 3072};
                case K4A_COLOR_RESOLUTION_1536P:
                    return {2048, 1536};
 
                default:
                    throw std::logic_error("Invalid color dimensions value!");
            }
        }
 
        /*!
         * @brief Returns the dimension of the depth images that will be produced for a certain resolution.
         * @param resolution The resolution that should be used for the depth image.
         * @return Pair of width and height that depth images will have.
         */
        static inline std::pair<int, int>
        GetDepthDimensions(const k4a_depth_mode_t depth_mode)
        {
            switch (depth_mode)
            {
                case K4A_DEPTH_MODE_NFOV_2X2BINNED:
                    return {320, 288};
                case K4A_DEPTH_MODE_NFOV_UNBINNED:
                    return {640, 576};
                case K4A_DEPTH_MODE_WFOV_2X2BINNED:
                    return {512, 512};
                case K4A_DEPTH_MODE_WFOV_UNBINNED:
                    return {1024, 1024};
                case K4A_DEPTH_MODE_PASSIVE_IR:
                    return {1024, 1024};
 
                default:
                    throw std::logic_error("Invalid depth dimensions value!");
            }
        }
 
        /*!
         * @brief Creates a string from a k4a_version_t.
         * @param version The verstion that should be transformed into a string.
         * @return The string representing the version.
         */
        static std::string
        VersionToString(const k4a_version_t& version)
        {
            std::stringstream s;
            s << version.major << "." << version.minor << "." << version.iteration;
            return s.str();
        }
 
        std::function<void(armarx::Duration)> createSwCallback(const std::string& description);
 
        void updateTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us,
                                   const std::chrono::nanoseconds& k4a_system_timestamp_ns);
 
        void initializeTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us);
 
 
        armarx::DateTime timestampToArmarX(const std::chrono::microseconds& k4a_timestamp_us);
 
    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::AzureKinectPointCloudProvider>::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;
 
        // Undistort the color image with all coefficients.
        bool enableColorUndistortion = false;
 
        //
        bool enableHeartbeat = true;
 
        // Path to a external calibration file.
        std::string externalCalibrationFilePath;
 
        // Camera parameters and distortion information.
        cv::Mat cameraMatrix;
        cv::Mat distCoeffs;
 
        cv::Mat colorDistortionMap;
 
        // K4A handles.
        k4a::device device;
        k4a_device_configuration_t config;
        k4a::calibration k4aCalibration;
 
        k4a::transformation transformation;
 
        k4a::image alignedDepthImage, xyzImage;
 
#ifdef INCLUDE_BODY_TRACKING
        k4abt::tracker bodyTracker;
        //        armarx::armem::human::HumanMemoryServerInterfacePrx humanMemoryServer;
 
        armarx::armem::human::client::Writer humanPoseWriter;
 
        armarx::RunningTask<AzureKinectPointCloudProvider>::pointer_type bodyTrackingPublishTask;
 
        void runPublishBodyTrackingResults();
#endif
 
        int mDeviceId = K4A_DEVICE_DEFAULT;
 
        struct diagnostics
        {
            unsigned int num_crashes = 0;
        };
 
        diagnostics mDiagnostics;
 
        bool bodyTrackingEnabled = false; // Initialize Body Tracking
        bool bodyTrackingRunAtStart = false;
        std::atomic<bool> bodyTrackingIsRunning = false;
        std::string bodyTrackingModelFilename = "${K4A_BODY_TRACKING_DNN_MODEL_FILEPATH}";
        std::int32_t bodyTrackingGPUDeviceID = 0;
        std::mutex bodyTrackingParameterMutex;
        float bodyTrackingTemporalSmoothingFactor = 0.0f;
        int bodyTrackingDepthMaskMinX = -1;
        int bodyTrackingDepthMaskMaxX = -1;
        int bodyTrackingDepthMaskMaxZ = -1;
        bool startIMU = false;
        bool useCPU = false;
 
        std::string bodyCameraFrameName = "AzureKinectDepthCamera";
        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;
    };
} // namespace visionx