void RSEDU_image_processing_OFFBOARD(void * buffer, int matchResult[80][120],int kimg)
{
struct timeval now;
static int counter = 0;
int i,j;
int row,col;
float hsv[3];
float yuv[3];
float rgb[3];
int nx=80, ny=120;
float feature_pps[3][4];
unsigned char matchLookuptable[128][128][128];
float camerapos[3];
float camerayaw;
//matching-Thresholds, global
//Landmarks
int lndmrk_nr = 5,lndmrk_best=0,lndmrk_cnd=0;
float featuredist_best = featdist_thrshld;
float featuredist_cnd = featdist_thrshld;
static lndmrk_t *lndmrks;
static double ldnmrk_pinv[4][4] =
{{-3.093396787626414, 2.082093991671625 ,0, 0.766805472932779},
{2.320047590719810 , 3.438429506246282 , 0 , 0.174895895300416},
{-1.737061273051754 , -2.676977989292088 , 0 , 0.299821534800714},
{2.510410469958359 , -2.843545508625819 , 0 , -0.241522903033909}}; //watch out: dimensions x-y switched compared to image matrices!
static double intrMatrx_inv[3][3] = {{ 0.006551831768882 ,0, -0.550082487527771},
{0, 0.006546559888686, -0.399495805347318},
{0, 0, 1.000000000000000}};
//communication
pixel2_t *image = buffer; /* Picture is a 160x120 pixels, stored in YUV422 interlaced format - TO BE CHECKED */
//time
struct timespec begin, current;
long long start, elapsed, microseconds;
/* set up start time data */
if (clock_gettime(CLOCK_MONOTONIC, &begin)) {
/* Oops, getting clock time failed */
exit(EXIT_FAILURE);
}
/* Start time in nanoseconds */
start = begin.tv_sec*NANOS + begin.tv_nsec;
counter++;
gettimeofday(&now,NULL);
if (counter==1)
{
//setup landmark database
lndmrks = malloc(lndmrk_nr*sizeof(lndmrk_t));
//yellow - not used for localization
lndmrks[0].X = 0.0;
lndmrks[0].Y = 0.0;
lndmrks[0].descr[0] = 70.0; //hue
lndmrks[0].descr[1] = 170.0; //green
lndmrks[0].descr[2] = 100.0; //blue
//green
lndmrks[1].X = 0.0;
lndmrks[1].Y = 0.3;
lndmrks[1].descr[0] = 130.0;
lndmrks[1].descr[1] = 160.0;
lndmrks[1].descr[2] = 110.0;
//pink
lndmrks[2].X = 0.5;
lndmrks[2].Y = 0.28;
lndmrks[2].descr[0] = 345.0;
lndmrks[2].descr[1] = 100.0;
lndmrks[2].descr[2] = 135.0;
//red
lndmrks[3].X = 0.25;
lndmrks[3].Y = 0.20;
lndmrks[3].descr[0] = 25.0;
lndmrks[3].descr[1] = 80.0;
lndmrks[3].descr[2] = 70.0;
//blue
lndmrks[4].X = 0.5;
lndmrks[4].Y = 0.0;
lndmrks[4].descr[0] = 220.0;
lndmrks[4].descr[1] = 120.0;
lndmrks[4].descr[2] = 160.0;
//reset posefile
mkdir("../../DroneExchange/imgs/",S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
mkdir("../../DroneExchange/imgs/processed/",S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
fclose(fopen("../../DroneExchange/imgs/processed/poses.txt","w"));
//load lookuptable for matching process
if (FEAT_NOLOOKUP==0)
{
FILE* data;
if ((data = fopen("../../DroneExchange/params/lookuptable.dat", "rb")) == NULL)
{
printf("Error opening lookupfile \n");
error(0);
}
fread(matchLookuptable, sizeof(matchLookuptable), 1, data);
fclose(data);
}
}
//reset landmark matching data
for (i=0;i<lndmrk_nr;i++)
{
lndmrks[i].n = 0;
lndmrks[i].weights = 0;
lndmrks[i].px = 0;
lndmrks[i].py = 0;
//printf("matches reset!\n");
}
//marker detection
//cycle through image, convert each pixel to hsv, threshold by s and v to find point, match h value to color-closest in "database", save average pixellocation with database-lndmrksID
//reconstruct position (3d pos with yaw that mininimzes errors seen with predicted marker position)
int filtersize = 5; //odd numbered! dummy for potential gaussian filter mask, etc.
int margin = (int)(filtersize-1)/4;
int weight = 1;
if(NULL!=image)
{
for (j=0;j<ny-2*margin;j++)
{
for (i=0;i<nx-2*margin;i++)
{
row = margin+j;
col = margin+i;
yuv[0] = (float)image[nx*row+col].y1;
yuv[1] = (float)image[nx*row+col].u;
yuv[2] = (float)image[nx*row+col].v;
//find best matching landmark
if (FEAT_NOLOOKUP)
{
YUVtoHSV(yuv,rgb,hsv);
//find matching landmark
lndmrk_best = 0;
//check if marker is candidate for colored marker by sat, match and store
if ((hsv[1]>=s_thrshld) && (hsv[2]>=va_thrshld) && ((hsv[1]+hsv[2])>=sva_sum_thrshld))
{
//find closest matching landmark
featuredist_best = featuredist(hsv,rgb,&(lndmrks[lndmrk_best].descr[0]));
for (lndmrk_cnd=1;lndmrk_cnd<lndmrk_nr;lndmrk_cnd++)
{
featuredist_cnd = featuredist(hsv,rgb,&(lndmrks[lndmrk_cnd].descr[0]));
if (featuredist_cnd < featuredist_best)
{
featuredist_best = featuredist_cnd;
lndmrk_best = lndmrk_cnd;
}
}
//update centerpoint of marker if matching succeeded
if (featuredist_best >= featdist_thrshld) //@TODO add more outlier handling like ransac
{
//set invalid (marker 0)
lndmrk_best = 0;
}
}
}
else
//use lookuptable
{
lndmrk_best = (int)matchLookuptable[(int)(yuv[0]/2)][(int)(yuv[1]/2)][(int)(yuv[2]/2)];
}
//Store matched landmarks
//----
if (lndmrk_best>0)
{
//increase weight if close to an existing, highly weighted cluster
if ( (lndmrks[lndmrk_best].weights>15) && (abs((col+1)-lndmrks[lndmrk_best].px)<20) && (abs((row+1)-lndmrks[lndmrk_best].py)<20))
{
weight=8;
}
else {
weight=1;
}
lndmrks[lndmrk_best].px = lndmrks[lndmrk_best].px*lndmrks[lndmrk_best].weights/(lndmrks[lndmrk_best].weights+weight) + (((double)(col+1))*2)*weight/(lndmrks[lndmrk_best].weights+weight);
lndmrks[lndmrk_best].py = lndmrks[lndmrk_best].py*lndmrks[lndmrk_best].weights/(lndmrks[lndmrk_best].weights+weight) + ((double)(row+1))*weight/(lndmrks[lndmrk_best].weights+weight);
//save that one more pixel lndmrks was added to this landmark
lndmrks[lndmrk_best].n += 1;
lndmrks[lndmrk_best].weights += weight;
matchResult[col][row] = lndmrk_best*40+80;
//if (lndmrk_best==0) {matchResult[col][row]=255;} else {matchResult[col][row]=0;}
}
}//end inner for
}//end outer for
printf("Result of matching: \n");
for (i=1;i<lndmrk_nr;i++)
{
printf("Lndmark %i: nr pxls matched: %i, pixelcoordinates: (%f,%f) \n",i,lndmrks[i].n,lndmrks[i].px,lndmrks[i].py);
}
//reconstruct pose
//choose lndmrks with some minimum number of pixels, add them to the feature_pps (ui,vi,1)
int features_valid=0;
int k;
for (k=1;k<lndmrk_nr;k++) //start from 1 as yellow not used!
{
if (lndmrks[k].n>=pxls_ftr_min)
{
features_valid+=1;
feature_pps[0][features_valid-1] = lndmrks[k].px;
feature_pps[1][features_valid-1] = lndmrks[k].py;
feature_pps[2][features_valid-1] = 1.0;
}
}
//reconstruct camera pose (3D position + yaw) //@TODO if want to be flexible with matrix size depending on found features, need to swap dimensions of matrix (first dim can be variable when passing 2D-array as parameter to a function);
if (features_valid==4)
{
reconstructCameraPose(camerapos,&camerayaw,feature_pps,ldnmrk_pinv,intrMatrx_inv);
printf("reconstructed cam pos: (%f, %f, %f,%f)\n",camerapos[0],camerapos[1],camerapos[2],camerayaw*180/3.1415);
FILE * posefile = fopen("../../DroneExchange/imgs/processed/poses.txt","a");
if (posefile == NULL)
{
printf("image_proc(): Error opening pose file! \n");
}
else
{
fprintf(posefile,"%i, %f, %f, %f,%f \n",kimg, camerapos[0],camerapos[1],camerapos[2],camerayaw*180/3.1415);
}
}
else printf("WARNING not enough distinct landmarks (colored markers) found for vision-based pose reconstruction! \n") ;
}
//out time
/* get elapsed time */
if (clock_gettime(CLOCK_MONOTONIC, ¤t)) {
/* getting clock time failed, what now? */
exit(EXIT_FAILURE);
}
/* Elapsed time in nanoseconds */
elapsed = current.tv_sec*NANOS + current.tv_nsec - start;
microseconds = elapsed / 1000 + (elapsed % 1000 >= 500); // round up halves
/* Display time in microseconds or something */
printf("elapsed time %lld \n",microseconds);
}
int main() {
//image variables
char row[80]; /* for reading data */
char in[80]; /* for reading data */
char out[80]; /* for writing data */
float **R; /* Red channel of RGB image */
float **G; /* Green channel of RGB image */
float **B; /* Blue channel of RGB image */
float **Y; /* Y channel of YUV image */
float **U; /* U channel of YUV image */
float **V; /* V channel of YUV image */
float **H; /* H channel of HSV image */
float **S; /* S channel of HSV image */
float **Va; /* Value channel of HSV image */
long nx =0, ny=0; /* image size in x, y direction */
int kimg; /*current image k*/
float yuv[3];
float rgb[3];
float hsv[3];
FILE *inimage, *outimage; /* input file, output file */
unsigned char byte; /* for data conversion */
char filename[20];
char filetype[20];
char ffilename[20]="image.uyvy";
pixel2_t fileimage[80*120];
//helper variables
long i, j; /* loop variables */
int lastImg;
int programMode;
float x_avg = 0;
int hits = 0;
//printf("ID of last image to process ? \n");
//scanf("%i", &lastImg);
/*
for (kimg=6;kimg<=lastImg;kimg=kimg+6)
{
sprintf(filename,"%s%i","../../DroneExchange/imgs/img",kimg);
sprintf(filetype,"%s","bin");
sprintf(ffilename,"%s.%s",filename,filetype);
printf("ffilename: %s \n",ffilename);
//open pgm file and read header
if (filetype[0] == 'p')
{
inimage = fopen(ffilename,"r");
if (inimage = NULL)
{
printf("ERROR: no image %s not found!\n",ffilename);
exit(0);
}
fgets (row, 80, inimage);
fgets (row, 80, inimage);
while (row[0]=='#') fgets(row, 80, inimage);
sscanf (row, "%ld %ld", &nx, &ny);
printf("image size (nx,ny)=(%i,%i) \n",nx,ny);
fgets (row, 80, inimage);
if (nx==160)
printf("WARNING: Image should be similar to 160x120 YUV422 interlaced, u.e. 80x120!\n");
//allocate storage
alloc_matrix (&R, nx+2, ny+2);
alloc_matrix (&G, nx+2, ny+2);
alloc_matrix (&B, nx+2, ny+2);
alloc_matrix (&Y, nx+2, ny+2);
alloc_matrix (&U, nx+2, ny+2);
alloc_matrix (&V, nx+2, ny+2);
alloc_matrix (&H, nx+2, ny+2);
alloc_matrix (&S, nx+2, ny+2);
alloc_matrix (&Va, nx+2, ny+2);
//read image data
for (j=0; j<ny; j++)
for (i=0; i<nx; i++)
{
R[i][j] = (float) getc (inimage);
G[i][j] = (float) getc (inimage);
B[i][j] = (float) getc (inimage);
}
fclose(inimage);
}
//bin image, saved from drone
else
{*/
FILE* data;
if ((data = fopen(ffilename, "rb")) == NULL)
{
printf("ERROR opening file!\n");
return 1;
}
nx=80;
ny=120;
fread(fileimage, sizeof(pixel2_t) *nx*ny, 1, data);
fclose(data);
//allocate storage
alloc_matrix (&R, nx+2, ny+2);
alloc_matrix (&G, nx+2, ny+2);
alloc_matrix (&B, nx+2, ny+2);
alloc_matrix (&Y, nx+2, ny+2);
alloc_matrix (&U, nx+2, ny+2);
alloc_matrix (&V, nx+2, ny+2);
alloc_matrix (&H, nx+2, ny+2);
alloc_matrix (&S, nx+2, ny+2);
alloc_matrix (&Va, nx+2, ny+2);
//read image data
for (j=0; j<ny; j++)
{
for (i=0; i<nx; i++)
{
Y[i][j] = (float)fileimage[nx*j+i].y1; //noneg yuv! transform to 0 centerd uav by (-16,-128,-128)
U[i][j] = (float)fileimage[nx*j+i].u; //noneg yuv!
V[i][j] = (float)fileimage[nx*j+i].v; //noneg yuv!
/* if (Y[i][j] > (float)240){
x_avg += i;
hits++;
}*/
//conversion
yuv[0] = Y[i][j];
yuv[1] = U[i][j];
yuv[2] = V[i][j];
YUVtoHSV(yuv,rgb,hsv); //this function takes non-zero yuv!
H[i][j] = hsv[0];
S[i][j] = hsv[1];
Va[i][j] = hsv[2];
R[i][j] = rgb[0];
G[i][j] = rgb[1];
B[i][j] = rgb[2];
printf("%0.f,", R[i][j]);
if (rgb[0]+rgb[1]+rgb[2] > 240*3){
x_avg += i;
hits++;
}
}
printf("\n");
}
if (hits > 0){
x_avg = x_avg/hits;
printf("x average is %f", x_avg);
}
}
void createMatchLookup()
{
int i,j,k;
float yuv[3],rgb[3],hsv[3];
unsigned char matchLookuptable[128][128][128];
//matching-Thresholds, global
//landmarks
int lndmrk_nr = 5,lndmrk_best=0,lndmrk_cnd=0;
float featuredist_best = featdist_thrshld;
float featuredist_cnd = featdist_thrshld;
static lndmrk_t *lndmrks;
//Init landmarks
lndmrks = malloc(lndmrk_nr*sizeof(lndmrk_t));
//yellow - not used for localization
lndmrks[0].X = 0.0;
lndmrks[0].Y = 0.0;
lndmrks[0].descr[0] = 70.0; //HUE
lndmrks[0].descr[1] = 170.0; //green
lndmrks[0].descr[2] = 100.0; //blue
//green
lndmrks[1].X = 0.0;
lndmrks[1].Y = 0.3;
lndmrks[1].descr[0] = 130.0;
lndmrks[1].descr[1] = 160.0;
lndmrks[1].descr[2] = 110.0;
//pink
lndmrks[2].X = 0.5;
lndmrks[2].Y = 0.28;
lndmrks[2].descr[0] = 345.0;
lndmrks[2].descr[1] = 100.0;
lndmrks[2].descr[2] = 135.0;
//red
lndmrks[3].X = 0.25;
lndmrks[3].Y = 0.20;
lndmrks[3].descr[0] = 25.0;
lndmrks[3].descr[1] = 80.0;
lndmrks[3].descr[2] = 70.0;
//blue
lndmrks[4].X = 0.5;
lndmrks[4].Y = 0.0;
lndmrks[4].descr[0] = 220.0;
lndmrks[4].descr[1] = 120.0;
lndmrks[4].descr[2] = 160.0;
//create table
for (i=0;i<255;i=i+2)
{
for (j=0;j<255;j=j+2)
{
for (k=0;k<255;k=k+2)
{
yuv[0] = i;
yuv[1] = j;
yuv[2] = k;
YUVtoHSV(yuv,rgb,hsv);
//Find marchting landmark
lndmrk_best = 0;
//check if marker is candidate for colored marker by sat, match and store
if ((hsv[1]>=s_thrshld) && (hsv[2]>=va_thrshld) && ((hsv[1]+hsv[2])>=sva_sum_thrshld))
{
//find closest matching landmark
featuredist_best = featuredist(hsv,rgb,&(lndmrks[lndmrk_best].descr[0]));
for (lndmrk_cnd=1;lndmrk_cnd<lndmrk_nr;lndmrk_cnd++)
{
featuredist_cnd = featuredist(hsv,rgb,&(lndmrks[lndmrk_cnd].descr[0]));
if (featuredist_cnd < featuredist_best)
{
featuredist_best = featuredist_cnd;
lndmrk_best = lndmrk_cnd;
}
}
if (featuredist_best >= featdist_thrshld) //@TODO add more outlier handling like ransac
{
//set invalid (marker 0)
lndmrk_best = 0;
}
}
matchLookuptable[(int)(i/2)][(int)(j/2)][(int)(k/2)] = lndmrk_best;
}//innermost forloop
}
}
//save lookuptable
FILE * lookupfile ;
if ( (lookupfile = fopen("../../DroneExchange/params/lookuptable.dat", "wb")) == NULL )
{
printf("matchlookupgenerator(): Error opening file\n");
}
fwrite(matchLookuptable, sizeof(matchLookuptable), 1, lookupfile);
}
