Object tracking

README.md

+Set video format
+
+v4l2-ctl --set-fmt-video=pixelformat=UYVY,width=64,height=64
\ No newline at end of file

f_raspinobo_object/CMakeLists.txt

+# Set the minimum version of cmake required to build this project
+cmake_minimum_required(VERSION 2.6)
+
+# Set the name of the project as the directory basename
+project(f_raspinobo_object)
+
+find_package(blc_channel)
+find_package(blc_program)
+
+add_definitions(${BL_DEFINITIONS} -march=native)
+include_directories(${BL_INCLUDE_DIRS} )
+add_executable(f_raspinobo_object f_raspinobo_object.cpp)
+target_link_libraries(f_raspinobo_object ${BL_LIBRARIES})
+
+
+
+

f_raspinobo_object/f_raspinobo_object.cpp

+//
+//  Created by Arnaud Blanchard on 22/12/15.
+//  Copyright ETIS 2015. All rights reserved.
+//
+#include <iostream>
+#include "blc_channel.h"
+#include "blc_program.h"
+#include <unistd.h>
+#include <math.h>
+
+#define DEFAULT_OUTPUT_NAME ":f_arma_detect_color<pid>"
+
+void detect_color(uchar *output, uchar *input, uint32_t format, int width, int height, uchar *u_min, uchar *u_max, uchar *v_min, uchar *v_max, uchar *coordinates){
+    int i,j;
+    int x, y;
+    div_t result;
+    x=128;
+    y=128;
+    j=1;
+        switch (format){
+        /*    case 'YUYV':
+                  BLC_COMMAND_LOOP(0){
+                      j=1;
+                      FOR(i, width*height){
+                          if ((input[1+i*4]>*u_min) && (input[1+i*4]<*u_max) &&  (input[3+i*4]>*v_min) && (input[3+i*4]<*v_min)) {
+                              result=div(i, width);
+                              x+=result.rem;
+                              y+=result.quot;
+                              j++;
+                          }
+                      }
+                      output[0]=x/j;
+                      output[1]=y/j;
+                }
+                break;*/
+            case 'UYVY':
+                BLC_COMMAND_LOOP(0){
+                    FOR(i, width*height/2){
+                        j=1;
+                        if ((input[i*4]>*u_min) && (input[i*4]<*u_max) &&  (input[2+i*4]>*v_min) && (input[2+i*4]<*v_max)) {
+                            output[i]=255;
+                            j++;
+                            result=div(i, width/2);
+                            x+=result.rem*255/(width/2);
+                            y+=result.quot*255/height;
+                        }
+                        else output[i]=0;
+                        coordinates[0]=x/j;
+                        coordinates[1]=y/j;
+                        x=coordinates[0];
+                        y=coordinates[1];
+                    }
+                }
+                break;
+            case 'yuv2':
+              //  u_mat=input_cube.subcube(0, 0, 0, size(1, width/2, height));
+              //  v_mat=input_cube.subcube(2, 0, 0, size(1, width/2, height));
+                break;
+            default: EXIT_ON_ERROR("This format is not managed");
+        }
+  //      output_mat=255*4/(exp(*sensibility*(*u_min-u_mat)/128.)+exp(*sensibility*(u_mat-*u_max)/128.)+exp(*sensibility*(*v_min-v_mat)/128.)+exp(*sensibility*(v_mat-*v_max)/128.)+1);
+    
+}
+
+int main(int argc, char **argv){
+    blc_channel input, output, coordinates, parameters;
+    char const  *output_name, *input_name, *input_parameter;
+    int output_width, output_height;
+    int iwidth, iheight;
+    uchar u_min, u_max, v_min, v_max;
+    uchar *u_min_pt, *u_max_pt, *v_min_pt, *v_max_pt;
+
+    char const* u_min_text, *u_max_text, *v_min_text, *v_max_text, *sensibility_text;
+    float sensibility, *sensibility_pt;
+    
+    blc_program_set_description("Detect color on yuv2 or YUYV image");
+    blc_program_add_option(&output_name, 'o', "output", "blc_channel-out", "channel name", DEFAULT_OUTPUT_NAME);
+    blc_program_add_option(&u_min_text, 'u', "u-min", "integer", "value U min", "128");
+    blc_program_add_option(&u_max_text, 'U', "u-max", "integer", "value U max", "128");
+    blc_program_add_option(&v_min_text, 'v', "v-min", "integer", "value V min", "128");
+    blc_program_add_option(&v_max_text, 'V', "v-max", "integer", "value V max", "128");
+    blc_program_add_option(&sensibility_text, 's', "sensibility", "real", "sensibility of the detection", "1");
+    blc_program_add_option(&input_parameter, 'p', "parameters", "blc_channel-in", "dynamic parameters in a array", NULL);
+    
+    blc_program_add_parameter(&input_name, "blc_channel-in", 1, "image where to detect color", NULL);
+    blc_program_init(&argc, &argv, blc_quit);
+    
+    
+    u_min=strtol(u_min_text, NULL, 10);
+    u_max=strtol(u_max_text, NULL, 10);
+    v_min=strtol(v_min_text, NULL, 10);
+    v_max=strtol(v_max_text, NULL, 10);
+    sensibility=strtof(sensibility_text, NULL);
+    
+    if (input_parameter) {
+        parameters.create_or_open(input_parameter, BLC_CHANNEL_READ, 'UIN8', 'NDEF', 1, 4);
+        u_min_pt=&parameters.uchars[0];
+        u_max_pt=&parameters.uchars[1];
+        v_min_pt=&parameters.uchars[2];
+        v_max_pt=&parameters.uchars[3];
+    }
+    else{
+        u_min_pt=&u_min;
+        u_max_pt=&u_max;
+        v_min_pt=&v_min;
+        v_max_pt=&v_max;
+        sensibility_pt=&sensibility;
+    }
+    
+    input.open(input_name, BLC_CHANNEL_READ);
+    iwidth=input.dims[1].length;
+    iheight=input.dims[2].length;
+    
+    //Eventual synchronization
+    blc_loop_try_add_waiting_semaphore(input.sem_new_data);
+    blc_loop_try_add_posting_semaphore(input.sem_ack_data);
+    
+    if (strcmp(output_name, DEFAULT_OUTPUT_NAME)==0) SYSTEM_ERROR_CHECK(asprintf((char**)&output_name,":f_arma_detect_color%d", getpid()), -1, NULL);
+
+    output.create_or_open(output_name, BLC_CHANNEL_WRITE, 'UIN8', 'Y800', 2, iwidth/2, iheight);
+    coordinates.create_or_open("/pinobo.coord", BLC_CHANNEL_WRITE, 'UIN8', 'NDEF', 1, 2);
+
+    blc_loop_try_add_waiting_semaphore(output.sem_ack_data);
+    blc_loop_try_add_posting_semaphore(output.sem_new_data);
+    output.publish();
+    fprintf(stderr, "%d %d %d %d\n", *u_min_pt, *u_max_pt, *v_min_pt, *v_max_pt);
+
+    detect_color(output.uchars, input.uchars, input.format, iwidth, iheight, u_min_pt, u_max_pt, v_min_pt, v_max_pt, coordinates.uchars);
+    
+    return EXIT_SUCCESS;
+}