Commit 986fe5af authored by Arnaud Blanchard's avatar Arnaud Blanchard

Object tracking

parent c10f528b
Set video format
v4l2-ctl --set-fmt-video=pixelformat=UYVY,width=64,height=64
\ No newline at end of file
# 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})
//
// 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;
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment