imagetr.cpp

00001 #pragma once
00002 #include "mbed.h"
00003 #include "global.h"
00004 #include "imagetr.h"
00005 #include "SDFileSystem.h"
00006 #include "SPI_TFT_ILI9341.h"
00007 
00008 
00009 //DigitalOut str(PB_3); // internal strobe for debug
00010 //DigitalOut str1(PC_8); // internal strobe for debug
00011 
00012 void imagetr::getimage( const char nf[])
00013 {
00014     fp = fopen(nf, "r");
00015     pc.printf("get %s\r\n", nf);
00016     for (int i=0; i<tmarray; i++) {
00017 
00018         bank[i] =fgetc(fp);
00019     }
00020     fclose(fp);
00021 }
00022 
00023 void imagetr::avera()
00024 {
00025     int i,j,k,l, av;
00026     for (i=1; i<nl-1; i=i+1) {
00027         for (j=1; j<nc-1; j=j+1) {
00028             av=0;
00029             for (k=0; k<3; k++) {
00030                 for (l=0; l<3; l++) {
00031                     av=av+banktc[i-1+k][j-1+l];
00032                 }
00033             }
00034             banktc[i][j]=av/9;
00035         }
00036     }
00037 }
00038 
00039 
00040 void imagetr::extrta()
00041 {
00042     for (int i=120-nlta; i<120; i++) {
00043         for (int j=0; j<ncta; j++) {
00044             bankta[i-120+nlta][j][0]=banktc[i][j];
00045         }
00046     }
00047 }
00048 int imagetr::ar(float a)
00049 {
00050     int ai;
00051     ai=a;
00052     if (a-ai >=0.5f) {
00053         return a+1;
00054     } else {
00055         return a;
00056     }
00057 }
00058 
00059 void imagetr::affine (int x, int y, float a, float z)
00060 {
00061     float cosa, sina, tif, tjf, dx, dy ;
00062     int ti,tj,i,j, tm1i, tm1j, til, tih, tjl, tjh;
00063     cosa=cos(a);
00064     sina=sin(a);
00065     for (i=0; i<nl; i++) {
00066         for (j=0; j<nc; j++) {
00067             banktc[i][j]=25;
00068         }
00069     }
00070     for (i=0; i<nl; i++) {
00071         for (j=0; j<nc; j++) {
00072 
00073             dx=i-61;
00074             dy=j-80;
00075             tif=dx*cosa - dy*sina + 60;
00076             tjf=dx*sina + dy*cosa + 80;
00077             tif=tif-x;
00078             tjf=tjf-y;
00079             ti=ar((tif-60) * z + 60);
00080             tj=ar((tjf-80) * z + 80 );
00081             tm1i =ti;
00082             if (tm1i<0) {
00083                 tm1i=0;
00084             }
00085             if (tm1i>nl-1) {
00086                 tm1i=nl-1;
00087             }
00088 
00089             dx=i-60;
00090             dy=j-81;
00091             tif=dx*cosa - dy*sina + 60;
00092             tjf=dx*sina + dy*cosa + 80;
00093             tif=tif-x;
00094             tjf=tjf-y;
00095             ti=ar((tif-60) * z + 60);
00096             tj=ar((tjf-80) * z + 80 );
00097             tm1j =tj;
00098             if (tm1j<0) {
00099                 tm1j=0;
00100             }
00101             if (tm1j>=nc) {
00102                 tm1j=nc-1;
00103             }
00104 
00105             dx=i-60;
00106             dy=j-80;
00107             tif=dx*cosa - dy*sina + 60;
00108             tjf=dx*sina + dy*cosa + 80;
00109             tif=tif-x;
00110             tjf=tjf-y;
00111             ti=ar((tif-60) * z + 60);
00112             tj=ar((tjf-80) * z + 80 );
00113 
00114             if((ti>=0) && (tj >=0) && (ti <nl) && (tj < nc)) {
00115                 banktc[ti][tj]=bankt[i][j];
00116                 if (ti> tm1i) {
00117                     til = tm1i;
00118                     tih = ti;
00119                 } else {
00120                     til =ti;
00121                     tih = tm1i;
00122                 }
00123 
00124                 if (tj> tm1j) {
00125                     tjl = tm1j;
00126                     tjh = tj;
00127                 } else {
00128                     tjl =tj;
00129                     tjh = tm1j;
00130                 }
00131                 for (int k = til; k<tih+1; k++) {
00132                     for (int l = tjl; l<tjh+1; l++) {
00133                         banktc[k][l]=bankt[i][j];
00134                     }
00135                 }
00136             }
00137         }
00138 
00139     }
00140 }
00141 void imagetr::genta (float tx, float ty, float a, float z)
00142 {
00143     // generate multiple ta
00144     float cosa, sina, tif, tjf, dx, dy ;
00145     int ti,tj,i,j, tm1i, tm1j, til, tih, tjl, tjh;
00146     cosa=cos(a);
00147     sina=sin(a);
00148     int nlta2=nlta/2;
00149     int ncta2=ncta/2;
00150     for (i=0; i<nlta; i++) {
00151         for (j=0; j<ncta; j++) {
00152             banktatc[i][j]=25;
00153         }
00154     }
00155     for (i=0; i<nlta; i++) {
00156         for (j=0; j<ncta; j++) {
00157 
00158             dx=(i-nlta2-1)*tx;
00159             dy=(j-ncta2)*ty;
00160             tif=dx*cosa - dy*sina + nlta2;
00161             tjf=dx*sina + dy*cosa + ncta2;
00162             ti=ar((tif-nlta2) * z + nlta2);
00163             tj=ar((tjf-ncta2) * z + ncta2 );
00164             tm1i =ti;
00165             if (tm1i<0) {
00166                 tm1i=0;
00167             }
00168             if (tm1i>nlta-1) {
00169                 tm1i=nlta-1;
00170             }
00171             dx=(i-nlta2)*tx;
00172             dy=(j-ncta2-1)*ty;
00173             tif=dx*cosa - dy*sina + nlta2;
00174             tjf=dx*sina + dy*cosa + ncta2;
00175             ti=ar((tif-nlta2) * z + nlta2);
00176             tj=ar((tjf-ncta2) * z + ncta2 );
00177             tm1j =tj;
00178             if (tm1j<0) {
00179                 tm1j=0;
00180             }
00181             if (tm1j>=ncta) {
00182                 tm1j=ncta-1;
00183             }
00184 
00185             dx=(i-nlta2)*tx;
00186             dy=(j-ncta2)*ty;
00187             tif=dx*cosa - dy*sina + nlta2;
00188             tjf=dx*sina + dy*cosa + ncta2;
00189             ti=ar((tif-nlta2) * z + nlta2);
00190             tj=ar((tjf-ncta2) * z + ncta2 );
00191 
00192             if((ti>=0) && (tj >=0) && (ti <nlta) && (tj < ncta)) {
00193                 banktatc[ti][tj]=bankta[i][j][0];
00194                 if (ti> tm1i) {
00195                     til = tm1i;
00196                     tih = ti;
00197                 } else {
00198                     til =ti;
00199                     tih = tm1i;
00200                 }
00201 
00202                 if (tj> tm1j) {
00203                     tjl = tm1j;
00204                     tjh = tj;
00205                 } else {
00206                     tjl =tj;
00207                     tjh = tm1j;
00208                 }
00209                 for (int k = til; k<tih+1; k++) {
00210                     for (int l = tjl; l<tjh+1; l++) {
00211                         banktatc[k][l]=bankta[i][j][0];
00212                     }
00213                 }
00214             }
00215         }
00216 
00217     }
00218 
00219 }
00220 void imagetr::lumi (float z)
00221 {
00222     int i,j;
00223     for (i=0; i<nl; i++) {
00224         for (j=0; j<nc; j++) {
00225             banktc[i][j]=banktc[i][j]*z;
00226         }
00227     }
00228 }
00229 
00230 void imagetr::rgbtoy(void)
00231 {
00232     //transform bank in Y in table bankt
00233     int R,G,G1,B,Y;
00234     for (int i=0; i<tmarray; i=i+2) {
00235 
00236         R=(bank[i+1]>>3);
00237         G=(bank[i+1]&0x07)<<3;
00238 
00239         G1=bank[i]>>5;
00240         G= G|G1;
00241         B=bank[i]&0x1f;
00242 
00243         Y=ar(0.299f*R+0.587f*G+0.114f*B);
00244         //pc.printf("i:%d\r\n",i);
00245         bankt[i/nc2][(i/2)%nc]=Y;
00246     }
00247 }
00248 void imagetr::rgbtoyta(void)
00249 {
00250     //transform bank in Y in table bankta, create muliple view
00251     int R,G,G1,B,Y;
00252     for (int i=0; i<tmarrayta; i=i+2) {
00253         R=(bank[i+1]>>3);
00254         G=(bank[i+1]&0x07)<<3;
00255 
00256         G1=bank[i]>>5;
00257         G= G|G1;
00258         B=bank[i]&0x1f;
00259 
00260         Y=ar(0.299f*R+0.587f*G+0.114f*B);
00261         bankta[i/nc2ta][(i/2)%ncta][0]=Y;
00262     }
00263 
00264 
00265     genta (1, 1, 0.17, 1); // 10 deg
00266     for (int i=0; i<nlta; i=i+1) {
00267         for (int j=0; j<ncta; j=j+1) {
00268             bankta[i][j][1]= banktatc[i][j];
00269         }
00270     }
00271     genta (1, 1, -0.17, 1); // -10 deg
00272     for (int i=0; i<nlta; i=i+1) {
00273         for (int j=0; j<ncta; j=j+1) {
00274             bankta[i][j][2]= banktatc[i][j];
00275         }
00276     }
00277     genta (1, 1, 0, 0.8); // zoom 0.8
00278     for (int i=0; i<nlta; i++) {
00279         for (int j=0; j<ncta; j=j+1) {
00280             bankta[i][j][3]= banktatc[i][j];
00281         }
00282     }
00283     genta (1, 1, 0, 1.20); // zoom 1.2
00284     for (int i=0; i<nlta; i++) {
00285         for (int j=0; j<ncta; j=j+1) {
00286             bankta[i][j][4]= banktatc[i][j];
00287         }
00288     }/*
00289      genta (0.9, 1, 0, 1);
00290     for (int i=0; i<nlta; i=i+1) {
00291         for (int j=0; j<ncta; j=j+1) {
00292             //           pc.printf("i %d, j %d b %02x\r\n", i, j, bankta[i][j][0]);
00293             bankta[i][j][5]= banktatc[i][j];
00294         }
00295     }
00296     //pc.printf("ty\r\n");
00297     genta (1, 0.9, 0, 1);
00298     for (int i=0; i<nlta; i=i+1) {
00299         for (int j=0; j<ncta; j=j+1) {
00300             bankta[i][j][6]= banktatc[i][j];
00301         }
00302     }*/
00303 }
00304 
00305 
00306 void imagetr::rgbtoytaed(void)
00307 {
00308     //transform bank in Y in table bankta, create muliple view
00309     int R,G,G1,B,Y;
00310     for (int i=0; i<tmarrayta; i=i+2) {
00311         R=(bank[i+1]>>3);
00312         G=(bank[i+1]&0x07)<<3;
00313 
00314         G1=bank[i]>>5;
00315         G= G|G1;
00316         B=bank[i]&0x1f;
00317 
00318         Y=ar(0.299f*R+0.587f*G+0.114f*B);
00319         bankta[i/nc2ta][(i/2)%ncta][0]=Y;
00320     }
00321 
00322     extedgeta(0,0,2);
00323 
00324     for (int i=0; i<nlta; i=i+1) {
00325         for (int j=0; j<ncta; j=j+1) {
00326             bankta[i][j][0]= banktatc[i][j];
00327         }
00328     }
00329 
00330     genta (1, 1, 0.17, 1); // 10 deg
00331     for (int i=0; i<nlta; i=i+1) {
00332         for (int j=0; j<ncta; j=j+1) {
00333             bankta[i][j][1]= banktatc[i][j];
00334         }
00335     }
00336     genta (1, 1, -0.17, 1); // -10 deg
00337     for (int i=0; i<nlta; i=i+1) {
00338         for (int j=0; j<ncta; j=j+1) {
00339             bankta[i][j][2]= banktatc[i][j];
00340         }
00341     }
00342     genta (1, 1, 0, 0.8); // zoom 0.8
00343     for (int i=0; i<nlta; i++) {
00344         for (int j=0; j<ncta; j=j+1) {
00345             bankta[i][j][3]= banktatc[i][j];
00346         }
00347     }
00348     genta (1, 1, 0, 1.20); // zoom 1.2
00349     for (int i=0; i<nlta; i++) {
00350         for (int j=0; j<ncta; j=j+1) {
00351             bankta[i][j][4]= banktatc[i][j];
00352         }
00353     }/*
00354      genta (0.9, 1, 0, 1);
00355     for (int i=0; i<nlta; i=i+1) {
00356         for (int j=0; j<ncta; j=j+1) {
00357             //           pc.printf("i %d, j %d b %02x\r\n", i, j, bankta[i][j][0]);
00358             bankta[i][j][5]= banktatc[i][j];
00359         }
00360     }
00361     //pc.printf("ty\r\n");
00362     genta (1, 0.9, 0, 1);
00363     for (int i=0; i<nlta; i=i+1) {
00364         for (int j=0; j<ncta; j=j+1) {
00365             bankta[i][j][6]= banktatc[i][j];
00366         }
00367     }*/
00368 }
00369 
00370 
00371 
00372 
00373 void imagetr::ytorgb(unsigned char b[nl][nc])
00374 {
00375     //populate bank with table b
00376     int R,G,G1,B,Y;
00377     for (int i=0; i<tarray; i=i+1) {
00378 
00379         Y=b[i/nc][i%nc];
00380         // Y=Y*yco;
00381         R=Y*0.71;
00382         if (R>31) {
00383             R=31;
00384         }
00385         G=Y*1.45;
00386         if (G>63) {
00387             G=63;
00388         }
00389         B=R;
00390         G1=G>>3;
00391         bank[2*i+1]=R<<3|G1;
00392         bank[2*i]=G<<5|B;
00393     }
00394 }
00395 
00396 void imagetr::ytorgbta(unsigned char b[nlta][ncta][nv], int nu,const char des[])
00397 {
00398     //p0pulate bankf with table b special target, write targetf.txt
00399     int R,G,G1,B,Y;
00400     fp = fopen(des, "w");
00401     for (int j=0; j<tmarray; j++) {
00402         bank[j]=25;
00403     }
00404     for (int i=0; i<tarrayta; i=i+1) {
00405 
00406         Y=b[i/ncta][i%ncta][nu];
00407         // Y=Y*yco;
00408         R=Y*0.71;
00409         if (R>31) {
00410             R=31;
00411         }
00412         G=Y*1.45;
00413         if (G>63) {
00414             G=63;
00415         }
00416         B=R;
00417         G1=G>>3;
00418         bank[2*i+1]=R<<3|G1;
00419         bank[2*i]=G<<5|B;
00420         fputc(bank[2*i], fp);
00421         fputc(bank[2*i+1], fp);
00422 
00423     }
00424     fclose(fp);
00425 }
00426 void imagetr::ytorgbtas(unsigned char b[nlta][ncta][nv], int nu)
00427 {
00428     //p0pulate bank with table b special target, without write targetfile
00429     int R,G,G1,B,Y;
00430 
00431     for (int j=0; j<tmarray; j++) {
00432         bank[j]=25;
00433     }
00434     for (int i=0; i<tarrayta; i=i+1) {
00435 
00436         Y=b[i/ncta][i%ncta][nu];
00437         // Y=Y*yco;
00438         R=Y*0.71;
00439         if (R>31) {
00440             R=31;
00441         }
00442         G=Y*1.45;
00443         if (G>63) {
00444             G=63;
00445         }
00446         B=R;
00447         G1=G>>3;
00448         bank[2*i+1]=R<<3|G1;
00449         bank[2*i]=G<<5|B;
00450     }
00451 }
00452 
00453 void imagetr::extcont(int minc, int maxc)
00454 {
00455     // contrast expand
00456     int Y;
00457     for (int i=0; i<tarray; i=i+1) {
00458 
00459         Y=bankt[i/nc][i%nc];
00460         if (Y<=minc) {
00461             Y=0;
00462         }
00463         if (Y>=maxc) {
00464             Y=50;
00465         }
00466         banktc[i/nc][i%nc]=Y;
00467     }
00468 }
00469 void imagetr::extedge(int minc, int maxc, int th)
00470 {
00471     //edge extract
00472     int Y,YG,YH,i,j;
00473     int M=0;
00474     for (i=0; i<nl; i++) {
00475         for (j=0; j<nc; j++) {
00476             M=M+bankt[i][j];
00477 
00478         }
00479     }
00480     M=M/tarray;
00481     //  if (M>maxc ||M<minc) {
00482     //     th=1;
00483     // } else {
00484     //    th=4;
00485     // }
00486     // th=6*M/minc;
00487     // pc.printf( "M: %d th %d: minc: %d maxc; %d \r\n",M,th, minc, maxc);
00488     for (i=1; i<nl; i++) {
00489         for (j=1; j<nc; j++) {
00490             Y=bankt[i][j];
00491             YG=bankt[i][j-1];
00492             YH=bankt[i-1][j];
00493             if (abs(Y-YG) >th||abs(Y-YH) >th) {
00494                 Y=50;
00495             } else {
00496                 Y=0;
00497             }
00498             banktc[i][j]=Y;
00499         }
00500     }
00501 }
00502 void imagetr::extedgeta(int minc, int maxc, int th)
00503 {
00504     //edge extract
00505     int Y,YG,YH,i,j;
00506     int M=0;
00507     for (i=0; i<nlta; i++) {
00508         for (j=0; j<ncta; j++) {
00509             M=M+bankta[i][j][0];
00510 
00511         }
00512     }
00513     M=M/tarray;
00514     //  if (M>maxc ||M<minc) {
00515     //     th=1;
00516     // } else {
00517     //    th=4;
00518     // }
00519     // th=6*M/minc;
00520     // pc.printf( "M: %d th %d: minc: %d maxc; %d \r\n",M,th, minc, maxc);
00521     for (i=1; i<nlta; i++) {
00522         for (j=1; j<ncta; j++) {
00523             Y=bankta[i][j][0];
00524             YG=bankta[i][j-1][0];
00525             YH=bankta[i-1][j][0];
00526             if (abs(Y-YG) >th||abs(Y-YH) >th) {
00527                 Y=50;
00528             } else {
00529                 Y=0;
00530             }
00531             banktatc[i][j]=Y;
00532         }
00533     }
00534 }
00535 
00536 
00537 void imagetr::searchpat(int th, char name[])
00538 {
00539     int i,j,k, l, maxi, maxj, car, tt,n;
00540     Time.start();
00541     for (n=0; n<=nv; n++) {
00542         for (i =0 ; i < nl-nlta; i++) {
00543             for (j=0; j<nc-ncta; j++) {
00544                 tt=0;
00545                 for (k=0; k<nlta; k++) {
00546                     for (l=0; l<ncta; l++) {
00547                         car= bankta[k][l][n]-banktc[i+k][j+l];
00548                         tt=tt+(car*car);
00549                         if (tt>th) {
00550                            break;
00551                         }
00552                     }
00553                 }
00554                 if (tt<th) {
00555                     maxi=i;
00556                     maxj=j;
00557                     i=nl;
00558                     j=nc;
00559                     n=nv;
00560                 }
00561             }
00562         }
00563     }
00564     Time.stop();
00565     if (tt<th) {
00566         //pc.printf("line: %d, column: %d, n: %d max: %d time: %f \r\n",maxi,maxj,n0,tt, Time.read());
00567         TFT.locate(160,20);
00568         printf("Pat found %4.2f s",Time.read());
00569         TFT.locate(160+maxj+(ncta/2),240-maxi-(nlta/2));
00570         printf(name);
00571         TFT.rect(160+maxj,240-maxi-nlta,160+maxj+ncta,240-maxi,Red);
00572     } else {
00573         TFT.locate(160,20);
00574         printf("Not found %4.2f s",Time.read());
00575         //pc.printf("not found time: %4f n:%d max: %d \r\n",Time.read(), n0,tt);
00576     }
00577     Time.reset();
00578 }
00579 
00580 int imagetr::BMP_tofile(unsigned int x, unsigned int y, const char *Name_BMP)
00581 {
00582 
00583 #define OffsetPixelWidth    18
00584 #define OffsetPixelHeigh    22
00585 #define OffsetFileSize      34
00586 #define OffsetPixData       10
00587 #define OffsetBPP           28
00588 
00589     char filename[50];
00590     unsigned char BMP_Header[54];
00591     unsigned short BPP_t;
00592     unsigned int PixelWidth,PixelHeigh,start_data;
00593     unsigned int    i,off;
00594     int padd,j;
00595     unsigned short *line;
00596 
00597     // get the filename
00598     i=0;
00599     while (*Name_BMP!='\0') {
00600         filename[i++]=*Name_BMP++;
00601     }
00602     filename[i] = 0;
00603 
00604     FILE *Image = fopen((const char *)&filename[0], "rb");  // open the bmp file
00605     if (!Image) {
00606         return(0);      // error file not found !
00607     }
00608 
00609     fread(&BMP_Header[0],1,54,Image);      // get the BMP Header
00610 
00611     if (BMP_Header[0] != 0x42 || BMP_Header[1] != 0x4D) {  // check magic byte
00612         fclose(Image);
00613         return(-1);     // error no BMP file
00614     }
00615 
00616     BPP_t = BMP_Header[OffsetBPP] + (BMP_Header[OffsetBPP + 1] << 8);
00617     if (BPP_t != 0x0010) {
00618         fclose(Image);
00619         return(-2);     // error no 16 bit BMP
00620     }
00621 
00622     PixelHeigh = BMP_Header[OffsetPixelHeigh] + (BMP_Header[OffsetPixelHeigh + 1] << 8) + (BMP_Header[OffsetPixelHeigh + 2] << 16) + (BMP_Header[OffsetPixelHeigh + 3] << 24);
00623     PixelWidth = BMP_Header[OffsetPixelWidth] + (BMP_Header[OffsetPixelWidth + 1] << 8) + (BMP_Header[OffsetPixelWidth + 2] << 16) + (BMP_Header[OffsetPixelWidth + 3] << 24);
00624     if (PixelHeigh > TFT.height() + y || PixelWidth > TFT.width() + x) {
00625         fclose(Image);
00626         return(-3);      // to big
00627     }
00628 
00629     start_data = BMP_Header[OffsetPixData] + (BMP_Header[OffsetPixData + 1] << 8) + (BMP_Header[OffsetPixData + 2] << 16) + (BMP_Header[OffsetPixData + 3] << 24);
00630 
00631     line = (unsigned short *) malloc (2 * PixelWidth); // we need a buffer for a line
00632     if (line == NULL) {
00633         return(-4);         // error no memory
00634     }
00635 
00636     // the bmp lines are padded to multiple of 4 bytes
00637     padd = -1;
00638     do {
00639         padd ++;
00640     } while ((PixelWidth * 2 + padd)%4 != 0);
00641 
00642     // GraphicsDisplay.window(x, y,PixelWidth ,PixelHeigh);
00643     //GraphicsDisplay.wr_cmd(0x2C);  // send pixel
00644     //GraphicsDisplay.spi_16(1);
00645     int compt=38400;
00646     unsigned char c,c1;
00647     for (j = PixelHeigh - 1; j >= 0; j--) {               //Lines bottom up
00648         off = j * (PixelWidth  * 2 + padd) + start_data;   // start of line
00649         fseek(Image, off ,SEEK_SET);
00650         fread(line,1,PixelWidth * 2,Image);       // read a line - slow
00651         //   for (i = 0; i < PixelWidth; i++) {        // copy pixel data to TFT
00652         for (int i = PixelWidth-1; i >=0; i--) {        // copy pixel data to TFT
00653             compt = compt -1;
00654             c=line[i]&0x00FF;
00655             c1=line[i]>>8;
00656             bank[compt] =c1      ;
00657             compt = compt -1;
00658             bank[compt] =c;          // one 16 bit pixel
00659         }
00660     }
00661     //spi_bsy();
00662     //_cs = 1;
00663     //spi_16(0);
00664     free (line);
00665     fclose(Image);
00666     //WindowMax();
00667     return(1);
00668 }
00669 
00670