modularization/src/api/perception_camera_yolo/main.cpp

#include <QCoreApplication>

#include <iostream>
#include "modulecomm.h"
#include "rawpic.pb.h"

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
//#include "opencv2/imgcodecs/legacy/constants_c.h"
#include <opencv2/imgproc/types_c.h>


extern "C"
{
#include "darknet.h"
}


#include <memory>
#include <thread>
#include <mutex>
#include <condition_variable>

image ** gpalphabet;
network * gpnet;
static void * gpa;

cv::Mat gmat;
bool gbUpdate = false;
std::mutex gmutex;
std::mutex gmutexcv;
std::condition_variable gcv;
bool gbrun = true;

char ** names;

int gnclasses = 80;
double gfthresh = 0.5;


namespace  iv {


image mat_to_image(cv::Mat mat)
{
    IplImage ipl = cvIplImage(mat);

    IplImage * src = &ipl;

    int h = src->height;
    int w = src->width;
    int c = src->nChannels;
    image im = make_image(w, h, c);
    unsigned char *data = (unsigned char *)src->imageData;
    int step = src->widthStep;
    int i, j, k;

    for(i = 0; i < h; ++i){
        for(k= 0; k < c; ++k){
            for(j = 0; j < w; ++j){
                im.data[k*w*h + i*w + j] = data[i*step + j*c + k]/255.;
            }
        }
    }

    for(i = 0; i < im.w*im.h; ++i){
        float swap = im.data[i];
        im.data[i] = im.data[i+im.w*im.h*2];
        im.data[i+im.w*im.h*2] = swap;
    }

    return im;

}


IplImage *image_to_ipl(image im)
{
    int x,y,c;
    IplImage *disp = cvCreateImage(cvSize(im.w,im.h), IPL_DEPTH_8U, im.c);
    int step = disp->widthStep;
    for(y = 0; y < im.h; ++y){
        for(x = 0; x < im.w; ++x){
            for(c= 0; c < im.c; ++c){
                float val = im.data[c*im.h*im.w + y*im.w + x];
                disp->imageData[y*step + x*im.c + c] = (unsigned char)(val*255);
            }
        }
    }
    return disp;
}

cv::Mat image_to_mat(image im)
{
    image copy = copy_image(im);
    constrain_image(copy);
    if(im.c == 3) rgbgr_image(copy);

    IplImage *ipl = image_to_ipl(copy);
    cv::Mat m = cv::cvarrToMat(ipl, true);
    cvReleaseImage(&ipl);
    free_image(copy);
    return m;
}


void dodetec(cv::Mat mat)
{

    image im = iv::mat_to_image(mat);

    image sized = letterbox_image(im, gpnet->w, gpnet->h);

    layer l = gpnet->layers[gpnet->n-1];

    std::cout<<" net layer: "<<gpnet->n<<"class: "<<l.classes<<std::endl;


    float *X = sized.data;
    double time=what_time_is_it_now();
    network_predict(gpnet, X);
    printf("Predicted in %f seconds.\n", what_time_is_it_now()-time);

    int nboxes = 0;
    float thresh = gfthresh;
    detection *dets = get_network_boxes(gpnet, im.w, im.h, thresh,thresh, 0, 1, &nboxes);

    float nms=.45;

    if(nms)do_nms_sort(dets, nboxes, gnclasses, nms);
    draw_detections(im, dets, nboxes, thresh, names, gpalphabet, gnclasses);
    free_detections(dets, nboxes);

    std::cout<<" nbox: "<<nboxes<<std::endl;

#ifdef  SHOW_RESULT
    cv::Mat matres = image_to_mat(im);
    cv::namedWindow("image" + std::to_string(0), cv::WINDOW_NORMAL);
    cv::imshow("image"+std::to_string(0), matres);
    cv::waitKey(1);
#endif

    free_image(im);
    free_image(sized);


    return;
}

}

void threaddetect()
{
    while (gbrun) {
        std::unique_lock<std::mutex> lk(gmutexcv);
        if(gcv.wait_for(lk, std::chrono::milliseconds(3000)) == std::cv_status::timeout)
        {
            lk.unlock();
        }
        else
        {
            lk.unlock();
        }
        if(gbUpdate)
        {
            cv::Mat xMat;
            gmutex.lock();
            xMat = gmat.clone();
            gbUpdate = false;
            gmutex.unlock();

            iv::dodetec(xMat);

        }
    }
}

static void CallDetect(iv::vision::rawpic & xrawpic)
{
    cv::Mat mat(xrawpic.height(),xrawpic.width(),xrawpic.mattype());

    if(xrawpic.type() == 1)
        memcpy(mat.data,xrawpic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
    else
    {
        std::vector<unsigned char> buff(xrawpic.picdata().data(),
                                        xrawpic.picdata().data()+xrawpic.picdata().size());
        mat = cv::imdecode(buff,1);
    }
//    cv::cvtColor(mat, mat, CV_BGR2RGB);
    gmutex.lock();
    gmat = mat.clone();
    gbUpdate = true;
    gmutex.unlock();
    gcv.notify_all();
    //Call Detect Funcion use mat
}

void Listenpic(const char * strdata,const unsigned int nSize,
               const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index; (void)dt;  (void)strmemname;
    iv::vision::rawpic pic;
    if(false == pic.ParseFromArray(strdata,nSize))
    {
        std::cout<<"picview Listenpic fail."<<std::endl;
        return;
    }
    CallDetect(pic);

}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    char * cfgfile = "./cfg/yolov3.cfg";
    char * weightfile = "./yolov3.weights";
    names = get_labels("./data/coco.names");
    gpalphabet = load_alphabet();
    gnclasses = 80;
    gfthresh =  0.5;
    network *net = load_network(cfgfile, weightfile, 0);
    set_batch_network(net, 1);
    gpnet = net;

    gpa = iv::modulecomm::RegisterRecv("image00",Listenpic);

#ifdef  SHOW_RESULT
    threaddetect();
#else
    std::thread * pthreaddet =   new std::thread(threaddetect);
    (void)pthreaddet;
#endif

    return a.exec();
}