海思3559实现KCF算法

这里我参考IVE模块，采用双通道VPSS，实现KCF的跟踪功能。
具体流程为：摄像头读取图像数据实时通过HDMI输出显示，中间经过VI-VPSS-VO。
1、编写cmake文件

CMAKE_MINIMUM_REQUIRED(VERSION 3.18)

project(3559Project)
SET(CMAKE_CXX_STANDARD 11)
# 设置编译器选项：O3优化，显示所有警告
SET(CMAKE_C_FLAGS "$ENV{CFLAGS} -O3 -Wall -g -lstdc++  -mcpu=cortex-a53")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2  -std=c++11 -Wall -g -lstdc++ ")

SET(CROSS_TOOLCHAIN_PREFIX "/opt/hisi-linux/x86-arm/aarch64-himix100-linux/bin")
SET(CMAKE_C_COMPILER ${CROSS_TOOLCHAIN_PREFIX}/aarch64-himix100-linux-gcc)
SET(CMAKE_CXX_COMPILER ${CROSS_TOOLCHAIN_PREFIX}/aarch64-himix100-linux-g++)

MESSAGE(STATUS "CMAKE_C_COMPILER:" ${CMAKE_C_COMPILER})
MESSAGE(STATUS "CMAKE_CXX_COMPILER:" ${CMAKE_CXX_COMPILER})

# 设置Cmake查找主路径
SET(CMAKE_FIND_ROOT_PATH /opt/hisi-linux/x86-arm/aarch64-himix100-linux)

# hisi
SET(HI_INCLUDE ${PROJECT_SOURCE_DIR}/thirdParty/Hisi/include)
SET(HI_LIB ${PROJECT_SOURCE_DIR}/thirdParty/Hisi/lib)
SET(HI_SVP_COMMON_INCLUDE ${PROJECT_SOURCE_DIR}/src/include)
INCLUDE_DIRECTORIES(${HI_INCLUDE})
INCLUDE_DIRECTORIES(${HI_SVP_COMMON_INCLUDE})
LINK_DIRECTORIES(${HI_LIB})

#opencv
SET(OPENCV_INCLUDE ${PROJECT_SOURCE_DIR}/thirdParty/opencv3.4.1/include)
SET(OPENCV_LIB ${PROJECT_SOURCE_DIR}/thirdParty/opencv3.4.1/lib)

MESSAGE(STATUS "OPENCV_INCLUDE:" ${OPENCV_INCLUDE})
MESSAGE(STATUS "OPENCV_LIB:" ${OPENCV_LIB})

#SET(OPENCV_INCLUDE /home/zynq/build_hi3559a/install/include)
#SET(OPENCV_LIB /home/zynq/build_hi3559a/install/lib)
INCLUDE_DIRECTORIES(${OPENCV_INCLUDE})
LINK_DIRECTORIES(${OPENCV_LIB})

FILE(GLOB OPENCV_LIBS "${OPENCV_LIB}/*")
FILE(GLOB HI_LIBS "${HI_LIB}/*")

MESSAGE(STATUS "OPENCV_LIBS:" ${OPENCV_LIBS})

#添加源文件路径
AUX_SOURCE_DIRECTORY(${PROJECT_SOURCE_DIR}/src/. SRC)
AUX_SOURCE_DIRECTORY(${PROJECT_SOURCE_DIR}/src/common/. SRC)

add_definitions(-DSENSOR0_TYPE=SONY_IMX334_MIPI_8M_30FPS_12BIT)
add_definitions(-DSENSOR1_TYPE=SONY_IMX334_MIPI_8M_30FPS_12BIT)
add_definitions(-DSENSOR2_TYPE=SONY_IMX334_MIPI_8M_30FPS_12BIT)
add_definitions(-DSENSOR3_TYPE=SONY_IMX334_MIPI_8M_30FPS_12BIT)
add_definitions(-DSENSOR4_TYPE=SONY_IMX277_SLVS_8M_30FPS_12BIT)
add_definitions(-DSENSOR5_TYPE=SONY_IMX277_SLVS_8M_30FPS_12BIT)
add_definitions(-DSENSOR6_TYPE=SONY_IMX277_SLVS_8M_30FPS_12BIT)
add_definitions(-DSENSOR7_TYPE=SONY_IMX277_SLVS_8M_30FPS_12BIT)



ADD_EXECUTABLE(sample_kcf_main ${SRC})
TARGET_LINK_LIBRARIES(sample_kcf_main ${HI_LIBS} -lpthread -lrt -ldl -lm)
TARGET_LINK_LIBRARIES(sample_kcf_main ${OPENCV_LIBS} -lpthread -lrt -ldl -lm)

2、主函数

#include <iostream>
#include "kcftracker.hpp"

#include<opencv2/opencv.hpp>

#ifdef __cplusplus
extern "C" {
#endif

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <pthread.h>
#include <sys/prctl.h>
#include <math.h>

#include "hi_common.h"
#include "hi_comm_sys.h"
#include "hi_comm_svp.h"
#include "sample_comm.h"
#include "sample_comm_svp.h"
#include "sample_comm_nnie.h"
#include "hi_comm_vgs.h"


#include "sample_comm_ive.h"
#ifdef __cplusplus
}
#endif




static HI_BOOL s_bNnieStopSignal = HI_FALSE;
static SAMPLE_IVE_SWITCH_S s_stRfcnSwitch = {HI_FALSE,HI_FALSE};

static SAMPLE_VI_CONFIG_S s_stViConfig;

static pthread_t s_hNnieThread = 0;




typedef struct tagIPC_IMAGE{
	HI_U64      u64PhyAddr;
	HI_U64      u64VirAddr;
	HI_U32      u32Width;
	HI_U32      u32Height;
}IPC_IMAGE;
 

HI_S32 yuvFrame2rgb(VIDEO_FRAME_INFO_S *srcFrame,IPC_IMAGE *dstImage)
{
    IVE_HANDLE hIveHandle;
    IVE_SRC_IMAGE_S pstSrc;
    IVE_DST_IMAGE_S pstDst;
    IVE_CSC_CTRL_S stCscCtrl;
    HI_S32 s32Ret = 0;
    stCscCtrl.enMode = IVE_CSC_MODE_PIC_BT709_YUV2RGB;//IVE_CSC_MODE_VIDEO_BT601_YUV2RGB;
    pstSrc.enType = IVE_IMAGE_TYPE_YUV420SP;
    pstSrc.au64VirAddr[0]=srcFrame->stVFrame.u64VirAddr[0];
    pstSrc.au64VirAddr[1]=srcFrame->stVFrame.u64VirAddr[1];
    pstSrc.au64VirAddr[2]=srcFrame->stVFrame.u64VirAddr[2];
 
    pstSrc.au64PhyAddr[0]=srcFrame->stVFrame.u64PhyAddr[0];
    pstSrc.au64PhyAddr[1]=srcFrame->stVFrame.u64PhyAddr[1];
    pstSrc.au64PhyAddr[2]=srcFrame->stVFrame.u64PhyAddr[2];
 
    pstSrc.au32Stride[0]=srcFrame->stVFrame.u32Stride[0];
    pstSrc.au32Stride[1]=srcFrame->stVFrame.u32Stride[1];
    pstSrc.au32Stride[2]=srcFrame->stVFrame.u32Stride[2];
 
    pstSrc.u32Width = srcFrame->stVFrame.u32Width;
    pstSrc.u32Height = srcFrame->stVFrame.u32Height;
 
    pstDst.enType = IVE_IMAGE_TYPE_U8C3_PACKAGE;
    pstDst.u32Width   = pstSrc.u32Width;
    pstDst.u32Height  = pstSrc.u32Height;
    pstDst.au32Stride[0]  = pstSrc.au32Stride[0];
    pstDst.au32Stride[1]  = 0;
    pstDst.au32Stride[2]  = 0;
    s32Ret = HI_MPI_SYS_MmzAlloc_Cached(&pstDst.au64PhyAddr[0], (void **)&pstDst.au64VirAddr[0], "User", HI_NULL, pstDst.u32Height*pstDst.au32Stride[0]*3);
    if(HI_SUCCESS != s32Ret)
    {       
        HI_MPI_SYS_MmzFree(pstDst.au64PhyAddr[0], (void *)pstDst.au64VirAddr[0]);
        std::cout<<"HI_MPI_SYS_MmzAlloc_Cached Failed with 0x"<<s32Ret<<std::endl;
        return s32Ret;
    }
    s32Ret = HI_MPI_SYS_MmzFlushCache(pstDst.au64PhyAddr[0], (void *)pstDst.au64VirAddr[0], pstDst.u32Height*pstDst.au32Stride[0]*3);
    if(HI_SUCCESS != s32Ret)
    {       
        HI_MPI_SYS_MmzFree(pstDst.au64PhyAddr[0], (void *)pstDst.au64VirAddr[0]);
        std::cout<<"HI_MPI_SYS_MmzFlushCache Failed with 0x"<<s32Ret<<std::endl;
        return s32Ret;
    }
    memset((void *)pstDst.au64VirAddr[0], 0, pstDst.u32Height*pstDst.au32Stride[0]*3);
    HI_BOOL bInstant = HI_TRUE;
    s32Ret = HI_MPI_IVE_CSC(&hIveHandle,&pstSrc,&pstDst,&stCscCtrl,bInstant);
    if(HI_SUCCESS != s32Ret)
    {       
        HI_MPI_SYS_MmzFree(pstDst.au64PhyAddr[0], (void *)pstDst.au64VirAddr[0]);
        std::cout<<"HI_MPI_IVE_CSC Failed with 0x"<<s32Ret<<std::endl;
        return s32Ret;
    }
    if (HI_TRUE == bInstant)
    {
        HI_BOOL bFinish = HI_TRUE;
        HI_BOOL bBlock = HI_TRUE;
        s32Ret = HI_MPI_IVE_Query(hIveHandle,&bFinish,bBlock);
        while(HI_ERR_IVE_QUERY_TIMEOUT == s32Ret)
        {
            usleep(100);
            s32Ret = HI_MPI_IVE_Query(hIveHandle,&bFinish,bBlock);
        }
    }
    dstImage->u64PhyAddr = pstDst.au64PhyAddr[0];
    dstImage->u64VirAddr = pstDst.au64VirAddr[0];
    dstImage->u32Width = pstDst.u32Width;
    dstImage->u32Height = pstDst.u32Height;
    return s32Ret;
}


HI_S32 frame2Mat(VIDEO_FRAME_INFO_S *srcFrame,cv::Mat &dstMat)
{
    HI_U32 w = srcFrame->stVFrame.u32Width;
    HI_U32 h = srcFrame->stVFrame.u32Height;
    int bufLen = w*h*3;
    HI_U8 *srcRGB = NULL;
    IPC_IMAGE dstImage;
    HI_S32 s32Ret = 0;
    s32Ret = yuvFrame2rgb(srcFrame,&dstImage);
    if(s32Ret!=HI_SUCCESS){
        std::cout<<"yuvFrame2rgb Err ."<<std::endl;
        return s32Ret;
    }
    srcRGB = (HI_U8 *)dstImage.u64VirAddr;
    dstMat.create(h, w, CV_8UC3);
    memcpy(dstMat.data, srcRGB, bufLen*sizeof(HI_U8));
    HI_MPI_SYS_MmzFree(dstImage.u64PhyAddr, (void *)&(dstImage.u64VirAddr));
    return HI_SUCCESS;
}
 





/*****************************************************************************
*   Prototype    : SAMPLE_COMM_SVP_NNIE_FillRect
*   Description  : Draw rect
*   Input        : VIDEO_FRAME_INFO_S             *pstFrmInfo   Frame info
* 		            SAMPLE_SVP_NNIE_RECT_ARRAY_S  *pstRect       Rect
*                  HI_U32                         u32Color      Color
*
*
*   Output       :
*   Return Value : HI_S32,HI_SUCCESS:Success,Other:failure
*****************************************************************************/
HI_S32 SAMPLE_COMM_KCF_FillRect(VIDEO_FRAME_INFO_S *pstFrmInfo, cv::Rect &roi, HI_U32 u32Color
                                ,HI_U32 u32SrcWidth, HI_U32 u32SrcHeight,HI_U32 u32DstWidth,HI_U32 u32DstHeight)
{
    VGS_HANDLE VgsHandle = -1;
    HI_S32 s32Ret = HI_SUCCESS;
    
    VGS_TASK_ATTR_S stVgsTask;
    VGS_ADD_COVER_S stVgsAddCover;
    static HI_U32 u32Frm = 0;
    u32Frm++;

    s32Ret = HI_MPI_VGS_BeginJob(&VgsHandle);
    if (s32Ret != HI_SUCCESS)
    {
        SAMPLE_PRT("Vgs begin job fail,Error(%#x)\n", s32Ret);
        return s32Ret;
    }

    memcpy(&stVgsTask.stImgIn, pstFrmInfo, sizeof(VIDEO_FRAME_INFO_S));
    memcpy(&stVgsTask.stImgOut, pstFrmInfo, sizeof(VIDEO_FRAME_INFO_S));

    stVgsAddCover.enCoverType = COVER_QUAD_RANGLE;
    stVgsAddCover.u32Color = u32Color;
    stVgsAddCover.stQuadRangle.bSolid = HI_FALSE;
    stVgsAddCover.stQuadRangle.u32Thick = 6;


    //memcpy(stVgsAddCover.stQuadRangle.stPoint, pstRect->astRect[i][j].astPoint, sizeof(pstRect->astRect[i][j].astPoint));
    stVgsAddCover.stQuadRangle.stPoint[0].s32X=(HI_S32)((HI_FLOAT)roi.x/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1) ;
    stVgsAddCover.stQuadRangle.stPoint[0].s32Y=(HI_S32)((HI_FLOAT)roi.y/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1) ;

    stVgsAddCover.stQuadRangle.stPoint[2].s32X=(HI_S32)((HI_FLOAT)(roi.x + roi.width)/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1) ;
    stVgsAddCover.stQuadRangle.stPoint[2].s32Y=(HI_S32)((HI_FLOAT)roi.y/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1);

    stVgsAddCover.stQuadRangle.stPoint[1].s32X=(HI_S32)((HI_FLOAT)roi.x/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1);
    stVgsAddCover.stQuadRangle.stPoint[1].s32Y=(HI_S32)((HI_FLOAT)(roi.y + roi.height)/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1);

    stVgsAddCover.stQuadRangle.stPoint[3].s32X=(HI_S32)((HI_FLOAT)(roi.x + roi.width)/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1);
    stVgsAddCover.stQuadRangle.stPoint[3].s32Y=(HI_S32)((HI_FLOAT)(roi.y + roi.height)/(HI_FLOAT)u32SrcWidth * (HI_FLOAT)u32DstWidth) & (~1);
    
    //printf("3_x=%d, 3_y=%d 3_width=%d, 3_height=%d \n", _roi.x, _roi.y, _roi.width, _roi.height);
    //if(_roi.x%2!=0) _roi.x=_roi.x+1;
    //if(_roi.y%2!=0) _roi.y=_roi.y+1;
    
    s32Ret = HI_MPI_VGS_AddCoverTask(VgsHandle, &stVgsTask, &stVgsAddCover);
    if (s32Ret != HI_SUCCESS)
    {
        SAMPLE_PRT("HI_MPI_VGS_AddCoverTask fail,Error(%#x)\n", s32Ret);
        HI_MPI_VGS_CancelJob(VgsHandle);
        return s32Ret;
    }   

    s32Ret = HI_MPI_VGS_EndJob(VgsHandle);
    if (s32Ret != HI_SUCCESS)
    {
        SAMPLE_PRT("HI_MPI_VGS_EndJob fail,Error(%#x)\n", s32Ret);
        HI_MPI_VGS_CancelJob(VgsHandle);
        return s32Ret;
    }

    return s32Ret;

}



/******************************************************************************
* function : Rfcn vi to vo thread entry
******************************************************************************/
static HI_VOID* SAMPLE_KCF_ViToVo(HI_VOID* pArgs)
{
    HI_S32 s32Ret;
    VIDEO_FRAME_INFO_S stBaseFrmInfo;
    VIDEO_FRAME_INFO_S stExtFrmInfo;
    HI_S32 s32MilliSec = 20000;
    VO_LAYER voLayer = 0;
    VO_CHN voChn = 0;
    HI_S32 s32VpssGrp = 0;
    HI_S32 as32VpssChn[] = {VPSS_CHN0, VPSS_CHN1};

    VPSS_CHN_ATTR_S stpstChnAttr[4];
    
    //memset(stpstChnAttr,0,sizeof(VPSS_CHN_ATTR_S*));
    cv::Mat frame;
    cv::Rect roi;
    cv::Rect result;
    int cnt=0;

	bool HOG = true;
	bool FIXEDWINDOW = true;
	bool MULTISCALE = false;
	
	bool LAB = false;

	// Create KCFTracker object
	KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB);
    clock_t start = 0;
    clock_t end = 0;
    result.x=2;
    result.y=2;
    result.width=100;
    result.height=100;
    while (HI_FALSE == s_bNnieStopSignal)
    {
        cnt += 1;
        s32Ret = HI_MPI_VPSS_GetChnFrame(s32VpssGrp, as32VpssChn[1], &stExtFrmInfo, s32MilliSec);
        if(HI_SUCCESS != s32Ret)
        {
            SAMPLE_PRT("Error(%#x),HI_MPI_VPSS_GetChnFrame failed, VPSS_GRP(%d), VPSS_CHN(%d)!\n",
                s32Ret,s32VpssGrp, as32VpssChn[1]);
            continue;
        }

        s32Ret = HI_MPI_VPSS_GetChnFrame(s32VpssGrp, as32VpssChn[0], &stBaseFrmInfo, s32MilliSec);
        SAMPLE_CHECK_EXPR_GOTO(HI_SUCCESS!=s32Ret, EXT_RELEASE,
            "Error(%#x),HI_MPI_VPSS_GetChnFrame failed, VPSS_GRP(%d), VPSS_CHN(%d)!\n",
            s32Ret,s32VpssGrp, as32VpssChn[0]);
        
        s32Ret = HI_MPI_VPSS_GetChnAttr(s32VpssGrp,as32VpssChn[0],stpstChnAttr);


        std::cout<<"base:"<<(stpstChnAttr[0].u32Width)<<" "<<(stpstChnAttr[0].u32Height)<<std::endl;
        frame2Mat(&stExtFrmInfo,frame);
        std::cout<<frame.rows<<" "<<frame.cols<<std::endl;
        start=clock();
        if(cnt == 1)
        {
            roi.x=2;
            roi.y=2;
            roi.width=100;
            roi.height=100;
            tracker.init(roi, frame);
        }
        else
        {
            
            
		    result = tracker.update(frame);

            if(result.width<1||result.height<1)
            {
                cnt=0;
                result.x=2;
                result.y=2;
                result.width=100;
                result.height=100;
                continue;
            }
        }

        end=clock();
        std::cout<<(double)(end-start)/1000<<std::endl;


        //Draw rect
        s32Ret = SAMPLE_COMM_KCF_FillRect(&stBaseFrmInfo, result,\
                                        frame.cols,frame.rows,stpstChnAttr[0].u32Width,stpstChnAttr[0].u32Height, 0x0000FF00);
        SAMPLE_CHECK_EXPR_GOTO(HI_SUCCESS!=s32Ret, BASE_RELEASE,
            "SAMPLE_COMM_SVP_NNIE_FillRect failed, Error(%#x)!\n", s32Ret);

        s32Ret = HI_MPI_VO_SendFrame(voLayer, voChn, &stBaseFrmInfo, s32MilliSec);
        SAMPLE_CHECK_EXPR_GOTO(HI_SUCCESS!=s32Ret, BASE_RELEASE,
            "HI_MPI_VO_SendFrame failed, Error(%#x)!\n", s32Ret);

        BASE_RELEASE:
            s32Ret = HI_MPI_VPSS_ReleaseChnFrame(s32VpssGrp,as32VpssChn[0], &stBaseFrmInfo);
            if (HI_SUCCESS != s32Ret)
            {
                SAMPLE_PRT("Error(%#x),HI_MPI_VPSS_ReleaseChnFrame failed,Grp(%d) chn(%d)!\n",
                    s32Ret,s32VpssGrp,as32VpssChn[0]);
            }

        EXT_RELEASE:
            s32Ret = HI_MPI_VPSS_ReleaseChnFrame(s32VpssGrp,as32VpssChn[1], &stExtFrmInfo);
            if (HI_SUCCESS != s32Ret)
            {
                SAMPLE_PRT("Error(%#x),HI_MPI_VPSS_ReleaseChnFrame failed,Grp(%d) chn(%d)!\n",
                    s32Ret,s32VpssGrp,as32VpssChn[1]);
            }

    }

    return HI_NULL;
}




void SAMPLE_KCF(void)
{
    PIC_SIZE_E enSize = PIC_1080P;
    SIZE_S stSize;

    HI_S32 s32Ret = HI_SUCCESS;
    HI_CHAR acThreadName[16] = {0};
    memset(&s_stViConfig,0,sizeof(SAMPLE_VI_CONFIG_S));
    /******************************************
     step 1: start vi vpss vo
     ******************************************/
    s_stRfcnSwitch.bVenc = HI_FALSE;
    s_stRfcnSwitch.bVo   = HI_TRUE;
    s32Ret = SAMPLE_COMM_IVE_StartViVpssVencVo(&s_stViConfig,&s_stRfcnSwitch,&enSize);
    SAMPLE_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret, END_RFCN_0,
        "Error(%#x),SAMPLE_COMM_IVE_StartViVpssVencVo failed!\n", s32Ret);
        s32Ret = SAMPLE_COMM_SYS_GetPicSize(enSize, &stSize);
    SAMPLE_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret, END_RFCN_0,
        "Error(%#x),SAMPLE_COMM_SYS_GetPicSize failed!\n", s32Ret);
           s_bNnieStopSignal = HI_FALSE;

    /******************************************
      step 2: Create work thread
     ******************************************/
    snprintf(acThreadName, 16, "KCF_ViToVo");
    prctl(PR_SET_NAME, (unsigned long)acThreadName, 0,0,0);
    pthread_create(&s_hNnieThread, 0, SAMPLE_KCF_ViToVo, NULL);

    SAMPLE_PAUSE();

    s_bNnieStopSignal = HI_TRUE;
    pthread_join(s_hNnieThread, HI_NULL);
    s_hNnieThread = 0;

END_RFCN_0:
    SAMPLE_COMM_IVE_StopViVpssVencVo(&s_stViConfig,&s_stRfcnSwitch);
    return ;
}

int main()
{
    SAMPLE_KCF();
    return 0;
}

3、全部代码还有很多优化的地方，比如KCF的优化，内存读写的优化等。