OpenCV 双边滤波器代码实现

void BilateralFilter(InputArray _src, OutputArray _dst, int d,
	double sigmaColor, double sigmaSpace,
	int borderType)
{
	Mat src = _src.getMat();
	_dst.create(src.size(), src.type());
	Mat dst = _dst.getMat();

	if (src.depth() == CV_8U)
		bilateralFilter_8u(src, dst, d, sigmaColor, sigmaSpace, borderType);
}


void
bilateralFilter_8u(const Mat& src, Mat& dst, int d,
	double sigma_color, double sigma_space,
	int borderType)
{

	int cn = src.channels();
	int i, j, k, maxk, radius;
	Size size = src.size();

	CV_Assert((src.type() == CV_8UC1 || src.type() == CV_8UC3) &&
		src.type() == dst.type() && src.size() == dst.size() &&
		src.data != dst.data);

	if (sigma_color <= 0)
		sigma_color = 1;
	if (sigma_space <= 0)
		sigma_space = 1;

	// 计算颜色域和空间域的权重的高斯核系数, 均值 μ = 0;  exp(-1/(2*sigma^2))  
	double gauss_color_coeff = -0.5 / (sigma_color*sigma_color);
	double gauss_space_coeff = -0.5 / (sigma_space*sigma_space);

	// radius 为空间域的大小: 其值是 windosw_size 的一半    
	if (d <= 0)
		radius = cvRound(sigma_space*1.5);
	else
		radius = d / 2;
	radius = MAX(radius, 1);
	d = radius * 2 + 1;

	cout << "radius: " << radius << endl;
	cout << "d: " << d << endl;

	Mat temp;
	copyMakeBorder(src, temp, radius, radius, radius, radius, borderType);
	cout << "copyMakeBorder = " << endl << temp << endl;

	vector _color_weight(cn * 256);
	vector _space_weight(d*d);
	vector _space_ofs(d*d);
	float* color_weight = &_color_weight[0];
	float* space_weight = &_space_weight[0];
	int* space_ofs = &_space_ofs[0];

	// 初始化颜色相关的滤波器系数: exp(-1*x^2/(2*sigma^2))
	cout << "颜色相关的滤波器系数：";
	for (i = 0; i < 256 * cn; i++)
	{
		color_weight[i] = (float)std::exp(i*i*gauss_color_coeff);
		cout << i << ":" << color_weight[i] << "; ";
	}

	// 初始化空间相关的滤波器系数和 offset:  
	cout << endl<<"空间相关的滤波器系数和offset：";
	for (i = -radius, maxk = 0; i <= radius; i++)
	{
		j = -radius;

		for (; j <= radius; j++)
		{
			double r = std::sqrt((double)i*i + (double)j*j);
			if (r > radius)
				continue;
			space_weight[maxk] = (float)std::exp(r*r*gauss_space_coeff);
			//space_ofs[maxk++] = (int)(i*temp.step + j * cn);
			space_ofs[maxk] = (int)(i*temp.step + j * cn);

			cout << "(" <