3516camshift实现

3516camshift实现

//H分量的camshift 跟踪
static HI_VOID *CYF_Cam_Proc_H(HI_VOID *pArgs)
{
    HI_U32 srcWidth = 768;
    HI_U32 srcHeight = 576;
    HI_U32 srcSize = 768*576;
    HI_S32 s32Ret;
    VI_FRAME_INFO_S stFrameInfo;
    VI_CHN viIveChn = 2;
    VENC_CHN vencChn = 0;
    IVE_HANDLE IveHandle;

    my_CvTermCriteria mycriteria;
    mycriteria.epsilon = 1;
    mycriteria.max_iter =10;
    my_CvBox2D camBox;
    HI_BOOL bHistDone = HI_FALSE;  

    HI_U32  *pImage = NULL;
    HI_U8 *pbuf_H;
    HI_U8 *pbuf_roiH;   //窗口y分量

    HI_U8 *pNormHist = NULL;
    HI_U8 *pBackPrj = NULL;  //反向投影结果


    IVE_SRC_INFO_S stSrcRoi;   
    IVE_MEM_INFO_S stDstRoi;


    IVE_IMAGE_S stHueRoi;   //结构体内含有物理地址和虚拟地址  
    IVE_IMAGE_S stHistRoi;


    IVE_MEM_INFO_S stDst;
    HI_VOID *pVirtDst;
    HI_VOID *pVirtDst2;

    CYF_IVE_MD_S pstMd_RGB;
    my_CvRect rect_rio;  //跟踪窗口区域，camshift会自动更新它


    //原始size的H通道分配空间
    pbuf_H = (HI_U8*)malloc(srcSize);   
    memset(pbuf_H,1,srcSize);

    //ROI区域的H通道分配空间
    pbuf_roiH = (HI_U8*)malloc(srcSize);    
    memset(pbuf_roiH,1,srcSize);

    //归一化直方图分配空间
    pNormHist = (HI_U8*)malloc(256);
    memset(pNormHist,1,256);

    //反向投影分配空间
    pBackPrj = (HI_U8*)malloc(srcSize);
    memset(pBackPrj,1,srcSize);




/******************************** step 1 初始化，分配内存等************************/


        //为yuv转rgb的目的地址分配空间，需要ive操作，用cached 分配
    s32Ret =  HI_MPI_SYS_MmzAlloc_Cached(&pstMd_RGB.stSrc.u32PhyAddr[0], (void**)&pstMd_RGB.stSrc.pu8VirAddr[0],NULL, HI_NULL, srcSize*3);
    if(s32Ret==HI_FAILURE)
    {
        printf("HI_MPI_SYS_MmzAlloc_Cached RGB failed!\n");
        return HI_FAILURE;
    }

    //ROI图像数据(H分量)开辟物理地址
    s32Ret =  HI_MPI_SYS_MmzAlloc_Cached(&stHueRoi.u32PhyAddr[0], (void**)&stHueRoi.pu8VirAddr[0],NULL, HI_NULL, srcSize);
    if(s32Ret==HI_FAILURE)
    {
        printf("HI_MPI_SYS_MmzAlloc_Cached ROI_H failed!\n");
        return ;
    }

    //ROI直方图操作后的目的区域开辟物理地址
    s32Ret =  HI_MPI_SYS_MmzAlloc_Cached(&stHistRoi.u32PhyAddr[0], (void**)&stHistRoi.pu8VirAddr[0],NULL, HI_NULL, 256*4);  //32位存储，4字节
    if(s32Ret==HI_FAILURE)
    {
        printf("HI_MPI_SYS_MmzAlloc_Cached Hist failed!\n");
        return ;
    }


    PhyAddr_Free1 =stHueRoi.u32PhyAddr[0];
    PhyAddr_Free2 =pstMd_RGB.stSrc.u32PhyAddr[0];
    PhyAddr_Free3 = stHistRoi.u32PhyAddr[0];


    signal(SIGINT, CYF_CAM_HandleSig);
    signal(SIGTERM, CYF_CAM_HandleSig);


    /******************************** step 2 设置帧缓存为1帧************************/

    if (HI_MPI_VI_SetFrameDepth(viIveChn, 1))
    {        
        printf("HI_MPI_VI_SetFrameDepth err, viIveChn chn %d \n", viIveChn);
        return ;
    }

    usleep(50000);



    while (1)
    {



        if(stCamObj.bisDetect == HI_TRUE) //如果检测到有效最大目标的话，开启viive通道做跟踪
         {

                /******************************** step 3 获取帧***********************/

            printf("find the CamObj ,start to get frame \n");

            s32Ret = HI_MPI_VI_GetFrame(viIveChn, &stFrameInfo);
            if (HI_SUCCESS != s32Ret)
            {
                printf("HI_MPI_VI_GetFrame fail,viIveChn(%d),Error(%#x)\n",viIveChn,s32Ret);
                return;
            }
            else
            {
                //printf("HI_MPI_VI_GetFrame sucess,(%#x)\n",s32Ret);
            }



            /******************************** step 4  调取原始帧的H 通道分量***********************/

            /********调用ive将原始帧转换成rgb数据,以BRG_pack顺序存储 *********/
            CYF_GetRGB_from_Frame(&stFrameInfo,&pstMd_RGB);   

            //查看像素
            /*
            HI_U8 *pVirtDst2 = pstMd_RGB.stSrc.pu8VirAddr[0];
            int pix = (int)(*(pVirtDst2+10000));
            printf("pbuf_ 101 is %d\n", pix);
            */


            /**********RGB转HS
            V 只得到H分量********/
            pVirtDst2 = pstMd_RGB.stSrc.pu8VirAddr[0];

            //保存rgb图
            //printf("the last pix is %d \n", *((uchar*)(pVirtDst2+srcSize*3-1000)));

            cyf_Get_H_from_RGB((uchar*)pVirtDst2,pbuf_H,srcWidth,srcHeight);

            if( bHistDone == FALSE)
            {
                //cyf_saveBMPFile((uchar*)pVirtDst2, srcWidth, srcHeight, "/home/rgb.dib"); 

                 //GenBmpFile((U8*)pVirtDst2, 24, srcWidth, srcHeight, "/home/out.bmp");//生成BMP文件 
            }


            //printf("cyf_Get_H_from_RGB \n");


            if( bHistDone == FALSE)   //做窗口直方图，测试时值做一次

            {

                //************提取roi区域里的H数据  ,做直方图的要求是宽高大于64x64 16字节对齐   


                rect_rio.x = stCamObj.stPosition.u16Left+10;        
                rect_rio.y = stCamObj.stPosition.u16Top+80;
                rect_rio.width = (stCamObj.stPosition.u16Right - stCamObj.stPosition.u16Left)/16*16;
                rect_rio.height = (stCamObj.stPosition.u16Bottom - stCamObj.stPosition.u16Top)/16*16;


                cyf_get_RIO_from_Y(pbuf_roiH, pbuf_H ,srcWidth,srcHeight, rect_rio );

                    //printf("cyf_get_RIO_from_Y \n");
                //int pix = (int)(*(pbuf_roiH+100));
                    //printf("pbuf_roiH100 is %d\n", pix);


                /******************************** step 5  对ROI区域做直方图(调用ive)**********************/

                // 将roi的H分量数据搬移到分配的物理地址对应的虚拟地址上 
                //cyf_roi_copyto_ive 自动将roi区域的长宽信息赋值给stHueRoi。
                cyf_roi_copyto_ive(&stHueRoi,(HI_U8*)pbuf_roiH,rect_rio.width,rect_rio.height);   //做直方图是可能需要8或者16字节对齐
                printf("cyf_roi_copyto_ive \n");


                //调用ive对stHueRoi的数据做ive的直方图操作，调用以后数据放在stHistRoi对应的物理和虚拟地址上了
                //cyf_getHist_from_Ive(&stHistRoi, &stHueRoi);


                //给ive函数参数的两个地址结构体赋值，分别对应上述已经分配好的物理地址
                HI_BOOL bFinish, bBlock;
                HI_BOOL bInstant = HI_TRUE;
                stSrcRoi.enSrcFmt = IVE_SRC_FMT_SINGLE;
                stSrcRoi.stSrcMem.u32PhyAddr = stHueRoi.u32PhyAddr[0];
                stSrcRoi.stSrcMem.u32Stride = stHueRoi.u32Width;
                stSrcRoi.u32Width = stHueRoi.u32Width;
                stSrcRoi.u32Height = stHueRoi.u32Height;

                stDstRoi.u32Stride = stHueRoi.u32Width;
                stDstRoi.u32PhyAddr = stHistRoi.u32PhyAddr[0];

                //  printf(" u32PhyAddr is %u \n", stHueRoi.u32PhyAddr[0]);

                s32Ret = HI_MPI_IVE_HIST(&IveHandle, &stSrcRoi,&stDstRoi,  bInstant);  //操作后将结果指针指向分配好的地址上
                  if (HI_SUCCESS != s32Ret)
                {
                    printf("%s: HI_MPI_IVE_HIST failed!Error(%#x)\n", s32Ret);
                    return ;
                }

                printf("HI_MPI_IVE_HIST \n");

                s32Ret  = HI_MPI_IVE_Query(IveHandle, &bFinish, bBlock);        
                if(s32Ret != HI_SUCCESS)
                {
                    printf("HI_MPI_IVE_Query fail,Error(%#x)\n",s32Ret);
                    return;
                }



                    //ive调用成功后只需要对物理地址对应的虚拟地址操作就可以了。


                    //usleep(10000);
                //  pImage = (HI_U32*)stHistRoi.pu8VirAddr[0];
                //  HI_U32 pix3 = (HI_U32)(*(pImage+10));

                //  printf("pix his is %u       \n",pix3);



                //归一化直方图 到(0-255)    @@@@@@@@@@@@@@@@注意，直方图操作后数据是4字节保存的，地址是(HI_U32*)
                pImage = (HI_U32*)stHistRoi.pu8VirAddr[0];
                cyf_Hist_normalize(pNormHist, (unsigned int*)pImage, 0, 255,256); //直方图有256个bin .归一到{0-255}

                printf(" *********stCamObj's  Hist normalize done  ******************    \n");

                bHistDone = HI_TRUE;
            }


        /******************************** step 6  对原始帧的H分量做反向投影**********************/
            cyf_calcBackProject(pbuf_H ,pBackPrj , pNormHist ,srcWidth, srcHeight);
            printf("cyf_calcBackProject done        \n");


        /******************************** step 7.camshift调用c接口函数?*********************/             

        //  camBox = interfaces( pBackPrj,&rect_rio, mycriteria , srcWidth, srcHeight);

        printf("myCamShift2 before rect_rio.x is %d   \n",rect_rio.x);
        camBox= myCamShift2( pBackPrj,&rect_rio, mycriteria , 768, 576  ) ; 



            stCamRectShow.x = camBox.center.x - camBox.size.width/2;
            stCamRectShow.y = camBox.center.y - camBox.size.height/2 ;  
            stCamRectShow.height = camBox.size.height;
            stCamRectShow.width = camBox.size.width;

            printf("cambox center x =  done  %f\n ",rect_rio.x);



            s32Ret = HI_MPI_VI_ReleaseFrame(viIveChn, &stFrameInfo);
            if (HI_SUCCESS != s32Ret)
            {
                printf("HI_MPI_VI_ReleaseFrame fail,viIveChn(%d),Error(%#x)\n",viIveChn,s32Ret);
            }


        }


        usleep(10000);




    }