OpenPose3DDepthImageConverter.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 "OpenPose3DDepthImageConverter.h"
 
using namespace visionx;
using namespace armarx;
 
 
void OpenPose3DDepthImageConverter::setupPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
    OpenPoseEstimation::setupPropertyDefinitions(def);
 
    def->optional(radius, "DepthMedianRadius", "Depth Median Radius");
    def->optional(maxDepth, "MaxDepth", "Pixels with a distance higher than this value are masked out. Only for depth camera mode.", PropertyDefinitionBase::eModifiable);
    def->optional(maxDepthDifference, "MaxDepthDifference", "Allowed difference of depth value for one keypoint to median of all keypoints.", PropertyDefinitionBase::eModifiable);
    def->optional(cameraNodeName, "CameraNodeName");
}
 
void OpenPose3DDepthImageConverter::setupLocalVariables()
{
    OpenPoseEstimation::setupLocalVariables();
}
 
void OpenPose3DDepthImageConverter::destroyLocalVariables()
{
    OpenPoseEstimation::destroyLocalVariables();
}
 
void OpenPose3DDepthImageConverter::calculate3DEntitiesFrom2DEntitiesInBuffer()
{
    if (!localRobot || !running3D_is_possible || !running3D)
    {
        return;
    }
 
    RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateInterface, timestamp_of_update);
 
    const int depthThreshold = maxDepthDifference;
    openposeResult3D.clear();
 
    for (const auto& entities_iterator : openposeResult)
    {
        const std::string& name = entities_iterator->first;
        const Entity2D& entity = entities_iterator->second;
 
        if (entity.keypointMap.size() == 0)
        {
            continue;
        }
 
        std::map<std::string, 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 (const auto& entity_iterator : entity.keypointMap)
        {
            const std::string& kp_name = entities_iterator->first;
            const Keypoint2D& point = entities_iterator->second;
 
            int depth = getMedianDepthFromImage(static_cast<int>(point.x), static_cast<int>(point.y), radius);
            depthStorage[kp_name] = 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& depthStorage_iterator : depthStorage)
        {
            const std::string& storage_name = depthStorage_iterator->first;
            int& depth = depthStorage_iterator->second;
 
            if (depth > medianDepth + depthThreshold || depth < medianDepth - depthThreshold)
            {
                depth = medianDepth;
            }
        }
 
        // Transform pixel + depth into world coordinates
        // and update stored 3d entity
        openposeResult3D[name] = Entity3D();
        for (const auto& entity_iterator : entity.keypointMap)
        {
            const std::string& kp_name = entity_iterator->first;
            const Keypoint3D& point = entity_iterator->second;
 
            Vec2d imagePoint;
            imagePoint.x = point.x;
            imagePoint.y = point.y;
 
            Vec3d result;
            ARMARX_CHECK_EXPRESSION(calibration);
            calibration->ImageToWorldCoordinates(imagePoint, result, static_cast<float>(depthStorage.at(i)), useDistortionParameters);
 
            //FramedPosition pos = FramedPosition(Eigen::Vector3f(result.x, result.y, result.z), cameraNodeName, localRobot->getName());
            openposeResult3D[name].keypointMap[kp_name].x = result.x;
            openposeResult3D[name].keypointMap[kp_name].y = result.y;
            openposeResult3D[name].keypointMap[kp_name].z = result.z;
        }
 
        float average_x = 0;
        float average_y = 0;
        float average_z = 0;
    }
}
 
int OpenPose3DDepthImageConverter::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];
}
 
 
void OpenPose3DDepthImageConverter::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;
 
        }
    }
}