KeypointManager.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    <PACKAGE_NAME>::<CATEGORY>::KeypointManager
 * @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 "KeypointManager.h"
 
#include <numeric> // std::iota
 
#include <ArmarXCore/core/logging/Logging.h>
 
using namespace armarx;
 
Keypoint::Keypoint(const Keypoint& p) : _id(p.getId()), _name(p.getName())
{
    if (p.isStereo())
    {
        auto pair = p.getStereo2D();
        setStereo2D(pair.first, pair.second);
    }
    else
    {
        set2D(p.get2D());
    }
    if (p.is3DSet())
    {
        set3D(p.get3D());
    }
    setConfidence(p.getConfidence());
}
 
Keypoint::Keypoint(float x, float y, unsigned int id, const std::string& name, float confidence) :
    Keypoint(Point2D(x, y), id, name, confidence)
{
}
 
Keypoint::Keypoint(const Point2D& point,
                   unsigned int id,
                   const std::string& name,
                   float confidence) :
    _id(id), _name(name), _dominantColor(DrawColor24Bit{0, 0, 0})
{
    set2D(point);
    setConfidence(confidence);
}
 
Keypoint::Keypoint(const Point2D& left,
                   const Point2D& right,
                   unsigned int id,
                   const std::string& name,
                   float confidence) :
    _id(id), _name(name), _dominantColor(DrawColor24Bit{0, 0, 0})
{
    setStereo2D(left, right);
    setConfidence(confidence);
}
 
bool
Keypoint::is3DSet() const
{
    return _pos3D ? true : false;
}
 
bool
Keypoint::isStereo() const
{
    return !_pointR.isnan();
}
 
void
Keypoint::set2D(const Point2D& point)
{
    ARMARX_CHECK_EXPRESSION(!point.isnan());
    _pointL = point;
}
 
void
Keypoint::setStereo2D(const Point2D& left, const Point2D& right)
{
    set2D(left);
    ARMARX_CHECK_EXPRESSION(!right.isnan());
    _pointR = right;
}
 
void
Keypoint::set3D(FramedPositionPtr pos)
{
    _pos3D = pos;
}
 
void
Keypoint::setConfidence(float confidence)
{
    _confidence = confidence;
}
 
void
Keypoint::setDominantColor(const DrawColor24Bit& color)
{
    _dominantColor = color;
}
 
Point2D
Keypoint::get2D() const
{
    return _pointL;
}
 
std::pair<Point2D, Point2D>
Keypoint::getStereo2D() const
{
    ARMARX_CHECK_EXPRESSION(isStereo());
    return std::make_pair(_pointL, _pointR);
}
 
FramedPositionPtr
Keypoint::get3D() const
{
    return _pos3D;
}
 
float
Keypoint::getConfidence() const
{
    return _confidence;
}
 
DrawColor24Bit
Keypoint::getDominantColor() const
{
    return _dominantColor;
}
 
unsigned int
Keypoint::getId() const
{
    return _id;
}
 
std::string
Keypoint::getName() const
{
    return _name;
}
 
std::string
Keypoint::toString2D() const
{
    std::stringstream ss;
    ss << "Id: " << std::to_string(_id) << ", ";
    ss << "Name: " << _name << ", ";
    ss << "x: " << _pointL._x << ", ";
    ss << "y: " << _pointL._y << ", ";
    ss << "Confindence: " << _confidence;
    return ss.str();
}
 
KeypointSetById::const_iterator
KeypointObject::begin() const
{
    return _keypoints.get<0>().begin();
}
 
KeypointSetById::const_iterator
KeypointObject::end() const
{
    return _keypoints.get<0>().end();
}
 
KeypointPtr
KeypointObject::getNode(const std::string& nodeName) const
{
    auto itFound = _keypoints.get<1>().find(nodeName);
    if (itFound != _keypoints.get<1>().end())
    {
        return *itFound;
    }
    else
    {
        return KeypointPtr();
    }
}
 
KeypointPtr
KeypointObject::getNode(unsigned int id) const
{
    auto itFound = _keypoints.get<0>().find(id);
    if (itFound != _keypoints.get<0>().end())
    {
        return *itFound;
    }
    else
    {
        return KeypointPtr();
    }
}
 
void
KeypointObject::insertNode(const KeypointPtr point)
{
    if (point)
    {
        _keypoints.insert(point);
    }
}
 
void
KeypointObject::removeNode(const std::string& nodeName)
{
    _keypoints.get<1>().erase(nodeName);
}
 
void
KeypointObject::removeNode(unsigned int id)
{
    _keypoints.get<0>().erase(id);
}
 
size_t
KeypointObject::size() const
{
    return _keypoints.get<0>().size();
}
 
float
KeypointObject::getAverageDepth() const
{
    if (size() == 0)
    {
        return std::numeric_limits<float>::quiet_NaN();
    }
    float result = 0.0f;
    int amountDepths = 0;
    for (KeypointPtr point : _keypoints.get<0>())
    {
        ARMARX_CHECK_EXPRESSION(
            point
                ->is3DSet()); // if any point in this object has no 3D-information, something went wrong.
        result += point->get3D()->z;
        amountDepths++;
    }
    return result / static_cast<float>(amountDepths);
}
 
Keypoint2DMap
KeypointObject::toIce2D(IdNameMap idNameMap) const
{
    Keypoint2DMap iceMap;
    for (auto [id, label] : idNameMap)
    {
        KeypointPtr keypoint = this->getNode(id);
        if (keypoint)
        {
            Keypoint2D iceKeypoint;
            iceKeypoint.x = keypoint->get2D()._x;
            iceKeypoint.y = keypoint->get2D()._y;
            iceKeypoint.label = keypoint->getName();
            iceKeypoint.confidence = keypoint->getConfidence();
            iceKeypoint.dominantColor = keypoint->getDominantColor();
            iceMap.insert(std::make_pair(iceKeypoint.label, iceKeypoint));
        }
        else
        {
            Keypoint2D iceKeypoint;
            iceKeypoint.x = 0.0f;
            iceKeypoint.y = 0.0f;
            iceKeypoint.label = label;
            iceKeypoint.confidence = 0.0f;
            iceKeypoint.dominantColor = DrawColor24Bit{0, 0, 0}; // Black
            iceMap.insert(std::make_pair(iceKeypoint.label, iceKeypoint));
        }
    }
    return iceMap;
}
 
Keypoint3DMap
KeypointObject::toIce3D(IdNameMap idNameMap, const VirtualRobot::RobotPtr& local_robot) const
{
    Keypoint3DMap iceMap;
    for (auto [id, label] : idNameMap)
    {
        KeypointPtr keypoint = this->getNode(id);
        if (keypoint && keypoint->is3DSet())
        {
            Keypoint3D iceKeypoint;
            iceKeypoint.x = keypoint->get3D()->x;
            iceKeypoint.y = keypoint->get3D()->y;
            iceKeypoint.z = keypoint->get3D()->z;
 
            Eigen::Vector3f globalPose = keypoint->get3D()->toGlobalEigen(local_robot);
 
            iceKeypoint.globalX = globalPose(0);
            iceKeypoint.globalY = globalPose(1);
            iceKeypoint.globalZ = globalPose(2);
 
            iceKeypoint.label = keypoint->getName();
            iceKeypoint.confidence = keypoint->getConfidence();
            iceKeypoint.dominantColor = keypoint->getDominantColor();
            iceMap.insert(std::make_pair(keypoint->getName(), iceKeypoint));
        }
        else
        {
            Keypoint3D iceKeypoint;
            iceKeypoint.x = 0.0f;
            iceKeypoint.y = 0.0f;
            iceKeypoint.z = 0.0f;
            iceKeypoint.globalX = 0.0f;
            iceKeypoint.globalY = 0.0f;
            iceKeypoint.globalZ = 0.0f;
            iceKeypoint.label = label;
            iceKeypoint.confidence = 0.0f;
            iceKeypoint.dominantColor = DrawColor24Bit{0, 0, 0}; // Black
            iceMap.insert(std::make_pair(iceKeypoint.label, iceKeypoint));
        }
    }
    return iceMap;
}
 
void
KeypointObject::matchWith(const KeypointObjectPtr object)
{
    if (object)
    {
        for (KeypointPtr keypoint : _keypoints.get<0>())
        {
            KeypointPtr otherPoint = object->getNode(keypoint->getId());
            if (otherPoint)
            {
                ARMARX_CHECK_EXPRESSION(keypoint->getName() == otherPoint->getName());
                keypoint->setStereo2D(keypoint->get2D(), otherPoint->get2D());
                // The combined confindence is defined as the average of both confidences
                keypoint->setConfidence((keypoint->getConfidence() + otherPoint->getConfidence()) /
                                        2.0f);
            }
        }
    }
}
 
std::string
KeypointObject::toString2D() const
{
    std::stringstream ss;
    for (KeypointPtr keypoint : _keypoints.get<0>())
    {
        ss << keypoint->toString2D() << "\n";
    }
    return ss.str();
}
 
KeypointManager::KeypointManager(IdNameMap idNameMap) : _idNameMap(idNameMap)
{
}
 
KeypointManager::~KeypointManager()
{
}
 
Keypoint2DMapList
KeypointManager::toIce2D() const
{
    Keypoint2DMapList list;
    for (KeypointObjectPtr o : _objects)
    {
        list.push_back(o->toIce2D(_idNameMap));
    }
    return list;
}
 
Keypoint2DMapList
KeypointManager::toIce2D_normalized(int width, int height) const
{
    Keypoint2DMapList list = this->toIce2D();
    for (Keypoint2DMap map : list)
    {
        for (auto pair : map)
        {
            Keypoint2D point = pair.second;
            if (point.x < 0.0f || point.x > width || point.y < 0.0f || point.y > height)
            {
                point.x = 0.0;
                point.y = 0.0;
            }
            else
            {
                point.x = point.x / static_cast<float>(width);
                point.y = point.y / static_cast<float>(height);
            }
        }
    }
    return list;
}
 
Keypoint3DMapList
KeypointManager::toIce3D(const VirtualRobot::RobotPtr& local_robot) const
{
    Keypoint3DMapList list;
    for (KeypointObjectPtr o : _objects)
    {
        list.push_back(o->toIce3D(_idNameMap, local_robot));
    }
    return list;
}
 
void
KeypointManager::matchWith(const KeypointManagerPtr other)
{
    // For every person in the left image, we need to calculate the distance of all keypoints to their corresponding keypoints of every object in the right image
    // This results in a sum for each possible object-pair, which is the distance between two objects
    // The persons in both images then have to be paired in such a way that the sum of distances between the two objects in a pair is minimized.
 
    // It is assumed, that this manager contains the objects of the left image and the other manager conatains the objects of the right image
    // Afterwards this manager contains still all objects of the left image, but as much as possible are matched to the objects in the other manager
 
    size_t leftSize = getData().size();
    size_t rightSize = other->getData().size();
 
    ARMARX_DEBUG << VAROUT(leftSize) << " " << VAROUT(rightSize);
 
    if (leftSize == 0 || rightSize == 0)
    {
        return;
    }
 
    std::vector<KeypointObjectPtr> moreObjects;
    std::vector<KeypointObjectPtr> lessObjects;
    if (leftSize > rightSize)
    {
        moreObjects = this->getData();
        lessObjects = other->getData();
    }
    else
    {
        moreObjects = other->getData();
        lessObjects = this->getData();
    }
 
    // With only one object in every manager we can simply match these two
    if (leftSize == 1 && rightSize == 1)
    {
        KeypointObjectPtr o = getData().at(0);
        KeypointObjectPtr o1 = other->getData().at(0);
        o->matchWith(o1);
        return;
    }
 
    Eigen::MatrixXf personDistances(lessObjects.size(),
                                    moreObjects.size()); // rows = lessPersons; cols = morePersons
    Eigen::MatrixXf validKeypointsRatio(
        lessObjects.size(),
        moreObjects
            .size()); // to prevent, that artifacts of the openpose algorithm are used in the pairs...
 
    for (unsigned int l = 0; l < lessObjects.size(); l++)
    {
        KeypointObjectPtr pl = lessObjects.at(l);
        for (unsigned int r = 0; r < moreObjects.size(); r++)
        {
            KeypointObjectPtr pr = moreObjects.at(r);
            double sum = 0.0;
            int addedDistances = 0;
            for (KeypointPtr kpl : *pl)
            {
                KeypointPtr kpr = pr->getNode(kpl->getId());
                if (kpr)
                {
                    sum += std::pow(kpl->get2D()._x - kpr->get2D()._x, 2) +
                           std::pow(kpr->get2D()._y - kpr->get2D()._y,
                                    2); // squared euclidean distance
                    addedDistances++;
                }
            }
 
            personDistances(l, r) = (addedDistances == 0)
                                        ? std::numeric_limits<float>::max()
                                        : static_cast<float>(sum) / addedDistances;
 
            // Calculate ratio of valid keypoints
            int validKeypointsPL = static_cast<int>(pl->size());
            int validKeypointsPR = static_cast<int>(pr->size());
            if (validKeypointsPL == 0 || validKeypointsPR == 0)
            {
                validKeypointsRatio(l, r) =
                    0.000000001f; // simply a very small number for a very bad ratio (ratio cannot be 0, because it is needed for a division later)
            }
            else if (validKeypointsPL > validKeypointsPR)
            {
                validKeypointsRatio(l, r) =
                    static_cast<float>(validKeypointsPR) / static_cast<float>(validKeypointsPL);
            }
            else
            {
                validKeypointsRatio(l, r) =
                    static_cast<float>(validKeypointsPR) / static_cast<float>(validKeypointsPL);
            }
        }
    }
 
    // Finding best pairs by generating permutations of the list with more persons and comparing the resulting sum of distances.
    std::vector<unsigned int> numbers(moreObjects.size());
    std::iota(numbers.begin(), numbers.end(), 0);
 
    std::vector<unsigned int> bestPermutation;
    float currentMin = std::numeric_limits<float>::max();
    do
    {
        float sum = 0.0;
        for (unsigned int i = 0; i < lessObjects.size(); i++)
        {
            sum +=
                personDistances(i, numbers.at(i)) /
                static_cast<float>(std::pow(
                    validKeypointsRatio(i, numbers.at(i)),
                    3)); // distances with bad ratios will be bigger than distances with good ratios
        }
        if (sum < currentMin)
        {
            currentMin = sum;
            bestPermutation = numbers;
        }
        std::reverse(
            numbers.begin() + static_cast<long>(lessObjects.size()),
            numbers
                .end()); // we only need permutations of the form (morePersons choose lessPersons)
    } while (std::next_permutation(numbers.begin(), numbers.end()));
 
    // Matching
    if (!bestPermutation.empty())
    {
        if (leftSize <= rightSize)
        {
            for (unsigned int i = 0; i < this->getData().size(); i++)
            {
                KeypointObjectPtr left = this->getData().at(i);
                KeypointObjectPtr right = other->getData().at(bestPermutation.at(i));
                left->matchWith(right);
            }
        }
        else
        {
            for (unsigned int i = 0; i < other->getData().size(); i++)
            {
                KeypointObjectPtr right = other->getData().at(i);
                KeypointObjectPtr left = this->getData().at(bestPermutation.at(i));
                left->matchWith(right);
            }
        }
    }
}
 
void
KeypointManager::filterToNearestN(unsigned int n)
{
    if (n > _objects.size())
    {
        return;
    }
    std::sort(_objects.begin(),
              _objects.end(),
              [](KeypointObjectPtr o1, KeypointObjectPtr o2)
              { return o1->getAverageDepth() < o2->getAverageDepth(); });
    _objects.resize(n);
}