Kernal-Based Object Tracking---基于核函数的目标跟踪 一次只能跟踪一个目标

2018-04-03 10:07:00 20

Kernal-Based Object Tracking---基于核函数的目标跟踪   一次只能跟踪一个目标

  1. #include "stdafx.h"  

  2. #include <windows.h>  

  3. #include <stdio.h>  

  4. #include <ctype.h>  

  5. #include <iostream>  

  6. #include <opencv2/opencv.hpp>  

  7.   

  8. using namespace std;  

  9. using namespace cv;  

  10.   

  11. Mat image;  

  12.    

  13. bool selectObject = false//selectObject的初始值为false,  

  14. int trackObject = 0;  //trackObject的初值是0  

  15. bool showTrace = true//是否显示MeanShift迭代收敛轨迹  

  16. Point origin;  

  17. Rect selection;  

  18.   

  19. //设定直方图中bin的数目  

  20. int histSize[] = /*{16,16,16}*/ {32,32,32};  

  21. vector<int> select_channels(3);  

  22. //设定取值范围  

  23. float range[] = {0,256};//灰度特征的范围都是[0,256)  

  24. const float* histRange[] = {range,range,range};    

  25. bool uniform = true//均匀分布,  

  26. bool accumu = false;//无累加  

  27.     

  28. //定义一个核函数指针,该函数指针可以指向任意一个标准的核函数  

  29. typedef float (*KernalFunc)(Point2f& center,Point2f& xy,int hx,int hy);  

  30. int weight_method = 0; //选择核函数 ==0:Epanechnikov;==1:Gaussian  

  31. KernalFunc  DeriveFuncOfKernal ;//声明一个指向核函数的导函数的函数指针变量  

  32.   

  33. //用鼠标选择目标图像区域  

  34. static void onMouse( int event, int x, int y, intvoid* );  

  35. //该函数用于计算给定矩形图像块的加权直方图  

  36. static void CalculateWeightedHistogram(const Mat& img,vector<int>& channels,Mat& _hist,int weighted_method);  

  37. //该函数用于计算给定矩形图像块的非加权直方图  

  38. static void CalculateHistogram(const Mat& img , Mat& _hist);  

  39. //自己写的计算图像一维直方图的函数  

  40. void  myCalcHist3D( const Mat& image, vector<int>& channels,OutputArray _hist,   

  41.                           int dims, const int* histSize,const float** ranges);  

  42. //计算图像一维加权直方图的函数  

  43. static void  myCalcWeightedHist3D( const Mat& image, vector<int>& channels,OutputArray _hist,   

  44.                           int dims, const int* histSize,const float** ranges,KernalFunc kernal);  

  45. /*核函数:Epanechnikov Kernal*/  

  46. static float  EpanechnikovKernalFunc(Point2f& center,Point2f& xy,int hx,int hy);  

  47. /*核函数的负导数:Derivation of Epanechnikov Kernal*/  

  48. static float  DerivationOfEpanechnikovKernal(Point2f& center,Point2f& xy,int hx,int hy);  

  49. /*核函数:Gaussian Kernal */  

  50. static float  GaussianKernalFunc(Point2f& center,Point2f& xy,int hx,int hy);  

  51. /*核函数的负导数:Derivation of Gaussian Kernal*/  

  52. static float  DerivationOfGaussianKernal(Point2f& center,Point2f& xy,int hx,int hy);  

  53. //计算两个直方图的巴式距离  

  54. static float  ComputeBhattacharyyaDistance3D(const Mat& hist1 ,const Mat& hist2 );  

  55. //根据当前搜索框下的图像块,加权直方图以及目标模板直方图计算更新权值矩阵  

  56. static void  UpdateWeightsMatrix3D(Mat& image,Mat& modal_hist,Mat& cur_hist,int* histSize,  

  57.                                  const float** ranges,OutputArray weights_matrix);  

  58. //根据均值向量漂移到新的位置(公式26)  

  59. static void ShiftNewLocationByMeanVector3D(Mat& weights_matrix,KernalFunc DeriveFuncOfKernal,  

  60.                                          Point& leftup,Point2f& old_center,Point2f& new_center);  

  61. //自己编写的Mean Shift算法,isJudgeOverShift参数用于控制是否在迭代过程中判断漂移冲过了头;  

  62. //最后两个参数用来保存迭代路径,用于显示搜索窗口的运动和记录窗口的巴氏系数  

  63. int MyMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria ,  

  64.                    bool isJudgeOverShift,vector<Rect>& iterRects ,vector<float>& iterBhattaCoeff);  

  65.  //具有尺度自适应的MeanShift算法  

  66. int MyScaleAdaptMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria ,  

  67.                              bool isJudgeOverShift,vector<Rect>& iterRects ,vector<float>& iterBhattaCoeff);  

  68.   

  69. int _tmain(int argc, _TCHAR* argv[])  

  70. {  

  71.       

  72.     //VideoCapture cap;  //声明一个读取视频流的类VideoCapture的对象  

  73.     Rect trackWindow;  //跟踪窗口   

  74.         

  75.     CvCapture* cap = 0; //视频获取结构   

  76.       

  77.     cap = cvCreateFileCapture("E:\calibration\视频文件\FFOutput\强光下的人(热成像广角)_20141127184435_20141127184456.avi");    //读本地视频文件  

  78.   

  79.     if( !cap )//判断相机是否被正常打开  

  80.     {   

  81.         cout << "***无法初始化视频读入流...*** ";  

  82.         cout << "当前命令行参数值:  ";   

  83.         return -1;  

  84.     }  

  85.    

  86.     namedWindow( "MeanShift Demo", 1 );//创建显示跟踪过程的窗口  

  87.     setMouseCallback( "MeanShift Demo", onMouse, 0 );//为窗口添加鼠标响应函数,用于选择跟踪目标  

  88.        

  89.      //在大循环外预先声明重复使用的变量  

  90.     Mat frame1, colorFrame,modal_hist,ms_hist;   

  91.     vector<Rect> IterRects;vector<float> IterBhattaCoeff; //记录MeanShift迭代过程的数组  

  92.     bool paused = false;  //暂停标志  

  93.        

  94. /**********控制算法运行行为的主要参数*******************************************************/  

  95.     bool isScaleAdapt = true;//是否启用尺度自适应功能  

  96.     int MaxIterNum = 50;  //该参数用于控制Mean Shift的最大迭代次数  

  97.     bool isJudgeOverShift = true;//用于Mean Shift迭代过程中判断是否冲过头的标志  

  98.     weight_method = 0; //选择加权核函数 ==0的话选择 Epanechnikov kernal;==1选择Gaussian kernal   

  99.     //选择特征通道  

  100.     select_channels[0] = 0;select_channels[1] = 1; select_channels[2] = 2; //选择blue,green和red通道构建直方图  

  101.     TermCriteria criteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER,MaxIterNum,1 );//MeanShift算法的迭代收敛条件   

  102.       

  103. /**********控制算法运行行为的主要参数*******************************************************/  

  104.   

  105.     IplImage *image1 =NULL;  

  106.     /*进入循环,处理视频图像序列,跟踪目标*/  

  107.     for(;;)  

  108.     {  

  109.         //if( !paused ) //如果没有暂停,则继续读入下一帧图像  

  110.        // {  

  111.             if (!cvGrabFrame(cap)) //从摄像头或者视频文件中抓取帧  

  112.                 break;  

  113.               

  114.             image1 = cvRetrieveFrame(cap); //取回由函数cvGrabFrame抓取的图像,返回由函数cvGrabFrame 抓取的图像的指针  

  115.              

  116.            Mat frame(image1);  

  117.             if( frame.empty() ) //判断读入的图像数据是否为空  

  118.                 break;  

  119.        // }  

  120.   

  121.         frame.copyTo(image);//将读入的图像拷贝到image中,深度拷贝  

  122.            

  123.         if(!paused) //判断是否暂停处理  

  124.         {  

  125.             frame.copyTo(colorFrame);//将读入的图像拷贝到colorFrame中,深度拷贝  

  126.   

  127.             if( trackObject )//trackObject的初始值为0,用鼠标选择了目标以后,trackObject == -1  

  128.             {  

  129.                 if( trackObject < 0 )  //如果trackObject小于0,就表示刚刚用鼠标选定目标,  

  130.                 {   //该if语句段就是在鼠标给出的矩形框中提取目标的特征直方图    

  131.                       

  132.                     //取出母图像中的感兴趣区域  

  133.                     Mat roi_img(colorFrame, selection);    

  134.                     //计算选中的roi图像块的加权直方图   

  135.                     CalculateWeightedHistogram(roi_img,select_channels,modal_hist,weight_method);                    

  136.                        

  137.                     //在第一帧中,跟踪窗口就是有鼠标指定的selection矩形区域  

  138.                     trackWindow = selection;    

  139.                     trackObject = 1;    //跟踪标志置1,所以下一次循环,该段if语句就不会执行了  

  140.   

  141.                 }//该if语句只在第一帧选中目标后被执行一次,用于计算目标特征直方图hist    

  142.   

  143.                 int IterNum;  //迭代次数  

  144.                 if(!isScaleAdapt)  

  145.                 {  

  146.                     //调用无尺度自适应的MeanShift算法搜索roi图像块   

  147.                     IterNum = MyMeanShift3D(colorFrame,modal_hist,trackWindow,criteria,  

  148.                                             isJudgeOverShift,IterRects,IterBhattaCoeff);      

  149.                 }  

  150.                 else  

  151.                 {  

  152.                     //调用尺度自适应的MeanShift算法搜索roi图像块  

  153.                     IterNum = MyScaleAdaptMeanShift3D(colorFrame,modal_hist,trackWindow,criteria,  

  154.                                             isJudgeOverShift,IterRects,IterBhattaCoeff);  

  155.                 }  

  156.                   

  157.                   

  158.                 //MeanShift给出的最后目标位置框  

  159.                 trackWindow  = IterRects[IterRects.size()-1];//trackWindow 继续作为下一帧的初始搜索位置  

  160.                 //在显示图像上绘制当前帧的跟踪结果  

  161.                 rectangle(image,trackWindow, Scalar(0,255,0),2);  

  162.                 //将MeanShift收敛过程绘制到当前帧上  

  163.                 cout<<"当前帧所用迭代次数: "<<IterNum<<endl;  

  164.                 for(int ii=1;(ii<=IterNum)&&(showTrace);ii++)  

  165.                 {  

  166.                     Point c1,c2;//前后两次迭代矩形搜索框的中心  

  167.                     c1 = Point(IterRects[ii-1].x+ IterRects[ii-1].width*0.5f,  

  168.                            IterRects[ii-1].y+ IterRects[ii-1].height*0.5f);  

  169.                     c2 = Point(IterRects[ii].x+ IterRects[ii].width*0.5f,  

  170.                             IterRects[ii].y+ IterRects[ii].height*0.5f);  

  171.                     circle(image,c1,3,Scalar(0,0,0),2,8);  

  172.                     circle(image,c2,3,Scalar(0,0,0),2,8);  

  173.                     //将所有迭代过程的中心连起来,显示在当前帧上均值漂移的过程  

  174.                     line(image,c1,c2,Scalar(0,0,255),2,8);  

  175.                 }   

  176.             }  

  177.         }  

  178.         else if( trackObject < 0 )  

  179.         {  

  180.              paused = false;  

  181.         }  

  182.   

  183.         //下面这段if语句代码也只有在第一帧上选定目标时才会被执行  

  184.         //在按下鼠标左键和抬起鼠标左键之间的这段时间,selectObject为真,  

  185.         //selection会随着鼠标的移动不断变化,直到抬起鼠标左键后,  

  186.         //selectObject为假,selection就是选中的目标矩形框  

  187.         if( selectObject && selection.width > 0 && selection.height > 0 )  

  188.         {  

  189.             Mat roi_img(image, selection);   

  190.             bitwise_not(roi_img, roi_img);  

  191.         }  

  192.          //显示当前要处理的图像  

  193.         imshow( "MeanShift Demo", image );   

  194.            

  195.         char c = (char)waitKey(2);//每处理完一帧图像,等待10毫秒  

  196.         if( c == 27 ) //按ESC键退出程序  

  197.             break;  

  198.         switch(c)  

  199.         {  

  200.         case 't'://显示迭代轨迹  

  201.             showTrace = !showTrace;  

  202.             break;  

  203.         case 'c':    //停止跟踪  

  204.             trackObject = 0;   

  205.             break;   

  206.         case 'p':     //是否暂停  

  207.             paused = !paused;  

  208.             break;  

  209.         default:  

  210.             ;  

  211.         }  

  212.     }  

  213.     return 0;  

  214. }  

  215.   

  216. /*鼠标相应函数,用于在第一帧上选取要跟踪的目标*/  

  217. static void onMouse( int event, int x, int y, intvoid* )  

  218. {  

  219.     if( selectObject ) //鼠标左键被按下后,该段语句开始执行  

  220.     {                  //按住左键拖动鼠标的时候,该鼠标响应函数  

  221.                        //会被不断的触发,不断计算目标矩形的窗口  

  222.         selection.x = MIN(x, origin.x);  

  223.         selection.y = MIN(y, origin.y);  

  224.         selection.width = std::abs(x - origin.x);  

  225.         selection.height = std::abs(y - origin.y);  

  226.   

  227.         selection &= Rect(0, 0, image.cols, image.rows);  

  228.     }  

  229.   

  230.     switch( event )  

  231.     {  

  232.     case CV_EVENT_LBUTTONDOWN:  

  233.         origin = Point(x,y);  

  234.         selection = Rect(x,y,0,0);  

  235.         selectObject = true;  //当在第一帧按下鼠标左键后,被置为true,拖动鼠标,开始选择目标的矩形区域  

  236.         break;  

  237.     case CV_EVENT_LBUTTONUP:  

  238.         selectObject = false;//直到鼠标左键抬起,标志着目标区域选择完毕。selectObject被置为false  

  239.         if( selection.width > 0 && selection.height > 0 )  

  240.             trackObject = -1; //当在第一帧用鼠标选定了合适的目标跟踪窗口后,trackObject的值置为 -1  

  241.         break;  

  242.     }  

  243. }  

  244.   

  245. //该函数用于计算给定矩形图像块的加权直方图  

  246. static void CalculateWeightedHistogram(const Mat& img ,vector<int>& channels, Mat& _hist,int weighted_method)  

  247. {   

  248.      

  249.     KernalFunc kernal;//声明一个KernalFunc类型的函数指针变量  

  250.   

  251.     //根据加权参数指定具体的核函数  

  252.     if(weighted_method == 0)  

  253.     {  

  254.         kernal = EpanechnikovKernalFunc; //调用Epanechnikov核函数  

  255.         DeriveFuncOfKernal = DerivationOfEpanechnikovKernal;//Epanechnikov的导函数  

  256.     }     

  257.     else if(weighted_method == 1)  

  258.     {  

  259.         kernal = GaussianKernalFunc;     //调用高斯Gaussian核函数   

  260.         DeriveFuncOfKernal = DerivationOfGaussianKernal;//高斯核函数的导函数  

  261.     }  

  262.     //调用自己写的函数计算选中图像块的加权直方图  

  263.     myCalcWeightedHist3D(img,channels,_hist,3,histSize,histRange,kernal);  

  264.        

  265. }  

  266.   

  267. //该函数用于计算给定矩形图像块的直方图  

  268. static void CalculateHistogram(const Mat& img,vector<int>& channels, Mat& _hist)  

  269. {  

  270.     //调用OpenCV函数计算选中图像块的灰度直方图  

  271.     //calcHist(&img,1,0,Mat(),_hist,1,&histSize,&histRange,uniform,accumu);  

  272.   

  273.     //调用自己写的函数计算选中图像块的灰度直方图  

  274.        

  275.     myCalcHist3D(img,channels,_hist,3,histSize,histRange);  

  276.    

  277. }  

  278.     

  279. /** 

  280.     计算给定图像的三维直方图,图像类型必须是CV_8UCx的,x = 3, or 4 or ...., 

  281.     channels规定了通道索引顺序,对于3D直方图,channels数组只能有3个值,且必须都 < x 

  282. */  

  283. void myCalcHist3D( const Mat& image, vector<int>& channels,OutputArray _hist,   

  284.                           int dims, const int* histSize,const float** ranges)  

  285. {  

  286.     int calc_channels = channels.size();//image图像中指定要被统计计算的通道的个数  

  287.     int img_channel = image.channels(); //image图像的总的通道个数  

  288.     if(img_channel < channels[calc_channels - 1] + 1 )  

  289.     {  

  290.         printf("channels中的最大通道索引数不能超过images的通道数 ");  

  291.         getchar();  

  292.     }  

  293.     if(image.depth() != CV_8U)  

  294.     {  

  295.         printf("该函数仅支持CV_8U类的图像 ");  

  296.         getchar();  

  297.     }  

  298.     if(dims != calc_channels)  

  299.     {  

  300.         printf("被索引的通道数目与直方图的维数不相等 ");  

  301.         getchar();  

  302.     }  

  303.     int img_width = image.cols; //图像宽度  

  304.     int img_height = image.rows;//图像高度  

  305.        

  306.     //参数_hist是输出参数,保存着计算好的直方图   

  307.     _hist.create(dims, histSize, CV_32F); //为输出直方图开辟空间  

  308.     Mat hist = _hist.getMat();    

  309.     hist.flags = (hist.flags & ~CV_MAT_TYPE_MASK)|CV_32S;//直方图矩阵的类型信息   

  310.     hist = Scalar(0.f); //清空原来的数据,当前直方图不累加到原来的直方图上去  

  311.        

  312.     //被统计的通道索引,对应直方图的第一维和第二维,和第三维  

  313.     int ch1 = channels[dims-3], ch2 = channels[dims-2],ch3 = channels[dims-1];   

  314.     //对应于直方图的第一维的区间范围,bin个数等  

  315.     float low_range1 = ranges[0][0] ,high_range1 = ranges[0][1];//要计算的直方图第一维区间的上下限  

  316.     float a1 = histSize[0]/(high_range1 - low_range1);/* 第一维单位区间内直方图bin的数目*/  

  317.     float b1 = -a1*low_range1;  

  318.     //对应于直方图的第二维的区间范围,bin个数等  

  319.     float low_range2= ranges[1][0] ,high_range2 = ranges[1][1];//要计算的直方图第二维区间的上下限  

  320.     float a2 = histSize[1]/(high_range2 - low_range2);/* 单位区间内直方图bin的数目*/  

  321.     float b2 = -a2*low_range2;  

  322.     //对应于直方图的第三维的区间范围,bin个数等  

  323.     float low_range3 = ranges[2][0] ,high_range3 = ranges[2][1];//要计算的直方图第三维区间的上下限  

  324.     float a3 = histSize[2]/(high_range3 - low_range3);/* 单位区间内直方图bin的数目*/  

  325.     float b3 = -a3*low_range3;  

  326.   

  327.     const uchar* hist_data = hist.data; //指向直方图矩阵的数据区的首指针  

  328.     size_t hist_step0 = hist.step[0];  //直方图第一维的步长  

  329.     size_t hist_step1 = hist.step[1];  //直方图第er维的步长  

  330.     size_t hist_step2 = hist.step[2];  //直方图第san维的步长  

  331.     Mat_<Vec3b> Myx = image;//Mxy是一个uchar类型的三元组,用于取出image中位于(x,y)位置的像素  

  332.       

  333.     //外循环按行从上往下扫描    

  334.     for(int y=0; y < img_height ; y++ )  

  335.     {          

  336.         //内循环从左往右扫描  

  337.         for(int x=0; x < img_width ; x++)  

  338.         {  

  339.             //取出输入图像的第y行第x列所有通道的像素值  

  340.             Vec3b val = Myx(y,x);//val三元组中按照B,G,R的顺序存放着三个通道的uchar值             

  341.             //计算像素值val在输入参数规定的灰度区间内的bin序号的索引值  

  342.             int idx1 = cvFloor(val[ch1]*a1 + b1);//输出直方图第一维的bin索引号  

  343.             int idx2 = cvFloor(val[ch2]*a2 + b2);//输出直方图第二维的bin索引号  

  344.             int idx3 = cvFloor(val[ch3]*a3 + b3);//输出直方图第三维的bin索引号  

  345.             //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引,第三维idx3为列索引  

  346.             float* hist_ptr =(float*)(hist_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2);    

  347.              (*hist_ptr)++; //累计(idx1,idx2,idx3)位置处的对应的直方图bin上的数量  

  348.         }  

  349.     }  

  350.   

  351.     return;  

  352. }  

  353.   

  354. /*核函数:Epanechnikov Kernal 

  355.   center: 核函数中心坐标 

  356.   xy    : 在核区域内的某一个点 

  357.   hx,hy : 核区域在x方向和y方向的半径(或叫 带宽) 

  358. */  

  359. static float  EpanechnikovKernalFunc(Point2f& center,Point2f& xy,int hx,int hy)  

  360. {  

  361.     //计算点xy到中心center的归一化距离  

  362.     float distx = (xy.x - center.x)/hx;  

  363.     float disty = (xy.y - center.y)/hy;  

  364.     float dist_square = distx*distx + disty*disty;//距离平方  

  365.   

  366.     float result = 0.f; //函数要返回的结果  

  367.   

  368.     //核区域就是以2hx和2hy为边长的矩形的内接圆,在该内接圆中的点  

  369.     //的函数值都不为0,若距离超过核区域,则返回0,  

  370.     //距离center越近,函数值越大  

  371.     if(dist_square>=1)    

  372.     {  

  373.         result = 0.f;   

  374.     }  

  375.     else  

  376.     {  

  377.         //float Cd = CV_PI;  //单位圆面积 pi*R^2 ,R==1  

  378.         float Cd_inv =  0.318309886f; // == 1.0/Cd;单位圆的面积的倒数  

  379.         int dimension = 2;  //坐标的维数  

  380.         //Epanechnikov核函数的截面断层函数:profile  

  381.         result = 0.5f*Cd_inv*( dimension + 2 )*( 1 - dist_square );  

  382.     }  

  383.   

  384.     return  result;  

  385. }  

  386. /*核函数的负导数:Derivation of Epanechnikov Kernal*/  

  387. static float  DerivationOfEpanechnikovKernal(Point2f& center,Point2f& xy,int hx,int hy)  

  388. {  

  389.     //计算点xy到中心center的归一化距离  

  390.     float distx = (xy.x - center.x)/hx;  

  391.     float disty = (xy.y - center.y)/hy;  

  392.     float dist_square = distx*distx + disty*disty;//距离平方  

  393.   

  394.     float result = 0.f; //函数要返回的结果  

  395.        

  396.     if(dist_square>=1)    

  397.     {  

  398.         result = 0.f;   

  399.     }  

  400.     else  

  401.     {  

  402.         //float Cd = CV_PI;  //单位圆面积 pi*R^2 ,R==1  

  403.         //float Cd_inv =  0.318309886f; // == 1.0/Cd;单位圆的面积的倒数  

  404.         //int dimension = 2;  //坐标的维数   

  405.         //result = 0.5f*Cd_inv*( dimension + 2 ) ;  

  406.         result = 0.63661977237f;  //是个常数 = 0.5f*Cd_inv*( dimension + 2 )  

  407.     }  

  408.   

  409.     return  result;  

  410. }  

  411. /*核函数:Gaussian Kernal 

  412.   center: 核函数中心坐标 

  413.   xy    : 在核区域内的某一个点 

  414.   hx,hy : 核区域在x方向和y方向的半径(或叫 带宽) 

  415. */  

  416. static float  GaussianKernalFunc(Point2f& center,Point2f& xy,int hx,int hy)  

  417. {  

  418.     //计算点xy到中心center的归一化距离  

  419.     float distx = (xy.x - center.x)/hx;  

  420.     float disty = (xy.y - center.y)/hy;  

  421.     float dist_square = distx*distx + disty*disty;//距离平方  

  422.   

  423.     float result = 0.f; //函数要返回的结果  

  424.   

  425.     //核区域就是以2hx和2hy为边长的矩形的内接圆,在该内接圆中的点  

  426.     //的函数值都不为0,若距离超过核区域,则返回0,  

  427.     //距离center越近,函数值越大,权重越高  

  428.     if(dist_square>=1)   

  429.     {  

  430.         result = 0.f;   

  431.     }  

  432.     else  

  433.     {  

  434.         float Cd = 6.2831853072f;  // ==2*CV_PI  

  435.         float Cd_inv = 1.0f/Cd;  

  436.         int dimension = 2;  //坐标的维数  

  437.         result = Cd_inv*expf( -0.5f*dist_square ); //高斯核函数的截面断层函数:profile  

  438.     }  

  439.   

  440.     return  result;  

  441. }  

  442. /*核函数的负导数:Derivation of Gaussian Kernal*/  

  443. static float  DerivationOfGaussianKernal(Point2f& center,Point2f& xy,int hx,int hy)  

  444. {  

  445.     //计算点xy到中心center的归一化距离  

  446.     float distx = (xy.x - center.x)/hx;  

  447.     float disty = (xy.y - center.y)/hy;  

  448.     float dist_square = distx*distx + disty*disty;//距离平方  

  449.   

  450.     float result = 0.f; //函数要返回的结果  

  451.   

  452.     //核区域就是以2hx和2hy为边长的矩形的内接圆,在该内接圆中的点  

  453.     //的函数值都不为0,若距离超过核区域,则返回0,  

  454.     //距离center越近,函数值越大,权重越高  

  455.     if(dist_square>=1)   

  456.     {  

  457.         result = 0.f;   

  458.     }  

  459.     else  

  460.     {  

  461.         float Cd = 12.566370614f;  // ==4*CV_PI  

  462.         float Cd_inv = 1.0f/Cd;  

  463.         int dimension = 2;  //坐标的维数  

  464.         result = Cd_inv*expf( -0.5f*dist_square ); //高斯核函数的导函数  

  465.     }  

  466.   

  467.     return  result;  

  468. }  

  469.   

  470. //计算图像三维加权直方图的函数  

  471. static void  myCalcWeightedHist3D( const Mat& image, vector<int>& channels,OutputArray _hist,   

  472.                       int dims, const int* histSize,const float** ranges,KernalFunc kernal)  

  473. {  

  474.     int calc_channels = channels.size();//image图像中指定要被统计计算的通道的个数  

  475.     int img_channel = image.channels(); //image图像的总的通道个数  

  476.     if(img_channel < channels[calc_channels - 1] + 1 )  

  477.     {  

  478.         printf("channels中的最大通道索引数不能超过images的通道数 ");  

  479.         getchar();  

  480.     }  

  481.     if(image.depth() != CV_8U)  

  482.     {  

  483.         printf("该函数仅支持CV_8U类的图像 ");  

  484.         getchar();  

  485.     }  

  486.     if(dims != calc_channels)  

  487.     {  

  488.         printf("被索引的通道数目与直方图的维数不相等 ");  

  489.         getchar();  

  490.     }  

  491.     int img_width = image.cols; //图像宽度  

  492.     int img_height = image.rows;//图像高度  

  493.       

  494.     //图像区域的中心,即核函数中心  

  495.     Point2f center(0.5f*img_width,0.5f*img_height);  

  496.     int hx = img_width/2 , hy = img_height/2;//核函数带宽  

  497.     float NormalConst =0.f;  //用于归一化加权系数  

  498.   

  499.     //参数_hist是输出参数,保存着计算好的直方图,二维直方图是histSize[0]行histSize[1]列的矩阵   

  500.     _hist.create(dims, histSize, CV_32F); //为输出直方图开辟空间  

  501.     Mat hist = _hist.getMat();    

  502.     hist.flags = (hist.flags & ~CV_MAT_TYPE_MASK)|CV_32S;//直方图矩阵的类型信息   

  503.     hist = Scalar(0.f); //清空原来的数据,当前直方图不累加到原来的直方图上去  

  504.        

  505.     //被统计的通道索引,对应直方图的第一维和第二维,和第三维  

  506.     int ch1 = channels[dims-3], ch2 = channels[dims-2],ch3 = channels[dims-1];   

  507.     //对应于直方图的第一维的区间范围,bin个数等  

  508.     float low_range1 = ranges[0][0] ,high_range1 = ranges[0][1];//要计算的直方图第一维区间的上下限  

  509.     float a1 = histSize[0]/(high_range1 - low_range1);/* 第一维单位区间内直方图bin的数目*/  

  510.     float b1 = -a1*low_range1;  

  511.     //对应于直方图的第二维的区间范围,bin个数等  

  512.     float low_range2= ranges[1][0] ,high_range2 = ranges[1][1];//要计算的直方图第二维区间的上下限  

  513.     float a2 = histSize[1]/(high_range2 - low_range2);/* 单位区间内直方图bin的数目*/  

  514.     float b2 = -a2*low_range2;  

  515.     //对应于直方图的第三维的区间范围,bin个数等  

  516.     float low_range3 = ranges[2][0] ,high_range3 = ranges[2][1];//要计算的直方图第三维区间的上下限  

  517.     float a3 = histSize[2]/(high_range3 - low_range3);/* 单位区间内直方图bin的数目*/  

  518.     float b3 = -a3*low_range3;  

  519.   

  520.     const uchar* hist_data = hist.data; //指向直方图矩阵的数据区的首指针  

  521.     size_t hist_step0 = hist.step[0];  //直方图第一维的步长,层(或叫“面”)索引  

  522.     size_t hist_step1 = hist.step[1];  //直方图第er维的步长,某层图像的行索引  

  523.     size_t hist_step2 = hist.step[2];  //直方图第san维的步长,某层图像的列索引  

  524.   

  525.     Mat_<Vec3b> Myx = image;//Mxy是一个uchar类型的三元组,用于取出image中位于(x,y)位置的像素  

  526.       

  527.     //外循环按行从上往下扫描    

  528.     for(int y=0; y < img_height ; y++ )  

  529.     {  

  530.         //指向图像第y行的指针  

  531.         const uchar* ptr_y = image.ptr<uchar>(y);  

  532.           

  533.         //内循环从左往右扫描  

  534.         for(int x=0; x < img_width ; x++)  

  535.         {  

  536.             //取出输入图像的第y行第x列所有通道的像素值  

  537.             Vec3b val = Myx(y,x);//val三元组中按照B,G,R的顺序存放着三个通道的uchar值             

  538.             //计算像素值val在输入参数规定的灰度区间内的bin序号的索引值  

  539.             int idx1 = cvFloor(val[ch1]*a1 + b1);//输出直方图第一维的bin索引号  

  540.             int idx2 = cvFloor(val[ch2]*a2 + b2);//输出直方图第二维的bin索引号  

  541.             int idx3 = cvFloor(val[ch3]*a3 + b3);//输出直方图第三维的bin索引号  

  542.             //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引,第三维idx3为列索引  

  543.             float* hist_ptr =(float*)(hist_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2);    

  544.              // 调用核函数,计算图像块(x,y)位置处的加权值  

  545.             float weighted_value = kernal(center,Point2f(x,y),hx,hy); //(x,y)位置处的加权值  

  546.             (*hist_ptr) += weighted_value; //加权累计直方图的每一个bin  

  547.             NormalConst += weighted_value; //加权系数归一化因子  

  548.         }  

  549.     }  

  550.     //归一化因子  

  551.     float  NomalConst_inv = 1.0f/(NormalConst + DBL_EPSILON);  

  552.     //归一化加权直方图  

  553.     for(int idx1=0; idx1 < histSize[0]; idx1++)  

  554.     {  

  555.         for(int idx2=0; idx2 < histSize[1]; idx2++)  

  556.         {  

  557.             for(int idx3=0; idx3 < histSize[2]; idx3++)  

  558.             {  

  559.                 //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引,第三维idx3为列索引  

  560.                 float* hist_ptr =(float*)(hist_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2);   

  561.                 *hist_ptr *=  NomalConst_inv; //乘以归一化因子  

  562.             }             

  563.         }         

  564.     }  

  565.     return;  

  566. }  

  567.   

  568. //计算两个三维直方图的巴式距离  

  569. static float  ComputeBhattacharyyaDistance3D(const Mat& hist1 ,const Mat& hist2 )  

  570. {  

  571.        

  572.     //判断两个直方图的维数是否一致   

  573.     int hist_layer = hist1.size[0] ;    //三维直方图矩阵的层数 == histSize[0]  

  574.     int hist_rows = hist1.size[1] ;     //一层图像的行数 == histSize[1]      

  575.     int hist_cols = hist1.size[2] ;     //一层图像的列数 == histSize[2]   

  576.    

  577.     if((hist_layer != hist2.size[0])||(hist_rows != hist2.size[1])  

  578.         ||(hist_cols != hist2.size[2]))  

  579.     {  

  580.         printf("两个直方图的维数不一致,无法计算距离!!! ");getchar();  

  581.     }  

  582.   

  583.     const uchar* hist1_data = hist1.data; //指向直方图矩阵的数据区的首指针  

  584.     const uchar* hist2_data = hist2.data; //指向直方图矩阵的数据区的首指针  

  585.     size_t hist_step0 = hist1.step[0];  //直方图第一维的步长  

  586.     size_t hist_step1 = hist1.step[1];  //直方图第er维的步长  

  587.     size_t hist_step2 = hist1.step[2];  //直方图第san维的步长  

  588.   

  589.     float distance = 0.0f;     

  590.     const float* phist1 =0,*phist2 =0;  

  591.   

  592.     for(int idx1=0; idx1 < hist_layer; idx1++)  

  593.     {  

  594.         for(int idx2=0; idx2 < hist_rows; idx2++)  

  595.         {  

  596.             for(int idx3=0; idx3 < hist_cols; idx3++)  

  597.             {  

  598.                //第一维idx1对应于直方图矩阵的面(层)索引,第二维idx2对应于行索引,第三维idx3为列索引  

  599.                 phist1 =(float*)(hist1_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2);   

  600.                 phist2 =(float*)(hist2_data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2);   

  601.                 distance += sqrtf((*phist1)*(*phist2));//累计距离值  

  602.             }   

  603.         }  

  604.           

  605.     }  

  606.     return distance;  

  607. }  

  608.   

  609. //根据当前搜索框下的图像块,加权直方图以及目标模板直方图计算更新权值矩阵weights_matrix  

  610. static void  UpdateWeightsMatrix3D(Mat& cur_image,vector<int>& channels,Mat& modal_hist,Mat& cur_hist,  

  611.                                  int* histSize,const float** ranges,OutputArray weights_matrix)  

  612. {  

  613.     if((cur_image.channels() < 2 )||(cur_image.depth() != CV_8U))  

  614.     {  

  615.         printf("该函数只支持CV_8UC3类型的多通道图像 "); getchar();  

  616.     }   

  617.     int img_channel = cur_image.channels();  

  618.     int rows = cur_image.rows;   

  619.     int cols = cur_image.cols;  

  620.     //给权值矩阵开辟空间,权值矩阵的大小与image的大小一样,每一个权值与image的一个像素位置一一对应  

  621.     weights_matrix.create(rows,cols,CV_32FC1);//创建一个rows行cols列的单通道浮点数矩阵  

  622.     Mat wei_mat = weights_matrix.getMat();  //从OutputArray类型的参数获取Mat类型的矩阵头索引  

  623.   

  624.     //遍历图像块的每一个像素位置,找到该像素点在直方图中的bin索引号,然后求两个直方图对应bin位置处的比值  

  625.     int dims = 3; //直方图维数  

  626.     //被统计的通道索引,对应直方图的第一维和第二维,和第三维  

  627.     int ch1 = channels[dims-3], ch2 = channels[dims-2],ch3 = channels[dims-1];   

  628.     //对应于直方图的第一维的区间范围,bin个数等  

  629.     float low_range1 = ranges[0][0] ,high_range1 = ranges[0][1];//要计算的直方图第一维区间的上下限  

  630.     float a1 = histSize[0]/(high_range1 - low_range1);/* 第一维单位区间内直方图bin的数目*/  

  631.     float b1 = -a1*low_range1;  

  632.     //对应于直方图的第二维的区间范围,bin个数等  

  633.     float low_range2= ranges[1][0] ,high_range2 = ranges[1][1];//要计算的直方图第二维区间的上下限  

  634.     float a2 = histSize[1]/(high_range2 - low_range2);/* 单位区间内直方图bin的数目*/  

  635.     float b2 = -a2*low_range2;  

  636.     //对应于直方图的第三维的区间范围,bin个数等  

  637.     float low_range3 = ranges[2][0] ,high_range3 = ranges[2][1];//要计算的直方图第三维区间的上下限  

  638.     float a3 = histSize[2]/(high_range3 - low_range3);/* 单位区间内直方图bin的数目*/  

  639.     float b3 = -a3*low_range3;   

  640.    

  641.     size_t hist_step0 = modal_hist.step[0];  //直方图第一维的步长,层(或叫“面”)索引  

  642.     size_t hist_step1 = modal_hist.step[1];  //直方图第er维的步长,某层图像的行索引  

  643.     size_t hist_step2 = modal_hist.step[2];  //直方图第san维的步长,某层图像的列索引  

  644.    

  645.     //按照从上往下,从左往右的顺序填充权值矩阵  

  646.     for(int row =0; row < rows; row++)  

  647.     {  

  648.         //指向image的第row行的指针  

  649.         uchar* curimg_ptr = cur_image.ptr<uchar>(row);  

  650.         //指向权重矩阵第row行的指针  

  651.         float* weimat_ptr = wei_mat.ptr<float>(row);  

  652.   

  653.         for(int col =0; col < cols; col++)  

  654.         {  

  655.             //取出输入图像的第y行第x列第ch1、ch2、ch3 通道的像素值  

  656.             uchar val1 = curimg_ptr[col*img_channel + ch1];  

  657.             uchar val2 = curimg_ptr[col*img_channel + ch2];   

  658.             uchar val3 = curimg_ptr[col*img_channel + ch3];  

  659.             //计算灰度值val在输入参数规定的灰度区间内的bin序号的索引值  

  660.             int idx1 = cvFloor(val1*a1 + b1);//直方图第一维的bin索引号  

  661.             int idx2 = cvFloor(val2*a2 + b2);//直方图第二维的bin索引号  

  662.             int idx3 = cvFloor(val3*a3 + b3);//直方图第二维的bin索引号  

  663.             //取出(idx1,idx2)对应位置的直方图的bin值  

  664.             float* pmodal = (float*)(modal_hist.data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2);   

  665.             float* pcur   = (float*)(cur_hist.data + idx1*hist_step0 + idx2*hist_step1+ idx3*hist_step2);   

  666.             //第row行第col列的权重值  

  667.             float weight = sqrtf( (*pmodal) / (*pcur + DBL_EPSILON) );  

  668.             *(weimat_ptr + col) = weight;  

  669.         }  

  670.     }  

  671.   

  672.     return;  

  673. }  

  674.   

  675. //根据均值向量漂移到新的位置(公式26)  

  676. static void ShiftNewLocationByMeanVector3D(Mat& weights_matrix,KernalFunc  DeriveFuncOfKernal,  

  677.                                          Point& leftup,Point2f& old_center,Point2f& new_center)  

  678. {  

  679.     //权值矩阵的行列数  

  680.     int rows = weights_matrix.rows;   

  681.     int cols = weights_matrix.cols;   

  682.     //核函数的横向半径和纵向半径  

  683.     int hx = cols/2 , hy = rows/2;   

  684.     //计算新的搜索框中心位置  

  685.     float x_sum=0.f,y_sum=0.f,const_sum=0.f;  

  686.     int x = 0 , y = 0;   

  687.   

  688.     for(int row =0; row < rows; row++)  

  689.     {   

  690.         //指向权重矩阵第row行的指针  

  691.         float* weimat_ptr = weights_matrix.ptr<float>(row);  

  692.         y  = leftup.y + row;   

  693.         for(int col =0; col < cols; col++)  

  694.         {  

  695.               x = leftup.x + col;  

  696.               //当前像素点在母图像中的坐标位置  

  697.               Point2f point(x,y);  

  698.               //调用对应核函数的导函数  

  699.               float g = DeriveFuncOfKernal(old_center,point,hx,hy);  

  700.               //取出对应位置(row,col)上的权重值  

  701.               float weight = weimat_ptr[col];  

  702.               //累加分子与分母  

  703.               x_sum += x*weight*g ;  

  704.               y_sum += y*weight*g ;  

  705.               const_sum += weight*g ;   

  706.         }  

  707.     }  

  708.     //计算新的中心点  

  709.     new_center.x = cvRound(x_sum/(const_sum+DBL_EPSILON));  

  710.     new_center.y = cvRound(y_sum/(const_sum+DBL_EPSILON));  

  711. }  

  712.   

  713. //自己编写的Mean Shift算法,isJudgeOverShift参数用于控制是否在迭代过程中判断漂移冲过了头;  

  714. //最后两个参数用来保存迭代路径,用于显示搜索窗口的运动和记录窗口的巴氏系数  

  715. int MyMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria ,  

  716.                    bool isJudgeOverShift,vector<Rect>& iterRects ,vector<float>& iterBhattaCoeff)  

  717. {  

  718.    

  719.     int  i = 0, eps;  

  720.       

  721.     Rect cur_rect ;  //当前搜索窗口  

  722.     Mat  cur_win; //当前搜索窗口下的图像块  

  723.     Mat  cur_hist; //当前搜索窗口下图像块的加权直方图  

  724.     Mat  cur_weights;//当前搜索窗口下的权值矩阵  

  725.     //初始化迭代路径,  

  726.     iterRects.clear();   

  727.     iterBhattaCoeff.clear();  

  728.        

  729.     Mat mat = image;  

  730.     Rect windowIn(initialWindow);  

  731.   

  732.     if( mat.channels()<2 )//目前只支持多通道图像  

  733.         CV_Error( CV_BadNumChannels, "目前只支持多通道图像" );  

  734.   

  735.     if( windowIn.height <= 0 || windowIn.width <= 0 )  

  736.         CV_Error( CV_StsBadArg, "窗口尺寸必须大于0" );  

  737.   

  738.      windowIn = Rect(windowIn) & Rect(0, 0, mat.cols, mat.rows); //边界检查  

  739.   

  740.      //检查迭代终止条件  

  741.     criteria = cvCheckTermCriteria( criteria, 1., 100 );  

  742.     eps = cvRound( criteria.epsilon * criteria.epsilon );  

  743.      //初始搜索框  

  744.     cur_rect =  windowIn;    

  745.     float BhattaCoeff = 0.0f;  

  746.     

  747.     //开始MeanShift迭代过程,实际迭代次数小于等于criteria.maxCount  

  748.     for( i = 0; i < criteria.maxCount; i++ )  

  749.     {  

  750.         int dx, dy, nx, ny;  

  751.            

  752.          //当前搜索窗口的中心  

  753.         Point2f cur_center(cur_rect.x +0.5f* cur_rect.width,  

  754.                         cur_rect.y + 0.5f* cur_rect.height);  

  755.         //当前搜索窗口的左上角顶点坐标  

  756.         Point2i cur_leftup(cur_rect.x,cur_rect.y);  

  757.   

  758.         //从当前帧图像中获取当前搜索窗口下的图像块   

  759.         cur_win = mat(cur_rect);   

  760.            

  761.         //计算当前窗口下的图像块的加权直方图  

  762.         CalculateWeightedHistogram(cur_win,select_channels,cur_hist,weight_method);   

  763.   

  764.         //计算当前搜索窗口下的直方图与给定的目标模板直方图的巴氏系数  

  765.         BhattaCoeff = ComputeBhattacharyyaDistance3D(modal_hist,cur_hist);  

  766.        

  767.          //根据公式25计算当前窗口下的权值矩阵,  

  768.         UpdateWeightsMatrix3D(cur_win,select_channels,modal_hist,cur_hist,  

  769.                                         histSize,histRange,cur_weights); //公式-25  

  770.           

  771.         //根据公式26计算均值漂移向量,得到新的搜索窗口的中心位置   

  772.         Point2f new_center;//新的搜索窗口的中心位置   

  773.         ShiftNewLocationByMeanVector3D(cur_weights,DeriveFuncOfKernal,  

  774.                                      cur_leftup,cur_center,new_center); //公式-26  

  775.            

  776.         //新的搜索区域的左上角坐标  

  777.         nx = cvRound(new_center.x - 0.5f*cur_rect.width);  

  778.         ny = cvRound(new_center.y - 0.5f*cur_rect.height);  

  779.         //x方向的边界检查  

  780.         if( nx < 0 )  

  781.             nx = 0;  

  782.         else if( nx + cur_rect.width > mat.cols )  

  783.             nx = mat.cols - cur_rect.width;  

  784.         //y方向的边界检查  

  785.         if( ny < 0 )  

  786.             ny = 0;  

  787.         else if( ny + cur_rect.height > mat.rows )  

  788.             ny = mat.rows - cur_rect.height;                  

  789.            

  790.         //计算漂移向量(dx,dy)  

  791.         dx = nx - cur_rect.x;  

  792.         dy = ny - cur_rect.y;  

  793.   

  794.         //记录迭代过程  

  795.         iterRects.push_back(cur_rect);  

  796.         iterBhattaCoeff.push_back(BhattaCoeff);  

  797.    

  798.         //形成新的搜索框,移动左上角顶点坐标,长宽不变  

  799.         Rect new_rect(cur_rect);  

  800.         new_rect.x = nx;  

  801.         new_rect.y = ny;  

  802.   

  803.         //判断均值漂移是否冲过了头  

  804.         if(isJudgeOverShift)  

  805.         {  

  806.             Mat new_win = mat(new_rect); //取出新窗口下的图像块  

  807.             Mat new_hist;  

  808.             //计算新窗口下的图像块的加权直方图  

  809.             CalculateWeightedHistogram(new_win,select_channels,new_hist,0);   

  810.             //计算新窗口下的直方图与给定的目标模板直方图的巴氏系数  

  811.             float new_BhattaCoeff = ComputeBhattacharyyaDistance3D(modal_hist,new_hist);  

  812.             //如果新位置上的巴氏系数小于旧位置上的的巴氏系数,说明冲过了头  

  813.             if(new_BhattaCoeff < BhattaCoeff)  

  814.             {   

  815.                 //若果冲过了头,就把两次的位置坐标平均一下,得出新的位置  

  816.                 new_rect.x = cvRound(0.5f*(cur_rect.x + new_rect.x));  

  817.                 new_rect.y = cvRound(0.5f*(cur_rect.y + new_rect.y));  

  818.   

  819.                 //由于调整了新窗口的位置,所以要重新计算漂移向量(dx,dy)  

  820.                 dx = new_rect.x - cur_rect.x;  

  821.                 dy = new_rect.y - cur_rect.y;  

  822.             }  

  823.         }  

  824.            

  825.         /* 检查窗口的移动距离是否小于eps,若小于,则算法收敛,退出循环 */  

  826.         if( dx*dx + dy*dy < eps )  

  827.         {  

  828.             cur_rect = new_rect;  

  829.             //如果在没有达到最大迭代次数之前该退出条件已经满足  

  830.             //则iterRects中的最后一个元素就是最终的迭代结果  

  831.             iterRects.push_back(cur_rect);  

  832.             iterBhattaCoeff.push_back(BhattaCoeff);  

  833.             break;  

  834.         }  

  835.         //进入下一次迭代  

  836.         cur_rect = new_rect; //这个搜索框在循环退出时才被加入到迭代轨迹数组中  

  837.   

  838.     }  

  839.     //如果超过最大迭代次数的时候,循环退出,把最后一次均值漂移形成的矩形框作为最终的结果  

  840.     if( i == criteria.maxCount)  

  841.     {  

  842.         iterRects.push_back(cur_rect);  

  843.         iterBhattaCoeff.push_back(BhattaCoeff);  

  844.     }   

  845.   

  846.     return iterRects.size();//返回实际使用的迭代次数(漂移次数)  

  847. }  

  848.   

  849. //把给定的矩形窗口缩放给定的尺度,但是保持中心位置不变  

  850. static Rect ScaleRectSize( Rect& rect , float scale)  

  851. {  

  852.     Rect new_rect;  

  853.     //获取rect的中心坐标  

  854.     int cx = rect.x + cvRound(rect.width/2.f);  

  855.     int cy = rect.y + cvRound(rect.height/2.f);  

  856.     //新的窗口尺寸  

  857.     new_rect.width =  cvRound(rect.width*scale);  

  858.     new_rect.height = cvRound(rect.height*scale);  

  859.     //新的左上角顶点坐标  

  860.     new_rect.x = cx -  cvRound(rect.width*scale*0.5f);  

  861.     new_rect.y = cy -  cvRound(rect.height*scale*0.5f);   

  862.   

  863.     return new_rect;   

  864. }  

  865. //具有尺度自适应的MeanShift算法  

  866. int MyScaleAdaptMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria ,  

  867.                              bool isJudgeOverShift,vector<Rect>& iterRects ,vector<float>& iterBhattaCoeff)  

  868. {   

  869.     float  scale = 0.1f; //缩放尺度  

  870.     float gama = 0.1f;  //对旧尺寸和新尺寸的加权系数  

  871.     Rect temp_rect;  

  872.        

  873.     //首先以原始窗口迭代  

  874.     MyMeanShift3D(image,modal_hist,initialWindow,criteria,isJudgeOverShift,iterRects,iterBhattaCoeff);  

  875.     temp_rect = iterRects[iterRects.size()-1];//原始尺寸窗口的搜索结果  

  876.     //然后以小窗口迭代,起始位置是原始尺寸窗口进行迭代后返回的位置      

  877.     Rect small_rect = ScaleRectSize(temp_rect,1 - scale);//比初始窗口小的窗口  

  878.     //迭代路径  

  879.     vector<Rect> small_iterRects ;vector<float>  small_iterBhattaCoeff;   

  880.     MyMeanShift3D(image,modal_hist,small_rect,criteria,isJudgeOverShift,small_iterRects,small_iterBhattaCoeff);  

  881.     //然后用大窗口迭代 起始位置是原始尺寸窗口进行迭代后返回的位置      

  882.     Rect large_rect = ScaleRectSize(temp_rect,1 + scale);//比初始窗口大的窗口  

  883.     vector<Rect> large_iterRects ;vector<float>  large_iterBhattaCoeff;   

  884.     MyMeanShift3D(image,modal_hist,large_rect,criteria,isJudgeOverShift,large_iterRects,large_iterBhattaCoeff);  

  885.   

  886.     //判断小尺寸窗口巴氏系数是否比原始窗口大  

  887.     if((small_iterBhattaCoeff[small_iterBhattaCoeff.size()-1] > iterBhattaCoeff[iterBhattaCoeff.size()-1])  

  888.         ||(small_iterBhattaCoeff[small_iterBhattaCoeff.size()-1] > large_iterBhattaCoeff[large_iterBhattaCoeff.size()-1]))  

  889.     {  

  890.         Rect new_rect = small_iterRects[small_iterRects.size()-1];  

  891.         //小尺寸窗口迭代返回结果的中心坐标  

  892.         int cx = new_rect.x + cvRound(new_rect.width/2.f);  

  893.         int cy = new_rect.y + cvRound(new_rect.height/2.f);  

  894.         //一阶滤波以后的窗口尺寸  

  895.         int  new_width = cvRound(gama*new_rect.width + (1-gama)*temp_rect.width);  

  896.         int  new_height = cvRound(gama*new_rect.height + (1-gama)*temp_rect.height);   

  897.         //新的左上角顶点坐标  

  898.         int nx = cx - cvRound(new_width*0.5f);  

  899.         int ny = cy - cvRound(new_height*0.5f);  

  900.         //把结果放入迭代记录数组,返回  

  901.         small_iterRects[small_iterRects.size()-1] = Rect(nx,ny,new_width,new_height);  

  902.         iterRects = small_iterRects;  

  903.         iterBhattaCoeff = small_iterBhattaCoeff;  

  904.           

  905.     }//判断大尺寸窗口巴氏系数是否比原始窗口大  

  906.     else if((large_iterBhattaCoeff[large_iterBhattaCoeff.size()-1] > iterBhattaCoeff[iterBhattaCoeff.size()-1])  

  907.         ||(large_iterBhattaCoeff[large_iterBhattaCoeff.size()-1] > small_iterBhattaCoeff[small_iterBhattaCoeff.size()-1]))  

  908.     {  

  909.         Rect new_rect = large_iterRects[large_iterRects.size()-1];  

  910.         //大尺寸窗口迭代返回结果的中心坐标  

  911.         int cx = new_rect.x + cvRound(new_rect.width/2.f);  

  912.         int cy = new_rect.y + cvRound(new_rect.height/2.f);  

  913.         //一阶滤波以后的窗口尺寸  

  914.         int  new_width = cvRound(gama*new_rect.width + (1-gama)*temp_rect.width);  

  915.         int  new_height = cvRound(gama*new_rect.height + (1-gama)*temp_rect.height);   

  916.         //新的左上角顶点坐标  

  917.         int nx = cx - cvRound(new_width*0.5f);  

  918.         int ny = cy - cvRound(new_height*0.5f);  

  919.         //把结果放入迭代记录数组,返回  

  920.         large_iterRects[large_iterRects.size()-1] = Rect(nx,ny,new_width,new_height);  

  921.         iterRects = large_iterRects;  

  922.         iterBhattaCoeff = large_iterBhattaCoeff;   

  923.     }  

  924.      return iterBhattaCoeff.size();  

  925. }