OpenPoseEstimation.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::OpenPoseEstimation
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
 
#include <RobotAPI/components/RobotState/RobotStateComponent.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
#include <VisionX/components/image_processor/OpenPoseEstimation/ImageKeypointBuffer.h>
#include <VisionX/components/image_processor/OpenPoseEstimation/KeypointManager.h>
#include <VisionX/components/image_processor/OpenPoseEstimation/Util.h>
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
#include <VisionX/interface/components/PointCloudAndImageAndCalibrationProviderInterface.h>
#include <VisionX/libraries/human/pose/model/openpose_body_25.h>
 
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
 
// OpenPose
#include <openpose/core/headers.hpp>
#include <openpose/pose/headers.hpp>
#include <openpose/utilities/headers.hpp>
 
//#ifdef OPENPOSE_MODELS
//#define MODEL_FOLDER OPENPOSE_MODELS
//#else
//#define MODEL_FOLDER ""
//#endif
 
 
namespace armarx
{
    enum OpenPoseEstimationMode
    {
        FromDepthImage,
        FromStereoImage,
        FromTopic
    };
 
    /**
     * @class OpenPoseEstimationPropertyDefinitions
     * @brief
     */
    class OpenPoseEstimationPropertyDefinitions : public visionx::ImageProcessorPropertyDefinitions
    {
    public:
        OpenPoseEstimationPropertyDefinitions(std::string prefix) :
            visionx::ImageProcessorPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("OpenPoseEstimation2DTopicName",
                                                "OpenPoseEstimation2D");
            defineOptionalProperty<std::string>("OpenPoseEstimation3DTopicName",
                                                "OpenPoseEstimation3D");
            defineOptionalProperty<int>(
                "DepthMedianRadius",
                10,
                "Radius of the circle around a target pixel in the depth image, which is used to "
                "calculate the median around that pixel.");
            defineOptionalProperty<bool>("UseDistortionParameters",
                                         false,
                                         "Whether to use distortion parameters when transforming "
                                         "image coordinates into world coordinates");
 
            defineRequiredProperty<std::string>("ImageProviderName",
                                                "Name of the ImageProviderComponent");
            defineOptionalProperty<std::string>(
                "DebugDrawerTopicName",
                "DebugDrawerUpdates",
                "Name of the debug drawer topic that should be used");
 
            defineOptionalProperty<OpenPoseEstimationMode>(
                "Mode",
                FromDepthImage,
                "Sets the mode of this instance of OpenPoseEstiamtion. There are currently three "
                "support modes:"
                "'FromDepthImage', which lets the image processor expect a rgb-image and a "
                "depth-image,"
                "'FromStereoImage', which lets the image processor expect two images as a stereo "
                "input "
                "which will be used for calculating the 3D-values and"
                "'FromTopic', which lets the image processor listen on a topic which provides "
                "openpose-keypoints.")
                .setMatchRegex("FromDepthImage|FromStereoImage|FromTopic")
                .map("FromDepthImage", OpenPoseEstimationMode::FromDepthImage)
                .map("FromStereoImage", OpenPoseEstimationMode::FromStereoImage)
                .map("FromTopic", OpenPoseEstimationMode::FromTopic);
 
            defineOptionalProperty<std::string>(
                "OP_net_resolution",
                "-1x368",
                "Multiples of 16. If it is increased, the accuracy potentially increases. If it is "
                "decreased, the speed increases. For maximum speed-accuracy balance, it should "
                "keep the "
                "closest aspect ratio possible to the images or videos to be processed.\n Using "
                "`-1` in "
                "any of the dimensions, OP will choose the optimal aspect ratio depending on the "
                "user's "
                "input value.\n E.g. the default `-1x368` is equivalent to `656x368` in 16:9 "
                "resolutions, "
                "e.g. full HD (1980x1080) and HD (1280x720) resolutions.");
            defineOptionalProperty<std::string>("OP_output_resolution",
                                                "-1x-1",
                                                "The image resolution (display and output). Use "
                                                "\"-1x-1\" to force the program to use the"
                                                " input image resolution.");
            defineOptionalProperty<double>("OP_scale_gap",
                                           0.3,
                                           "Scale gap between scales. No effect unless "
                                           "scale_number > 1. Initial scale is always 1. "
                                           "If you want to change the initial    calib->get scale, "
                                           "you actually want to multiply the "
                                           "`net_resolution` by your desired initial scale.");
            defineOptionalProperty<int>("OP_scale_number", 1, "Number of scales to average.");
            defineOptionalProperty<std::string>(
                "OP_model_pose",
                human::pose::model::openpose_body_25::ModelId,
                "Model to be used. E.g. `BODY_25` (25 keypoints, best model), `COCO` (18 "
                "keypoints), `MPI` (15 keypoints, ~10% faster), "
                "MPI_4_layers` (15 keypoints, even faster but less accurate).");
            defineOptionalProperty<std::string>("OP_model_folder",
                                                "models/",
                                                "Folder path (absolute or relative) where the "
                                                "models (pose, face, ...) are located.");
            defineOptionalProperty<int>("OP_num_gpu_start", 0, "GPU device start number.");
            defineOptionalProperty<float>("OP_render_threshold",
                                          0.05f,
                                          "Only estimated keypoints whose score confidences are "
                                          "higher than this threshold will be"
                                          " rendered.\n Generally, a high threshold (> 0.5) will "
                                          "only render very clear body parts;"
                                          " while small thresholds (~0.1) will also output guessed "
                                          "and occluded keypoints, but also"
                                          " more false positives (i.e. wrong detections).");
            defineOptionalProperty<int>("MaxDepth",
                                        3000,
                                        "Pixels with a distance higher than this value are masked "
                                        "out. Only for depth camera mode.",
                                        PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<int>(
                "MaxDepthDifference",
                700,
                "Allowed difference of depth value for one keypoint to median of all keypoints.",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<int>(
                "MaskBrightnessIncrease",
                100,
                "All masked pixels will be increased by this brightness for visualization. If "
                "value is <0, the original mask is shown.",
                PropertyDefinitionBase::eModifiable);
 
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent");
            defineOptionalProperty<std::string>(
                "CameraNodeName", "DepthCamera", "Name of the robot node for the input camera");
            defineOptionalProperty<bool>(
                "FilterWithWorkspacePolygonActive",
                true,
                "Whether to use the WorkspacePolygon for filtering keypoints.");
            defineOptionalProperty<std::string>(
                "WorkspacePolygon",
                "-5000,-5000;5000,-5000;5000,5000;-5000,5000;-5000,-5000",
                "A string that describes a polygon which represents the workspace of the robot.\n"
                "Every keypoint outside of this polygon will be masked out.\n"
                "Every point is seperated by a ';' (semicolon) and every point is described as "
                "'x-value, y-value' (comma-seperated).\n"
                "The last point must be identical to the first point.");
            defineOptionalProperty<int>("MinimalAmountKeypoints",
                                        5,
                                        "Minimal amount of keypoints per person. Detected persons "
                                        "with less valid keypoints will be discarded.");
            defineOptionalProperty<bool>("ReportOnlyNearestPerson",
                                         false,
                                         "If true, only one person is reported in the 3DTopic. The "
                                         "reported person is the nearest person to the robot.");
            defineOptionalProperty<bool>("ActivateOnStartup",
                                         true,
                                         "If true, poseEstimation-tasks are started after starting "
                                         "the component. If false, the component idles.");
 
            defineOptionalProperty<std::string>(
                "Topic_Dimensions",
                "480x360",
                "Only for: 'Mode'='FromTopic'!! The dimensions of the image whose keypoints are "
                "reported over the topic.");
        }
    };
 
    /**
     * @defgroup Component-OpenPoseEstimation OpenPoseEstimation
     * @ingroup VisionX-Components
     * A description of the component OpenPoseEstimation.
     *
     * @class OpenPoseEstimation
     * @ingroup Component-OpenPoseEstimation
     * @brief Brief description of class OpenPoseEstimation.
     *
     * Detailed description of class OpenPoseEstimation.
     */
    class OpenPoseEstimation :
        public visionx::ImageProcessor,
        public OpenPoseEstimationInterface,
        virtual public armarx::ArVizComponentPluginUser
    {
    public:
        using PoseKeypoints = op::Array<float>;
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "OpenPoseEstimation";
        }
 
        void start(const Ice::Current& = Ice::emptyCurrent) override;
        void stop(const Ice::Current& = Ice::emptyCurrent) override;
        void start3DPoseEstimation(const Ice::Current& = Ice::emptyCurrent) override;
        void stop3DPoseEstimation(const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
        static void Render2DResultImage(const CByteImage& inputImage,
                                        const CByteImage& maskedInputImage,
                                        KeypointManagerPtr keypointManager,
                                        CByteImage& resultImage,
                                        op::PoseModel poseModel,
                                        float renderThreshold,
                                        int brightnessIncrease = 100);
        static void Render2DResultImage(const CByteImage& inputImage,
                                        const CByteImage& maskedInputImage,
                                        PoseKeypoints& keypoints,
                                        CByteImage& resultImage,
                                        op::PoseModel poseModel,
                                        float renderThreshold,
                                        int brightnessIncrease = 100);
        static void VisualizeTransparentImageMask(CByteImage& resultImage,
                                                  const CByteImage& maskedInputImage,
                                                  int brightnessIncrease,
                                                  const CByteImage& inputImage);
 
    protected:
        // ImageProcessor interface
        void onInitImageProcessor() override;
        void onConnectImageProcessor() override;
        void onDisconnectImageProcessor() override;
        void onExitImageProcessor() override;
        void process() override;
 
        KeypointManagerPtr generate2DKeypoints(PoseKeypoints& keypoints,
                                               const CByteImage& rgbImage) const;
        void calculate2DFromOpenPose();
        void calculate2DFromTopic();
        void calculate3DFromDepthImage(KeypointManagerPtr manager);
        void calculate3DFromStereoImage(KeypointManagerPtr manager);
        void filterKeypointsBasedOnWorkspacePolygon(KeypointObjectPtr object);
        void filterToNearest();
        void visualize3DKeypoints();
 
        int getMedianDepthFromImage(int x, int y, int radius) const;
        void maskOutBasedOnDepth(CByteImage& image, int maxDepth);
        DrawColor24Bit getDominantColorOfPatch(const CByteImage& image,
                                               const Vec2d& point,
                                               int windowSize = 10) const;
 
        //Texting interface
        void onMessage(const Texting::TextMessage& text,
                       const Ice::Current& = Ice::emptyCurrent) override;
 
    private:
        // Meta (Properties)
        OpenPoseEstimationMode mode;
        std::string providerName;
        int radius;
        bool useDistortionParameters;
        bool reportOnlyNearestPerson;
        bool filterWithWorkspacePolygonActive;
        std::string cameraNodeName;
        Polygon2D workspacePolygon;
        int minimalValidKeypoints;
        float renderThreshold;
 
        // Robot
        RobotStateComponentInterfacePrx robotStateInterface;
        VirtualRobot::RobotPtr localRobot;
 
        // Keypoints
        KeypointManagerPtr keypointManager;
        std::mutex keypointManagerMutex;
 
        // ImageBuffer und ImageInformations
        CByteImage** imageBuffer;
        CByteImage *rgbImageBuffer, *maskedrgbImageBuffer;
        std::mutex rgbImageBufferMutex;
        CByteImage* depthImageBuffer;
        std::mutex depthImageBufferMutex;
        CByteImage** openPoseResultImage;
        std::mutex resultImageBufferMutex;
        visionx::ImageProviderInfo imageProviderInfo;
        armarx::MetaInfoSizeBasePtr imageMetaInfo;
        unsigned int numImages;
 
        // SecondHandsTopic
        ImageKeypointBufferPtr imageKeypointBuffer;
        visionx::ImageDimension incomingKeypointDimensions;
 
        // Calibrations (IVT-Objects)
        const CCalibration* calibration = nullptr;
        CStereoCalibration* stereoCalibration = nullptr;
 
        // Visualization
        armarx::DebugDrawerInterfacePrx debugDrawerTopic;
        std::string layerName;
        int layerCounter = 0;
 
        // Topics
        OpenPose2DListenerPrx listener2DPrx;
        OpenPose3DListenerPrx listener3DPrx;
 
        // Threads and program flow information
        RunningTask<OpenPoseEstimation>::pointer_type task2DKeypoints;
        RunningTask<OpenPoseEstimation>::pointer_type task3DKeypoints;
        bool running2D;
        bool running3D;
        void run();
        bool
            imageUpdated; // Is true, if new images are available (when mode is 'FromTopic' this also means that corresponding keypoints are available)
        long
            timeProvidedImage; // Contains the timestamp of the currently available images (when mode is 'FromTopic' this is also the timestamp of the corresponding keypoints)
 
        // OpenPose
        std::shared_ptr<op::ScaleAndSizeExtractor> scaleAndSizeExtractor;
        std::shared_ptr<op::CvMatToOpInput> cvMatToOpInput;
        std::shared_ptr<op::CvMatToOpOutput> cvMatToOpOutput;
        std::shared_ptr<op::PoseExtractorCaffe> poseExtractorCaffe;
        std::shared_ptr<op::OpOutputToCvMat> opOutputToCvMat;
        void setupOpenPoseEnvironment();
        PoseKeypoints getPoseKeypoints(CByteImage* imageBuffer);
        op::PoseModel poseModel;
 
        // ErrorCounters
        std::uint64_t timeoutCounter2d{0};
        std::uint64_t readErrorCounter2d{0};
        std::uint64_t sucessCounter2d{0};
    };
} // namespace armarx