change perception_camera_yolo.

src/api/perception_camera_yolo/main.cpp

@@ -22,6 +22,7 @@ extern "C"
 #include <mutex>
 #include <condition_variable>
 
+image ** gpalphabet;
 network * gpnet;
 static void * gpa;
 
@@ -32,142 +33,16 @@ std::mutex gmutexcv;
 std::condition_variable gcv;
 bool gbrun = true;
 
+char ** names;
 
-namespace  iv {
-
-float colors[6][3] = { {1,0,1}, {0,0,1},{0,1,1},{0,1,0},{1,1,0},{1,0,0} };
+int gnclasses = 80;
+double gfthresh = 0.5;
 
-float get_color(int c, int x, int max)
-{
-    float ratio = ((float)x/max)*5;
-    int i = floor(ratio);
-    int j = ceil(ratio);
-    ratio -= i;
-    float r = (1-ratio) * colors[i][c] + ratio*colors[j][c];
-    //printf("%f\n", r);
-    return r;
-}
-
-static float get_pixel(image m, int x, int y, int c)
-{
-    assert(x < m.w && y < m.h && c < m.c);
-    return m.data[c*m.h*m.w + y*m.w + x];
-}
 
-static void set_pixel(image m, int x, int y, int c, float val)
-{
-    if (x < 0 || y < 0 || c < 0 || x >= m.w || y >= m.h || c >= m.c) return;
-    assert(x < m.w && y < m.h && c < m.c);
-    m.data[c*m.h*m.w + y*m.w + x] = val;
-}
-
-void embed_image(image source, image dest, int dx, int dy)
-{
-    int x,y,k;
-    for(k = 0; k < source.c; ++k){
-        for(y = 0; y < source.h; ++y){
-            for(x = 0; x < source.w; ++x){
-                float val = get_pixel(source, x,y,k);
-                set_pixel(dest, dx+x, dy+y, k, val);
-            }
-        }
-    }
-}
-
-void draw_detections1(image  im, detection *dets, int num, float thresh, char **names, image **alphabet, int classes)
-{
-    int i,j;
-
-    for(i = 0; i < num; ++i){
-        char labelstr[4096] = {0};
-        int classnum = -1;
-        for(j = 0; j < classes; ++j){
-            if (dets[i].prob[j] > thresh){
-                if (classnum < 0) {
-                    if(names != nullptr)
-                        strcat(labelstr, names[j]);
-                    else
-                    {
-                        char strtem[100];
-                        snprintf(strtem,4096,"id%02d",j);
-                        strcat(labelstr, strtem);
-                    }
-                    classnum = j;
-                } else {
-                    if(names != nullptr){
-                        strcat(labelstr, ", ");
-                        strcat(labelstr, names[j]);
-                    }
-                    else
-                    {
-                        strcat(labelstr, ", ");
-                        char strtem[100];
-                        snprintf(strtem,4096,"id%02d",j);
-                        strcat(labelstr, strtem);
-                    }
-                }
-                if(names != nullptr)
-                    printf("%s: %.0f%%\n", names[j], dets[i].prob[j]*100);
-                else
-                    printf("id%02d: %.0f%%\n", j, dets[i].prob[j]*100);
-            }
-        }
-        if(classnum >= 0){
-            int width = im.h * .006;
-
-            /*
-               if(0){
-               width = pow(prob, 1./2.)*10+1;
-               alphabet = 0;
-               }
-             */
-
-            //printf("%d %s: %.0f%%\n", i, names[class], prob*100);
-            int offset = classnum *123457 % classes;
-            float red = get_color(2,offset,classes);
-            float green = get_color(1,offset,classes);
-            float blue = get_color(0,offset,classes);
-            float rgb[3];
-
-            //width = prob*20+2;
-
-            rgb[0] = red;
-            rgb[1] = green;
-            rgb[2] = blue;
-            box b = dets[i].bbox;
-            //printf("%f %f %f %f\n", b.x, b.y, b.w, b.h);
-
-            int left  = (b.x-b.w/2.)*im.w;
-            int right = (b.x+b.w/2.)*im.w;
-            int top   = (b.y-b.h/2.)*im.h;
-            int bot   = (b.y+b.h/2.)*im.h;
-
-            if(left < 0) left = 0;
-            if(right > im.w-1) right = im.w-1;
-            if(top < 0) top = 0;
-            if(bot > im.h-1) bot = im.h-1;
-
-            draw_box_width(im, left, top, right, bot, width, red, green, blue);
-            if (alphabet) {
-                image label = get_label(alphabet, labelstr, (im.h*.03));
-                draw_label(im, top + width, left, label, rgb);
-                free_image(label);
-            }
-            if (dets[i].mask){
-                image mask = float_to_image(14, 14, 1, dets[i].mask);
-                image resized_mask = resize_image(mask, b.w*im.w, b.h*im.h);
-                image tmask = threshold_image(resized_mask, .5);
-                embed_image(tmask, im, left, top);
-                free_image(mask);
-                free_image(resized_mask);
-                free_image(tmask);
-            }
-        }
-    }
-}
+namespace  iv {
 
 
-void dodetec(cv::Mat mat)
+image mat_to_image(cv::Mat mat)
 {
     IplImage ipl = cvIplImage(mat);
 
@@ -195,10 +70,52 @@ void dodetec(cv::Mat mat)
         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();
@@ -206,16 +123,28 @@ void dodetec(cv::Mat mat)
     printf("Predicted in %f seconds.\n", what_time_is_it_now()-time);
 
     int nboxes = 0;
-    float thresh = 0.5;
-    detection *dets = get_network_boxes(gpnet, im.w, im.h, 0.5, 0.5, 0, 1, &nboxes);
+    float thresh = gfthresh;
+    detection *dets = get_network_boxes(gpnet, im.w, im.h, thresh,thresh, 0, 1, &nboxes);
+
+    float nms=.45;
 
-    draw_detections1(im, dets, nboxes, thresh, nullptr, nullptr, l.classes);
+    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;
 }
 
@@ -288,6 +217,10 @@ int main(int argc, char *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;