skeleton.h

/******************************************************************************
 *                                                                            *
 * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT)        *
 * All Rights Reserved.                                                       *
 *                                                                            *
 ******************************************************************************/
 
#ifndef ASSISTIVE_REHAB_SKELETON_H
#define ASSISTIVE_REHAB_SKELETON_H
 
#include <string>
#include <vector>
#include <unordered_map>
#include <utility>
#include <ostream>
#include <iostream>
#include <limits>
#include <yarp/os/Bottle.h>
#include <yarp/os/Property.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
 
namespace assistive_rehab
{
 
namespace KeyPointTag
{
extern const std::string shoulder_center;
extern const std::string head;
extern const std::string shoulder_left;
extern const std::string elbow_left;
extern const std::string hand_left;
extern const std::string shoulder_right;
extern const std::string elbow_right;
extern const std::string hand_right;
extern const std::string hip_center;
extern const std::string hip_left;
extern const std::string knee_left;
extern const std::string ankle_left;
extern const std::string foot_left;
extern const std::string hip_right;
extern const std::string knee_right;
extern const std::string ankle_right;
extern const std::string foot_right;
}
 
namespace SkeletonType
{
extern const std::string Skeleton;
extern const std::string SkeletonStd;
}
 
class Skeleton;
class SkeletonStd;
 
class KeyPoint
{
    friend class Skeleton;
    friend class SkeletonStd;
 
    bool updated;
    std::string tag;
    yarp::sig::Vector point;
    yarp::sig::Vector pixel;
    std::vector<KeyPoint*> parent;
    std::vector<KeyPoint*> child;
 
    void stale() { updated=false; }
 
public:
    KeyPoint();
 
    KeyPoint(const std::string &tag_, const yarp::sig::Vector &point_=yarp::sig::Vector(3,std::numeric_limits<double>::quiet_NaN()),
             const yarp::sig::Vector &pixel_=yarp::sig::Vector(2,std::numeric_limits<double>::quiet_NaN()),const bool updated_=false);
 
    KeyPoint(const KeyPoint&) = delete;
 
    KeyPoint& operator=(const KeyPoint&) = delete;
 
    virtual ~KeyPoint() { }
 
    bool isUpdated() const { return updated; }
 
    const std::string& getTag() const { return tag; }
 
    const yarp::sig::Vector &getPoint() const { return point; }
 
    bool setPoint(const yarp::sig::Vector &point);
 
    bool setPoint(const yarp::sig::Vector &point,
                  const yarp::sig::Vector &pixel);
 
    const yarp::sig::Vector &getPixel() const { return pixel; }
 
    unsigned int getNumParent() const { return (unsigned int)parent.size(); }
 
    const KeyPoint* getParent(const unsigned int i) const;
 
    unsigned int getNumChild() const { return (unsigned int)child.size(); }
 
    const KeyPoint* getChild(const unsigned int i) const;
};
 
class Skeleton
{
protected:
    std::string type; 
    std::string tag; 
    std::vector<KeyPoint*> keypoints; 
    std::unordered_map<std::string,KeyPoint*> tag2key; 
    std::unordered_map<KeyPoint*,unsigned int> key2id; 
    yarp::sig::Matrix T; 
    yarp::sig::Vector coronal; 
    yarp::sig::Vector sagittal; 
    yarp::sig::Vector transverse; 
    yarp::os::Property helper_toproperty(KeyPoint *k) const;
    void helper_fromproperty(yarp::os::Bottle *prop, KeyPoint *parent);
    void helper_updatefromproperty(yarp::os::Bottle *prop);
    void helper_normalize(KeyPoint* k, const std::vector<yarp::sig::Vector> &helperpoints, const double n);
    void helper_scale(KeyPoint* k, const std::vector<yarp::sig::Vector> &helperpoints, const double s);
    double helper_getmaxpath(KeyPoint* k, std::vector<bool> &visited) const;
 
public:
    Skeleton();
 
    Skeleton(const Skeleton&) = delete;
 
    Skeleton& operator=(const Skeleton&) = delete;
 
    virtual ~Skeleton();
 
    const std::string& getType() const { return type; }
 
    void setTag(const std::string &tag) { this->tag=tag; }
 
    const std::string& getTag() const { return tag; }
 
    bool setTransformation(const yarp::sig::Matrix &T);
 
    const yarp::sig::Matrix& getTransformation() const { return T; }
 
    bool setCoronal(const yarp::sig::Vector &coronal);
 
    bool setSagittal(const yarp::sig::Vector &sagittal);
 
    bool setTransverse(const yarp::sig::Vector &transverse);
 
    yarp::sig::Vector getCoronal() const;
 
    yarp::sig::Vector getSagittal() const;
 
    yarp::sig::Vector getTransverse() const;
 
    double getMaxPath() const;
 
    virtual yarp::os::Property toProperty();
 
    virtual void fromProperty(const yarp::os::Property &prop);
 
    unsigned int getNumKeyPoints() const { return (unsigned int)keypoints.size(); }
 
    int getNumFromKey(const std::string &tag) const;
 
    const KeyPoint* operator[](const std::string &tag) const;
 
    const KeyPoint* operator[](const unsigned int i) const;
 
    virtual void update();
 
    virtual void update(const std::vector<yarp::sig::Vector> &ordered);
 
    virtual void update(const std::vector<std::pair<std::string,yarp::sig::Vector>> &unordered);
 
    virtual void update_withpixels(const std::vector<std::pair<yarp::sig::Vector,yarp::sig::Vector>> &ordered);
 
    virtual void update_withpixels(const std::vector<std::pair<std::string,std::pair<yarp::sig::Vector,yarp::sig::Vector>>> &unordered);
 
    virtual void update(const yarp::os::Property &prop);
 
    virtual bool update_planes() = 0;
 
    virtual std::vector<yarp::sig::Vector> get_ordered() const;
 
    virtual std::vector<std::pair<std::string,yarp::sig::Vector>> get_unordered() const;
 
    virtual std::vector<std::pair<yarp::sig::Vector,yarp::sig::Vector>> get_ordered_withpixels() const;
 
    virtual std::vector<std::pair<std::string,std::pair<yarp::sig::Vector,yarp::sig::Vector>>> get_unordered_withpixels() const;
 
    void normalize(const double n=1.0);
 
    void scale(const double s);
 
    void print(std::ostream &os=std::cout) const;
};
 
class SkeletonStd : public Skeleton
{
public:
    SkeletonStd();
 
    bool update_planes() override;
};
 
Skeleton *skeleton_factory(const yarp::os::Property &prop);
 
}
 
#endif