Kernal-Based Object Tracking---基于核函数的目标跟踪

这篇博文主要实现了2003年发表于PAMI上的文章Kernal-Based Object Tracking，读者可以去读原始的文章了解其中的原理，或者参考本人的博文

   Mean Shift原理  与  最大化巴氏系数和Mean Shift的关系

下面是这个算法的主要流程图：

上述流程图中所提的（17），（19），（22）式在我的博文  巴氏系数和Mean Shift 中可以找到。

另外还要说说该算法的尺度自适应问题。

论文中是这样说的：在上一帧的窗口尺寸的基础上，在当前帧上分三次运行Mean Shift迭代过程，第一次的窗口尺寸比上一帧的尺寸小一点，第二次迭代的窗口尺寸是上一帧的尺寸，第三次的窗口尺寸比上一帧的尺寸大一点。就这样，总共得到了三次运行Mean Shift 算法的最好结果：共有三个巴氏系数。选择其中巴氏系数最大的那个窗口尺寸作为当前帧上的最佳尺寸。同时为了让跟踪窗口的尺寸调整更加平滑，将上述最佳尺寸与上一帧的尺寸做个加权组合（也可以说是一阶滤波）。将滤波后的尺寸作为当前帧上目标的最终尺寸。在我的程序中，实现自适应尺度的Mean Shift 的函数为：

                      int MyScaleAdaptMeanShift3D(...)

也可以通过参数isScaleAdapt来控制是否开启尺度自适应。

下面请看完整代码（基于三维直方图模型）：

// KernalBasedMeanShiftVideoTracking_ScaleAdaptation.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include 
  
   
#include 
   
    
#include 
    
     

using namespace std;
using namespace cv;

Mat image;
 
bool selectObject = false; //selectObject的初始值为false,
int trackObject = 0;  //trackObject的初值是0
bool showTrace = true; //是否显示MeanShift迭代收敛轨迹
Point origin;
Rect selection;

//设定直方图中bin的数目
int histSize[] = /*{16,16,16}*/ {32,32,32};
vector
     
       select_channels(3);
//设定取值范围
float range[] = {0,256};//灰度特征的范围都是[0,256)
const float* histRange[] = {range,range,range};  
bool uniform = true; //均匀分布，
bool accumu = false;//无累加
  
//定义一个核函数指针，该函数指针可以指向任意一个标准的核函数
typedef float (*KernalFunc)(Point2f& center,Point2f& xy,int hx,int hy);
int weight_method = 0; //选择核函数 ==0：Epanechnikov；==1：Gaussian
KernalFunc  DeriveFuncOfKernal ;//声明一个指向核函数的导函数的函数指针变量

//用鼠标选择目标图像区域
static void onMouse( int event, int x, int y, int, void* );
//该函数用于计算给定矩形图像块的加权直方图
static void CalculateWeightedHistogram(const Mat& img,vector
      
       & channels,Mat& _hist,int weighted_method);
//该函数用于计算给定矩形图像块的非加权直方图
static void CalculateHistogram(const Mat& img , Mat& _hist);
//自己写的计算图像一维直方图的函数
void  myCalcHist3D( const Mat& image, vector
       
        & channels,OutputArray _hist, int dims, const int* histSize,const float** ranges); //计算图像一维加权直方图的函数 static void myCalcWeightedHist3D( const Mat& image, vector
        
         & channels,OutputArray _hist, int dims, const int* histSize,const float** ranges,KernalFunc kernal); /*核函数：Epanechnikov Kernal*/ static float EpanechnikovKernalFunc(Point2f& center,Point2f& xy,int hx,int hy); /*核函数的负导数：Derivation of Epanechnikov Kernal*/ static float DerivationOfEpanechnikovKernal(Point2f& center,Point2f& xy,int hx,int hy); /*核函数：Gaussian Kernal */ static float GaussianKernalFunc(Point2f& center,Point2f& xy,int hx,int hy); /*核函数的负导数：Derivation of Gaussian Kernal*/ static float DerivationOfGaussianKernal(Point2f& center,Point2f& xy,int hx,int hy); //计算两个直方图的巴式距离 static float ComputeBhattacharyyaDistance3D(const Mat& hist1 ,const Mat& hist2 ); //根据当前搜索框下的图像块，加权直方图以及目标模板直方图计算更新权值矩阵 static void UpdateWeightsMatrix3D(Mat& image,Mat& modal_hist,Mat& cur_hist,int* histSize, const float** ranges,OutputArray weights_matrix); //根据均值向量漂移到新的位置(公式26) static void ShiftNewLocationByMeanVector3D(Mat& weights_matrix,KernalFunc DeriveFuncOfKernal, Point& leftup,Point2f& old_center,Point2f& new_center); //自己编写的Mean Shift算法，isJudgeOverShift参数用于控制是否在迭代过程中判断漂移冲过了头； //最后两个参数用来保存迭代路径，用于显示搜索窗口的运动和记录窗口的巴氏系数 int MyMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria , bool isJudgeOverShift,vector
         
          & iterRects ,vector
          
           & iterBhattaCoeff); //具有尺度自适应的MeanShift算法 int MyScaleAdaptMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria , bool isJudgeOverShift,vector
           
            & iterRects ,vector
            
             & iterBhattaCoeff); int _tmain(int argc, _TCHAR* argv[]) { VideoCapture cap; //声明一个读取视频流的类VideoCapture的对象 Rect trackWindow; //跟踪窗口 cap.open(1); //调用VideoCapture的对象的open函数打开摄像头 if( !cap.isOpened() )//判断相机是否被正常打开 { cout << "***无法初始化视频读入流...***\n"; cout << "当前命令行参数值： \n"; return -1; } namedWindow( "MeanShift Demo", 1 );//创建显示跟踪过程的窗口 setMouseCallback( "MeanShift Demo", onMouse, 0 );//为窗口添加鼠标响应函数，用于选择跟踪目标 //在大循环外预先声明重复使用的变量 Mat frame, colorFrame,modal_hist,ms_hist; vector
             
               IterRects;vector
              
                IterBhattaCoeff; //记录MeanShift迭代过程的数组 bool paused = false; //暂停标志 /**********控制算法运行行为的主要参数*******************************************************/ bool isScaleAdapt = true;//是否启用尺度自适应功能 int MaxIterNum = 50; //该参数用于控制Mean Shift的最大迭代次数 bool isJudgeOverShift = true;//用于Mean Shift迭代过程中判断是否冲过头的标志 weight_method = 0; //选择加权核函数 ==0的话选择 Epanechnikov kernal；==1选择Gaussian kernal //选择特征通道 select_channels[0] = 0;select_channels[1] = 1; select_channels[2] = 2; //选择blue,green和red通道构建直方图 TermCriteria criteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER,MaxIterNum,1 );//MeanShift算法的迭代收敛条件 /**********控制算法运行行为的主要参数*******************************************************/ /*进入循环，处理视频图像序列，跟踪目标*/ for(;;) { if( !paused ) //如果没有暂停，则继续读入下一帧图像 { cap >> frame; //cap对象将从摄像头读入的原始图像保存在frame中 if( frame.empty() ) //判断读入的图像数据是否为空 break; } frame.copyTo(image);//将读入的图像拷贝到image中，深度拷贝 if(!paused) //判断是否暂停处理 { frame.copyTo(colorFrame);//将读入的图像拷贝到colorFrame中，深度拷贝 if( trackObject )//trackObject的初始值为0，用鼠标选择了目标以后，trackObject == -1 { if( trackObject < 0 ) //如果trackObject小于0,就表示刚刚用鼠标选定目标， { //该if语句段就是在鼠标给出的矩形框中提取目标的特征直方图 //取出母图像中的感兴趣区域 Mat roi_img(colorFrame, selection); //计算选中的roi图像块的加权直方图 CalculateWeightedHistogram(roi_img,select_channels,modal_hist,weight_method); //在第一帧中，跟踪窗口就是有鼠标指定的selection矩形区域 trackWindow = selection; trackObject = 1; //跟踪标志置1，所以下一次循环，该段if语句就不会执行了 }//该if语句只在第一帧选中目标后被执行一次，用于计算目标特征直方图hist int IterNum; //迭代次数 if(!isScaleAdapt) { //调用无尺度自适应的MeanShift算法搜索roi图像块 IterNum = MyMeanShift3D(colorFrame,modal_hist,trackWindow,criteria, isJudgeOverShift,IterRects,IterBhattaCoeff); } else { //调用尺度自适应的MeanShift算法搜索roi图像块 IterNum = MyScaleAdaptMeanShift3D(colorFrame,modal_hist,trackWindow,criteria, isJudgeOverShift,IterRects,IterBhattaCoeff); } //MeanShift给出的最后目标位置框 trackWindow = IterRects[IterRects.size()-1];//trackWindow 继续作为下一帧的初始搜索位置 //在显示图像上绘制当前帧的跟踪结果 rectangle(image,trackWindow, Scalar(0,255,0),2); //将MeanShift收敛过程绘制到当前帧上 cout<<"当前帧所用迭代次数： "<
               
                <
                
                  0 && selection.height > 0 ) { Mat roi_img(image, selection); bitwise_not(roi_img, roi_img); } //显示当前要处理的图像 imshow( "MeanShift Demo", image ); char c = (char)waitKey(5);//每处理完一帧图像，等待10毫秒 if( c == 27 ) //按ESC键退出程序 break; switch(c) { case 't'://显示迭代轨迹 showTrace = !showTrace; break; case 'c': //停止跟踪 trackObject = 0; break; case 'p': //是否暂停 paused = !paused; break; default: ; } } return 0; } /*鼠标相应函数，用于在第一帧上选取要跟踪的目标*/ static void onMouse( int event, int x, int y, int, void* ) { if( selectObject ) //鼠标左键被按下后，该段语句开始执行 { //按住左键拖动鼠标的时候，该鼠标响应函数 //会被不断的触发，不断计算目标矩形的窗口 selection.x = MIN(x, origin.x); selection.y = MIN(y, origin.y); selection.width = std::abs(x - origin.x); selection.height = std::abs(y - origin.y); selection &= Rect(0, 0, image.cols, image.rows); } switch( event ) { case CV_EVENT_LBUTTONDOWN: origin = Point(x,y); selection = Rect(x,y,0,0); selectObject = true; //当在第一帧按下鼠标左键后，被置为true，拖动鼠标，开始选择目标的矩形区域 break; case CV_EVENT_LBUTTONUP: selectObject = false;//直到鼠标左键抬起，标志着目标区域选择完毕。selectObject被置为false if( selection.width > 0 && selection.height > 0 ) trackObject = -1; //当在第一帧用鼠标选定了合适的目标跟踪窗口后，trackObject的值置为 -1 break; } } //该函数用于计算给定矩形图像块的加权直方图 static void CalculateWeightedHistogram(const Mat& img ,vector
                 
                  & channels, Mat& _hist,int weighted_method) { KernalFunc kernal;//声明一个KernalFunc类型的函数指针变量 //根据加权参数指定具体的核函数 if(weighted_method == 0) { kernal = EpanechnikovKernalFunc; //调用Epanechnikov核函数 DeriveFuncOfKernal = DerivationOfEpanechnikovKernal;//Epanechnikov的导函数 } else if(weighted_method == 1) { kernal = GaussianKernalFunc; //调用高斯Gaussian核函数 DeriveFuncOfKernal = DerivationOfGaussianKernal;//高斯核函数的导函数 } //调用自己写的函数计算选中图像块的加权直方图 myCalcWeightedHist3D(img,channels,_hist,3,histSize,histRange,kernal); } //该函数用于计算给定矩形图像块的直方图 static void CalculateHistogram(const Mat& img,vector
                  
                   & channels, Mat& _hist) { //调用OpenCV函数计算选中图像块的灰度直方图 //calcHist(&img,1,0,Mat(),_hist,1,&histSize,&histRange,uniform,accumu); //调用自己写的函数计算选中图像块的灰度直方图 myCalcHist3D(img,channels,_hist,3,histSize,histRange); } /** 计算给定图像的三维直方图，图像类型必须是CV_8UCx的,x = 3, or 4 or ...., channels规定了通道索引顺序，对于3D直方图,channels数组只能有3个值，且必须都 < x */ void myCalcHist3D( const Mat& image, vector
                   
                    & channels,OutputArray _hist, int dims, const int* histSize,const float** ranges) { int calc_channels = channels.size();//image图像中指定要被统计计算的通道的个数 int img_channel = image.channels(); //image图像的总的通道个数 if(img_channel < channels[calc_channels - 1] + 1 ) { printf("channels中的最大通道索引数不能超过images的通道数\n"); getchar(); } if(image.depth() != CV_8U) { printf("该函数仅支持CV_8U类的图像\n"); getchar(); } if(dims != calc_channels) { printf("被索引的通道数目与直方图的维数不相等\n"); getchar(); } int img_width = image.cols; //图像宽度 int img_height = image.rows;//图像高度 //参数_hist是输出参数，保存着计算好的直方图 _hist.create(dims, histSize, CV_32F); //为输出直方图开辟空间 Mat hist = _hist.getMat(); hist.flags = (hist.flags & ~CV_MAT_TYPE_MASK)|CV_32S;//直方图矩阵的类型信息 hist = Scalar(0.f); //清空原来的数据，当前直方图不累加到原来的直方图上去 //被统计的通道索引，对应直方图的第一维和第二维,和第三维 int ch1 = channels[dims-3], ch2 = channels[dims-2],ch3 = channels[dims-1]; //对应于直方图的第一维的区间范围，bin个数等 float low_range1 = ranges[0][0] ,high_range1 = ranges[0][1];//要计算的直方图第一维区间的上下限 float a1 = histSize[0]/(high_range1 - low_range1);/* 第一维单位区间内直方图bin的数目*/ float b1 = -a1*low_range1; //对应于直方图的第二维的区间范围，bin个数等 float low_range2= ranges[1][0] ,high_range2 = ranges[1][1];//要计算的直方图第二维区间的上下限 float a2 = histSize[1]/(high_range2 - low_range2);/* 单位区间内直方图bin的数目*/ float b2 = -a2*low_range2; //对应于直方图的第三维的区间范围，bin个数等 float low_range3 = ranges[2][0] ,high_range3 = ranges[2][1];//要计算的直方图第三维区间的上下限 float a3 = histSize[2]/(high_range3 - low_range3);/* 单位区间内直方图bin的数目*/ float b3 = -a3*low_range3; const uchar* hist_data = hist.data; //指向直方图矩阵的数据区的首指针 size_t hist_step0 = hist.step[0]; //直方图第一维的步长 size_t hist_step1 = hist.step[1]; //直方图第er维的步长 size_t hist_step2 = hist.step[2]; //直方图第san维的步长 Mat_
                    
                      Myx = image;//Mxy是一个uchar类型的三元组，用于取出image中位于(x,y)位置的像素 //外循环按行从上往下扫描 for(int y=0; y < img_height ; y++ ) { //内循环从左往右扫描 for(int x=0; x < img_width ; x++) { //取出输入图像的第y行第x列所有通道的像素值 Vec3b val = Myx(y,x);//val三元组中按照B,G,R的顺序存放着三个通道的uchar值 //计算像素值val在输入参数规定的灰度区间内的bin序号的索引值 int idx1 = cvFloor(val[ch1]*a1 + b1);//输出直方图第一维的bin索引号 int idx2 = cvFloor(val[ch2]*a2 + b2);//输出直方图第二维的bin索引号 int idx3 = cvFloor(val[ch3]*a3 + b3);//输出直方图第三维的bin索引号 //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引，第三维idx3为列索引 float* hist_ptr =(float*)(hist_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2); (*hist_ptr)++; //累计(idx1,idx2,idx3)位置处的对应的直方图bin上的数量 } } return; } /*核函数：Epanechnikov Kernal center: 核函数中心坐标 xy : 在核区域内的某一个点 hx,hy : 核区域在x方向和y方向的半径（或叫 带宽） */ static float EpanechnikovKernalFunc(Point2f& center,Point2f& xy,int hx,int hy) { //计算点xy到中心center的归一化距离 float distx = (xy.x - center.x)/hx; float disty = (xy.y - center.y)/hy; float dist_square = distx*distx + disty*disty;//距离平方 float result = 0.f; //函数要返回的结果 //核区域就是以2hx和2hy为边长的矩形的内接圆，在该内接圆中的点 //的函数值都不为0，若距离超过核区域，则返回0， //距离center越近，函数值越大 if(dist_square>=1) { result = 0.f; } else { //float Cd = CV_PI; //单位圆面积 pi*R^2 ,R==1 float Cd_inv = 0.318309886f; // == 1.0/Cd;单位圆的面积的倒数 int dimension = 2; //坐标的维数 //Epanechnikov核函数的截面断层函数：profile result = 0.5f*Cd_inv*( dimension + 2 )*( 1 - dist_square ); } return result; } /*核函数的负导数：Derivation of Epanechnikov Kernal*/ static float DerivationOfEpanechnikovKernal(Point2f& center,Point2f& xy,int hx,int hy) { //计算点xy到中心center的归一化距离 float distx = (xy.x - center.x)/hx; float disty = (xy.y - center.y)/hy; float dist_square = distx*distx + disty*disty;//距离平方 float result = 0.f; //函数要返回的结果 if(dist_square>=1) { result = 0.f; } else { //float Cd = CV_PI; //单位圆面积 pi*R^2 ,R==1 //float Cd_inv = 0.318309886f; // == 1.0/Cd;单位圆的面积的倒数 //int dimension = 2; //坐标的维数 //result = 0.5f*Cd_inv*( dimension + 2 ) ; result = 0.63661977237f; //是个常数 = 0.5f*Cd_inv*( dimension + 2 ) } return result; } /*核函数：Gaussian Kernal center: 核函数中心坐标 xy : 在核区域内的某一个点 hx,hy : 核区域在x方向和y方向的半径（或叫 带宽） */ static float GaussianKernalFunc(Point2f& center,Point2f& xy,int hx,int hy) { //计算点xy到中心center的归一化距离 float distx = (xy.x - center.x)/hx; float disty = (xy.y - center.y)/hy; float dist_square = distx*distx + disty*disty;//距离平方 float result = 0.f; //函数要返回的结果 //核区域就是以2hx和2hy为边长的矩形的内接圆，在该内接圆中的点 //的函数值都不为0，若距离超过核区域，则返回0， //距离center越近，函数值越大,权重越高 if(dist_square>=1) { result = 0.f; } else { float Cd = 6.2831853072f; // ==2*CV_PI float Cd_inv = 1.0f/Cd; int dimension = 2; //坐标的维数 result = Cd_inv*expf( -0.5f*dist_square ); //高斯核函数的截面断层函数：profile } return result; } /*核函数的负导数：Derivation of Gaussian Kernal*/ static float DerivationOfGaussianKernal(Point2f& center,Point2f& xy,int hx,int hy) { //计算点xy到中心center的归一化距离 float distx = (xy.x - center.x)/hx; float disty = (xy.y - center.y)/hy; float dist_square = distx*distx + disty*disty;//距离平方 float result = 0.f; //函数要返回的结果 //核区域就是以2hx和2hy为边长的矩形的内接圆，在该内接圆中的点 //的函数值都不为0，若距离超过核区域，则返回0， //距离center越近，函数值越大,权重越高 if(dist_square>=1) { result = 0.f; } else { float Cd = 12.566370614f; // ==4*CV_PI float Cd_inv = 1.0f/Cd; int dimension = 2; //坐标的维数 result = Cd_inv*expf( -0.5f*dist_square ); //高斯核函数的导函数 } return result; } //计算图像三维加权直方图的函数 static void myCalcWeightedHist3D( const Mat& image, vector
                     
                      & channels,OutputArray _hist, int dims, const int* histSize,const float** ranges,KernalFunc kernal) { int calc_channels = channels.size();//image图像中指定要被统计计算的通道的个数 int img_channel = image.channels(); //image图像的总的通道个数 if(img_channel < channels[calc_channels - 1] + 1 ) { printf("channels中的最大通道索引数不能超过images的通道数\n"); getchar(); } if(image.depth() != CV_8U) { printf("该函数仅支持CV_8U类的图像\n"); getchar(); } if(dims != calc_channels) { printf("被索引的通道数目与直方图的维数不相等\n"); getchar(); } int img_width = image.cols; //图像宽度 int img_height = image.rows;//图像高度 //图像区域的中心，即核函数中心 Point2f center(0.5f*img_width,0.5f*img_height); int hx = img_width/2 , hy = img_height/2;//核函数带宽 float NormalConst =0.f; //用于归一化加权系数 //参数_hist是输出参数，保存着计算好的直方图，二维直方图是histSize[0]行histSize[1]列的矩阵 _hist.create(dims, histSize, CV_32F); //为输出直方图开辟空间 Mat hist = _hist.getMat(); hist.flags = (hist.flags & ~CV_MAT_TYPE_MASK)|CV_32S;//直方图矩阵的类型信息 hist = Scalar(0.f); //清空原来的数据，当前直方图不累加到原来的直方图上去 //被统计的通道索引，对应直方图的第一维和第二维,和第三维 int ch1 = channels[dims-3], ch2 = channels[dims-2],ch3 = channels[dims-1]; //对应于直方图的第一维的区间范围，bin个数等 float low_range1 = ranges[0][0] ,high_range1 = ranges[0][1];//要计算的直方图第一维区间的上下限 float a1 = histSize[0]/(high_range1 - low_range1);/* 第一维单位区间内直方图bin的数目*/ float b1 = -a1*low_range1; //对应于直方图的第二维的区间范围，bin个数等 float low_range2= ranges[1][0] ,high_range2 = ranges[1][1];//要计算的直方图第二维区间的上下限 float a2 = histSize[1]/(high_range2 - low_range2);/* 单位区间内直方图bin的数目*/ float b2 = -a2*low_range2; //对应于直方图的第三维的区间范围，bin个数等 float low_range3 = ranges[2][0] ,high_range3 = ranges[2][1];//要计算的直方图第三维区间的上下限 float a3 = histSize[2]/(high_range3 - low_range3);/* 单位区间内直方图bin的数目*/ float b3 = -a3*low_range3; const uchar* hist_data = hist.data; //指向直方图矩阵的数据区的首指针 size_t hist_step0 = hist.step[0]; //直方图第一维的步长，层(或叫“面”)索引 size_t hist_step1 = hist.step[1]; //直方图第er维的步长，某层图像的行索引 size_t hist_step2 = hist.step[2]; //直方图第san维的步长，某层图像的列索引 Mat_
                      
                        Myx = image;//Mxy是一个uchar类型的三元组，用于取出image中位于(x,y)位置的像素 //外循环按行从上往下扫描 for(int y=0; y < img_height ; y++ ) { //指向图像第y行的指针 const uchar* ptr_y = image.ptr
                       
                        (y); //内循环从左往右扫描 for(int x=0; x < img_width ; x++) { //取出输入图像的第y行第x列所有通道的像素值 Vec3b val = Myx(y,x);//val三元组中按照B,G,R的顺序存放着三个通道的uchar值 //计算像素值val在输入参数规定的灰度区间内的bin序号的索引值 int idx1 = cvFloor(val[ch1]*a1 + b1);//输出直方图第一维的bin索引号 int idx2 = cvFloor(val[ch2]*a2 + b2);//输出直方图第二维的bin索引号 int idx3 = cvFloor(val[ch3]*a3 + b3);//输出直方图第三维的bin索引号 //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引，第三维idx3为列索引 float* hist_ptr =(float*)(hist_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2); // 调用核函数，计算图像块（x，y）位置处的加权值 float weighted_value = kernal(center,Point2f(x,y),hx,hy); //（x,y）位置处的加权值 (*hist_ptr) += weighted_value; //加权累计直方图的每一个bin NormalConst += weighted_value; //加权系数归一化因子 } } //归一化因子 float NomalConst_inv = 1.0f/(NormalConst + DBL_EPSILON); //归一化加权直方图 for(int idx1=0; idx1 < histSize[0]; idx1++) { for(int idx2=0; idx2 < histSize[1]; idx2++) { for(int idx3=0; idx3 < histSize[2]; idx3++) { //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引，第三维idx3为列索引 float* hist_ptr =(float*)(hist_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2); *hist_ptr *= NomalConst_inv; //乘以归一化因子 } } } return; } //计算两个三维直方图的巴式距离 static float ComputeBhattacharyyaDistance3D(const Mat& hist1 ,const Mat& hist2 ) { //判断两个直方图的维数是否一致 int hist_layer = hist1.size[0] ; //三维直方图矩阵的层数 == histSize[0] int hist_rows = hist1.size[1] ; //一层图像的行数 == histSize[1] int hist_cols = hist1.size[2] ; //一层图像的列数 == histSize[2] if((hist_layer != hist2.size[0])||(hist_rows != hist2.size[1]) ||(hist_cols != hist2.size[2])) { printf("两个直方图的维数不一致,无法计算距离！！！\n");getchar(); } const uchar* hist1_data = hist1.data; //指向直方图矩阵的数据区的首指针 const uchar* hist2_data = hist2.data; //指向直方图矩阵的数据区的首指针 size_t hist_step0 = hist1.step[0]; //直方图第一维的步长 size_t hist_step1 = hist1.step[1]; //直方图第er维的步长 size_t hist_step2 = hist1.step[2]; //直方图第san维的步长 float distance = 0.0f; const float* phist1 =0,*phist2 =0; for(int idx1=0; idx1 < hist_layer; idx1++) { for(int idx2=0; idx2 < hist_rows; idx2++) { for(int idx3=0; idx3 < hist_cols; idx3++) { //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引，第三维idx3为列索引 phist1 =(float*)(hist1_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2); phist2 =(float*)(hist2_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2); distance += sqrtf((*phist1)*(*phist2));//累计距离值 } } } return distance; } //根据当前搜索框下的图像块，加权直方图以及目标模板直方图计算更新权值矩阵weights_matrix static void UpdateWeightsMatrix3D(Mat& cur_image,vector
                        
                         & channels,Mat& modal_hist,Mat& cur_hist, int* histSize,const float** ranges,OutputArray weights_matrix) { if((cur_image.channels() < 2 )||(cur_image.depth() != CV_8U)) { printf("该函数只支持CV_8UC3类型的多通道图像\n"); getchar(); } int img_channel = cur_image.channels(); int rows = cur_image.rows; int cols = cur_image.cols; //给权值矩阵开辟空间，权值矩阵的大小与image的大小一样，每一个权值与image的一个像素位置一一对应 weights_matrix.create(rows,cols,CV_32FC1);//创建一个rows行cols列的单通道浮点数矩阵 Mat wei_mat = weights_matrix.getMat(); //从OutputArray类型的参数获取Mat类型的矩阵头索引 //遍历图像块的每一个像素位置，找到该像素点在直方图中的bin索引号，然后求两个直方图对应bin位置处的比值 int dims = 3; //直方图维数 //被统计的通道索引，对应直方图的第一维和第二维,和第三维 int ch1 = channels[dims-3], ch2 = channels[dims-2],ch3 = channels[dims-1]; //对应于直方图的第一维的区间范围，bin个数等 float low_range1 = ranges[0][0] ,high_range1 = ranges[0][1];//要计算的直方图第一维区间的上下限 float a1 = histSize[0]/(high_range1 - low_range1);/* 第一维单位区间内直方图bin的数目*/ float b1 = -a1*low_range1; //对应于直方图的第二维的区间范围，bin个数等 float low_range2= ranges[1][0] ,high_range2 = ranges[1][1];//要计算的直方图第二维区间的上下限 float a2 = histSize[1]/(high_range2 - low_range2);/* 单位区间内直方图bin的数目*/ float b2 = -a2*low_range2; //对应于直方图的第三维的区间范围，bin个数等 float low_range3 = ranges[2][0] ,high_range3 = ranges[2][1];//要计算的直方图第三维区间的上下限 float a3 = histSize[2]/(high_range3 - low_range3);/* 单位区间内直方图bin的数目*/ float b3 = -a3*low_range3; size_t hist_step0 = modal_hist.step[0]; //直方图第一维的步长，层(或叫“面”)索引 size_t hist_step1 = modal_hist.step[1]; //直方图第er维的步长，某层图像的行索引 size_t hist_step2 = modal_hist.step[2]; //直方图第san维的步长，某层图像的列索引 //按照从上往下，从左往右的顺序填充权值矩阵 for(int row =0; row < rows; row++) { //指向image的第row行的指针 uchar* curimg_ptr = cur_image.ptr
                         
                          (row); //指向权重矩阵第row行的指针 float* weimat_ptr = wei_mat.ptr
                          
                           (row); for(int col =0; col < cols; col++) { //取出输入图像的第y行第x列第ch1、ch2、ch3 通道的像素值 uchar val1 = curimg_ptr[col*img_channel + ch1]; uchar val2 = curimg_ptr[col*img_channel + ch2]; uchar val3 = curimg_ptr[col*img_channel + ch3]; //计算灰度值val在输入参数规定的灰度区间内的bin序号的索引值 int idx1 = cvFloor(val1*a1 + b1);//直方图第一维的bin索引号 int idx2 = cvFloor(val2*a2 + b2);//直方图第二维的bin索引号 int idx3 = cvFloor(val3*a3 + b3);//直方图第二维的bin索引号 //取出(idx1,idx2)对应位置的直方图的bin值 float* pmodal = (float*)(modal_hist.data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2); float* pcur = (float*)(cur_hist.data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2); //第row行第col列的权重值 float weight = sqrtf( (*pmodal) / (*pcur + DBL_EPSILON) ); *(weimat_ptr + col) = weight; } } return; } //根据均值向量漂移到新的位置(公式26) static void ShiftNewLocationByMeanVector3D(Mat& weights_matrix,KernalFunc DeriveFuncOfKernal, Point& leftup,Point2f& old_center,Point2f& new_center) { //权值矩阵的行列数 int rows = weights_matrix.rows; int cols = weights_matrix.cols; //核函数的横向半径和纵向半径 int hx = cols/2 , hy = rows/2; //计算新的搜索框中心位置 float x_sum=0.f,y_sum=0.f,const_sum=0.f; int x = 0 , y = 0; for(int row =0; row < rows; row++) { //指向权重矩阵第row行的指针 float* weimat_ptr = weights_matrix.ptr
                           
                            (row); y = leftup.y + row; for(int col =0; col < cols; col++) { x = leftup.x + col; //当前像素点在母图像中的坐标位置 Point2f point(x,y); //调用对应核函数的导函数 float g = DeriveFuncOfKernal(old_center,point,hx,hy); //取出对应位置（row，col）上的权重值 float weight = weimat_ptr[col]; //累加分子与分母 x_sum += x*weight*g ; y_sum += y*weight*g ; const_sum += weight*g ; } } //计算新的中心点 new_center.x = cvRound(x_sum/(const_sum+DBL_EPSILON)); new_center.y = cvRound(y_sum/(const_sum+DBL_EPSILON)); } //自己编写的Mean Shift算法，isJudgeOverShift参数用于控制是否在迭代过程中判断漂移冲过了头； //最后两个参数用来保存迭代路径，用于显示搜索窗口的运动和记录窗口的巴氏系数 int MyMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria , bool isJudgeOverShift,vector
                            
                             & iterRects ,vector
                             
                              & iterBhattaCoeff) { int i = 0, eps; Rect cur_rect ; //当前搜索窗口 Mat cur_win; //当前搜索窗口下的图像块 Mat cur_hist; //当前搜索窗口下图像块的加权直方图 Mat cur_weights;//当前搜索窗口下的权值矩阵 //初始化迭代路径, iterRects.clear(); iterBhattaCoeff.clear(); Mat mat = image; Rect windowIn(initialWindow); if( mat.channels()<2 )//目前只支持多通道图像 CV_Error( CV_BadNumChannels, "目前只支持多通道图像" ); if( windowIn.height <= 0 || windowIn.width <= 0 ) CV_Error( CV_StsBadArg, "窗口尺寸必须大于0" ); windowIn = Rect(windowIn) & Rect(0, 0, mat.cols, mat.rows); //边界检查 //检查迭代终止条件 criteria = cvCheckTermCriteria( criteria, 1., 100 ); eps = cvRound( criteria.epsilon * criteria.epsilon ); //初始搜索框 cur_rect = windowIn; float BhattaCoeff = 0.0f; //开始MeanShift迭代过程，实际迭代次数小于等于criteria.maxCount for( i = 0; i < criteria.maxCount; i++ ) { int dx, dy, nx, ny; //当前搜索窗口的中心 Point2f cur_center(cur_rect.x +0.5f* cur_rect.width, cur_rect.y + 0.5f* cur_rect.height); //当前搜索窗口的左上角顶点坐标 Point2i cur_leftup(cur_rect.x,cur_rect.y); //从当前帧图像中获取当前搜索窗口下的图像块 cur_win = mat(cur_rect); //计算当前窗口下的图像块的加权直方图 CalculateWeightedHistogram(cur_win,select_channels,cur_hist,weight_method); //计算当前搜索窗口下的直方图与给定的目标模板直方图的巴氏系数 BhattaCoeff = ComputeBhattacharyyaDistance3D(modal_hist,cur_hist); //根据公式25计算当前窗口下的权值矩阵， UpdateWeightsMatrix3D(cur_win,select_channels,modal_hist,cur_hist, histSize,histRange,cur_weights); //公式-25 //根据公式26计算均值漂移向量，得到新的搜索窗口的中心位置 Point2f new_center;//新的搜索窗口的中心位置 ShiftNewLocationByMeanVector3D(cur_weights,DeriveFuncOfKernal, cur_leftup,cur_center,new_center); //公式-26 //新的搜索区域的左上角坐标 nx = cvRound(new_center.x - 0.5f*cur_rect.width); ny = cvRound(new_center.y - 0.5f*cur_rect.height); //x方向的边界检查 if( nx < 0 ) nx = 0; else if( nx + cur_rect.width > mat.cols ) nx = mat.cols - cur_rect.width; //y方向的边界检查 if( ny < 0 ) ny = 0; else if( ny + cur_rect.height > mat.rows ) ny = mat.rows - cur_rect.height; //计算漂移向量(dx,dy) dx = nx - cur_rect.x; dy = ny - cur_rect.y; //记录迭代过程 iterRects.push_back(cur_rect); iterBhattaCoeff.push_back(BhattaCoeff); //形成新的搜索框，移动左上角顶点坐标，长宽不变 Rect new_rect(cur_rect); new_rect.x = nx; new_rect.y = ny; //判断均值漂移是否冲过了头 if(isJudgeOverShift) { Mat new_win = mat(new_rect); //取出新窗口下的图像块 Mat new_hist; //计算新窗口下的图像块的加权直方图 CalculateWeightedHistogram(new_win,select_channels,new_hist,0); //计算新窗口下的直方图与给定的目标模板直方图的巴氏系数 float new_BhattaCoeff = ComputeBhattacharyyaDistance3D(modal_hist,new_hist); //如果新位置上的巴氏系数小于旧位置上的的巴氏系数，说明冲过了头 if(new_BhattaCoeff < BhattaCoeff) { //若果冲过了头，就把两次的位置坐标平均一下，得出新的位置 new_rect.x = cvRound(0.5f*(cur_rect.x + new_rect.x)); new_rect.y = cvRound(0.5f*(cur_rect.y + new_rect.y)); //由于调整了新窗口的位置，所以要重新计算漂移向量(dx,dy) dx = new_rect.x - cur_rect.x; dy = new_rect.y - cur_rect.y; } } /* 检查窗口的移动距离是否小于eps,若小于，则算法收敛，退出循环 */ if( dx*dx + dy*dy < eps ) { cur_rect = new_rect; //如果在没有达到最大迭代次数之前该退出条件已经满足 //则iterRects中的最后一个元素就是最终的迭代结果 iterRects.push_back(cur_rect); iterBhattaCoeff.push_back(BhattaCoeff); break; } //进入下一次迭代 cur_rect = new_rect; //这个搜索框在循环退出时才被加入到迭代轨迹数组中 } //如果超过最大迭代次数的时候，循环退出，把最后一次均值漂移形成的矩形框作为最终的结果 if( i == criteria.maxCount) { iterRects.push_back(cur_rect); iterBhattaCoeff.push_back(BhattaCoeff); } return iterRects.size();//返回实际使用的迭代次数(漂移次数) } //把给定的矩形窗口缩放给定的尺度，但是保持中心位置不变 static Rect ScaleRectSize( Rect& rect , float scale) { Rect new_rect; //获取rect的中心坐标 int cx = rect.x + cvRound(rect.width/2.f); int cy = rect.y + cvRound(rect.height/2.f); //新的窗口尺寸 new_rect.width = cvRound(rect.width*scale); new_rect.height = cvRound(rect.height*scale); //新的左上角顶点坐标 new_rect.x = cx - cvRound(rect.width*scale*0.5f); new_rect.y = cy - cvRound(rect.height*scale*0.5f); return new_rect; } //具有尺度自适应的MeanShift算法 int MyScaleAdaptMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria , bool isJudgeOverShift,vector
                              
                               & iterRects ,vector
                               
                                & iterBhattaCoeff) { float scale = 0.1f; //缩放尺度 float gama = 0.1f; //对旧尺寸和新尺寸的加权系数 Rect temp_rect; //首先以原始窗口迭代 MyMeanShift3D(image,modal_hist,initialWindow,criteria,isJudgeOverShift,iterRects,iterBhattaCoeff); temp_rect = iterRects[iterRects.size()-1];//原始尺寸窗口的搜索结果 //然后以小窗口迭代，起始位置是原始尺寸窗口进行迭代后返回的位置 Rect small_rect = ScaleRectSize(temp_rect,1 - scale);//比初始窗口小的窗口 //迭代路径 vector
                                
                                  small_iterRects ;vector
                                 
                                   small_iterBhattaCoeff; MyMeanShift3D(image,modal_hist,small_rect,criteria,isJudgeOverShift,small_iterRects,small_iterBhattaCoeff); //然后用大窗口迭代 起始位置是原始尺寸窗口进行迭代后返回的位置 Rect large_rect = ScaleRectSize(temp_rect,1 + scale);//比初始窗口大的窗口 vector
                                  
                                    large_iterRects ;vector
                                   
                                     large_iterBhattaCoeff; MyMeanShift3D(image,modal_hist,large_rect,criteria,isJudgeOverShift,large_iterRects,large_iterBhattaCoeff); //判断小尺寸窗口巴氏系数是否比原始窗口大 if((small_iterBhattaCoeff[small_iterBhattaCoeff.size()-1] > iterBhattaCoeff[iterBhattaCoeff.size()-1]) ||(small_iterBhattaCoeff[small_iterBhattaCoeff.size()-1] > large_iterBhattaCoeff[large_iterBhattaCoeff.size()-1])) { Rect new_rect = small_iterRects[small_iterRects.size()-1]; //小尺寸窗口迭代返回结果的中心坐标 int cx = new_rect.x + cvRound(new_rect.width/2.f); int cy = new_rect.y + cvRound(new_rect.height/2.f); //一阶滤波以后的窗口尺寸 int new_width = cvRound(gama*new_rect.width + (1-gama)*temp_rect.width); int new_height = cvRound(gama*new_rect.height + (1-gama)*temp_rect.height); //新的左上角顶点坐标 int nx = cx - cvRound(new_width*0.5f); int ny = cy - cvRound(new_height*0.5f); //把结果放入迭代记录数组，返回 small_iterRects[small_iterRects.size()-1] = Rect(nx,ny,new_width,new_height); iterRects = small_iterRects; iterBhattaCoeff = small_iterBhattaCoeff; }//判断大尺寸窗口巴氏系数是否比原始窗口大 else if((large_iterBhattaCoeff[large_iterBhattaCoeff.size()-1] > iterBhattaCoeff[iterBhattaCoeff.size()-1]) ||(large_iterBhattaCoeff[large_iterBhattaCoeff.size()-1] > small_iterBhattaCoeff[small_iterBhattaCoeff.size()-1])) { Rect new_rect = large_iterRects[large_iterRects.size()-1]; //大尺寸窗口迭代返回结果的中心坐标 int cx = new_rect.x + cvRound(new_rect.width/2.f); int cy = new_rect.y + cvRound(new_rect.height/2.f); //一阶滤波以后的窗口尺寸 int new_width = cvRound(gama*new_rect.width + (1-gama)*temp_rect.width); int new_height = cvRound(gama*new_rect.height + (1-gama)*temp_rect.height); //新的左上角顶点坐标 int nx = cx - cvRound(new_width*0.5f); int ny = cy - cvRound(new_height*0.5f); //把结果放入迭代记录数组，返回 large_iterRects[large_iterRects.size()-1] = Rect(nx,ny,new_width,new_height); iterRects = large_iterRects; iterBhattaCoeff = large_iterBhattaCoeff; } return iterBhattaCoeff.size(); } 
                                   
                                  
                                 
                                
                               
                              
                             
                            
                           
                          
                         
                        
                       
                      
                     
                    
                   
                  
                 
                
               
              
             
            
           
          
         
        
       
      
     
    
   
  

跟踪效果展示：