KeypointManager.h

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
 */
 
#pragma once
 
#include <cmath>
 
#include <boost/multi_index/mem_fun.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index_container.hpp>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
 
namespace armarx
{
    static void
    checkInput(std::initializer_list<float> f)
    {
        for (float fe : f)
        {
            ARMARX_CHECK_EXPRESSION(fe >= 0.0f)
                << "Within KeypointManager it is asserted that every value is greater than or "
                   "equal to zero. That is not true for "
                << fe;
        }
    }
 
    struct Point2D
    {
        Point2D()
        {
            _x = NAN;
            _y = NAN;
        }
 
        Point2D(float x, float y)
        {
            checkInput({x, y});
            _x = x;
            _y = y;
        }
 
        float _x = NAN;
        float _y = NAN;
 
        bool
        isnan() const
        {
            return std::isnan(_x) || std::isnan(_y);
        }
    };
 
    class Keypoint
    {
    public:
        Keypoint() = delete;
        Keypoint(const Keypoint& p);
        Keypoint(float x,
                 float y,
                 unsigned int id,
                 const std::string& name,
                 float confidence = 0.0f);
        Keypoint(const Point2D& point,
                 unsigned int id,
                 const std::string& name,
                 float confidence = 0.0f);
        Keypoint(const Point2D& left,
                 const Point2D& right,
                 unsigned int id,
                 const std::string& name,
                 float confidence = 0.0f);
 
        ~Keypoint()
        {
        }
 
        bool is3DSet() const;
        bool isStereo() const;
 
        void set2D(const Point2D& point);
        void setStereo2D(const Point2D& left, const Point2D& right);
        void set3D(FramedPositionPtr pose);
        void setConfidence(float confidence);
        void setDominantColor(const DrawColor24Bit& color);
 
        Point2D get2D() const;
        std::pair<Point2D, Point2D> getStereo2D() const;
        FramedPositionPtr get3D() const;
        float getConfidence() const;
        DrawColor24Bit getDominantColor() const;
        unsigned int getId() const;
        std::string getName() const;
        std::string toString2D() const;
 
    private:
        unsigned int _id;
        std::string _name;
        Point2D _pointL; // point in left image if stereo image is used; otherwise value of keypoint
        Point2D _pointR; // point in right image if stereo image is used
        float _confidence;
        FramedPositionPtr _pos3D;
        DrawColor24Bit _dominantColor;
    };
 
    using KeypointPtr = std::shared_ptr<Keypoint>;
 
 
    // defining boost multi index container
    using boost::multi_index_container;
    using namespace boost::multi_index;
    using KeypointSet = multi_index_container<
        KeypointPtr,
        indexed_by<ordered_unique<const_mem_fun<Keypoint, unsigned int, &Keypoint::getId>>,
                   ordered_non_unique<const_mem_fun<Keypoint, std::string, &Keypoint::getName>>>>;
    using KeypointSetById = typename KeypointSet::nth_index<0>::type;
    using KeypointSetByName = typename KeypointSet::nth_index<1>::type;
 
 
    using IdNameMap = const std::map<unsigned int, std::string>&;
 
    class KeypointObject;
    using KeypointObjectPtr = std::shared_ptr<KeypointObject>;
 
    // An object that contains serveral labeled keypoints.
    class KeypointObject
    {
 
    public:
        KeypointObject()
        {
        }
 
        ~KeypointObject()
        {
        }
 
        typename KeypointSetById::const_iterator begin() const;
        typename KeypointSetById::const_iterator end() const;
 
        KeypointPtr getNode(const std::string& nodeName) const;
        KeypointPtr getNode(unsigned int id) const;
        void insertNode(const KeypointPtr point);
        void removeNode(const std::string& nodeName);
        void removeNode(unsigned int id);
        size_t size() const;
        float getAverageDepth() const;
 
        Keypoint2DMap toIce2D(IdNameMap idNameMap) const;
        Keypoint3DMap toIce3D(IdNameMap idNameMap, const VirtualRobot::RobotPtr&) const;
 
        // updates stereo2D-information of the entries of this KeypointObject with the 2D-information of the other KeypointObject
        // This object keeps all its entries but a stereo2D-information is only set if the other object has a corresponding valid node
        void matchWith(const KeypointObjectPtr object);
 
        std::string toString2D() const;
 
    private:
        KeypointSet _keypoints;
    };
 
 
    class KeypointManager;
    using KeypointManagerPtr = std::shared_ptr<KeypointManager>;
 
    /**
     * @class KeypointManager
     * @brief A brief description
     *
     * Detailed Description
     */
    class KeypointManager
    {
    public:
        /**
         * KeypointManager Constructor
         */
        KeypointManager() = delete;
        KeypointManager(IdNameMap idNameMap);
 
        /**
         * KeypointManager Destructor
         */
        ~KeypointManager();
 
        std::vector<KeypointObjectPtr>&
        getData()
        {
            return _objects;
        }
 
        IdNameMap
        getIdNameMap() const
        {
            return _idNameMap;
        }
 
        Keypoint2DMapList toIce2D() const;
        Keypoint2DMapList toIce2D_normalized(int width, int height) const;
        Keypoint3DMapList toIce3D(const VirtualRobot::RobotPtr&) const;
 
        void matchWith(const KeypointManagerPtr other);
        // Filters keypointObjects to the nearest n objects
        void filterToNearestN(unsigned int n);
 
    private:
        std::vector<KeypointObjectPtr> _objects;
        IdNameMap _idNameMap;
    };
 
 
} // namespace armarx