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 <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/Component.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();
        }
    }
} // namespace armarx