OpenPoseAdapter.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::MonocularOpenPoseEstimation
 * @author     Fabian Peller <fabian.peller@kit.edu>
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
// Header
#include "OpenPoseAdapter.h"
 
// OpenPose
#include <openpose/pose/poseParameters.hpp>
#include <openpose/pose/poseParametersRender.hpp>
 
// IVT
#include <Image/IplImageAdaptor.h>
#include <Image/PrimitivesDrawer.h>
#include <Image/ImageProcessor.h>
 
// ArmarX
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
using namespace visionx;
using namespace armarx;
 
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);
    }
}
 
 
OpenPoseAdapter::OpenPoseAdapter(const OpenPoseSettings& settings) :
    settings(settings)
{
    setupOpenPoseEnvironment();
}
 
// Initialize all variables to interact with openpose
void OpenPoseAdapter::setupOpenPoseEnvironment()
{
    std::lock_guard l(op_running_mutex);
 
    TIMING_START(setupOpenPose);
    ARMARX_INFO_S << "setting up open pose environment";
 
    auto op_model_pose_conv = ConvertToOPCustomType(settings.model_pose);
    poseModel = op::flagsToPoseModel(op_model_pose_conv);
 
    auto op_net_resolution_conv = ConvertToOPCustomType(settings.net_resolution);
    auto op_output_resolution_conv = ConvertToOPCustomType(settings.output_resolution);
 
    const op::Point<int> netInputSize = op::flagsToPoint(op_net_resolution_conv, "-1x368");
    const op::Point<int> outputSize = op::flagsToPoint(op_output_resolution_conv, "-1x-1");
 
    #if (OPENPOSE_VERSION_MAJOR > 1) || (OPENPOSE_VERSION_MAJOR == 1 && OPENPOSE_VERSION_MINOR > 7) || (OPENPOSE_VERSION_MAJOR == 1 && OPENPOSE_VERSION_MINOR == 7 && OPENPOSE_VERSION_PATCH > 0)
    scaleAndSizeExtractor = std::make_shared<op::ScaleAndSizeExtractor>(netInputSize,
                                                                        -1, // unclear what this value should be from openpose documentation
                                                                        outputSize, settings.scale_number, settings.scale_gap);
    #else
    scaleAndSizeExtractor = std::make_shared<op::ScaleAndSizeExtractor>(netInputSize,
                                                                        outputSize, settings.scale_number, settings.scale_gap);
    #endif
    cvMatToOpInput = std::make_shared<op::CvMatToOpInput>(poseModel);
    cvMatToOpOutput.reset(new op::CvMatToOpOutput());
    opOutputToCvMat.reset(new op::OpOutputToCvMat());
 
    std::string modelFolder = settings.model_folder;
    auto found = ArmarXDataPath::getAbsolutePath(modelFolder, modelFolder, {"/usr/share/OpenPose/", "/usr/local/share/OpenPose/", OPENPOSE_MODELS});
    if (!found)
    {
        ARMARX_ERROR << "Could not find model folder at " << modelFolder;
        return;
    }
    if (!modelFolder.empty() && *modelFolder.rbegin() != '/')
    {
        modelFolder += "/";
    }
    ARMARX_INFO << "Found model path at: " << modelFolder;
    poseExtractorCaffe = std::make_shared<op::PoseExtractorCaffe>(poseModel, modelFolder, settings.num_gpu_start);
    poseExtractorCaffe->initializationOnThread();
 
    TIMING_END(setupOpenPose);
}
 
// run openpose and return the openpose representation of keypoints
op::Array<float> OpenPoseAdapter::getOpenposeKeypoints(const CByteImage* inputImage)
{
    std::lock_guard l(op_running_mutex);
 
    // Step 1 - Convert image to cv::Mat
    IplImage* iplImage = IplImageAdaptor::Adapt(inputImage);
    cv::Mat opInputImage = cv::cvarrToMat(iplImage);
    const op::Point<int> imageSize {opInputImage.cols, opInputImage.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
    auto inputImage_conv = ConvertToOPCustomType(opInputImage);
    const auto netInputArray = cvMatToOpInput->createArray(inputImage_conv, scaleInputToNetInputs, netInputSizes);
 
    // Step 4 - Estimate poseKeypoints
    poseExtractorCaffe->forwardPass(netInputArray, imageSize, scaleInputToNetInputs);
    const op::Array<float> poseKeypoints = poseExtractorCaffe->getPoseKeypoints();
 
    return poseKeypoints;
}
 
// convert the openpose representation of keypoints into our ice version
HumanPose2DMap OpenPoseAdapter::convert2DKeypointsToIce(const op::Array<float>& op_keypoints, const CByteImage* rgbImage) const
{
    ARMARX_CHECK_NOT_NULL(rgbImage);
 
    HumanPose2DMap ice_entities;
    const std::map<unsigned int, std::string> poseBodyPartMapping = op::getPoseBodyPartMapping(poseModel);
 
    if (op_keypoints.getSize().empty())
    {
        return ice_entities;
    }
 
    int entities = op_keypoints.getSize().at(0);
    int points = op_keypoints.getSize().at(1);
 
    ARMARX_CHECK_EXPRESSION(poseBodyPartMapping.size() > 0);
    ARMARX_CHECK_EXPRESSION(points == static_cast<int>(poseBodyPartMapping.size()) - 1);
 
    for (int i = 0; i < entities; i++)
    {
        std::string entity_id = std::to_string(i);
 
        ice_entities[entity_id] = HumanPose2D();
        Keypoint2DMap ice_keypoint_map;
        for (int id = 0; id < points; id++)
        {
            Keypoint2D keypoint;
            float x = op_keypoints.at({i, id, 0});
            float y = op_keypoints.at({i, id, 1});
            float c = op_keypoints.at({i, id, 2});
            if (x == 0.0f && y == 0.0f && c == 0.0f)
            {
                continue;    // Invalid keypoint from openpose
            }
            keypoint.id = id;
            keypoint.label = poseBodyPartMapping.at(id);
            keypoint.x = x;
            keypoint.y = y;
            keypoint.confidence = c;
            keypoint.dominantColor = getDominantColorOfPatch(rgbImage, Vec2d {x, y}, rgbImage->width / 50);
            ice_keypoint_map[keypoint.label] = keypoint;
        }
 
        if (ice_keypoint_map.size() >= settings.minimum_number_of_valid_keypoints_per_entitiy)
        {
            ice_entities[entity_id].keypointMap = ice_keypoint_map;
        }
    }
 
    return ice_entities;
}
 
armarx::DrawColor24Bit OpenPoseAdapter::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 OpenPoseAdapter::render2DResultImage(const CByteImage& inputImage, const op::Array<float>& keypoints, CByteImage& resultImage, float renderThreshold)
{
    //ARMARX_CHECK_EXPRESSION(inputImage.width == resultImage.width);
    //ARMARX_CHECK_EXPRESSION(inputImage.height == resultImage.height);
    resultImage.Set(inputImage.width, inputImage.height, inputImage.type);
    ::ImageProcessor::CopyImage(&inputImage, &resultImage);
 
    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);
                }
            }
        }
    }
}