nanodet_hand.cpp

// Tencent is pleased to support the open source community by making ncnn available.
//
// Copyright (C) 2020 THL A29 Limited, a Tencent company. All rights reserved.
//
// Licensed under the BSD 3-Clause License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// https://opensource.org/licenses/BSD-3-Clause
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.

// modifies 4-4-2021 Q-engineering

#include "net.h"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdlib.h>
#include <float.h>
#include <stdio.h>
#include <vector>
#include <iostream>

ncnn::Net nanodet;
ncnn::Net nanopnt;

const int target_size = 320;
const float prob_threshold = 0.4f;
const float nms_threshold = 0.5f;
const float mean_vals[3] = {103.53f, 116.28f, 123.675f};
const float norm_vals[3] = {0.017429f, 0.017507f, 0.017125f};

struct Object
{
    cv::Rect_<float> rect;
    std::vector<cv::Point2f> pts;
    int label;
    float prob;
};

static inline float intersection_area(const Object& a, const Object& b)
{
    cv::Rect_<float> inter = a.rect & b.rect;
    return inter.area();
}

static void qsort_descent_inplace(std::vector<Object>& faceobjects, int left, int right)
{
    int i = left;
    int j = right;
    float p = faceobjects[(left + right) / 2].prob;

    while (i <= j)
    {
        while (faceobjects[i].prob > p)
            i++;

        while (faceobjects[j].prob < p)
            j--;

        if (i <= j)
        {
            // swap
            std::swap(faceobjects[i], faceobjects[j]);

            i++;
            j--;
        }
    }

    #pragma omp parallel sections
    {
        #pragma omp section
        {
            if (left < j) qsort_descent_inplace(faceobjects, left, j);
        }
        #pragma omp section
        {
            if (i < right) qsort_descent_inplace(faceobjects, i, right);
        }
    }
}

static void qsort_descent_inplace(std::vector<Object>& faceobjects)
{
    if (faceobjects.empty())
        return;

    qsort_descent_inplace(faceobjects, 0, faceobjects.size() - 1);
}

static void nms_sorted_bboxes(const std::vector<Object>& faceobjects, std::vector<int>& picked, float nms_threshold)
{
    picked.clear();

    const int n = faceobjects.size();

    std::vector<float> areas(n);
    for (int i = 0; i < n; i++)
    {
        areas[i] = faceobjects[i].rect.width * faceobjects[i].rect.height;
    }

    for (int i = 0; i < n; i++)
    {
        const Object& a = faceobjects[i];

        int keep = 1;
        for (int j = 0; j < (int)picked.size(); j++)
        {
            const Object& b = faceobjects[picked[j]];

            // intersection over union
            float inter_area = intersection_area(a, b);
            float union_area = areas[i] + areas[picked[j]] - inter_area;
            // float IoU = inter_area / union_area
            if (inter_area / union_area > nms_threshold)
                keep = 0;
        }

        if (keep)
            picked.push_back(i);
    }
}

static void generate_proposals(const ncnn::Mat& cls_pred, const ncnn::Mat& dis_pred, int stride, const ncnn::Mat& in_pad, float prob_threshold, std::vector<Object>& objects)
{
    const int num_grid = cls_pred.h;

    int num_grid_x;
    int num_grid_y;
    if (in_pad.w > in_pad.h)
    {
        num_grid_x = in_pad.w / stride;
        num_grid_y = num_grid / num_grid_x;
    }
    else
    {
        num_grid_y = in_pad.h / stride;
        num_grid_x = num_grid / num_grid_y;
    }

    const int num_class = cls_pred.w;
    const int reg_max_1 = dis_pred.w / 4;

    for (int i = 0; i < num_grid_y; i++)
    {
        for (int j = 0; j < num_grid_x; j++)
        {
            const int idx = i * num_grid_x + j;

            const float* scores = cls_pred.row(idx);

            // find label with max score
            int label = -1;
            float score = -FLT_MAX;
            for (int k = 0; k < num_class; k++)
            {
                if (scores[k] > score)
                {
                    label = k;
                    score = scores[k];
                }
            }

            if (score >= prob_threshold)
            {
                ncnn::Mat bbox_pred(reg_max_1, 4, (void*)dis_pred.row(idx));
                {
                    ncnn::Layer* softmax = ncnn::create_layer("Softmax");

                    ncnn::ParamDict pd;
                    pd.set(0, 1); // axis
                    pd.set(1, 1);
                    softmax->load_param(pd);

                    ncnn::Option opt;
                    opt.num_threads = 1;
                    opt.use_packing_layout = false;

                    softmax->create_pipeline(opt);

                    softmax->forward_inplace(bbox_pred, opt);

                    softmax->destroy_pipeline(opt);

                    delete softmax;
                }

                float pred_ltrb[4];
                for (int k = 0; k < 4; k++)
                {
                    float dis = 0.f;
                    const float* dis_after_sm = bbox_pred.row(k);
                    for (int l = 0; l < reg_max_1; l++)
                    {
                        dis += l * dis_after_sm[l];
                    }

                    pred_ltrb[k] = dis * stride;
                }

                float pb_cx = (j + 0.5f) * stride;
                float pb_cy = (i + 0.5f) * stride;

                float x0 = pb_cx - pred_ltrb[0];
                float y0 = pb_cy - pred_ltrb[1];
                float x1 = pb_cx + pred_ltrb[2];
                float y1 = pb_cy + pred_ltrb[3];

                Object obj;
                obj.rect.x = x0;
                obj.rect.y = y0;
                obj.rect.width = x1 - x0;
                obj.rect.height = y1 - y0;
                obj.label = label;
                obj.prob = score;

                objects.push_back(obj);
            }
        }
    }
}

static int detect_nanodet(const cv::Mat& bgr, std::vector<Object>& objects)
{
    int width = bgr.cols;
    int height = bgr.rows;

    // pad to multiple of 32
    int w = width;
    int h = height;
    float scale = 1.f;
    if (w > h)
    {
        scale = (float)target_size / w;
        w = target_size;
        h = h * scale;
    }
    else
    {
        scale = (float)target_size / h;
        h = target_size;
        w = w * scale;
    }

    ncnn::Mat in = ncnn::Mat::from_pixels_resize(bgr.data, ncnn::Mat::PIXEL_BGR, width, height, w, h);

    // pad to target_size rectangle
    int wpad = (w + 31) / 32 * 32 - w;
    int hpad = (h + 31) / 32 * 32 - h;
    ncnn::Mat in_pad;
    ncnn::copy_make_border(in, in_pad, hpad / 2, hpad - hpad / 2, wpad / 2, wpad - wpad / 2, ncnn::BORDER_CONSTANT, 0.f);

    in_pad.substract_mean_normalize(mean_vals, norm_vals);

    ncnn::Extractor ex = nanodet.create_extractor();

    ex.input("input.1", in_pad);

    std::vector<Object> proposals;

    // stride 8
    {
        ncnn::Mat cls_pred;
        ncnn::Mat dis_pred;
        ex.extract("cls_pred_stride_8", cls_pred);
        ex.extract("dis_pred_stride_8", dis_pred);

        std::vector<Object> objects8;
        generate_proposals(cls_pred, dis_pred, 8, in_pad, prob_threshold, objects8);

        proposals.insert(proposals.end(), objects8.begin(), objects8.end());
    }

    // stride 16
    {
        ncnn::Mat cls_pred;
        ncnn::Mat dis_pred;
        ex.extract("cls_pred_stride_16", cls_pred);
        ex.extract("dis_pred_stride_16", dis_pred);

        std::vector<Object> objects16;
        generate_proposals(cls_pred, dis_pred, 16, in_pad, prob_threshold, objects16);

        proposals.insert(proposals.end(), objects16.begin(), objects16.end());
    }

    // stride 32
    {
        ncnn::Mat cls_pred;
        ncnn::Mat dis_pred;
        ex.extract("cls_pred_stride_32", cls_pred);
        ex.extract("dis_pred_stride_32", dis_pred);

        std::vector<Object> objects32;
        generate_proposals(cls_pred, dis_pred, 32, in_pad, prob_threshold, objects32);

        proposals.insert(proposals.end(), objects32.begin(), objects32.end());
    }

    // sort all proposals by score from highest to lowest
    qsort_descent_inplace(proposals);

    // apply nms with nms_threshold
    std::vector<int> picked;
    nms_sorted_bboxes(proposals, picked, nms_threshold);

    int count = picked.size();

    objects.resize(count);
    for (int i = 0; i < count; i++)
    {
        objects[i] = proposals[picked[i]];

        // adjust offset to original unpadded
        float x0 = (objects[i].rect.x - (wpad / 2)) / scale;
        float y0 = (objects[i].rect.y - (hpad / 2)) / scale;
        float x1 = (objects[i].rect.x + objects[i].rect.width - (wpad / 2)) / scale;
        float y1 = (objects[i].rect.y + objects[i].rect.height - (hpad / 2)) / scale;

        // clip
        x0 = std::max(std::min(x0, (float)(width - 1)), 0.f);
        y0 = std::max(std::min(y0, (float)(height - 1)), 0.f);
        x1 = std::max(std::min(x1, (float)(width - 1)), 0.f);
        y1 = std::max(std::min(y1, (float)(height - 1)), 0.f);

        objects[i].rect.x = x0;
        objects[i].rect.y = y0;
        objects[i].rect.width = x1 - x0;
        objects[i].rect.height = y1 - y0;
        {
            int xx = x0;
            int yy = y0;
            int ww = x1-x0;
            int hh = y1-y0;
            //detect pts

            cv::Mat input = bgr(cv::Rect(xx,yy,ww,hh)).clone();
            int target_size = 224;
            int w = input.cols;
            int h = input.rows;
            float scale = 1.f;
            if (w > h)
            {
                scale = (float)target_size / w;
                w = target_size;
                h = h * scale;
            }
            else
            {
                scale = (float)target_size / h;
                h = target_size;
                w = w * scale;
            }

            ncnn::Mat in = ncnn::Mat::from_pixels_resize(input.data, ncnn::Mat::PIXEL_RGB, input.cols, input.rows, w, h);
            int wpad = target_size - w;
            int hpad = target_size - h;
            ncnn::Mat in_pad;
            ncnn::copy_make_border(in, in_pad, hpad / 2, hpad - hpad / 2, wpad / 2, wpad - wpad / 2, ncnn::BORDER_CONSTANT, 0.f);

            const float norm_vals[3] = { 1 / 255.f, 1 / 255.f, 1 / 255.f };
            in_pad.substract_mean_normalize(0, norm_vals);
            ncnn::Mat points,score;
            ncnn::Extractor ex = nanopnt.create_extractor();
            ex.input("input", in_pad);
            ex.extract("points", points);
            ex.extract("score",score);

            float* points_data = (float*)points.data;

            for (int j = 0; j < 21; j++){
                cv::Point2f pt;
                pt.x = (points_data[j * 3] - (wpad / 2)) / scale + (float)xx;
                pt.y = (points_data[j * 3 + 1]- (hpad / 2)) / scale + (float)yy;

                objects[i].pts.push_back(pt);
            }
        }
    }

    return 0;
}

static void draw_objects(cv::Mat& rgb, const std::vector<Object>& objects)
{
    for (size_t i = 0; i < objects.size(); i++){
        const Object& obj = objects[i];
        cv::Scalar color1(10, 215, 255);
        cv::Scalar color2(255, 115, 55);
        cv::Scalar color3(5, 255, 55);
        cv::Scalar color4(25, 15, 255);
        cv::Scalar color5(225, 15, 55);
        for(size_t j = 0; j < obj.pts.size(); j++){
            cv::circle(rgb, obj.pts[j],4,cv::Scalar(255,0,0),-1);
            if (j < 4)            cv::line(rgb, obj.pts[j], obj.pts[j+1], color1, 2, 8);
            if (j < 8 && j > 4)   cv::line(rgb, obj.pts[j], obj.pts[j+1], color2, 2, 8);
            if (j < 12 && j > 8)  cv::line(rgb, obj.pts[j], obj.pts[j+1], color3, 2, 8);
            if (j < 16 && j > 12) cv::line(rgb, obj.pts[j], obj.pts[j+1], color4, 2, 8);
            if (j < 20 && j > 16) cv::line(rgb, obj.pts[j], obj.pts[j+1], color5, 2, 8);
        }
        cv::line(rgb, obj.pts[0], obj.pts[5], color2, 2, 8);
        cv::line(rgb, obj.pts[0], obj.pts[9], color3, 2, 8);
        cv::line(rgb, obj.pts[0], obj.pts[13], color4, 2, 8);
        cv::line(rgb, obj.pts[0], obj.pts[17], color5, 2, 8);
    }
}

int main(int argc, char** argv)
{
    float f;
    float FPS[16];
    int i,Fcnt=0;
    cv::Mat frame;
    //some timing
    std::chrono::steady_clock::time_point Tbegin, Tend;

    for(i=0;i<16;i++) FPS[i]=0.0;

    nanodet.opt = ncnn::Option();
    nanopnt.opt = ncnn::Option();

    nanodet.opt.num_threads = 4;
    nanopnt.opt.num_threads = 4;

    nanodet.load_param("nanodet-hand.param");
    nanodet.load_model("nanodet-hand.bin");

    nanopnt.load_param("hand_lite-op.param");
    nanopnt.load_model("hand_lite-op.bin");

//    cv::VideoCapture cap("Walkers.mp4");
//    if (!cap.isOpened()) {
//        std::cerr << "ERROR: Unable to open the camera" << std::endl;
//        return 0;
//    }

    std::cout << "Start grabbing, press ESC on Live window to terminate" << std::endl;
	while(1){
        frame=cv::imread("hand.jpg");  //need to refresh frame before dnn class detection
//        cap >> frame;
        if (frame.empty()) {
            std::cerr << "ERROR: Unable to grab from the camera" << std::endl;
            break;
        }

        Tbegin = std::chrono::steady_clock::now();

        std::vector<Object> objects;
        detect_nanodet(frame, objects);
        draw_objects(frame, objects);

        Tend = std::chrono::steady_clock::now();

        //calculate frame rate
        f = std::chrono::duration_cast <std::chrono::milliseconds> (Tend - Tbegin).count();
        if(f>0.0) FPS[((Fcnt++)&0x0F)]=1000.0/f;
        for(f=0.0, i=0;i<16;i++){ f+=FPS[i]; }
        putText(frame, cv::format("FPS %0.2f", f/16),cv::Point(10,20),cv::FONT_HERSHEY_SIMPLEX,0.6, cv::Scalar(0, 0, 255));

        //show output
        imshow("RPi 4 - 1,95 GHz - 2 Mb RAM", frame);
//        cv::imwrite("test.jpg",frame);

        char esc = cv::waitKey(5);
        if(esc == 27) break;
    }

    std::cout << "Closing the camera" << std::endl;
    cv::destroyAllWindows();
    std::cout << "Bye!" << std::endl;

    return 0;
}