void RSEDU_image_processing(void * buffer)
{
//process control
static int counter = 0;
//communication
static float vis_data[4];
int status;
static int vis_fifo;
//image variables
int i, j;
int row, col;
float yuv[3];
int nx = 80, ny = 120;
float feature_pps[3][4];
static unsigned char matchLookup[128][128][128];
//camera variables
float camerapos[3]; //@TODO static?
float camerayaw; //@TODO static?
//Image and Matching Thresholds
static int filtersize = 5; //odd numbered! dummy for potential gaussian filter mask, etc.
int pxls_ftr_min = 5; //minimum required nr of detected pixels per landmark
//Landmarks, Pose Estimation Matrices
int lndmrk_nr = 5, lndmrk_best = 0;
static lndmrk_t lndmrks[5];
//matrices for reconstructing camera pose by intrMatrx_inv*landmark-pixellocation*ldnmrk_pinv
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}
};
static double intrMatrx_inv[3][3] = {{ 0.006551831768882 , 0, -0.550082487527771},
{0, 0.006546559888686, -0.399495805347318},
{0, 0, 1.000000000000000}
};
//wait on first call
if(counter == 1)
{
usleep(20000);
}
//Init Communication, Streaming
int fifo;
if(FEAT_IMSAVE == 2)
{
u8 * raw = buffer;
};
pixel2_t *image = buffer; /* Picture is a 160x120 pixels, stored in YUV422 interlaced format - TO BE CHECKED */
/*
* PROGRAM
*/
//ptiming - declare and start
//------------
long long start;
static FILE *ptfile;
ptimer_start(FEAT_TIME, counter, &(start));
//------------
//process control
counter++;
if(counter == 1)
{
//ptiming - init file
//------------
ptimer_init(FEAT_TIME, __func__, &(ptfile), NULL);
//------------
printf("rsedu_vis(): Init fifo-communication...\n");
//open fifo to dump visual position estimates to control code
if(access("/tmp/vis_fifo", F_OK) != -1)
{
printf("rsedu_vis(): SUCCESS POSVIS FIFO exists! \n");
vis_fifo = open("/tmp/vis_fifo", O_WRONLY);
if(vis_fifo)
{
vis_data[0] = -99.0;
write(vis_fifo, (float*)(&vis_data), sizeof(vis_data));
close(vis_fifo);
printf("rsedu_vis(): SUCCESS opening POSVIS-fifo!\n");
}
else
{
printf("rsedu_vis(): ERROR opening POSVIS-fifo!\n");
}
}
else
{
printf("rsedu_vis(): ERROR opening POSVIS-fifo!\n");
}
//load lookuptable for matching process
if(FEAT_NOLOOK == 0)
{
FILE* data;
if((data = fopen("/data/edu/params/lookuptable.dat", "rb")) == NULL)
{
printf("rsedu_vis(): ERROR opening lookupfile \n");
}
fread(matchLookup, sizeof(matchLookup), 1, data);
fclose(data);
}
}
//landmark detection
//------------
//1 cycle through image, convert each pixel to hsv, threshold by s and v to find colored balls, match h value to color-closest in "database" (use precomputed lookup), save weighted average pixellocation with database-lndmrkID
//2 reconstruct pose of camera (3D-position with yaw)
if((FEAT_POSVIS_RUN) && ((counter % 15) == 0) && (NULL != image)) //@pseudo4Hz
{
//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;
}
int margin = (int)(filtersize - 1) / 4;
int weight = 1; // weight used to emphasize pixellocation that is close to existing estimate for sufficiently robust, identified landmark
//cycle through image
for(j = 0; j < ny - 2 * margin; j++)
{
for(i = 0; i < nx - 2 * margin; i++)
{
row = margin + j;
col = margin + i;
//Get YUV-values for pixel
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 for current pixel
if(FEAT_NOLOOK)
{
printf("rsedu_vis(): ERROR Unfortunately, color conversion, etc. too slow onboard. Please set FEAT_NOLOOK = 0. \n");
}
else
//use pre-computed lookup table to find match
{
//printf("here uselu \n");
lndmrk_best = (int)matchLookup[(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); //@TODO is it row?
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); //@TODO is it row?
//save that one more pixel to this landmark
lndmrks[lndmrk_best].n += 1;
lndmrks[lndmrk_best].weights += weight;
//if (lndmrk_best==0) {matchResult[col][row]=255;} else {matchResult[col][row]=0;}
}
}//end inner image-for
}//end outer image-for
/*
printf("image_proc(): Result of matching: \n");
for (i=0;i<lndmrk_nr;i++)
{
printf("image_proc(): Lndmrk %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_pixelposition-matrix (ui,vi,1). TODO: remove hardcoded number of landmarks
int features_valid = 0;
int k;
for(k = 1; k < lndmrk_nr; k++) //start from 1 as yellow (landmark 0) 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;
}
}
if(features_valid == 4)
{
reconstructCameraPose(camerapos, &camerayaw, feature_pps, ldnmrk_pinv, intrMatrx_inv);
printf("rsedu_vis(): SUCCESS reconstructed camera pose: (%f, %f, %f,%f)\n", camerapos[0], camerapos[1], camerapos[2], camerayaw * 180 / 3.1415);
/*
* dump pose into FIFO to make available to controls code
*/
//compile data
vis_data[0] = camerapos[0];
vis_data[1] = camerapos[1];
vis_data[2] = camerapos[2];
vis_data[3] = camerayaw;
vis_fifo = open("/tmp/vis_fifo", O_WRONLY);
if(vis_fifo)
{
write(vis_fifo, (float*)(&vis_data), sizeof(vis_data));
close(vis_fifo);
}
}
else
{
printf("rsedu_vis(): WARNING not enough distinct markers (colored balls) found! \n") ;
}
}
//-----------
//STREAMING INSTRUCTIONS
//-----------
/* Enabling image streaming, copies the picture into a named FIFO. Picture can then be sent to a remote Ubuntu computer using standard commands:
Run this one-liner in a shell on the RollingSpider (open terminal, log onto drone via telnet 192.168.1.1) :
(remember to connect via the Bluetooth link, since pluging the USB cable deactivates the camera !!!)
while [ 1 ]; do cat /tmp/picture | nc 192.168.1.2 1234; done
Run these two commands in two different shells on the remote Ubuntu computer:
mkfifo /tmp/rollingspiderpicture ; while [ 1 ]; do nc -l 1234 > /tmp/rollingspiderpicture; done
mplayer -demuxer rawvideo -rawvideo w=160:h=120:format=yuy2 -loop 0 /tmp/rollingspiderpicture
*/
//stream image
//-----------
if((FEAT_IMSAVE == 2) && ((counter % 60) == 0) && (NULL != image)) //@pseudo1Hz
{
printf("image_proc(): Write image to fifo...\n");
mkfifo("/tmp/picture", 0777);
fifo = open("/tmp/picture", O_WRONLY);
if(fifo)
{
//char word = "asd";
//write(fifo,word,320*120);
write(fifo, buffer, 320 * 120);
close(fifo);
usleep(5000);
}
}
//save image
//-----------
if((FEAT_IMSAVE == 1) && ((counter % 6) == 0) && (NULL != image)) //@10Hz
{
FILE* data;
char filename[15];
//sprintf(filename,"/data/edu/imgs/img%i.bin",counter);
sprintf(filename, "/tmp/edu/imgs/img%i.bin", counter);
//sprintf(filename,"/tmp/imgs/img.bin");
//printf("image_proc(): img name: %s \n",filename);
mkdir("/tmp/edu", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
mkdir("/tmp/edu/imgs", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
if((data = fopen(filename, "wb")) == NULL)
{
printf("rsedu_vis(): ERROR opening img file\n");
}
fwrite(image, sizeof(pixel2_t) * 80 * 120, 1, data);
fclose(data);
usleep(5000);
}
usleep(4000);
//ptiming - store
//----------
ptimer_stopstore(FEAT_TIME, counter, start, ptfile);
//----------
}
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);
}
