OpenPoseEstimation.cpp

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
 */
 
#include "OpenPoseEstimation.h"
 
#include <boost/algorithm/string.hpp>
#include <boost/make_shared.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/math/ColorUtils.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
#include <VisionX/interface/components/RGBDImageProvider.h>
#include <VisionX/libraries/ArViz/HumanPoseBody25.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Image/IplImageAdaptor.h>
#include <Image/PrimitivesDrawer.h>
#include <openpose/pose/poseParameters.hpp>
#include <openpose/pose/poseParametersRender.hpp>
 
 
// json
#include <SimoxUtility/json/json.hpp>
 
using namespace armarx;
 
#define MAX_LAYER 5
 
namespace
{
    // This is a workaround because in OpenPose 1.4, this function was called `intRound`, whereas
    // in OpenPose 1.5.1 it was renamed to `positiveIntRound`.  This function is now defined in this
    // implementation as long as we use several OpenPose versions.  As soon as we move to 1.5.1 we
    // can remove this and use 'op::positiveIntRound` instead.
    template <typename T>
    inline int
    positiveIntRound(const T a)
    {
        return int(a + 0.5f);
    }
} // namespace
 
armarx::PropertyDefinitionsPtr
OpenPoseEstimation::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new OpenPoseEstimationPropertyDefinitions(getConfigIdentifier()));
}
 
void
OpenPoseEstimation::onInitImageProcessor()
{
    providerName = getProperty<std::string>("ImageProviderName").getValue();
 
    usingImageProvider(providerName);
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    radius = getProperty<int>("DepthMedianRadius").getValue();
    useDistortionParameters = getProperty<bool>("UseDistortionParameters").getValue();
    layerName = "OpenPoseEstimation";
    renderThreshold = getProperty<float>("OP_render_threshold").getValue();
 
    cameraNodeName = getProperty<std::string>("CameraNodeName").getValue();
    minimalValidKeypoints = getProperty<int>("MinimalAmountKeypoints").getValue();
    reportOnlyNearestPerson = getProperty<bool>("ReportOnlyNearestPerson").getValue();
 
    // Setup workspace-polygon
    std::vector<Polygon2D::Point> points;
    std::vector<std::string> pointStrings;
    std::string input = getProperty<std::string>("WorkspacePolygon").getValue();
    boost::split(pointStrings, input, boost::is_any_of(";"));
    for (auto s : pointStrings)
    {
        ARMARX_VERBOSE << "WorkspacePolygon: " << s;
        std::vector<std::string> workSpacePolygonPoint;
        boost::split(workSpacePolygonPoint, s, boost::is_any_of(","));
        ARMARX_CHECK_EXPRESSION(workSpacePolygonPoint.size() == 2);
        Polygon2D::Point point;
        point.x = std::strtof(workSpacePolygonPoint.at(0).c_str(), nullptr);
        point.y = std::strtof(workSpacePolygonPoint.at(1).c_str(), nullptr);
        points.push_back(point);
    }
    workspacePolygon = Polygon2D(points);
 
    mode = getProperty<OpenPoseEstimationMode>("Mode").getValue();
    if (mode == OpenPoseEstimationMode::FromTopic)
    {
        usingTopic("2Dposes");
        poseModel = op::flagsToPoseModel("MPI");
        ARMARX_INFO << "Switching human model from \""
                    << getProperty<std::string>("OP_model_pose").getValue()
                    << "\" to \"MPI\" because the OpenPoseEstiamtionMode is set to \"FromTopic\".";
 
        // parse Topic_Dimensions
        std::string s = getProperty<std::string>("Topic_Dimensions").getValue();
        std::vector<std::string> split = armarx::Split(s, "x", true);
        ARMARX_CHECK_EXPRESSION(split.size() == 2);
        this->incomingKeypointDimensions.width = armarx::toInt(split.at(0));
        this->incomingKeypointDimensions.height = armarx::toInt(split.at(1));
    }
    else
    {
        poseModel = op::flagsToPoseModel(getProperty<std::string>("OP_model_pose").getValue());
    }
 
    timeoutCounter2d = 0;
    readErrorCounter2d = 0;
    sucessCounter2d = 0;
}
 
void
OpenPoseEstimation::onConnectImageProcessor()
{
    visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
    imageProviderInfo = getImageProvider(providerName, imageDisplayType);
 
    numImages = static_cast<unsigned int>(imageProviderInfo.numberImages);
    if (numImages < 1 || numImages > 2)
    {
        ARMARX_FATAL << "invalid number of images. aborting";
        return;
    }
    if (mode == OpenPoseEstimationMode::FromStereoImage ||
        mode == OpenPoseEstimationMode::FromTopic)
    {
        if (numImages != 2)
        {
            ARMARX_FATAL << "invalid number of images. aborting";
            return;
        }
    }
 
    imageBuffer = new CByteImage*[numImages];
    for (unsigned int i = 0; i < numImages; i++)
    {
        imageBuffer[i] = visionx::tools::createByteImage(imageProviderInfo);
    }
    rgbImageBuffer = visionx::tools::createByteImage(imageProviderInfo);
    maskedrgbImageBuffer = visionx::tools::createByteImage(imageProviderInfo);
    ::ImageProcessor::Zero(maskedrgbImageBuffer);
    depthImageBuffer = visionx::tools::createByteImage(imageProviderInfo);
    ::ImageProcessor::Zero(depthImageBuffer);
    openPoseResultImage = new CByteImage*[1];
    openPoseResultImage[0] = visionx::tools::createByteImage(imageProviderInfo);
    enableResultImages(
        1, imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type);
 
    offeringTopic(getProperty<std::string>("OpenPoseEstimation2DTopicName").getValue());
    listener2DPrx = getTopic<OpenPose2DListenerPrx>(
        getProperty<std::string>("OpenPoseEstimation2DTopicName").getValue());
    offeringTopic(getProperty<std::string>("OpenPoseEstimation3DTopicName").getValue());
    listener3DPrx = getTopic<OpenPose3DListenerPrx>(
        getProperty<std::string>("OpenPoseEstimation3DTopicName").getValue());
 
    // Calibration
    stereoCalibration = nullptr;
    calibration = nullptr;
    {
        ARMARX_VERBOSE << "Trying to get StereoCalibrationInterface proxy: "
                       << getIceManager()->getCommunicator()->proxyToString(
                              imageProviderInfo.proxy);
        visionx::StereoCalibrationInterfacePrx calib =
            visionx::StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
        if (!calib)
        {
            ARMARX_WARNING
                << "Image provider does not provide a stereo calibration - 3D will not work - "
                << imageProviderInfo.proxy->ice_ids();
        }
        else
        {
            ARMARX_VERBOSE << "got StereoCalibrationInterface proxy";
            stereoCalibration = visionx::tools::convert(calib->getStereoCalibration());
            ARMARX_VERBOSE << "got StereoCalibration";
            calibration = stereoCalibration->GetLeftCalibration();
            ARMARX_VERBOSE << "got mono Calibration";
        }
    }
 
    debugDrawerTopic = getTopic<armarx::DebugDrawerInterfacePrx>(
        getProperty<std::string>("DebugDrawerTopicName").getValue());
    debugDrawerTopic->clearLayer(layerName);
    ARMARX_VERBOSE << "Trying to get RobotStateComponent proxy";
    // Trying to access robotStateComponent if it is avaiable
    robotStateInterface = getProxy<RobotStateComponentInterfacePrx>(
        getProperty<std::string>("RobotStateComponentName").getValue(), false, "", false);
    if (robotStateInterface)
    {
        localRobot = RemoteRobot::createLocalClone(robotStateInterface);
        ARMARX_VERBOSE << "RobotStateComponent available";
    }
    else
    {
        ARMARX_ERROR << "RobotStateCompontent is not avaiable, 3D-Estimation will be deactivated!!";
    }
 
    if (mode == OpenPoseEstimationMode::FromTopic)
    {
        imageKeypointBuffer.reset(new ImageKeypointBuffer(imageProviderInfo));
    }
 
    if (getProperty<bool>("ActivateOnStartup").getValue())
    {
        running2D = true;
        task2DKeypoints = new RunningTask<OpenPoseEstimation>(this, &OpenPoseEstimation::run);
        task2DKeypoints->start();
 
        running3D = true;
    }
    else
    {
        running2D = false;
        running3D = false;
    }
    ARMARX_INFO << "OpenPoseEstimation connect done";
}
 
void
OpenPoseEstimation::onDisconnectImageProcessor()
{
    running2D = false;
    task2DKeypoints->stop();
    running3D = false;
 
    delete rgbImageBuffer;
    delete maskedrgbImageBuffer;
    delete depthImageBuffer;
    delete[] imageBuffer;
    delete[] openPoseResultImage;
    delete stereoCalibration;
 
    imageKeypointBuffer.reset();
}
 
void
OpenPoseEstimation::onExitImageProcessor()
{
}
 
void
OpenPoseEstimation::process()
{
    if (running2D)
    {
        if (!waitForImages(providerName))
        {
            ++timeoutCounter2d;
            ARMARX_WARNING << "Timeout or error in wait for images"
                           << " (#timeout " << timeoutCounter2d << ", #read error "
                           << readErrorCounter2d << ", #success " << sucessCounter2d << ")";
        }
        else
        {
            if (static_cast<unsigned int>(getImages(providerName, imageBuffer, imageMetaInfo)) !=
                numImages)
            {
                ++readErrorCounter2d;
                ARMARX_WARNING << "Unable to transfer or read images"
                               << " (#timeout " << timeoutCounter2d << ", #read error "
                               << readErrorCounter2d << ", #success " << sucessCounter2d << ")";
                return;
            }
            else
            {
                ++sucessCounter2d;
                if (mode == OpenPoseEstimationMode::FromTopic)
                {
                    imageKeypointBuffer->addRGBImage(imageBuffer[0], imageMetaInfo->timeProvided);
                    if (imageKeypointBuffer->addDepthImage(imageBuffer[1],
                                                           imageMetaInfo->timeProvided))
                    {
                        timeProvidedImage = imageMetaInfo->timeProvided;
                        imageUpdated = true;
                    }
                }
                else
                {
                    {
                        std::unique_lock lock_rgb(rgbImageBufferMutex);
                        ::ImageProcessor::CopyImage(imageBuffer[0], rgbImageBuffer);
                    }
 
                    if (numImages == 2)
                    {
                        {
                            std::unique_lock lock_depth(depthImageBufferMutex);
                            ::ImageProcessor::CopyImage(imageBuffer[1], depthImageBuffer);
                        }
                    }
                    timeProvidedImage = imageMetaInfo->timeProvided;
                    imageUpdated = true;
                }
            }
        }
    }
    else
    {
        usleep(10000);
    }
}
 
void
OpenPoseEstimation::run()
{
    // If we receive Keypoints from Topic, then we don't need to setup the OpenPose-Environment
    if (mode != OpenPoseEstimationMode::FromTopic)
    {
        setupOpenPoseEnvironment();
    }
 
    while (running2D && task2DKeypoints->isRunning())
    {
        if (!imageUpdated)
        {
            usleep(100);
            continue;
        }
 
        imageUpdated = false;
 
        // 1. If mode is not listening for topic --> create 2D-Keypoints from images
        if (mode != OpenPoseEstimationMode::FromTopic)
        {
            calculate2DFromOpenPose();
        }
        else if (mode == OpenPoseEstimationMode::FromTopic)
        {
            calculate2DFromTopic();
        }
 
        if (running3D && localRobot && numImages > 1)
        {
            // 2. Calculate 3D values
            RemoteRobot::synchronizeLocalCloneToTimestamp(
                localRobot, robotStateInterface, timeProvidedImage);
            if (mode == OpenPoseEstimationMode::FromDepthImage ||
                mode == OpenPoseEstimationMode::FromTopic)
            {
                calculate3DFromDepthImage(keypointManager);
            }
            else if (mode == OpenPoseEstimationMode::FromStereoImage)
            {
                calculate3DFromStereoImage(keypointManager);
            }
            else
            {
                //                ARMARX_ERROR << "Something went really wrong. The mode is in an unexpected state!! Skipping frame...";
                continue;
            }
 
            // 3. Filter based on workspacePolygon
            {
                std::unique_lock lock(keypointManagerMutex);
                std::vector<KeypointObjectPtr>::iterator iter = keypointManager->getData().begin();
                while (iter != keypointManager->getData().end())
                {
                    KeypointObjectPtr p = *iter;
                    //                filterKeypointsBasedOnWorkspacePolygon(p);
                    if (p->size() == 0)
                    {
                        iter = keypointManager->getData().erase(iter);
                    }
                    else
                    {
                        iter++;
                    }
                }
            }
 
            // 4. Filter to nearest objects (optional)
            if (reportOnlyNearestPerson)
            {
                filterToNearest();
            }
 
            // 5. Visualize and report to topic
            visualize3DKeypoints();
 
            ARMARX_VERBOSE << deactivateSpam(0.5) << "Reporting 2Dkeypoints for "
                           << keypointManager->getData().size() << " objects";
            listener2DPrx->report2DKeypoints(keypointManager->toIce2D(), timeProvidedImage);
            listener2DPrx->report2DKeypointsNormalized(
                keypointManager->toIce2D_normalized(imageProviderInfo.imageFormat.dimension.width,
                                                    imageProviderInfo.imageFormat.dimension.height),
                timeProvidedImage);
            ARMARX_VERBOSE << deactivateSpam(0.5) << "Reporting 3Dkeypoints for "
                           << keypointManager->getData().size() << " objects";
            listener3DPrx->report3DKeypoints(keypointManager->toIce3D(localRobot),
                                             timeProvidedImage);
        }
        else
        {
            ARMARX_VERBOSE << deactivateSpam(0.5) << "Reporting 2Dkeypoints for "
                           << keypointManager->getData().size() << " objects";
            listener2DPrx->report2DKeypoints(keypointManager->toIce2D(), timeProvidedImage);
            listener2DPrx->report2DKeypointsNormalized(
                keypointManager->toIce2D_normalized(imageProviderInfo.imageFormat.dimension.width,
                                                    imageProviderInfo.imageFormat.dimension.height),
                timeProvidedImage);
        }
 
        // Always provide result image
        provideResultImages(openPoseResultImage, imageMetaInfo);
    }
}
 
void
OpenPoseEstimation::setupOpenPoseEnvironment()
{
    ARMARX_INFO << "setting up open pose environment";
    TIMING_START(setupOpenPose);
 
    const op::Point<int> netInputSize =
        op::flagsToPoint(getProperty<std::string>("OP_net_resolution").getValue(), "-1x368");
    const op::Point<int> outputSize =
        op::flagsToPoint(getProperty<std::string>("OP_output_resolution").getValue(), "-1x-1");
    int scaleNumber = getProperty<int>("OP_scale_number").getValue();
    double scaleGap = getProperty<double>("OP_scale_gap").getValue();
    scaleAndSizeExtractor = std::make_shared<op::ScaleAndSizeExtractor>(
        netInputSize, outputSize, scaleNumber, scaleGap);
 
    cvMatToOpInput = std::make_shared<op::CvMatToOpInput>(poseModel);
    cvMatToOpOutput.reset(new op::CvMatToOpOutput());
    opOutputToCvMat.reset(new op::OpOutputToCvMat());
 
    std::string modelFolder;
 
    modelFolder = getProperty<std::string>("OP_model_folder").getValue();
    auto found = ArmarXDataPath::getAbsolutePath(
        modelFolder, modelFolder, {"/usr/share/OpenPose/", OPENPOSE_MODELS});
    if (!found)
    {
        ARMARX_ERROR << "Could not find model folder!";
        return;
    }
    if (!modelFolder.empty() && *modelFolder.rbegin() != '/')
    {
        modelFolder += "/";
    }
    ARMARX_INFO << "Found model path at: " << modelFolder;
    int numGpuStart = getProperty<int>("OP_num_gpu_start").getValue();
    poseExtractorCaffe =
        std::make_shared<op::PoseExtractorCaffe>(poseModel, modelFolder, numGpuStart);
    poseExtractorCaffe->initializationOnThread();
 
    TIMING_END(setupOpenPose);
}
 
OpenPoseEstimation::PoseKeypoints
OpenPoseEstimation::getPoseKeypoints(CByteImage* imageBuffer)
{
    // Step 1 - Convert image to cv::Mat
    IplImage* iplImage = IplImageAdaptor::Adapt(imageBuffer);
    cv::Mat inputImage = cv::cvarrToMat(iplImage);
    const op::Point<int> imageSize{inputImage.cols, inputImage.rows};
 
    // Step 2 - Get desired scale sizes
    std::vector<double> scaleInputToNetInputs;
    std::vector<op::Point<int>> netInputSizes;
    double scaleInputToOutput;
    op::Point<int> outputResolution;
    std::tie(scaleInputToNetInputs, netInputSizes, scaleInputToOutput, outputResolution) =
        scaleAndSizeExtractor->extract(imageSize);
 
    // Step 3 - Format input image to OpenPose input and output formats
    const auto netInputArray =
        cvMatToOpInput->createArray(inputImage, scaleInputToNetInputs, netInputSizes);
 
    // Step 4 - Estimate poseKeypoints
    poseExtractorCaffe->forwardPass(netInputArray, imageSize, scaleInputToNetInputs);
    const PoseKeypoints poseKeypoints = poseExtractorCaffe->getPoseKeypoints();
 
    return poseKeypoints;
}
 
void
OpenPoseEstimation::Render2DResultImage(const CByteImage& inputImage,
                                        const CByteImage& maskedInputImage,
                                        KeypointManagerPtr keypointManager,
                                        CByteImage& resultImage,
                                        op::PoseModel poseModel,
                                        float renderThreshold,
                                        int brightnessIncrease)
{
    IdNameMap m1 = keypointManager->getIdNameMap();
    IdNameMap poseBodyPartMapping = getPoseBodyPartMapping(poseModel);
    ARMARX_CHECK_EXPRESSION(m1.size() == poseBodyPartMapping.size() &&
                            std::equal(m1.begin(), m1.end(), poseBodyPartMapping.begin()))
        << "The idNameMap used in the keypointManager must be equal to the poseBodyPartMapping "
           "defined by the given poseModel.";
 
    // Creating openpose-representation of keypoints stored in keypointManager
    const int objects = keypointManager->getData().size();
    const int numberKeypoints = m1.size();
 
    PoseKeypoints opKeypoints({objects, numberKeypoints, 3});
    int objectNumber = 0;
    for (KeypointObjectPtr obj : keypointManager->getData())
    {
        for (int i = 0; i < numberKeypoints; i++)
        {
            const int index = (objectNumber * numberKeypoints + i) * opKeypoints.getSize(2);
            KeypointPtr p = obj->getNode(static_cast<unsigned int>(i));
            if (p)
            {
                opKeypoints[index] = p->get2D()._x;
                opKeypoints[index + 1] = p->get2D()._y;
                opKeypoints[index + 2] = p->getConfidence();
            }
            else
            {
                opKeypoints[index] = 0.0f;
                opKeypoints[index + 1] = 0.0f;
                opKeypoints[index + 2] = 0.0f;
            }
        }
        objectNumber++;
    }
    std::cout << opKeypoints.toString() << std::endl;
 
    // Render image
    Render2DResultImage(inputImage,
                        maskedInputImage,
                        opKeypoints,
                        resultImage,
                        poseModel,
                        renderThreshold,
                        brightnessIncrease);
}
 
void
OpenPoseEstimation::VisualizeTransparentImageMask(CByteImage& resultImage,
                                                  const CByteImage& maskedInputImage,
                                                  int brightnessIncrease,
                                                  const CByteImage& inputImage)
{
    Ice::Byte mask[3] = {0, 255, 0};
    auto pixelCount = inputImage.width * inputImage.height;
 
    // first create a smoothed mask
    CByteImage imageMask(maskedInputImage.width, maskedInputImage.height, CByteImage::eGrayScale);
    CByteImage smoothedImageMask(&imageMask);
    ::ImageProcessor::Zero(&imageMask);
    if (brightnessIncrease > 0)
    {
        for (int i = 0; i < pixelCount; i += 1)
        {
            if (maskedInputImage.pixels[i * 3] == mask[0] &&
                maskedInputImage.pixels[i * 3 + 1] == mask[1] &&
                maskedInputImage.pixels[i * 3 + 2] == mask[2])
            {
                imageMask.pixels[i] = 255;
            }
        }
        ::ImageProcessor::GaussianSmooth5x5(&imageMask, &smoothedImageMask);
    }
    // now apply mask to image
    for (int i = 0; i < pixelCount; i += 1)
    {
        if (brightnessIncrease > 0)
        {
            float perc = static_cast<float>(smoothedImageMask.pixels[i]) / 255.f;
            int effectiveBrightnessIncrease = brightnessIncrease * perc;
            resultImage.pixels[i * 3] =
                std::min<int>(resultImage.pixels[i * 3] + effectiveBrightnessIncrease, 255);
            resultImage.pixels[i * 3 + 1] =
                std::min<int>(resultImage.pixels[i * 3 + 1] + effectiveBrightnessIncrease, 255);
            resultImage.pixels[i * 3 + 2] =
                std::min<int>(resultImage.pixels[i * 3 + 2] + effectiveBrightnessIncrease, 255);
        }
        // use original mask
        else if (brightnessIncrease < 0)
        {
            if (maskedInputImage.pixels[i * 3] == mask[0] &&
                maskedInputImage.pixels[i * 3 + 1] == mask[1] &&
                maskedInputImage.pixels[i * 3 + 2] == mask[2])
            {
                resultImage.pixels[i * 3] = maskedInputImage.pixels[i * 3];
                resultImage.pixels[i * 3 + 1] = maskedInputImage.pixels[i * 3 + 1];
                resultImage.pixels[i * 3 + 2] = maskedInputImage.pixels[i * 3 + 2];
            }
        }
    }
}
 
void
OpenPoseEstimation::Render2DResultImage(const CByteImage& inputImage,
                                        const CByteImage& maskedInputImage,
                                        OpenPoseEstimation::PoseKeypoints& keypoints,
                                        CByteImage& resultImage,
                                        op::PoseModel poseModel,
                                        float renderThreshold,
                                        int brightnessIncrease)
{
    ARMARX_CHECK_EXPRESSION(inputImage.width == resultImage.width);
    ARMARX_CHECK_EXPRESSION(inputImage.height == resultImage.height);
    ::ImageProcessor::CopyImage(&inputImage, &resultImage);
 
    VisualizeTransparentImageMask(resultImage, maskedInputImage, brightnessIncrease, inputImage);
 
    if (keypoints.getSize().empty())
    {
        return;
    }
 
    const std::vector<unsigned int>& posePartPairs = op::getPoseBodyPartPairsRender(poseModel);
 
    // Get colors for points and lines from openpose
    std::vector<float> keypointColors = op::getPoseColors(poseModel);
 
    const auto thicknessCircleRatio = 1.f / 75.f;
    const auto thicknessLineRatioWRTCircle = 0.75f;
    const auto area = inputImage.width * inputImage.height;
    const int numberKeypoints = keypoints.getSize(1);
 
    for (int person = 0; person < keypoints.getSize(0); person++)
    {
        const auto personRectangle = op::getKeypointsRectangle(keypoints, person, 0.1f);
        if (personRectangle.area() > 0)
        {
            // define size-dependent variables
            const auto ratioAreas = op::fastMin(
                1.f,
                op::fastMax(personRectangle.width / static_cast<float>(inputImage.width),
                            personRectangle.height / static_cast<float>(inputImage.height)));
            const auto thicknessRatio =
                op::fastMax(positiveIntRound<float>(static_cast<float>(std::sqrt(area)) *
                                                    thicknessCircleRatio * ratioAreas),
                            2);
            // Negative thickness in ivt::drawCircle means that a filled circle is to be drawn.
            const auto thicknessCircle = op::fastMax(1, (ratioAreas > 0.05f ? thicknessRatio : -1));
            const auto thicknessLine =
                op::fastMax(1, positiveIntRound(thicknessRatio * thicknessLineRatioWRTCircle));
            const auto radius = thicknessRatio / 2;
 
            // Draw Lines
            for (unsigned int i = 0; i < posePartPairs.size(); i = i + 2)
            {
                const int index1 = (person * numberKeypoints + static_cast<int>(posePartPairs[i])) *
                                   keypoints.getSize(2);
                const int index2 =
                    (person * numberKeypoints + static_cast<int>(posePartPairs[i + 1])) *
                    keypoints.getSize(2);
 
                float x1 = keypoints[index1 + 0];
                float y1 = keypoints[index1 + 1];
                float x2 = keypoints[index2 + 0];
                float y2 = keypoints[index2 + 1];
 
                if (!(x1 == 0.0f && y1 == 0.0f) && !(x2 == 0.0f && y2 == 0.0f))
                {
                    if (keypoints[index1 + 2] > renderThreshold &&
                        keypoints[index2 + 2] > renderThreshold)
                    {
                        unsigned int colorIndex = posePartPairs[i + 1] * 3;
                        ::PrimitivesDrawer::DrawLine(
                            &resultImage,
                            Vec2d({x1, y1}),
                            Vec2d({x2, y2}),
                            static_cast<int>(keypointColors[colorIndex + 2]),
                            static_cast<int>(keypointColors[colorIndex + 1]),
                            static_cast<int>(keypointColors[colorIndex + 0]),
                            thicknessLine);
                    }
                }
            }
 
            // Draw points
            for (int i = 0; i < numberKeypoints; i++)
            {
                const int index = (person * numberKeypoints + i) * keypoints.getSize(2);
                float x = keypoints[index + 0];
                float y = keypoints[index + 1];
 
                if (!(x == 0.0f && y == 0.0f) && keypoints[index + 2] > renderThreshold)
                {
                    unsigned int colorIndex = static_cast<unsigned int>(i * 3);
 
                    ::PrimitivesDrawer::DrawCircle(&resultImage,
                                                   x,
                                                   y,
                                                   radius,
                                                   static_cast<int>(keypointColors[colorIndex + 2]),
                                                   static_cast<int>(keypointColors[colorIndex + 1]),
                                                   static_cast<int>(keypointColors[colorIndex + 0]),
                                                   thicknessCircle);
                }
            }
        }
    }
}
 
void
OpenPoseEstimation::start(const Ice::Current&)
{
    if (running2D)
    {
        return;
    }
    else
    {
        ARMARX_INFO << "Starting OpenposeEstimation";
        running2D = true;
        task2DKeypoints = new RunningTask<OpenPoseEstimation>(this, &OpenPoseEstimation::run);
        task2DKeypoints->start();
    }
}
 
void
OpenPoseEstimation::stop(const Ice::Current&)
{
    if (running2D)
    {
        ARMARX_INFO << "Stopping OpenposeEstimation";
        running2D = false;
        task2DKeypoints->stop(true);
        stop3DPoseEstimation();
    }
}
 
void
OpenPoseEstimation::start3DPoseEstimation(const Ice::Current&)
{
    if (running3D)
    {
        return;
    }
    else
    {
        ARMARX_INFO << "Starting OpenposeEstimation -- 3D";
 
        running3D = true;
        start();
    }
}
 
void
OpenPoseEstimation::stop3DPoseEstimation(const Ice::Current&)
{
    if (running3D)
    {
        ARMARX_INFO << "Stopping OpenposeEstimation -- 3D";
        running3D = false;
 
        // Remove all layers we might have used
        for (int i = 0; i < MAX_LAYER; i++)
        {
            debugDrawerTopic->removeLayer(layerName + std::to_string(i));
        }
    }
}
 
void
OpenPoseEstimation::maskOutBasedOnDepth(CByteImage& image, int maxDepth)
{
    std::unique_lock lock(depthImageBufferMutex);
 
    if (maxDepth <= 0)
    {
        return;
    }
    int pixelCount = depthImageBuffer->width * depthImageBuffer->height;
    int depthThresholdmm = maxDepth;
    CByteImage maskImage(depthImageBuffer->width, depthImageBuffer->height, CByteImage::eGrayScale);
    for (int i = 0; i < pixelCount; i += 1)
    {
        int z_value = depthImageBuffer->pixels[i * 3 + 0] +
                      (depthImageBuffer->pixels[i * 3 + 1] << 8) +
                      (depthImageBuffer->pixels[i * 3 + 2] << 16);
        maskImage.pixels[i] = z_value > depthThresholdmm || z_value == 0 ? 0 : 255;
    }
    ::ImageProcessor::Erode(&maskImage, &maskImage, 5);
    ::ImageProcessor::Dilate(&maskImage, &maskImage, 20);
    for (int i = 0; i < pixelCount; i += 1)
    {
        if (maskImage.pixels[i] == 0)
        {
            image.pixels[i * 3] = 0;
            image.pixels[i * 3 + 1] = 255;
            image.pixels[i * 3 + 2] = 0;
        }
    }
}
 
DrawColor24Bit
OpenPoseEstimation::getDominantColorOfPatch(const CByteImage& image,
                                            const Vec2d& point,
                                            int windowSize) const
{
    if (point.x < 0 || point.y < 0 || point.x >= image.width || point.y >= image.height)
        return DrawColor24Bit{0, 0, 0}; // Black
    int divisor = 256 / 3; // split into 3*3*3 bins
    typedef std::tuple<Ice::Byte, Ice::Byte, Ice::Byte> RGBTuple;
    std::map<RGBTuple, int> histogram;
    int halfWindowSize = static_cast<int>(windowSize * 0.5);
    int left = std::max<int>(0, static_cast<int>(point.x) - halfWindowSize);
    int top = std::max<int>(0, static_cast<int>(point.y) - halfWindowSize);
    int right = std::min<int>(image.width, static_cast<int>(point.x) + halfWindowSize);
    int bottom = std::min<int>(image.height, static_cast<int>(point.y) + halfWindowSize);
 
    for (int x = left; x < right; x++)
    {
        for (int y = top; y < bottom; y++)
        {
            int pixelPos = (y * image.width + x) * 3;
            auto tuple = std::make_tuple<Ice::Byte, Ice::Byte, Ice::Byte>(
                static_cast<Ice::Byte>(image.pixels[pixelPos] / divisor),
                static_cast<Ice::Byte>(image.pixels[pixelPos + 1] / divisor),
                static_cast<Ice::Byte>(image.pixels[pixelPos + 2] / divisor));
            if (histogram.count(tuple))
            {
                histogram.at(tuple)++;
            }
            else
            {
                histogram[tuple] = 1;
            }
        }
    }
 
    float maxHistogramValue = 0;
    RGBTuple dominantColor;
    for (auto& pair : histogram)
    {
        if (pair.second > maxHistogramValue)
        {
            dominantColor = pair.first;
            maxHistogramValue = pair.second;
        }
    }
    auto rgb = DrawColor24Bit{static_cast<Ice::Byte>(std::get<0>(dominantColor) * divisor),
                              static_cast<Ice::Byte>(std::get<1>(dominantColor) * divisor),
                              static_cast<Ice::Byte>(std::get<2>(dominantColor) * divisor)};
    return rgb;
}
 
void
OpenPoseEstimation::onMessage(const Texting::TextMessage& text, const Ice::Current&)
{
    using json = nlohmann::json;
    // parsing message
    json jsonView = json::parse(text.message);
 
    // Reading values
    long timestamp = jsonView["timestamp"].get<long>();
    json jsonValues = jsonView["values"];
 
    // Constructing and populating KeypointManager
    IdNameMap poseBodyPartMapping = getPoseBodyPartMapping(poseModel);
    KeypointManagerPtr manager = std::make_shared<KeypointManager>(poseBodyPartMapping);
    KeypointObjectPtr object = std::make_shared<KeypointObject>();
 
    float adjustingWidth = static_cast<float>(imageProviderInfo.imageFormat.dimension.width) /
                           static_cast<float>(this->incomingKeypointDimensions.width);
    float adjustingHeight = static_cast<float>(imageProviderInfo.imageFormat.dimension.height) /
                            static_cast<float>(this->incomingKeypointDimensions.height);
 
    for (json::iterator it = jsonValues.begin(); it != jsonValues.end(); ++it)
    {
        std::string jsonLabel = it.key();
 
        auto pair = MPII_TO_MPI.at(jsonLabel);
        unsigned int id = pair.first;
        std::string name = pair.second;
 
        json jsonPosition = it.value();
        json jsonPoint = jsonPosition["position"];
        float x = jsonPoint["y"]; // wrong naming in secondHands output
        float y = jsonPoint["x"];
 
        // Adjust Keypoints to fit to image dimensions
        x *= adjustingWidth;
        y *= adjustingHeight;
 
        if (x > 0.0f && y > 0.0f)
        {
            KeypointPtr keypoint = std::make_shared<Keypoint>(
                x,
                y,
                id,
                name,
                1.0f); // Since to Topic from SecondHands does not provide a confidence, we simply use 1 for every keypoint;
            object->insertNode(keypoint);
        }
    }
    manager->getData().push_back(object);
 
    // Inserting into buffer
    if (imageKeypointBuffer->addKeypoints(manager, timestamp))
    {
        timeProvidedImage = timestamp;
        imageUpdated = true;
    }
}
 
KeypointManagerPtr
OpenPoseEstimation::generate2DKeypoints(OpenPoseEstimation::PoseKeypoints& keypoints,
                                        const CByteImage& rgbImage) const
{
    IdNameMap poseBodyPartMapping = getPoseBodyPartMapping(poseModel);
    KeypointManagerPtr manager = std::make_shared<KeypointManager>(poseBodyPartMapping);
 
    if (keypoints.getSize().empty())
    {
        debugDrawerTopic->removeLayer(
            layerName +
            std::to_string(
                layerCounter)); // TODO find better way to clear if no persons are visible
        return manager;
    }
 
    int entities = keypoints.getSize().at(0);
    int points = keypoints.getSize().at(1);
    ARMARX_CHECK_EXPRESSION(points == static_cast<int>(poseBodyPartMapping.size()) - 1);
 
    for (int i = 0; i < entities; i++)
    {
        KeypointObjectPtr object = std::make_shared<KeypointObject>();
        for (int id = 0; id < points; id++)
        {
            float x = keypoints.at({i, id, 0});
            float y = keypoints.at({i, id, 1});
            if (x == 0.0f && y == 0.0f)
            {
                continue; // Invalid keypoint from openpose
            }
 
            KeypointPtr keypoint =
                std::make_shared<Keypoint>(x,
                                           y,
                                           static_cast<unsigned int>(id),
                                           poseBodyPartMapping.at(static_cast<unsigned int>(id)),
                                           static_cast<float>(keypoints.at({i, id, 2})));
 
            keypoint->setDominantColor(
                getDominantColorOfPatch(rgbImage, Vec2d{x, y}, rgbImage.width / 50));
            object->insertNode(keypoint);
        }
 
        if (static_cast<int>(object->size()) >= minimalValidKeypoints)
        {
            manager->getData().push_back(object);
        }
    }
 
    return manager;
}
 
void
OpenPoseEstimation::filterKeypointsBasedOnWorkspacePolygon(KeypointObjectPtr object)
{
    if (!localRobot)
    {
        return;
    }
 
    for (unsigned int i = 0; i < getPoseBodyPartMapping(poseModel).size(); i++)
    {
        KeypointPtr point = object->getNode(i);
        if (point && point->is3DSet())
        {
            FramedPositionPtr pos = point->get3D()->toGlobal(localRobot); // Changing frame of copy
            if (!workspacePolygon.isInside(pos))
            {
                ARMARX_VERBOSE << deactivateSpam(1) << "removing keypoint due out of workspace";
                object->removeNode(i);
            }
        }
    }
}
 
void
OpenPoseEstimation::filterToNearest()
{
    this->keypointManager->filterToNearestN(1);
}
 
void
OpenPoseEstimation::calculate2DFromOpenPose()
{
    std::unique_lock lock(keypointManagerMutex);
    if (mode == OpenPoseEstimationMode::FromDepthImage)
    {
        // OpenPoseCall nur auf erstem Bild
        std::unique_lock lock_rgb(rgbImageBufferMutex);
        // remove pixels that are far away
        ::ImageProcessor::CopyImage(rgbImageBuffer, maskedrgbImageBuffer);
        if (numImages > 1)
        {
            maskOutBasedOnDepth(*maskedrgbImageBuffer, getProperty<int>("MaxDepth").getValue());
        }
        PoseKeypoints p = getPoseKeypoints(maskedrgbImageBuffer);
        keypointManager = generate2DKeypoints(p, maskedrgbImageBuffer);
 
        // Render result image
        {
            std::unique_lock lock(resultImageBufferMutex);
            OpenPoseEstimation::Render2DResultImage(
                *rgbImageBuffer,
                *maskedrgbImageBuffer,
                p,
                *openPoseResultImage[0],
                poseModel,
                renderThreshold,
                getProperty<int>("MaskBrightnessIncrease").getValue());
        }
    }
    else if (mode == OpenPoseEstimationMode::FromStereoImage)
    {
        // OpenPoseCall auf beiden Bildern
        std::unique_lock lock_rgb(rgbImageBufferMutex);
        std::unique_lock lock_depth(depthImageBufferMutex);
        PoseKeypoints left = getPoseKeypoints(rgbImageBuffer);
        keypointManager = generate2DKeypoints(left, rgbImageBuffer);
        PoseKeypoints right = getPoseKeypoints(depthImageBuffer);
        KeypointManagerPtr m2 = generate2DKeypoints(right, depthImageBuffer);
        keypointManager->matchWith(m2);
 
        // Render result imageint)
        {
            std::unique_lock lock(resultImageBufferMutex);
            OpenPoseEstimation::Render2DResultImage(
                *rgbImageBuffer,
                *rgbImageBuffer,
                left,
                *openPoseResultImage[0],
                poseModel,
                renderThreshold,
                getProperty<int>("MaskBrightnessIncrease").getValue());
        }
    }
}
 
void
OpenPoseEstimation::calculate2DFromTopic()
{
    // ImageKeypointBuffer contains a triple for the timestamp saved in this->timeProvidedImage
 
    ImageKeypointBuffer::ImageKeypointTriplePtr imageKeypointTriple =
        imageKeypointBuffer->getTripleAtTimestamp(timeProvidedImage, true);
    std::unique_lock lock_rgb(rgbImageBufferMutex);
    ::ImageProcessor::CopyImage(imageKeypointTriple->rgbImage, rgbImageBuffer);
    std::unique_lock lock_depth(depthImageBufferMutex);
    ::ImageProcessor::CopyImage(imageKeypointTriple->depthImage, depthImageBuffer);
    keypointManager = imageKeypointTriple->keypoints;
 
    // Render result image
    {
        std::unique_lock lock(resultImageBufferMutex);
        OpenPoseEstimation::Render2DResultImage(
            *rgbImageBuffer,
            *rgbImageBuffer,
            keypointManager,
            *openPoseResultImage[0],
            poseModel,
            renderThreshold,
            getProperty<int>("MaskBrightnessIncrease").getValue());
    }
}
 
void
OpenPoseEstimation::calculate3DFromDepthImage(KeypointManagerPtr manager)
{
    if (!localRobot)
    {
        return;
    }
 
    const int depthThreshold = getProperty<int>("MaxDepthDifference").getValue();
 
    for (KeypointObjectPtr object : manager->getData())
    {
        if (object->size() == 0)
        {
            continue;
        }
 
        std::map<unsigned int, int>
            depthStorage; // we do not want to store perspective depth-values in a Keypoint, we transfer them into world-coordiantes (in cameraframe) anyways
        std::vector<int> depthsCopy;
 
        // Get camera depth information from image coordinates
        for (unsigned int i = 0; i < getPoseBodyPartMapping(poseModel).size(); i++)
        {
            KeypointPtr point = object->getNode(i);
            if (point)
            {
                int depth = getMedianDepthFromImage(static_cast<int>(point->get2D()._x),
                                                    static_cast<int>(point->get2D()._y),
                                                    radius);
                depthStorage[i] = depth;
                depthsCopy.push_back(depth);
            }
        }
 
        // Find outlier in depth values and set them to median of depth values
        std::sort(depthsCopy.begin(), depthsCopy.end());
        const int medianDepth = depthsCopy.at(depthsCopy.size() / 2);
        for (auto& m : depthStorage)
        {
            int depth = m.second;
            if (depth > medianDepth + depthThreshold || depth < medianDepth - depthThreshold)
            {
                m.second = medianDepth;
            }
        }
 
        // Transform pixel + depth into world coordinates
        for (unsigned int i = 0; i < op::getPoseBodyPartMapping(poseModel).size(); i++)
        {
            KeypointPtr point = object->getNode(i);
            if (point)
            {
                Vec2d imagePoint;
                imagePoint.x = point->get2D()._x;
                imagePoint.y = point->get2D()._y;
                Vec3d result;
                ARMARX_CHECK_EXPRESSION(calibration);
                calibration->ImageToWorldCoordinates(imagePoint,
                                                     result,
                                                     static_cast<float>(depthStorage.at(i)),
                                                     useDistortionParameters);
 
                point->set3D(new FramedPosition(Eigen::Vector3f(result.x, result.y, result.z),
                                                cameraNodeName,
                                                localRobot->getName()));
            }
        }
    }
}
 
void
OpenPoseEstimation::calculate3DFromStereoImage(KeypointManagerPtr manager)
{
    for (KeypointObjectPtr object : manager->getData())
    {
        for (unsigned int i = 0; i < getPoseBodyPartMapping(poseModel).size(); i++)
        {
            KeypointPtr point = object->getNode(i);
            if (point)
            {
                ARMARX_CHECK_EXPRESSION(point->isStereo());
                auto pair = point->getStereo2D();
                const Vec2d imagePointL = {pair.first._x, pair.first._y};
                const Vec2d imagePointR = {pair.second._x, pair.second._y};
                ARMARX_DEBUG << "PointL: " << imagePointL.x << " " << imagePointL.y;
                ARMARX_DEBUG << "PointR: " << imagePointR.x << " " << imagePointR.y;
 
                Vec3d result;
                PointPair3d* rD = new PointPair3d();
                stereoCalibration->Calculate3DPoint(
                    imagePointL, imagePointR, result, false, useDistortionParameters, rD);
                ARMARX_DEBUG << "IVT- ConnectionLine: " << rD->p1.x << " " << rD->p1.y << " "
                             << rD->p1.z << " --- " << rD->p2.x << " " << rD->p2.y << " "
                             << rD->p2.z;
 
                point->set3D(new FramedPosition(Eigen::Vector3f(result.x, result.y, result.z),
                                                cameraNodeName,
                                                localRobot->getName()));
            }
        }
    }
}
 
void
OpenPoseEstimation::visualize3DKeypoints()
{
    std::unique_lock lock(keypointManagerMutex);
    debugDrawerTopic->removeLayer(layerName + std::to_string(layerCounter));
 
    viz::Layer openPoseArVizLayer = arviz.layer(layerName);
    if (keypointManager->getData().empty() || !localRobot)
    {
        arviz.commit(openPoseArVizLayer);
        return;
    }
 
    const std::vector<unsigned int>& posePartPairs = op::getPoseBodyPartPairsRender(poseModel);
    const std::vector<float> keypointColors = op::getPoseColors(poseModel);
 
 
    int human = 1;
 
    layerCounter = (layerCounter == MAX_LAYER - 1) ? 0 : layerCounter + 1;
 
    int objectIndex = 0;
    for (KeypointObjectPtr object : keypointManager->getData())
    {
        std::string objectName = "object_" + std::to_string(objectIndex) + "_";
 
        // Draw Lines
        for (unsigned int i = 0; i < posePartPairs.size(); i = i + 2)
        {
            KeypointPtr point1 = object->getNode(posePartPairs[i]);
            KeypointPtr point2 = object->getNode(posePartPairs[i + 1]);
 
            if (point1 && point2)
            {
                FramedPositionPtr p1 = point1->get3D()->toGlobal(localRobot);
                FramedPositionPtr p2 = point2->get3D()->toGlobal(localRobot);
 
                std::string name = point1->getName() + "_" + point2->getName();
                unsigned int colorIndex = posePartPairs[i + 1] * 3;
                DrawColor color({keypointColors[colorIndex + 2] / 255.f,
                                 keypointColors[colorIndex + 1] / 255.f,
                                 keypointColors[colorIndex] / 255.f,
                                 1.0f});
                debugDrawerTopic->setLineVisu(layerName + std::to_string(layerCounter),
                                              objectName + name,
                                              new Vector3(p1->x, p1->y, p1->z),
                                              new Vector3(p2->x, p2->y, p2->z),
                                              10,
                                              color);
            }
        }
 
        Keypoint3DMap kpm;
 
        // Draw Points
        for (unsigned int i = 0; i < getPoseBodyPartMapping(poseModel).size(); i++)
        {
            KeypointPtr point = object->getNode(i);
            if (point)
            {
                FramedPositionPtr p = point->get3D()->toGlobal(localRobot);
                auto color24Bit = point->getDominantColor();
                auto color = DrawColor{0.0039215f * color24Bit.b,
                                       0.0039215f * color24Bit.g,
                                       0.0039215f * color24Bit.r,
                                       1.0f};
                debugDrawerTopic->setSphereVisu(layerName + std::to_string(layerCounter),
                                                objectName + point->getName(),
                                                new Vector3(p->x, p->y, p->z),
                                                color,
                                                20);
                Keypoint3D kp;
                kp.label = point->getName();
                kp.x = p->x;
                kp.y = p->y;
                kp.z = p->z;
                kp.confidence = point->getConfidence();
                kpm[point->getName()] = kp;
            }
        }
 
        visionx::viz::HumanPoseBody25::addPoseToLayer(
            kpm, openPoseArVizLayer, "human" + std::to_string(human));
        human++;
    }
 
    arviz.commit(openPoseArVizLayer);
}
 
int
OpenPoseEstimation::getMedianDepthFromImage(int x, int y, int radius) const
{
    std::vector<int> depths;
    for (int xoffset = -radius; xoffset < radius; xoffset++)
    {
        int xo = x + xoffset;
        if (xo < 0 || xo > depthImageBuffer->width)
        {
            continue;
        }
        for (int yoffset = -radius; yoffset < radius; yoffset++)
        {
            int yo = y + yoffset;
            if (yo < 0 || yo > depthImageBuffer->height)
            {
                continue;
            }
 
            // Check whether (x,y) is in circle:
            if (xoffset * xoffset + yoffset * yoffset <= radius * radius)
            {
                unsigned int pixelPos =
                    static_cast<unsigned int>(3 * (yo * depthImageBuffer->width + xo));
                int z_value = depthImageBuffer->pixels[pixelPos + 0] +
                              (depthImageBuffer->pixels[pixelPos + 1] << 8) +
                              (depthImageBuffer->pixels[pixelPos + 2] << 16);
                if (z_value > 0)
                {
                    depths.push_back(z_value);
                }
            }
        }
    }
    std::sort(depths.begin(), depths.end());
 
    return depths.empty() ? 0 : depths[depths.size() / 2];
}