HumanObstacleDetection.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    Armar6Skills::ArmarXObjects::HumanAvoidance
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @author     Fabian Peller
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "HumanObstacleDetection.h"
 
 
// STD/STL
#include <cmath>
#include <limits>
#include <string>
#include <map>
 
// Simox
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/XML/RobotIO.h>
 
// Ice
#include <Ice/Current.h>
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/time/StopWatch.h>
 
using namespace armarx;
using namespace visionx;
 
const std::string
HumanObstacleDetection::default_name = "HumanObstacleDetection";
 
 
HumanObstacleDetection::HumanObstacleDetection() noexcept :
    m_enabled(true),
    m_onlyUseFirstNResults(-1),
    m_warn_distance(100),
    m_human_confidence_filter_value(0.3),
    m_min_velocity_treshold(15),
    m_keypoint_after(IceUtil::Time::milliSeconds(1500))
{
 
}
 
 
void
HumanObstacleDetection::onInitComponent()
{
    ARMARX_DEBUG << "Initializing " << getName() << ".";
 
    ARMARX_DEBUG << "Initialized " << getName() << ".";
}
 
 
void
HumanObstacleDetection::onConnectComponent()
{
    ARMARX_DEBUG << "Connecting " << getName() << ".";
 
    const unsigned int periodic_task_interval = 100;
    m_check_human_task = new PeriodicTask<HumanObstacleDetection>(
        this,
        &HumanObstacleDetection::checkHumanVisibility, periodic_task_interval);
    m_check_human_task->start();
 
    ARMARX_DEBUG << "Connected " << getName() << ".";
}
 
 
void
HumanObstacleDetection::onDisconnectComponent()
{
    ARMARX_DEBUG << "Disconnecting " << getName() << ".";
 
    m_check_human_task->stop();
 
    ARMARX_DEBUG << "Disconnected " << getName() << ".";
}
 
 
void
HumanObstacleDetection::onExitComponent()
{
    ARMARX_DEBUG << "Exiting " << getName() << ".";
 
    ARMARX_DEBUG << "Exited " << getName() << ".";
}
 
void
HumanObstacleDetection::report3DKeypoints(
    const armarx::HumanPose3DMap& kpml,
    long timestamp,
    const Ice::Current&)
{
    ARMARX_DEBUG << "Got human pose";
    if (m_onlyUseFirstNResults == 0)
    {
        return;
    }
 
    if (m_onlyUseFirstNResults > 0)
    {
        m_onlyUseFirstNResults--;
    }
 
    IceUtil::Time start = IceUtil::Time::now();
    // Exit if component disabled or kpml is empty.
    {
        std::lock_guard l{m_enabled_mutex};
        if (not m_enabled or kpml.empty())
        {
            return;
        }
    }
    ARMARX_DEBUG << "TIme delta: " << start - IceUtil::Time::microSeconds(timestamp);
    ARMARX_DEBUG << "start report keypoints";
 
    std::lock_guard l{m_human_mutex};
    const IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
    armarx::core::time::StopWatch sw;
 
    // Create vector of maps
    std::vector<Keypoint3DMap> legacy_kpml;
    for (const auto& [name, entity] : kpml)
    {
        legacy_kpml.push_back(entity.keypointMap);
    }
 
    const OpenPoseResult closestHuman = find_closest_human(legacy_kpml, time);
    ARMARX_DEBUG << "find closest human" << sw.stop();
 
    if (not closestHuman.valid)
    {
        return;
    }
 
    if (closestHuman.distance < m_warn_distance)
    {
 
        ARMARX_IMPORTANT << deactivateSpam(30) << "HUMAN CLOSE! Attention!";
    }
    sw.reset();
    HumanApproximation human = approximate_human(closestHuman);
    ARMARX_DEBUG << "approximation" << sw.stop();
 
 
    sw.reset();
    m_obstacle_manager->directly_update_obstacle("human", Eigen::Vector2f(human.posX, human.posY), human.axisLengthX, human.axisLengthY, human.yaw);
    ARMARX_DEBUG << "manager" << sw.stop();
 
    ARMARX_DEBUG << "Drawing Human Obsacle at position (" << human.posX << ", " << human.posY << "). Needed time: " << IceUtil::Time::now() - start;
}
 
 
HumanObstacleDetection::OpenPoseResult
HumanObstacleDetection::find_closest_human(
    const std::vector<Keypoint3DMap>& kpml,
    const IceUtil::Time& timestamp)
{
    ARMARX_DEBUG << "Find closest human.";
    OpenPoseResult closest_human;
    closest_human.mean_position = Eigen::Vector3f(0, 0, 0);
    closest_human.timestamp = timestamp;
    closest_human.distance = std::numeric_limits<double>::max();
    closest_human.confidence = 0.0;
    closest_human.index = -1;
    closest_human.valid = false;
 
    for (unsigned int i = 0; i < kpml.size(); ++i)
    {
        // Calculate mean distance of keypoints to camera.
        auto& kpm = kpml[i];
        Eigen::Vector3f sum_positions(0, 0, 0);
        float sum_distance = 0;
        float sum_confidence = 0;
        int amount = 0;
 
        for (const auto& [key, value] : kpm)
        {
            if (std::isfinite(value.x) and value.x != 0 and std::isfinite(value.y) and value.y != 0 and std::isfinite(value.z) and value.z != 0)
            {
                sum_confidence += value.confidence;
                sum_positions += Eigen::Vector3f(value.globalX, value.globalY, value.globalZ);
                sum_distance += value.z;
                ++amount;
            }
        }
 
        if (amount == 0)
        {
            continue;
        }
 
        const Eigen::Vector3f mean_position = sum_positions * (1.0 / amount);
        const float mean_confidence = sum_confidence / amount;
        const float mean_distance = sum_distance / amount;
        ARMARX_DEBUG << "Found a human at a distance of " << sum_distance << "mm with confidence " << mean_confidence << ".";
 
        if (sum_distance < closest_human.distance and mean_confidence >= m_human_confidence_filter_value)
        {
            closest_human.mean_position = mean_position;
            closest_human.distance = mean_distance;
            closest_human.index = i;
            closest_human.keypoints = kpml[i];
            closest_human.confidence = mean_confidence;
            closest_human.valid = true;
        }
    }
 
    return closest_human;
}
 
 
HumanObstacleDetection::HumanApproximation
HumanObstacleDetection::approximate_human(const OpenPoseResult& closest_human)
{
    ARMARX_DEBUG << "Approximate human";
 
    std::map<std::string, std::pair<IceUtil::Time, Eigen::Vector2f>> transformed_points_with_timestamp;
 
    // Initialize transformed points with old ones.
    if (m_pose_buffer_fillctr > 0)
    {
        auto& pose_buf = m_pose_buffer.getUpToDateReadBuffer();
        for (unsigned int i = 0; i < m_pose_buffer_fillctr; ++i)
        {
            for (const auto& [key, timestamped_keypoint] : pose_buf[i].transformed_keypoints)
            {
                IceUtil::Time keypoint_timestamp = timestamped_keypoint.first;
                if (closest_human.timestamp - keypoint_timestamp > m_keypoint_after)
                {
                    transformed_points_with_timestamp[key] = timestamped_keypoint;
                }
            }
        }
    }
 
    // Update new transformed keypoints
    for (const auto& [key, value] : closest_human.keypoints)
    {
        //ARMARX_DEBUG << "kpm[" << key << "] = (" << value.x << ", " << value.y << ", "
        //             << value.z << ")";
        if (std::isfinite(value.globalX) and value.globalX != 0 and std::isfinite(value.globalY) and value.globalY != 0
            and std::isfinite(value.globalZ) and value.globalZ != 0)
        {
            Eigen::Vector3f pos{value.globalX, value.globalY, value.globalZ};
 
            transformed_points_with_timestamp[key] = std::make_pair(closest_human.timestamp, Eigen::Vector2f{pos(0), pos(1)});
        }
    }
 
    // Generate sum of all transformed keypoints in order to calculate mean.
    float sum_x = 0;
    float sum_y = 0;
    unsigned int amount = 0;
    for (const auto& [key, timestamped_keypoint] : transformed_points_with_timestamp)
    {
        Eigen::Vector2f keypoint = timestamped_keypoint.second;
 
        sum_x += keypoint(0);
        sum_y += keypoint(1);
        ++amount;
    }
 
    // Calculate mean.
    Eigen::Vector2f mean = Eigen::Vector2f::Zero();
    if (amount != 0)
    {
        mean = Eigen::Vector2f{sum_x / amount, sum_y / amount};
    }
 
    // Calculate max distance from mean position.  Needs mean in advance.
    float max_distance = 0;
    for (const auto& [key, timestamped_keypoint] : transformed_points_with_timestamp)
    {
        Eigen::Vector2f keypoint = timestamped_keypoint.second;
        max_distance = std::max(max_distance, (keypoint - mean).norm());
    }
 
    ARMARX_DEBUG << "Found positions in current frame [X,Y,Distance]: ("
                 << mean(0) << ", " << mean(1) << ", " << max_distance << ").";
    if ((last_human_approximation.valid and mean.isZero()) or max_distance == 0.0)
    {
        // Safety return to avoid 0,0 positions (although a human has been found).
        return last_human_approximation;
    }
 
    // Calculate yaw from the last poses (according to velocity).
    Eigen::Vector2f velocity;
    float velocity_length = 0.0;
    float yaw = 0.0;
    if (m_pose_buffer_fillctr > 0)
    {
        Eigen::Vector2f sum{0, 0};
        auto& pose_buf = m_pose_buffer.getUpToDateReadBuffer();
        for (unsigned int i = 0; i < m_pose_buffer_fillctr; ++i)
        {
            sum += Eigen::Vector2f{pose_buf[i].posX, pose_buf[i].posY};
        }
        Eigen::Vector2f temp_mean = sum / m_pose_buffer_fillctr;
        velocity = mean - temp_mean;
        velocity_length = velocity.norm();
        Eigen::Vector2f velocity_normalized = velocity.normalized();
 
        const float cross_product = velocity_normalized(1);
        const float dot_product = velocity_normalized(0);
        yaw = acos(dot_product);
 
        if (cross_product < 0)
        {
            yaw = 2 * M_PI - yaw;
        }
    }
 
    ARMARX_DEBUG << "Got yaw: " << yaw << " and velocity vector " << velocity << " and length "
                 << velocity_length << ".";
    float velocity_multiplier = velocity_length / 15.0;
 
    HumanApproximation current_frame_human;
    current_frame_human.valid = true;
    current_frame_human.transformed_keypoints = transformed_points_with_timestamp;
    current_frame_human.posX = mean(0);
    current_frame_human.posY = mean(1);
    current_frame_human.axisLengthX = max_distance * velocity_multiplier;
    current_frame_human.axisLengthY = max_distance;
    current_frame_human.yaw = yaw;
    current_frame_human.timestamp = closest_human.timestamp;
 
    // Add human of current frame to buffer.
    auto& buf = m_pose_buffer.getWriteBuffer();
    buf.at(m_pose_buffer_index++) = current_frame_human;
    m_pose_buffer_index %= buf.size();
    m_pose_buffer.commitWrite();
    ++m_pose_buffer_fillctr;
    m_pose_buffer_fillctr = std::min(m_pose_buffer_fillctr, static_cast<unsigned int>(buf.size()));
 
    // Calculate means of last frames.
    HumanApproximation mean_frame_human = current_frame_human;
    if (m_pose_buffer_fillctr > 0)
    {
        Eigen::Vector2f pose_sum{0, 0};
        Eigen::Vector2f axis_length_sum{0, 0};
        double sum_cos_yaw = 0;
        double sum_sin_yaw = 0;
        auto& pose_buf = m_pose_buffer.getUpToDateReadBuffer();
        for (unsigned int i = 0; i < m_pose_buffer_fillctr; ++i)
        {
            pose_sum += Eigen::Vector2f{pose_buf[i].posX, pose_buf[i].posY};
            axis_length_sum += Eigen::Vector2f{pose_buf[i].axisLengthX, pose_buf[i].axisLengthY};
            sum_sin_yaw += std::sin(pose_buf[i].yaw);
            sum_cos_yaw += std::cos(pose_buf[i].yaw);
        }
        mean_frame_human.posX = pose_sum(0) / m_pose_buffer_fillctr;
        mean_frame_human.posY = pose_sum(1) / m_pose_buffer_fillctr;
        //mean_frame_human.axisLengthX = axis_length_sum(0) / m_pose_buffer_fillctr;
        //mean_frame_human.axisLengthX = axis_length_sum(1) / m_pose_buffer_fillctr;
 
        // In order to avoid jitter, we only use the mean yaw if the velocity is great enough.
        if (velocity_length > m_min_velocity_treshold)
        {
            double mean_yaw = std::atan2((1.0 / m_pose_buffer_fillctr) * sum_sin_yaw,
                                         (1.0 / m_pose_buffer_fillctr) * sum_cos_yaw);
            if (mean_yaw < 0)
            {
                mean_yaw += 2.0 * M_PI;
            }
 
            mean_frame_human.yaw = mean_yaw;
        }
        else
        {
            mean_frame_human.yaw = last_human_approximation.yaw;
        }
    }
 
    last_human_approximation = mean_frame_human;
    return mean_frame_human;
}
 
 
void
HumanObstacleDetection::checkHumanVisibility()
{
    std::lock_guard l{m_human_mutex};
    const bool is_data_outdated =
        (IceUtil::Time::now() - last_human_approximation.timestamp) > m_keypoint_after;
    if (last_human_approximation.valid and is_data_outdated)
    {
        ARMARX_IMPORTANT << "Have not seen a human for " << m_keypoint_after << ".  " << "Deleting obstacle.";
        HumanApproximation human;
        last_human_approximation = human;
 
        m_obstacle_manager->remove_obstacle("human");
    }
}
 
 
void
HumanObstacleDetection::setEnabled(bool enable, const Ice::Current&)
{
    std::lock_guard l{m_enabled_mutex};
    m_enabled = enable;
}
 
 
void
HumanObstacleDetection::enable(const Ice::Current&)
{
    std::lock_guard l{m_enabled_mutex};
    m_enabled = true;
}
 
 
void
HumanObstacleDetection::disable(const Ice::Current&)
{
    std::lock_guard l{m_enabled_mutex};
    m_enabled = false;
}
 
 
std::string
HumanObstacleDetection::getDefaultName()
const
{
    return default_name;
}
 
 
PropertyDefinitionsPtr
HumanObstacleDetection::createPropertyDefinitions()
{
    PropertyDefinitionsPtr def{new ComponentPropertyDefinitions{getConfigIdentifier()}};
 
    def->topic<armarx::OpenPose3DListener>("OpenPoseEstimation3D");
 
    def->component(m_obstacle_manager, "DynamicObstacleManager", "ObstacleManager", "The name of the obstacle manager proxy");
 
    def->optional(m_enabled, "ActivateOnStartup", "Activate the component on startup.");
    def->optional(m_onlyUseFirstNResults, "OnlyUseFirstNResults", "Only use the first n results of OpenPose.");
    def->optional(m_keypoint_after, "ForgetOpenPoseKeypointAfter",
                  "Forget the OpenPose keypoints after X ms if not visible anymore.");
    def->optional(m_min_velocity_treshold, "MinVelocityThreshold",
                  "Velocities below this value will be considered zero.");
    def->optional(m_human_confidence_filter_value, "HumanConfidenceFilterValue",
                  "Poses with a mean confidence below this value will be discarded.");
    def->optional(m_warn_distance, "WarnDistance",
                  "Distance in [mm] at which a warning should be issued.").setMin(100);
    return def;
}