STBTrAPI.c

00001 /*---------------------------------------------------------------------------*/
00002 /* Copyright(C)  2017  OMRON Corporation                                     */
00003 /*                                                                           */
00004 /* Licensed under the Apache License, Version 2.0 (the "License");           */
00005 /* you may not use this file except in compliance with the License.          */
00006 /* You may obtain a copy of the License at                                   */
00007 /*                                                                           */
00008 /*     http://www.apache.org/licenses/LICENSE-2.0                            */
00009 /*                                                                           */
00010 /* Unless required by applicable law or agreed to in writing, software       */
00011 /* distributed under the License is distributed on an "AS IS" BASIS,         */
00012 /* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  */
00013 /* See the License for the specific language governing permissions and       */
00014 /* limitations under the License.                                            */
00015 /*---------------------------------------------------------------------------*/
00016 
00017 #include "STBTrAPI.h"
00018 
00019 #define STB_INT_MAX       2147483647    /* maximum (signed) int value */
00020 
00021 /*---------------------------------------------------------------------*/
00022 //    TrSlideRec
00023 /*---------------------------------------------------------------------*/
00024 void TrSlideRec ( ROI_SYS *rec )
00025 {
00026     STB_INT32 t , i ;
00027 
00028     for( t = STB_TR_BACK_MAX  - 2 ; t >= 0 ;  t-- ) 
00029     {
00030         rec [ t + 1 ].cnt = rec[ t + 0 ].cnt;
00031         for( i = 0 ; i < rec [ t + 1 ].cnt ;  i++ ) 
00032         {
00033             rec [ t + 1 ].nDetID    [i]    = rec [ t ].nDetID    [i] ;
00034             rec [ t + 1 ].nTraID    [i]    = rec [ t ].nTraID    [i] ;
00035             rec [ t + 1 ].posX        [i]    = rec [ t ].posX    [i] ;
00036             rec [ t + 1 ].posY        [i]    = rec [ t ].posY    [i] ;
00037             rec [ t + 1 ].size        [i]    = rec [ t ].size    [i]  ;
00038             rec [ t + 1 ].conf        [i]    = rec [ t ].conf    [i]  ;
00039             rec [ t + 1 ].retryN    [i]    = rec [ t ].retryN    [i]  ;
00040         }
00041     }
00042 }
00043 /*---------------------------------------------------------------------*/
00044 //    TrCurRec
00045 /*---------------------------------------------------------------------*/
00046 void TrCurRec ( ROI_SYS *rec , ROI_DET *det , STB_INT32 num)
00047 {
00048     STB_INT32  i ;
00049 
00050 
00051     rec [ 0 ].cnt =num;
00052     for( i = 0 ; i < rec [ 0 ].cnt ;  i++ ) 
00053     {
00054         rec [ 0 ].nDetID    [i]    = i                    ;
00055         rec [ 0 ].nTraID    [i]    = -1                ;
00056         rec [ 0 ].posX        [i]    = det[i].posX        ;
00057         rec [ 0 ].posY        [i]    = det[i].posY        ;
00058         rec [ 0 ].size        [i]    = det[i].size        ;
00059         rec [ 0 ].conf        [i]    = det[i].conf        ;
00060         rec [ 0 ].retryN    [i]    = 0                    ;
00061     }
00062 
00063 }
00064 
00065 
00066 /*---------------------------------------------------------------------*/
00067 //    TrDelRetry
00068 /*---------------------------------------------------------------------*/
00069 void
00070 TrDelRetry( ROI_SYS *preData , STB_INT32 thrRetryCnt )
00071 {
00072     //delete data exceeding the number of retries
00073     //If the face isn't find out during tracking, set until how many frames can look for it.
00074     //If tracking fails for the specified number of consecutive frames, tracking is terminated assuming that face is lost.
00075     STB_INT32    i,  tmpCnt      ;
00076 
00077     tmpCnt        = 0;
00078     for( i = 0 ; i < preData->cnt ;  i++ ) 
00079     {
00080         if( preData->retryN[i] <=  thrRetryCnt )
00081         {
00082 
00083             preData->nDetID        [tmpCnt    ]    = preData->nDetID    [i] ; 
00084             preData->nTraID        [tmpCnt    ]    = preData->nTraID    [i]    ;
00085             preData->posX        [tmpCnt    ]    = preData->posX        [i]    ;
00086             preData->posY        [tmpCnt    ]    = preData->posY        [i]    ; 
00087             preData->size        [tmpCnt    ]    = preData->size        [i]    ;
00088             preData->conf        [tmpCnt    ]    = preData->conf        [i]    ;
00089             preData->retryN        [tmpCnt    ]    = preData->retryN    [i]    ;
00090             tmpCnt++;
00091         }
00092     }
00093     preData->cnt    = tmpCnt    ;
00094 
00095 
00096 }
00097 /*---------------------------------------------------------------------*/
00098 //    TrCheckSameROI
00099 /*---------------------------------------------------------------------*/
00100 STB_INT32
00101 TrCheckSameROI(    STB_INT32 curX        ,STB_INT32 curY        ,STB_INT32 curS        ,
00102                 STB_INT32 preX        ,STB_INT32 preY        ,STB_INT32 preS        
00103                 )
00104 {
00105 
00106     STB_INT32 difP    ;//the percentage of detection position change
00107     STB_INT32 difS    ;//the percentage of detection size change
00108     float tmpVal;
00109     STB_INT32 retVal;
00110 
00111     if( preS < 1 )
00112     {
00113         return STB_INT_MAX;
00114     }
00115 
00116     //the percentage of detect position change
00117     //It is "Absolute value of detected position change amount from previous frame / Detected size of previous frame * 100".
00118     tmpVal = (float)sqrt( (float) (preX-curX)*(preX-curX) + (preY-curY)*(preY-curY) );
00119     difP   = (STB_INT32)( tmpVal * 100 / preS );
00120     //the percentage of detect size change
00121     //It is "Absolute value of detected size change amount from previous frame / Detected size of previous frame * 100".
00122     tmpVal =  (float)(preS-curS);
00123     if( tmpVal < 0 )
00124     {
00125         tmpVal *= (-1);
00126     }
00127     difS   = (STB_INT32)( tmpVal * 100 / preS );
00128     retVal = (difP+1)*(difS+1);
00129 
00130     return retVal;//The return value is the similarity of the rectangle. Always a value more than or equal to zero. The closer to zero, the more similar they are.
00131 
00132 }
00133 /*---------------------------------------------------------------------*/
00134 //    TrSetDistTbl
00135 /*---------------------------------------------------------------------*/
00136 void
00137 TrSetDistTbl
00138     (
00139         STB_INT32    *dst            ,
00140         ROI_SYS        *curData        ,
00141         ROI_SYS        *preData        ,
00142         STB_INT32    traCntMax
00143     )
00144 {
00145     STB_INT32    ip ,ic ;
00146     STB_INT32    distMax = STB_INT_MAX;
00147 
00148     // init
00149     for( ip = 0 ; ip < traCntMax ;  ip++ ) 
00150     {
00151         for( ic = 0 ; ic < traCntMax ;  ic++ ) 
00152         {
00153             dst [ ip * traCntMax + ic ] = distMax;
00154         }
00155     }
00156 
00157 
00158     for( ip = 0 ; ip < preData->cnt ;  ip++ )        
00159     {
00160         for( ic = 0 ; ic < curData->cnt ;  ic++ )        
00161         {
00162             dst [ ip * traCntMax + ic ] 
00163                 = TrCheckSameROI//The return value is the similarity of the rectangle. Always a value more than or equal to zero. The closer to zero, the more similar they are.
00164                     (
00165                         curData->posX[ic],curData->posY[ic],curData->size[ic],
00166                         preData->posX[ip],preData->posY[ip],preData->size[ip]
00167                     );
00168         }
00169     }
00170 
00171 }
00172 /*---------------------------------------------------------------------*/
00173 //    TrSteadinessXYS
00174 /*---------------------------------------------------------------------*/
00175 void
00176 TrSteadinessXYS
00177     (    
00178         STB_INT32    curX    ,STB_INT32    curY    ,STB_INT32    curS    ,
00179         STB_INT32    preX    ,STB_INT32    preY    ,STB_INT32    preS    ,
00180         STB_INT32*    dstX    ,STB_INT32*    dstY    ,STB_INT32*    dstS    ,
00181         STB_INT32    thrP    ,STB_INT32    thrS    
00182     )
00183 {
00184 
00185     STB_INT32 difP    ;//the percentage of detection position change
00186     STB_INT32 difS    ;//the percentage of detection size change
00187     float tmpVal;
00188 
00189 
00190     if( preS < 1 )
00191     {
00192         *dstX = curX    ;        *dstY = curY    ;        *dstS = curS    ;
00193         return ;
00194     }
00195 
00196     //the percentage of detect position change
00197     //It is "Absolute value of detected position change amount from previous frame / Detected size of previous frame * 100".
00198     tmpVal = (float)sqrt( (float) (preX-curX)*(preX-curX) + (preY-curY)*(preY-curY) );
00199     difP   = (STB_INT32)( tmpVal * 100 / preS );
00200     if( difP <= thrP )
00201     {
00202         *dstX = preX    ;        *dstY = preY    ;
00203     }else
00204     {
00205         *dstX = curX    ;        *dstY = curY    ;
00206     }
00207 
00208     //the percentage of detect size change
00209     //It is "Absolute value of detected size change amount from previous frame / Detected size of previous frame * 100".
00210     tmpVal =  (float)(preS-curS);
00211     if( tmpVal < 0 )
00212     {
00213         tmpVal *= (-1);
00214     }
00215     difS   = (STB_INT32)( tmpVal * 100 / preS );
00216     if( difS <= thrS )
00217     {
00218         *dstS = preS    ;
00219     }else
00220     {
00221         *dstS = curS    ;
00222     }
00223 
00224 }
00225 /*---------------------------------------------------------------------*/
00226 //    TrStabilizeTR
00227 /*---------------------------------------------------------------------*/
00228 void TrStabilizeTR
00229     (
00230         ROI_SYS        *wData            ,    //present data after the stabilization
00231         STB_INT32    *wCnt            ,    //a number of present data after the stabilization
00232         ROI_SYS        *rec            ,    //past data
00233         STB_INT32    *cntAcc            ,
00234         TRHANDLE    handle
00235     )
00236 {
00237 
00238     STB_INT32    stedinessPos    = handle->stedPos    ;
00239     STB_INT32    stedinessSize    = handle->stedSize    ;
00240     STB_INT32    thrRetryCnt        = handle->retryCnt    ;
00241     STB_INT32    traCntMax        = handle->traCntMax    ;
00242     STB_INT32    *idPreCur        = handle->wIdPreCur    ;
00243     STB_INT32    *idCurPre        = handle->wIdCurPre    ;
00244     STB_INT32    *dstTbl            = handle->wDstTbl    ;
00245     ROI_SYS        *curData        = &rec[0];//current frame data
00246     ROI_SYS        *preData        = &rec[1];//previous frame data
00247     STB_INT32   tmpAccCnt        ;
00248     STB_INT32    ip    ,ic        ;
00249     STB_INT32    ipp ,icc    ;
00250     STB_INT32    tmpWCnt    ;
00251     STB_INT32    tmpVal        ;
00252     STB_INT32    tmpX,tmpY,tmpS        ;
00253     const STB_INT32 LinkNot        = -1    ;
00254     
00255 
00256     //------------------------------------------------------------------------------//
00257     //Initialization
00258     //------------------------------------------------------------------------------//
00259     for( ip = 0 ; ip < traCntMax ;  ip++ ) 
00260     {
00261         idPreCur[ip]    = LinkNot;
00262         idCurPre[ip]    = LinkNot;
00263     }
00264 
00265 
00266     //------------------------------------------------------------------------------//
00267     //previous preparation
00268     //------------------------------------------------------------------------------//
00269     //Delete the data exceeding the retry count from the previous frame data.
00270     TrDelRetry ( preData    ,thrRetryCnt );
00271 
00272 
00273     //------------------------------------------------------------------------------//
00274     //main processing
00275     //------------------------------------------------------------------------------//
00276     tmpWCnt    = 0    ;//a number of present data after the stabilization
00277 
00278     // "It's reflected in the previous frame" and "It's reflected in the current frame".
00279     //Create dstTbl. The value of dstTbl is the similarity of the rectangle. Always a value more than or equal to zero. The closer to zero, the more similar they are.
00280     TrSetDistTbl( dstTbl,curData,preData,    traCntMax);
00281     for( ;; )    
00282     {
00283         //Get the combination (icc, ipp) that minimizes the value of dstTbl.
00284         tmpVal = STB_INT_MAX;        icc = -1;        ipp = -1;
00285         for( ic = 0 ; ic < curData->cnt ;  ic++ ) 
00286         {
00287             for( ip = 0 ; ip < preData->cnt ;  ip++ )        
00288             {
00289                 if( tmpVal > dstTbl [ ip * traCntMax + ic ]  )
00290                 {
00291                     tmpVal = dstTbl [ ip * traCntMax + ic ] ;        icc = ic;    ipp = ip;
00292                 }
00293             }
00294         }
00295         if( tmpVal == STB_INT_MAX )
00296         {
00297             break;
00298         }
00299 
00300         //Link ipp and icc
00301         idCurPre[ icc    ]    = ipp    ;
00302         idPreCur[ ipp    ]    = icc    ;
00303         // steadiness
00304         TrSteadinessXYS
00305             (    
00306                 curData->posX[icc]    ,curData->posY[icc]    ,curData->size[icc]    ,
00307                 preData->posX[ipp]    ,preData->posY[ipp]    ,preData->size[ipp]    ,
00308                 &tmpX                ,&tmpY                ,&tmpS                ,
00309                 stedinessPos        ,stedinessSize
00310             );
00311         // set 
00312         wData->nTraID[tmpWCnt]    = preData->nTraID[ipp];
00313         wData->nDetID[tmpWCnt]    = curData->nDetID[icc];
00314         wData->posX  [tmpWCnt]    = tmpX    ;    
00315         wData->posY  [tmpWCnt]    = tmpY    ;    
00316         wData->size  [tmpWCnt]    = tmpS    ;    
00317         wData->conf  [tmpWCnt]    = ( ( curData->conf[icc] + preData->conf[ipp] ) /2 );
00318         wData->retryN[tmpWCnt]    = 0    ;//"It's reflected(linked) in the current frame"so that 0.
00319         tmpWCnt++;
00320         //Renewal "dstTbl" not to refer the associated data.
00321         for( ic = 0 ; ic < curData->cnt ;  ic++ ) 
00322         {
00323             dstTbl [ ipp * traCntMax + ic  ]  = STB_INT_MAX ;
00324         }
00325         for( ip = 0 ; ip < preData->cnt ;  ip++ ) 
00326         {
00327             dstTbl [ ip  * traCntMax + icc ]  = STB_INT_MAX ;
00328         }
00329 
00330         if( tmpWCnt == traCntMax  )
00331         {
00332             *wCnt = tmpWCnt;
00333             return;
00334         }
00335     }
00336 
00337     // "It is reflected in the previous frame" and "It is not reflected in the current frame".
00338     for( ip = 0 ; ip < preData->cnt ;  ip++ )        //"It's reflected in the previous frame"
00339     {
00340         if( idPreCur[ip] == LinkNot )                //"It's not reflected in the current frame"
00341         {
00342             // set 
00343             wData->nTraID[tmpWCnt]    = preData->nTraID[ip];
00344             wData->nDetID[tmpWCnt]    = -1;//"It's not reflected in the current frame so the detection number is -1"
00345             wData->posX  [tmpWCnt]    = preData->posX     [ip];
00346             wData->posY  [tmpWCnt]    = preData->posY     [ip];
00347             wData->size  [tmpWCnt]    = preData->size     [ip];
00348             wData->conf  [tmpWCnt]    = preData->conf[ip];
00349             wData->retryN[tmpWCnt]    = preData->retryN[ip]  + 1 ;//"It's not reflected in the current frame"so that +1.
00350             tmpWCnt++;
00351         }
00352         if( tmpWCnt == traCntMax)
00353         {
00354             *wCnt = tmpWCnt        ;
00355             return;
00356         }
00357     }
00358 
00359     // "It is not reflected in the previous frame" and "It is reflected in the current frame".
00360     tmpAccCnt = *cntAcc;
00361     for( ic = 0 ; ic < curData->cnt ;  ic++ )        //"It's reflected in the current frame"
00362     {
00363         if( idCurPre[ic] == LinkNot )                //"It's not reflected in the previous frame"
00364         {
00365             // set 
00366             wData->nTraID[tmpWCnt]    = tmpAccCnt;
00367             wData->nDetID[tmpWCnt]    = curData->nDetID[ic];
00368             wData->posX  [tmpWCnt]    = curData->posX     [ic];
00369             wData->posY  [tmpWCnt]    = curData->posY     [ic];
00370             wData->size  [tmpWCnt]    = curData->size     [ic];
00371             wData->conf  [tmpWCnt]    = curData->conf[ic];
00372             wData->retryN[tmpWCnt]    = 0    ;//"It's reflected in the current frame" so that 0.
00373             tmpWCnt++;
00374             tmpAccCnt++;
00375         }
00376         if( tmpWCnt == traCntMax )
00377         {
00378             *wCnt    = tmpWCnt;
00379             *cntAcc = tmpAccCnt;
00380             return;
00381         }
00382     }
00383 
00384     
00385     *wCnt    = tmpWCnt    ;
00386     *cntAcc = tmpAccCnt;
00387 
00388 
00389 }
00390 /*---------------------------------------------------------------------*/
00391 //    TrSetRes
00392 /*---------------------------------------------------------------------*/
00393 void TrSetRes( ROI_SYS* wRoi,STB_TR_RES* resData , STB_INT32* resCnt )
00394 {
00395     STB_INT32 i;
00396 
00397     *resCnt = wRoi->cnt;
00398     for( i = 0 ; i < wRoi->cnt ; i++ )
00399     {
00400         resData[i].nTraID = wRoi->nTraID[i];
00401         resData[i].nDetID = wRoi->nDetID[i];
00402         resData[i].pos.x  = wRoi->posX  [i];
00403         resData[i].pos.y  = wRoi->posY  [i];
00404         resData[i].size   = wRoi->size  [i];
00405         resData[i].conf   = wRoi->conf[i];
00406     }
00407 }
00408 /*---------------------------------------------------------------------*/
00409 //    TrEditCur
00410 /*---------------------------------------------------------------------*/
00411 void TrEditCur( ROI_SYS* wRoi,ROI_SYS* curData )
00412 {
00413     STB_INT32 i;
00414 
00415     curData->cnt = wRoi->cnt;
00416     for( i = 0 ; i < wRoi->cnt ; i++ )
00417     {
00418         curData->nTraID[i] = wRoi->nTraID[i];
00419         curData->nDetID[i] = wRoi->nDetID[i];
00420         curData->posX  [i] = wRoi->posX  [i];
00421         curData->posY  [i] = wRoi->posY  [i];
00422         curData->size  [i] = wRoi->size  [i];
00423         curData->conf  [i] = wRoi->conf  [i];
00424         curData->retryN[i] = wRoi->retryN[i];
00425     }
00426 }
00427 /*---------------------------------------------------------------------*/
00428 //    StbTrExec
00429 /*---------------------------------------------------------------------*/
00430 int  StbTrExec ( TRHANDLE handle )
00431 {
00432 
00433 
00434     /* Face --------------------------------------*/
00435     if( handle->execFlg->faceTr    == STB_TRUE )
00436     {
00437         //Move the time series of past data.
00438         TrSlideRec( handle->fcRec );
00439         //"the present data" set to the past data 
00440         TrCurRec( handle->fcRec ,handle->stbTrDet->fcDet, handle->stbTrDet->fcNum );
00441         //Calculate "stabilized current data wRoi" from "past data".
00442         TrStabilizeTR( handle->wRoi ,&(handle->wRoi->cnt) ,handle->fcRec ,&(handle->fcCntAcc) ,handle );
00443         //Set "wRoi" data to output data "resFaces".
00444         TrSetRes( handle->wRoi, handle->resFaces->face, &(handle->resFaces->cnt) );
00445         //set "wRoi" data to accumulated data (current) "fcRec [0]".
00446         TrEditCur( handle->wRoi, &(handle->fcRec[0]) );
00447     }
00448 
00449     /* Body --------------------------------------*/
00450     if( handle->execFlg->bodyTr    == STB_TRUE )
00451     {
00452         //Move the time series of past data.
00453         TrSlideRec( handle->bdRec );
00454         //"the present data" set to the past data 
00455         TrCurRec( handle->bdRec ,handle->stbTrDet->bdDet ,handle->stbTrDet->bdNum );
00456         //Calculate "stabilized current data wRoi" from "past data".
00457         TrStabilizeTR( handle->wRoi ,&(handle->wRoi->cnt) , handle->bdRec, &(handle->bdCntAcc) , handle    );    
00458         //Set "wRoi" data to output data "resFaces".
00459         TrSetRes( handle->wRoi, handle->resBodys->body, &(handle->resBodys->cnt) );
00460         //set "wRoi" data to accumulated data (current) "bdRec [0]".
00461         TrEditCur( handle->wRoi, &(handle->bdRec[0]) );
00462     }
00463 
00464 
00465     return STB_NORMAL;
00466 }
00467 
00468 
00469