OpenPoseEstimationComponent.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 "OpenPoseEstimationComponent.h"
 
#include <ArmarXCore/core/Component.h>
#include <SimoxUtility/algorithm/string.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <VisionX/core/ImageProcessor.h>
 
namespace armarx
{
    void OpenPoseEstimationComponentPluginUser::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        def->topic(listener2DPrx, "OpenPoseEstimation2D", "OpenPoseEstimation2DTopicName");
 
        def->optional(op_settings.net_resolution, "OP_net_resolution", "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.");
        def->optional(op_settings.output_resolution, "OP_output_resolution", "The image resolution (display and output). Use \"-1x-1\" to force the program to use the"
                                                                    " input image resolution.");
        def->optional(op_settings.scale_gap, "OP_scale_gap", "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.");
        def->optional(op_settings.scale_number, "OP_scale_number", "Number of scales to average.");
        def->optional(op_settings.model_pose, "OP_model_pose", "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).");
        def->optional(op_settings.model_folder, "OP_model_folder", "Folder path (absolute or relative) where the models (pose, face, ...) are located.");
        def->optional(op_settings.num_gpu_start, "OP_num_gpu_start", "GPU device start number.");
        def->optional(op_settings.minimum_number_of_valid_keypoints_per_entitiy, "MinimalAmountKeypoints", "Minimal amount of keypoints per person. Detected persons with less valid keypoints will be discarded.");
 
        def->optional(op_render_threshold, "OP_render_threshold", "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).");
 
        def->optional(active_upon_startup, "ActivateOnStartup", "If true, poseEstimation-tasks are started after starting the component. If false, the component idles.");
    }
 
    void OpenPoseEstimationComponentPluginUser::postOnConnectImageProcessor()
    {
        if (active_upon_startup)
        {
            start();
        }
    }
 
    void OpenPoseEstimationComponentPluginUser::preOnInitImageProcessor()
    {
 
    }
 
    void OpenPoseEstimationComponentPluginUser::preOnConnectImageProcessor()
    {
 
    }
 
    void OpenPoseEstimationComponentPluginUser::postOnDisconnectImageProcessor()
    {
 
    }
 
    void OpenPoseEstimationComponentPluginUser::preOnDisconnectImageProcessor()
    {
 
        stop();
        running2D = false;
 
        delete openposeResultImage;
    }
 
    OpenPoseEstimationComponentPluginUser::OpenPoseEstimationComponentPluginUser()
    {
    }
 
    void OpenPoseEstimationComponentPluginUser::renderOutputImage(const op::Array<float>& op_keypoints)
    {
        openposePtr->render2DResultImage(*rgbImageBuffer, op_keypoints, *openposeResultImage[0], op_render_threshold);
    }
 
    void OpenPoseEstimationComponentPluginUser::reportEntities()
    {
        ARMARX_DEBUG << deactivateSpam(0.5) << "Reporting 2Dkeypoints for " << openposeResult.size() << " entities";
        listener2DPrx->report2DKeypoints(openposeResult, timestamp_of_update);
    }
 
    void OpenPoseEstimationComponentPluginUser::visualize()
    {
        // nothing to visualize since no 3d info available
        openposeResult.clear(); // last step in pipeline
 
    }
 
    void OpenPoseEstimationComponentPluginUser::loop()
    {
        // Check if openpose is setup
        if (!openposePtr)
        {
            std::lock_guard l(openpose_initializing_mutex);
            ARMARX_INFO << "Initialize OpenPose...";
            openposePtr = MonocularOpenPoseEstimationPtr(new OpenPoseAdapter(op_settings));
        }
 
        while (running2D && openposeTask->isRunning())
        {
            if (!update_ready)
            {
                usleep(100);
                continue;
            }
 
            update_ready = false;
 
            // 0. An update is ready.
            // The new input image is already stored in rgbImageBuffer
            // We use this image to get the openpose data
            // After that the openpose image is stored in openPoseResultImage and the
            // OpenPose values are stored in openposeResult
 
            // 1. create 2D-Keypoints from images
            std::lock_guard rgbImage_lock(rgbImageBufferMutex);
            auto op_array = openposePtr->getOpenposeKeypoints(rgbImageBuffer);
            openposeResult = openposePtr->convert2DKeypointsToIce(op_array, rgbImageBuffer);
 
            if (openposeResult.empty())
            {
                continue;
            }
 
            // 2. renderOutputImage
            {
                std::lock_guard output_lock(openposeResultImageMutex);
                renderOutputImage(op_array);
            }
 
            // 3. send keypoints via topic
            reportEntities();
 
            // 4. visualize
            visualize();
 
            result_image_ready = true;
        }
    }
 
    void OpenPoseEstimationComponentPluginUser::start(const Ice::Current&)
    {
        if (running2D)
        {
            return;
        }
        else
        {
            ARMARX_INFO << "Starting OpenposeEstimation";
            openposeTask = new RunningTask<OpenPoseEstimationComponentPluginUser>(this, &OpenPoseEstimationComponentPluginUser::loop);
            running2D = true;
            openposeTask->start();
        }
    }
 
    void OpenPoseEstimationComponentPluginUser::stop(const Ice::Current&)
    {
        if (running2D)
        {
            ARMARX_INFO << "Stopping OpenposeEstimation";
            running2D = false;
            openposeTask->stop(true);
        }
 
        // free memory
        openposePtr.reset();
    }
 
    void OpenPoseEstimationComponentPluginUser::start3DPoseEstimation(const Ice::Current&)
    {
        ARMARX_WARNING << "Could not start 3D pose estimation in 2D component.";
    }
 
    void OpenPoseEstimationComponentPluginUser::stop3DPoseEstimation(const Ice::Current&)
    {
 
    }
 
    void OpenPoseEstimationComponentPluginUser::enableHumanPoseEstimation(const EnableHumanPoseEstimationInput &input, const Ice::Current&)
    {
        if (input.enable2d)
        {
            start();
        }
        else
        {
            stop();
        }
    }
}