OpenPose C++实现多人姿态估计

代码实现

flyfish

#include<opencv2/dnn.hpp>
#include<opencv2/imgproc.hpp>
#include<opencv2/highgui.hpp>

#include<iostream>
#include<chrono>
#include<random>
#include<set>
#include<cmath>

cv::Mat g_test_output_frame;

struct KeyPoint{
    KeyPoint(cv::Point point,float probability){
        this->id = -1;
        this->point = point;
        this->probability = probability;
    }

    int id;
    cv::Point point;
    float probability;
};
//重载 << 输出 keypoint
std::ostream& operator << (std::ostream& os, const KeyPoint& kp)
{
    os << "Id:" << kp.id << ", Point:" << kp.point << ", Prob:" << kp.probability << std::endl;
    return os;
}

////////////////////////////////
struct ValidPair{
    ValidPair(int aId,int bId,float score){
        this->aId = aId;
        this->bId = bId;
        this->score = score;
    }

    int aId;
    int bId;
    float score;
};

//重载 << 输出 pair
std::ostream& operator << (std::ostream& os, const ValidPair& vp)
{
    os << "A:" << vp.aId << ", B:" << vp.bId << ", score:" << vp.score << std::endl;
    return os;
}

////////////////////////////////
//重载 << 输出 vector
template < class T > std::ostream& operator << (std::ostream& os, const std::vector<T>& v)
{
    os << "[";
    bool first = true;
    for (typename std::vector<T>::const_iterator ii = v.begin(); ii != v.end(); ++ii, first = false)
    {
        if(!first) os << ",";
        os << " " << *ii;
    }
    os << "]";//<<std::endl;;
    return os;
}
//重载 << 输出 set
template < class T > std::ostream& operator << (std::ostream& os, const std::set<T>& v)
{
    os << "[";
    bool first = true;
    for (typename std::set<T>::const_iterator ii = v.begin(); ii != v.end(); ++ii, first = false)
    {
        if(!first) os << ",";
        os << " " << *ii;
    }
    os << "]";
    return os;
}

////////////////////////////////
//COCO的模型
const int kPoints = 18;
//为每个关键点命名，一共18个，不包含背景
//鼻子-0, 脖子-1，右肩-2，右肘-3，右手腕-4，左肩-5，左肘-6，左手腕-7，右臀-8，右膝盖-9，
//右脚踝-10，左臀-11，左膝盖-12，左脚踝-13，右眼-14，左眼-15，有耳朵-16，左耳朵-17
const std::string keypointsMapping[] = {
    "Nose", "Neck",
    "R-Sho", "R-Elb", "R-Wr",
    "L-Sho", "L-Elb", "L-Wr",
    "R-Hip", "R-Knee", "R-Ank",
    "L-Hip", "L-Knee", "L-Ank",
    "R-Eye", "L-Eye", "R-Ear", "L-Ear"
};
//posePairs看下一个结构，posePairs的输出索引 例如1,2 对应，31,32；1,5对应39,40
//一共19对
const std::vector<std::pair<int,int>> mapIdx = {
    {31,32}, {39,40}, {33,34}, {35,36}, {41,42}, {43,44},
    {19,20}, {21,22}, {23,24}, {25,26}, {27,28}, {29,30},
    {47,48}, {49,50}, {53,54}, {51,52}, {55,56}, {37,38},
    {45,46}
};

//0分别与1,14,15连接
//Nose分别与Neck，Right Eye，Left Eye连接
//一共19对
const std::vector<std::pair<int,int>> posePairs = {
    {1,2}, {1,5}, {2,3}, {3,4}, {5,6}, {6,7},
    {1,8}, {8,9}, {9,10}, {1,11}, {11,12}, {12,13},
    {1,0}, {0,14}, {14,16}, {0,15}, {15,17}, {2,17},
    {5,16}
};

void output_draw_contours(cv::OutputArrayOfArrays& in)
{
    cv::drawContours(g_test_output_frame, in, -1, cv::Scalar::all(255));
    cv::imshow("Contours", g_test_output_frame);
}

//对 Confidence Map 采用 NMS(Non Maximum Suppression) 来检测关键点.
//probMap  smoothProbMap maskedProbMap
//probMap->smoothProbMap->maskedProbMap

//findContours函数
//    contour
//    美[ 'kɑːntʊr ]
//    英[ 'kɒntʊə ]
//    n. 等高线 / 周线 / 电路 / 概要
//    v. 使与某轮廓吻合 / 循地形轮廓而行 / 画轮廓 / 画等高线

//void cv::findContours(
//    cv::InputOutputArray image, // 输入的8位单通道“二值”图像
//    cv::OutputArrayOfArrays contours, // 包含points的vectors的vector
//    cv::OutputArray hierarchy, // (可选) 拓扑信息
//    int mode, // 轮廓检索模式
//    int method, // 近似方法
//    cv::Point offset = cv::Point() // (可选) 所有点的偏移
//);

//下面的findContours是当前使用的
//void cv::findContours(
//    cv::InputOutputArray image, // 输入的8位单通道“二值”图像
//    cv::OutputArrayOfArrays contours, // 包含points的vectors的vector 等同于  std::vector<std::vector<cv::Point> >
//    int mode, // 轮廓检索模式
//    int method, // 近似方法
//    cv::Point offset = cv::Point() //  (可选) 所有点的偏移
//);

//当前使用的轮廓检索模式是cv::RETR_TREE
//cv::RETR_EXTERNAL：表示只提取最外面的轮廓；
//cv::RETR_LIST：表示提取所有轮廓并将其放入列表；
//cv::RETR_CCOMP:表示提取所有轮廓并将组织成一个两层结构，其中顶层轮廓是外部轮廓，第二层轮廓是“洞”的轮廓；
//cv::RETR_TREE：表示提取所有轮廓并组织成轮廓嵌套的完整层级结构。
//---------------------------------------------------------------------------------------------
//void GaussianBlur(InputArray src, OutputArray dst, Size ksize, double sigmaX, double sigmaY=0, int borderType=BORDER_DEFAULT ) ;
//功能：对输入的图像src进行高斯滤波后用dst输出。
//参数：src和dst当然分别是输入图像和输出图像。Ksize为高斯滤波器模板大小，sigmaX和sigmaY分别为高斯滤波在横线和竖向的滤波系数。borderType为边缘扩展点插值类型。

void getKeyPoints(cv::Mat& probMap,double threshold,std::vector<KeyPoint>& keyPoints){
    //对于每个关键点，我们将阈值应用于置信度图(在本例中为0.1)
    cv::Mat smoothProbMap;
    cv::GaussianBlur( probMap, smoothProbMap, cv::Size( 3, 3 ), 0, 0 );

    cv::Mat maskedProbMap;
    //去噪声 https://docs.opencv.org/4.2.0/d7/d1b/group__imgproc__misc.html#gae8a4a146d1ca78c626a53577199e9c57
    cv::threshold(smoothProbMap,maskedProbMap,threshold,255,cv::THRESH_BINARY);

    maskedProbMap.convertTo(maskedProbMap,CV_8U,1);

    //-显示3个不同类型的mat
    cv::imshow("probMap", probMap);
    cv::imshow("smoothProbMap", smoothProbMap);
    cv::imshow("maskedProbMap", maskedProbMap);

    //关键点区域    keypoint region
    //找出对应于关键点的所有区域的轮廓(contour)
    std::vector<std::vector<cv::Point> > contours;
    cv::findContours(maskedProbMap,contours,cv::RETR_TREE,cv::CHAIN_APPROX_SIMPLE);
    std::cout<<contours;
    output_draw_contours(contours);
    //对于每个关键点轮廓区域，找到最大值.
    for(size_t i = 0; i < contours.size();++i){
        cv::Mat blobMask = cv::Mat::zeros(smoothProbMap.rows,smoothProbMap.cols,smoothProbMap.type());

        //填充凸多边形,只需要提供凸多边形的顶点
        cv::fillConvexPoly(blobMask,contours[i],cv::Scalar(1));


        double maxVal;
        cv::Point maxLoc;
        //提取关键点区域的局部最大值,与之前目标检测相似
        // MatExpr cv::Mat::mul 	( 	InputArray  	m,double  	scale = 1) 		const

        //        Performs an element-wise multiplication or division of the two matrices.
        //        The method returns a temporary object encoding per-element array multiplication, with optional scale.
        //                Note that this is not a matrix multiplication that corresponds to a simpler "\*" operator.
        //https://docs.opencv.org/4.2.0/d3/d63/classcv_1_1Mat.html#a385c09827713dc3e6d713bfad8460706
        //        执行两个矩阵的逐元素乘法或除法。

        //        该方法返回编码每个元素数组乘法的临时对象，具有可选的小数位数位数。请注意，这不是与更简单的“\*”运算符相对应的矩阵乘法。
        cv::minMaxLoc(smoothProbMap.mul(blobMask),0,&maxVal,0,&maxLoc);

        //我们为每一个关键点存储坐标(x,y),置信度分数
        //maxLoc 是坐标
        keyPoints.push_back(KeyPoint(maxLoc, probMap.at<float>(maxLoc.y,maxLoc.x)));
    }
}

void populateColorPalette(std::vector<cv::Scalar>& colors,int nColors){
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> dis1(64, 200);
    std::uniform_int_distribution<> dis2(100, 255);
    std::uniform_int_distribution<> dis3(100, 255);

    for(int i = 0; i < nColors;++i){
        colors.push_back(cv::Scalar(dis1(gen),dis2(gen),dis3(gen)));
    }
}

void splitNetOutputBlobToParts(cv::Mat& netOutputBlob,const cv::Size& targetSize,std::vector<cv::Mat>& netOutputParts){
    //size是指多维矩阵中每一维的大小，nParts是57
    int nParts = netOutputBlob.size[1];

    int h = netOutputBlob.size[2];
    int w = netOutputBlob.size[3];


    for(int i = 0; i< nParts;++i){
        cv::Mat part(h, w, CV_32F, netOutputBlob.ptr(0,i));

        cv::Mat resizedPart;

        //将输出结果的图像大小还原到原来的大小
        cv::resize(part,resizedPart,targetSize);

        netOutputParts.push_back(resizedPart);
    }
}
//a，b两点之间采样，a和b之间连接一条线段，在线上均匀的取numPoints个点
void populateInterpPoints(const cv::Point& a,const cv::Point& b,int numPoints,std::vector<cv::Point>& interpCoords){
    float xStep = ((float)(b.x - a.x))/(float)(numPoints-1);
    float yStep = ((float)(b.y - a.y))/(float)(numPoints-1);

    interpCoords.push_back(a);

    for(int i = 1; i< numPoints-1;++i){
        interpCoords.push_back(cv::Point(a.x + xStep*i,a.y + yStep*i));
    }

    interpCoords.push_back(b);
}

//不同的关键点连接起来就是pair，获取有效的pair
void getValidPairs(const std::vector<cv::Mat>& netOutputParts,
                   const std::vector<std::vector<KeyPoint>>& detectedKeypoints,
                   std::vector<std::vector<ValidPair>>& validPairs,
                   std::set<int>& invalidPairs) {

    int nInterpSamples = 10;//采样个数
    float pafScoreTh = 0.1;
    float confTh = 0.7;
    //mapIdx 大小19,4层循环 能否减少 计算次数
    //mapIdx与posePairs是一一对应的
    for(size_t k = 0; k < mapIdx.size();++k ){

        //A->B constitute a limb
        //Part Affinity Fields
        //从netOutputParts中按照mapIdx的键值对 取出，按key取出的值算作PAF_A，按value取出的值算作一个PAF_B
        //这里first指的key，second指的value
        cv::Mat pafA = netOutputParts[mapIdx[k].first];
        cv::Mat pafB = netOutputParts[mapIdx[k].second];

        //Find the keypoints for the first and second limb
        //查找第一个 limb 和第二个 limb 的关键点位置
        //从detectedKeypoints中按照 posePairs的键值对 取出,按key取出的值算作CAND_A，按value取出的值算作一个CAND_B
        //CAND_A和CAND_B连起来就是 candidate limb
        const std::vector<KeyPoint>& candA = detectedKeypoints[posePairs[k].first];
        const std::vector<KeyPoint>& candB = detectedKeypoints[posePairs[k].second];

        //candA和candB 因为检测结果，有相等的情况，也有不相等的情况
        int nA = candA.size();
        int nB = candB.size();

        /*
          # If keypoints for the joint-pair is detected
          # check every joint in candA with every joint in candB
          # Calculate the distance vector between the two joints
          # Find the PAF values at a set of interpolated points between the joints
          # Use the above formula to compute a score to mark the connection valid
         */
        /*
如果检测到 joint-pair 的关键点位置，则，
检查 candA 和 candB 中每个 joint.
计算两个 joints 之间的距离向量(distance vector).
计算两个 joints 之间插值点集合的 PAF 值.
使用论文里的公式，计算 score 值，判断连接的有效性.

 */

        if(nA != 0 && nB != 0){
            std::vector<ValidPair> localValidPairs;

            for(int i = 0; i< nA;++i){
                int maxJ = -1;
                float maxScore = -1;
                bool found = false;

                for(int j = 0; j < nB;++j){
                    std::pair<float,float> distance(candB[j].point.x - candA[i].point.x,candB[j].point.y - candA[i].point.y);

                    float norm = std::sqrt(distance.first*distance.first + distance.second*distance.second);

                    if(!norm){
                        continue;
                    }

                    distance.first /= norm;
                    distance.second /= norm;

                    //Find p(u)
                    std::vector<cv::Point> interpCoords; //A，B两点之间采样，A和B之间连接一条线段，在线上均匀的取nInterpSamples=10个点
                    populateInterpPoints(candA[i].point,candB[j].point,nInterpSamples,interpCoords);
                    //Find L(p(u))　　pafA，pafB 存储了模型输出的值
                    std::vector<std::pair<float,float>> pafInterp;
                    for(size_t l = 0; l < interpCoords.size();++l){
                        pafInterp.push_back(
                                    std::pair<float,float>(
                                        pafA.at<float>(interpCoords[l].y,interpCoords[l].x),
                                        pafB.at<float>(interpCoords[l].y,interpCoords[l].x)
                                        ));
                    }
                    //计算点积得到相似度
                    std::vector<float> pafScores;
                    float sumOfPafScores = 0;
                    int numOverTh = 0;
                    for(size_t l = 0; l< pafInterp.size();++l){
                        float score = pafInterp[l].first*distance.first + pafInterp[l].second*distance.second;
                        sumOfPafScores += score;
                        if(score > pafScoreTh){
                            ++numOverTh;
                        }

                        pafScores.push_back(score);
                    }
                    //计算相似度的平均值
                    float avgPafScore = sumOfPafScores/((float)pafInterp.size());

                    //选最大的
                    if(((float)numOverTh)/((float)nInterpSamples) > confTh){
                        if(avgPafScore > maxScore){
                            maxJ = j;
                            maxScore = avgPafScore;
                            found = true;
                        }
                    }

                }/* j */

                if(found){
                    localValidPairs.push_back(ValidPair(candA[i].id,candB[maxJ].id,maxScore));
                }

            }/* i */

            validPairs.push_back(localValidPairs);

        } else {
            invalidPairs.insert(k);
            validPairs.push_back(std::vector<ValidPair>());
        }
    }/* k */
}

//获取属于每个人的关键点集合
//先把关键点两个两个的连接起来，成为一对对的，再把一对对的关键点组成了人体的姿态
void getPersonwiseKeypoints(const std::vector<std::vector<ValidPair>>& validPairs,
                            const std::set<int>& invalidPairs,
                            std::vector<std::vector<int>>& personwiseKeypoints) {
    for(size_t k = 0; k < mapIdx.size();++k){
        if(invalidPairs.find(k) != invalidPairs.end()){
            continue;
        }

        const std::vector<ValidPair>& localValidPairs(validPairs[k]);

        int indexA(posePairs[k].first);
        int indexB(posePairs[k].second);

        for(size_t i = 0; i< localValidPairs.size();++i){
            bool found = false;
            int personIdx = -1;

            for(size_t j = 0; !found && j < personwiseKeypoints.size();++j){
                if(indexA < static_cast<int>( personwiseKeypoints[j].size()) &&
                        personwiseKeypoints[j][indexA] == localValidPairs[i].aId){
                    personIdx = j;
                    found = true;
                }
            }/* j */

            if(found){
                personwiseKeypoints[personIdx].at(indexB) = localValidPairs[i].bId;
            } else if(k < 17){
                std::vector<int> lpkp(std::vector<int>(18,-1));

                lpkp.at(indexA) = localValidPairs[i].aId;
                lpkp.at(indexB) = localValidPairs[i].bId;

                personwiseKeypoints.push_back(lpkp);
            }

        }/* i */
    }/* k */
}


int main(int argc,char** argv) {
    std::string inputFile = "2.jpg";

    if(argc > 1){
        inputFile = std::string(argv[1]);
    }

    cv::Mat input = cv::imread(inputFile, cv::IMREAD_COLOR);
    //-----------------------------------------


    std::chrono::time_point<std::chrono::system_clock> startTP = std::chrono::system_clock::now();
    //加载模型
    cv::dnn::Net inputNet = cv::dnn::readNetFromCaffe("openpose_pose_coco_multi_person.prototxt","pose_iter_440000.caffemodel");

    //输入的height是固定的368，根据长宽比计算输入的width.

    cv::Mat inputBlob = cv::dnn::blobFromImage(input,1.0/255.0,cv::Size((int)((368*input.cols)/input.rows),368),cv::Scalar(0,0,0),false,false);

    inputNet.setInput(inputBlob);

    cv::Mat netOutputBlob = inputNet.forward();



    //netOutputBlob为4维矩阵：
    //    第一个维度 忽略
    //    第二个维度 对于COCO模型，它由57个部分组成,size[1]
    //    18个关键点置信度图(confidence Map)+1个背景+19*2个Part Affinity Map。
    //    18+1+38=57
    //    第三个维度是输出图像的高度。row size[2]
    //    第四个维度是输出图像的宽度。col size[3]

    std::vector<cv::Mat> netOutputParts;//一维,大小是57

    //参数是原始图片的大小，不是resize后的大小 ,要把57个部分 分开
    splitNetOutputBlobToParts(netOutputBlob,cv::Size(input.cols,input.rows),netOutputParts);


    std::chrono::time_point<std::chrono::system_clock> finishTP = std::chrono::system_clock::now();

    std::cout << "Time Taken in forward pass = " << std::chrono::duration_cast<std::chrono::milliseconds>(finishTP - startTP).count() << " ms" << std::endl;

    int keyPointId = 0;
    //如果设计接口的输出 可以设计下面的数据结构 1（接口要输出part 和 pair）
    //detectedKeypoints,keyPointsList存储相同的内容，一个按二维存，一个是按一维存
    std::vector<std::vector<KeyPoint>> detectedKeypoints;//二维，存储了所有人的所有关键点
    std::vector<KeyPoint> keyPointsList;//一维,为了在两个关键点之间画线，容易将keypoint取出来，所以又定义了一个keyPointsList


    g_test_output_frame = input.clone();
    for(int i = 0; i < kPoints;++i){
        std::vector<KeyPoint> keyPoints;
        // 0.1是阈值
        //-----------------------
        //        cv::Mat output_part = netOutputParts[i].clone();//用于展示每个part 分析代码用
        //        cv::imshow("netOutputParts", output_part);
        //        cv::waitKey(0);
        //------------------------------------------
        getKeyPoints(netOutputParts[i],0.1,keyPoints);

        //std::cout << "Keypoints - " << keypointsMapping[i] << " : " << keyPoints << std::endl;
        //以输出左耳为例
        // 只有一个人的时候
        //Keypoints - L-Ear : [ Id:0, Point:[281, 89], Prob:0.784812]

        // 多个人的时候
        //        Keypoints - L-Ear :
        //        [ Id:88, Point:[335, 175], Prob:0.62902
        //        , Id:89, Point:[189, 175], Prob:0.590987
        //        , Id:90, Point:[260, 175], Prob:0.714218
        //        , Id:91, Point:[76, 173], Prob:0.573334
        //        , Id:92, Point:[125, 167], Prob:0.353028
        //        , Id:93, Point:[455, 159], Prob:0.258485
        //        ]

        // 这样依次输出18个关键点

        for(size_t i = 0; i< keyPoints.size();++i,++keyPointId){
            keyPoints[i].id = keyPointId;
        }
        std::cout << "Keypoints - " << keypointsMapping[i] << " : " << keyPoints << std::endl;

        //id是这样，从0开始，依次向后累加，假设一共6个人，检测出了3个鼻子，就是0,1,2，那么脖子的id就是从3开始。
        //而不是从6开始，不需要给6个人的鼻子都保留位置。
        detectedKeypoints.push_back(keyPoints);
        keyPointsList.insert(keyPointsList.end(),keyPoints.begin(),keyPoints.end());
    }

    //填充调色板
    std::vector<cv::Scalar> colors;
    populateColorPalette(colors,kPoints);

    cv::Mat outputFrame = input.clone();

    //将图片上所有关键点keypoint都画出来，
    for(int i = 0; i < kPoints;++i){
        for(size_t j = 0; j < detectedKeypoints[i].size();++j){
            cv::circle(outputFrame,detectedKeypoints[i][j].point,10,colors[i],-1,cv::LINE_AA);
        }
    }
    // 此时只是画出 keypoint,pair还没计算




    std::vector<std::vector<ValidPair>> validPairs;
    std::set<int> invalidPairs;
    getValidPairs(netOutputParts,detectedKeypoints,validPairs,invalidPairs);

    //一个point，有xy；两个point就可以连接成pair。
    //Nose 1分别与Neck 2 ，LAnkle 5连接 {1,2}, {1,5}
    //这里只能按照确定的一个方向去连接，例如nose 连接到 neck，而不是反方向的neck连接到nose
    std::vector<std::vector<int>> personwiseKeypoints;//二维

    //所有的pair，分配到不同的人
    getPersonwiseKeypoints(validPairs,invalidPairs,personwiseKeypoints);




    //如果设计接口的输出 可以设计下面的数据结构 2
    //假设是一个人,18关键点没有遮挡全部检测出来 personwiseKeypoints结果就是（这里是关键点的id）
    //[ [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17]]

    //假设是两个人，下面的 -1表示关键点没检测出来或者关键点没有连接到其他的关键点
    //一行表示一个人的的关键点的id，该关键点是可以连接到其他关键点的
    //    [ [ 0, 2, 4, -1, -1, 8, -1, -1, -1, -1, -1, -1, -1, -1, 10, -1, 12, -1],
    //    [ 1, 3, 5, -1, -1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, 11, -1, 13]]

    //人多一点就可以看的清楚一些，personwiseKeypoints的结果
    //    [
    //    [ -1, 3, 9, 17, 20, 26, 31, 36, 41, 48, 54, 59, 66, 73, -1, -1, -1, -1],
    //    [ -1, 4, 11, 16, 22, 27, 33, 38, 43, 50, 56, 61, 68, 74, -1, -1, -1, -1],
    //    [ -1, 5, 10, 15, 21, 28, 32, 37, 42, 49, 55, 62, 67, 72, -1, -1, -1, -1],
    //    [ 1, 6, 13, 19, 25, -1, -1, -1, 45, 51, 58, 63, 69, 76, 78, 80, -1, 91],
    //    [ 2, 7, 12, 17, -1, 29, 34, 39, 44, 47, 53, 60, 65, 71, 79, 81, 87, 93],
    //    [ -1, 8, 14, 18, 23, 30, 35, 40, 46, 52, 57, 64, 70, 75, -1, -1, -1, -1],
    //    [ 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 77, -1, 86, -1]]



    //下面是将每两个点之间画上线，按照posePairs的定义去画，是确定的哪个关键点可以连接哪个关键点
    for(int i = 0; i< kPoints-1;++i){
        for(size_t n  = 0; n < personwiseKeypoints.size();++n){
            const std::pair<int,int>& posePair = posePairs[i];
            int indexA = personwiseKeypoints[n][posePair.first];
            int indexB = personwiseKeypoints[n][posePair.second];

            //看personwiseKeypoints的值，就知道要判断-1的情况
            if(indexA == -1 || indexB == -1){
                continue;
            }

            const KeyPoint& kpA = keyPointsList[indexA];
            const KeyPoint& kpB = keyPointsList[indexB];

            cv::line(outputFrame,kpA.point,kpB.point,colors[i],3,cv::LINE_AA);

        }
    }

    cv::imshow("Detected Pose",outputFrame);
    cv::waitKey(0);

    return 0;
}