MIT_MatlabToolbox/trunk/utils/VisionPrePostProcessor/image_module.c
//----------------------------------
// Create lookup-table for pixel-to-marker matching
//----------------------------------

Create lookup-table for pixel-to-marker matching
Create lookup - table for pixel - to - marker matching
void createMatchLookup()
{
	int i,j,k;
	float yuv[3],rgb[3],hsv[3];
	unsigned char matchLookuptable[128][128][128];
	int lndmrk_nr = 5,lndmrk_best=0,lndmrk_cnd=0;
	float featuredist_best = featdist_thrshld;
	float featuredist_cnd = featdist_thrshld;
	static lndmrk_t *lndmrks;
	lndmrks = malloc(lndmrk_nr*sizeof(lndmrk_t));
	lndmrks[0].X = 0.0;
	lndmrks[0].Y = 0.0;
	lndmrks[0].descr[0] = 70.0;	
	lndmrks[0].descr[1] = 170.0;	
	lndmrks[0].descr[2] = 100.0;	
	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;
	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;
	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;
	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;
	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);
					 lndmrk_best = 0;
					 if ((hsv[1]>=s_thrshld) && (hsv[2]>=va_thrshld) && ((hsv[1]+hsv[2])>=sva_sum_thrshld))
					 {
						 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) 
						 	 {
							 lndmrk_best = 0;
						 	 }
					 	 }
					 matchLookuptable[(int)(i/2)][(int)(j/2)][(int)(k/2)] = lndmrk_best;
				 	 }
			 	 }
			 }
	FILE * lookupfile ;
    if ( (lookupfile = fopen("../../DroneExchange/params/lookuptable.dat", "wb")) == NULL )
    {
        printf("matchlookupgenerator(): Error opening file\n");
    }
    fwrite(matchLookuptable, sizeof(matchLookuptable), 1, lookupfile);
}
void createMatchLookup ( ) { int i , j , k ; float yuv [ 3 ] , rgb [ 3 ] , hsv [ 3 ] ; unsigned char matchLookuptable [ 128 ] [ 128 ] [ 128 ] ; int lndmrk_nr = 5 , lndmrk_best = 0 , lndmrk_cnd = 0 ; float featuredist_best = featdist_thrshld ; float featuredist_cnd = featdist_thrshld ; static lndmrk_t * lndmrks ; lndmrks = malloc ( lndmrk_nr * sizeof ( lndmrk_t ) ) ; lndmrks [ 0 ] . X = 0.0 ; lndmrks [ 0 ] . Y = 0.0 ; lndmrks [ 0 ] . descr [ 0 ] = 70.0 ; lndmrks [ 0 ] . descr [ 1 ] = 170.0 ; lndmrks [ 0 ] . descr [ 2 ] = 100.0 ; 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 ; 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 ; 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 ; 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 ; 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 ) ; lndmrk_best = 0 ; if ( ( hsv [ 1 ] >= s_thrshld ) && ( hsv [ 2 ] >= va_thrshld ) && ( ( hsv [ 1 ] + hsv [ 2 ] ) >= sva_sum_thrshld ) ) { 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 ) { lndmrk_best = 0 ; } } matchLookuptable [ ( int ) ( i / 2 ) ] [ ( int ) ( j / 2 ) ] [ ( int ) ( k / 2 ) ] = lndmrk_best ; } } } FILE * lookupfile ; if ( ( lookupfile = fopen ( " " , " " ) ) == NULL ) { printf ( " \n " ) ; } fwrite ( matchLookuptable , sizeof ( matchLookuptable ) , 1 , lookupfile ) ; }
Create lookup-table for pixel-to-marker matching
Create lookup - table for pixel - to - marker matching
//matching-Thresholds, global
 //landmarks
 //Init landmarks
 //yellow - not used for localization
 //HUE
 //green
 //blue
 //green
 //pink
 //red
 //blue
 //create table
 //Find marchting landmark
 //check if marker is candidate for colored marker by sat, match and store
 //find closest matching landmark
 //@TODO add more outlier handling like ransac
 //set invalid (marker 0)
 //innermost forloop
 //save lookuptable
