CscDetectorEnginePersonPose.h

Go to the documentation of this file.

//
//  CscDetectorEnginePersonPose.hpp
//  Conscience Core
//
//  Created by Ilies Zaoui on 22/07/2021.
//  Copyright © 2021 Conscience Robotics. All rights reserved.
//
 
#ifndef CscDetectorEnginePersonPose_h
#define CscDetectorEnginePersonPose_h
 
#include "DetectorEngine/CscDNNPool.h"
#include "CoreObjects/CscWorldObject.h"
#include "DetectorEngine/CscDetectorEngine.h"
#include <opencv2/opencv.hpp>
#include <ctime>
#include <array>
 
using std::array;
using namespace conscience_core::axiomes;
 
namespace conscience_core::detector_engine {
 
    ENUM(CscDetectedPersonPose, 
            NonePose,
            LyingDown, 
            StandingUp,
            Sit
    );
 
    using SafeNeuralNetwork = CscDNNPool::SafeNeuralNetwork;
 
    class CscDetectorEnginePersonPose : public CscDetectorEngine {
    public:
        
        struct Configuration {
            bool drawPredictionOnImage;
            optional<cv::Size> minSize;
            optional<cv::Size> maxSize;
            Configuration(bool drawPredictionOnImage = false,
                          optional<cv::Size> minSize = {},
                          optional<cv::Size> maxSize = {}): drawPredictionOnImage(drawPredictionOnImage), minSize(minSize), maxSize(maxSize) {}
        };
 
        CscDetectorEnginePersonPose(ptr<CscDNNPool> dnnPool, const Configuration &configuration = Configuration());
 
        ptr<DetectionResult> detectOnImage(
            const DetectorSourceImage &image, ptr<DetectionParameters> parameters
        ) override;
        
        virtual bool equals(CscDetectorEngine *) const override;
 
    private:
        
        bool drawPredictionOnImage;
        
        optional<cv::Size> minSize;
        optional<cv::Size> maxSize;
        
        /**********/
        // FIXME: hardcoded limits
        static constexpr float standingPersonMaxWidth = 200;
        static constexpr float standingPersonMinHeight = 360;
        static constexpr float standingPersonMaxVerticalAngleRadians = .3;
        
        static constexpr float maxBoundingBoxArea = 600 * 500;
        
        static constexpr float maxDistanceBetweenDots = 320;
        /**********/
        
        ptr<SafeNeuralNetwork> neuralNetwork;
        string modelTxt;
        string modelBin;
        
        const int NET_INPUT_SIZE = 368;
        
        std::unique_ptr<CscLogger> logger;
        
        struct KeyPoint{
            KeyPoint(cv::Point point,float probability){
                this->id = -1;
                this->point = point;
                this->probability = probability;
            }
 
            int id;
            cv::Point point;
            float probability;
        };
 
        struct ValidPair{
            ValidPair(int aId,int bId,float score){
                this->aId = aId;
                this->bId = bId;
                this->score = score;
            }
 
            int aId;
            int bId;
            float score;
        };
        
        struct PersonPair {
        public:
            const KeyPoint *pointA;
            const KeyPoint *pointB;
            bool invalid;
        };
        
        class Person {
        public:
            Person(int id, const vector<PersonPair *> &pairs);
            ~Person();
            const int id;
            
            bool hasInvalidPair() const;
            
            optional<cv::Point>  getLegsBottomPoint() const { return legsBottomPoint; }
            optional<CscPoint2d> getTopAverage() const { return topAverage; }
            optional<CscPoint2d> getMiddleAverage() const { return middleAverage; }
            optional<CscPoint2d> getBottomAverage() const { return bottomAverage; }
            optional<cv::Rect> getBoundingBox() const { return boundingBox; }
            optional<cv::Rect> getBodyBoundingBox() const { return bodyBoundingBox; }
            bool isBottomToMiddleLinkFound() const { return bottomToMiddleLinkFound; }
            bool isTopToMiddleLinkFound() const { return topToMiddleLinkFound; }
            bool getTopPointsCount() const { return topCount; }
            bool getMiddlePointsCount() const { return middleCount; }
            bool getBottomPointsCount() const { return bottomCount; }
            
            void drawPoints(Mat &imageMatrix, const cv::Scalar &color) const;
            void registerPublicAIData(map<string, CscPublicAIData*> &skeletonPublicAIData, double imageHeight) const;
            
            bool areLegsParallelAndStraight() const;
            
            optional<double> getLegsAngle() const { return legsAngle; }
            optional<double> getRightAnkleKneeAngle() const { return rightKneeFootAngle; }
            optional<double> getLeftAnkleKneeAngle() const { return leftKneeFootAngle; }
 
            optional<double> getRightHipKneeAngle() const { return rightHipKneeAngle; }
            optional<double> getLeftHipKneeAngle() const { return leftHipKneeAngle; }
 
            optional<double> getAnklesFeetAngle() const { return anklesFeetAngle; }
            optional<double> getLegsLength() const { return legsLength; }
            optional<double> getTorsoAngle() const { return torsoAngle; }
            optional<double> getTorsoLength() const { return torsoLength; }
            optional<cv::Point> getMiddleHipPoint() const;
            optional<cv::Point> getLeftHipPoint() const;
            optional<cv::Point> getRightHipPoint() const;
            
            optional<cv::Point> getTorsoTopPoint() const;
            
            optional<cv::Point> getLeftFootPoint() const;
            optional<cv::Point> getRightFootPoint() const;
            
            optional<cv::Point> getLeftKneePoint() const;
            optional<cv::Point> getRightKneePoint() const;
            optional<cv::Point> getLeftWristPoint() const;
            optional<cv::Point> getRightWristPoint() const;
 
        private:
            map<string, const KeyPoint *> points;
            const KeyPoint *getPointOrNull(const string &name) const;
            
            template <class ... FallbackNames>
            const KeyPoint *getPointOrOtherOrNull(const char *name, FallbackNames ... otherNames) const {
                const auto list = {name, otherNames...};
                
                for (const char *requestedName : list) {
                    const KeyPoint *result = getPointOrNull(string(requestedName));
                    if (result != nullptr) {
                        return result;
                    }
                }
                
                return nullptr;
            }
            
            const vector<PersonPair *> pairs;
            bool topToMiddleLinkFound = false;
            bool bottomToMiddleLinkFound = false;
            bool hasInvalidPairValue = false;
            optional<CscPoint2d> topAverage = {};
            optional<CscPoint2d> middleAverage = {};
            optional<CscPoint2d> bottomAverage = {};
            optional<cv::Point> legsBottomPoint = {};
            optional<cv::Rect> boundingBox = {};
            optional<cv::Rect> bodyBoundingBox = {};
            int topCount = 0;
            int middleCount = 0;
            int bottomCount = 0;
            
            optional<double> torsoAngle = {};
            optional<double> torsoLength = {};
            
            optional<double> leftLegAngle = {};
            optional<double> leftHipKneeAngle = {};
            optional<double> hipNeckAngle = {};
            optional<double> leftKneeFootAngle = {};
            optional<double> leftAnkleFootAngle = {};
            optional<double> rightAnkleFootAngle = {};
            optional<double> anklesFeetAngle = {};
            optional<double> rightLegAngle = {};
            optional<double> rightHipKneeAngle = {};
            optional<double> rightKneeFootAngle = {};
            optional<double> legsAngle = {};
            optional<double> legsLength = {};
        };
        
        vector<cv::Mat> splitNetOutputBlobToParts(cv::Mat& netOutputBlob, const cv::Size& targetSize) const;
        vector<vector<int>> getPersonwiseKeypoints(const std::vector<std::vector<ValidPair>>& validPairs, const std::set<int>& invalidPairs) const;
        vector<KeyPoint *> getKeyPoints(cv::Mat& probMap, double threshold) const;
        void getValidPairs(const vector<cv::Mat>& netOutputParts,
                           const vector<vector<KeyPoint *>>& detectedKeypoints,
                           vector<vector<ValidPair>>& validPairs,
                           std::set<int>& invalidPairs) const;
        
        CscDetectedPersonPose isPersonLyingOrStandingUp(const Mat &originalImage, Mat &resultingImage, Person *person, const map<string, vector<cv::Point2f>> &ignoredZones);
        
    };
}
 
#endif