Kernal-Based Object Tracking---基于核函数的目标跟踪 一次只能跟踪一个目标
#include "stdafx.h"
#include <windows.h>
#include <stdio.h>
#include <ctype.h>
#include <iostream>
#include <opencv2/opencv.hpp>
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<int> 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<int>& channels,Mat& _hist,int weighted_method);
//该函数用于计算给定矩形图像块的非加权直方图
static void CalculateHistogram(const Mat& img , Mat& _hist);
//自己写的计算图像一维直方图的函数
void myCalcHist3D( const Mat& image, vector<int>& channels,OutputArray _hist,
int dims, const int* histSize,const float** ranges);
//计算图像一维加权直方图的函数
static void myCalcWeightedHist3D( const Mat& image, vector<int>& 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<Rect>& iterRects ,vector<float>& iterBhattaCoeff);
//具有尺度自适应的MeanShift算法
int MyScaleAdaptMeanShift3D(Mat& image, Mat& modal_hist, Rect& initialWindow,TermCriteria criteria ,
bool isJudgeOverShift,vector<Rect>& iterRects ,vector<float>& iterBhattaCoeff);
int _tmain(int argc, _TCHAR* argv[])
{
//VideoCapture cap; //声明一个读取视频流的类VideoCapture的对象
Rect trackWindow; //跟踪窗口
CvCapture* cap = 0; //视频获取结构
cap = cvCreateFileCapture("E:\calibration\视频文件\FFOutput\强光下的人（热成像广角）_20141127184435_20141127184456.avi"); //读本地视频文件
if( !cap )//判断相机是否被正常打开
{
cout << "***无法初始化视频读入流...*** ";
cout << "当前命令行参数值： ";
return -1;
}
namedWindow( "MeanShift Demo", 1 );//创建显示跟踪过程的窗口
setMouseCallback( "MeanShift Demo", onMouse, 0 );//为窗口添加鼠标响应函数，用于选择跟踪目标
//在大循环外预先声明重复使用的变量
Mat frame1, colorFrame,modal_hist,ms_hist;
vector<Rect> IterRects;vector<float> 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算法的迭代收敛条件
/**********控制算法运行行为的主要参数*******************************************************/
IplImage *image1 =NULL;
/*进入循环，处理视频图像序列，跟踪目标*/
for(;)
{
//if( !paused ) //如果没有暂停，则继续读入下一帧图像
// {
if (!cvGrabFrame(cap)) //从摄像头或者视频文件中抓取帧
break;
image1 = cvRetrieveFrame(cap); //取回由函数cvGrabFrame抓取的图像,返回由函数cvGrabFrame 抓取的图像的指针
Mat frame(image1);
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<<"当前帧所用迭代次数： "<<IterNum<<endl;
for(int ii=1;(ii<=IterNum)&&(showTrace);ii++)
{
Point c1,c2;//前后两次迭代矩形搜索框的中心
c1 = Point(IterRects[ii-1].x+ IterRects[ii-1].width*0.5f,
IterRects[ii-1].y+ IterRects[ii-1].height*0.5f);
c2 = Point(IterRects[ii].x+ IterRects[ii].width*0.5f,
IterRects[ii].y+ IterRects[ii].height*0.5f);
circle(image,c1,3,Scalar(0,0,0),2,8);
circle(image,c2,3,Scalar(0,0,0),2,8);
//将所有迭代过程的中心连起来，显示在当前帧上均值漂移的过程
line(image,c1,c2,Scalar(0,0,255),2,8);
}
}
}
else if( trackObject < 0 )
{
paused = false;
}
//下面这段if语句代码也只有在第一帧上选定目标时才会被执行
//在按下鼠标左键和抬起鼠标左键之间的这段时间，selectObject为真，
//selection会随着鼠标的移动不断变化，直到抬起鼠标左键后，
//selectObject为假，selection就是选中的目标矩形框
if( selectObject && selection.width > 0 && selection.height > 0 )
{
Mat roi_img(image, selection);
bitwise_not(roi_img, roi_img);
}
//显示当前要处理的图像
imshow( "MeanShift Demo", image );
char c = (char)waitKey(2);//每处理完一帧图像，等待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<int>& 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<int>& 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<int>& 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的通道数 ");
getchar();
}
if(image.depth() != CV_8U)
{
printf("该函数仅支持CV_8U类的图像 ");
getchar();
}
if(dims != calc_channels)
{
printf("被索引的通道数目与直方图的维数不相等 ");
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_<Vec3b> 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<int>& 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的通道数 ");
getchar();
}
if(image.depth() != CV_8U)
{
printf("该函数仅支持CV_8U类的图像 ");
getchar();
}
if(dims != calc_channels)
{
printf("被索引的通道数目与直方图的维数不相等 ");
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_<Vec3b> Myx = image;//Mxy是一个uchar类型的三元组，用于取出image中位于(x,y)位置的像素
//外循环按行从上往下扫描
for(int y=0; y < img_height y++ )
{
//指向图像第y行的指针
const uchar* ptr_y = image.ptr<uchar>(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("两个直方图的维数不一致,无法计算距离！！！ ");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<int>& 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类型的多通道图像 "); 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<uchar>(row);
//指向权重矩阵第row行的指针
float* weimat_ptr = wei_mat.ptr<float>(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<float>(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<Rect>& iterRects ,vector<float>& 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<Rect>& iterRects ,vector<float>& 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<Rect> small_iterRects vector<float> small_iterBhattaCoeff;
MyMeanShift3D(image,modal_hist,small_rect,criteria,isJudgeOverShift,small_iterRects,small_iterBhattaCoeff);
//然后用大窗口迭代 起始位置是原始尺寸窗口进行迭代后返回的位置
Rect large_rect = ScaleRectSize(temp_rect,1 + scale);//比初始窗口大的窗口
vector<Rect> large_iterRects vector<float> 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();
}
我司举办举行首期ADAS开发技术内部培训
知远2015-3-27走进上汽集团智能化&前瞻技术交流会
ADAS为什么如此重要？
知远如何打造一款廉价又不失情怀的安全产品