[求助] mean shift图像平滑算法bug求解

function [ flt_img ] = msflt( img,sigma_s,sigma_r )
    k_constant = 1;%uniform kernel的常量
    sigma =0.1;%mean shift终止时的允许误差
    [r,c,p] = size(img);
    flt_img = zeros(r,c,p);
    %若输入图像是RGB彩色图像，将RGB图像转换成LAB颜色空间图像
    if p == 3
        if exist('applycform','file')
            img = applycform(img,makecform('srgb2lab'));
        else
            img = colorspace('Lab<-RGB',img);
        end
    end
   
    x = zeros(r*c,2+p);
    z = zeros(r*c,2+p);
    %将二维数据转换成一维数据
    if p == 3
        for i = 1:r
            for j = 1:c
                x((i-1)*c+j,:) = [i,j,img(i,j,1),img(i,j,2),img(i,j,3)];
            end
        end
    end
    if p == 1
        for i = 1:r
            for j = 1:c
                x((i-1)*c+j,:) = [i,j,img(i,j)];
            end
        end
    end
    %mean shift部分
    for i = 1:r*c
        
        pre_y = x(i,:);
        tmp_y = x(i,:);
        
        while 1 %计算某个点的收敛点
            pre_y = tmp_y;
            sum1 = zeros(1,p+2);
            sum2 = 0;
            axis_x = fix(tmp_y(1));
            axis_y = fix(tmp_y(2));
            for j = axis_x-sigma_s-1:axis_x+sigma_s+1        %计算偏移均值
                for k = axis_y-sigma_s-1:axis_y+sigma_s+1
                    if j > 0 && j <= r && k>0 && k <= c %&& j ~= fix((i-1)/c) + 1 && k ~= mod(i-1,c) + 1
                        m = (j-1)*c + k;
                        tmp1 = [pre_y(1)-x(m,1),pre_y(2)-x(m,2)]/sigma_s;
                        %tmp1 = [x(m,1),x(m,2)]/sigma_s;
                        %tmp1 = [x(i,1)-x(m,1),x(i,2)-x(m,2)]/sigma_s;
                        if p == 3
                            tmp2 = [pre_y(3)-x(m,3),pre_y(4)-x(m,4),pre_y(5)-x(m,5)]/sigma_r;
                        end
                        if p == 1
                            tmp2 = (pre_y(3)-x(m,3))/sigma_r;
                        end
                        tmp1 = tmp1.*tmp1;
                        tmp2 = tmp2.*tmp2;
                        if sum(tmp1(:)) <= 1 && sum(tmp2(:)) <= 1%采用uniform kernel
                            sum1 = sum1 + k_constant*x(m,:);
                            sum2 = sum2 + k_constant;
                        end
                    end
                end
            end
            tmp_y = sum1/sum2;
            tmp = tmp_y - pre_y;
            if abs(tmp) < sigma
                break;
            end
        end
        
        z(i,:) = tmp_y;
        a = fix((i-1)/c) + 1;
        b = rem(i-1,c) + 1;
        if p == 3
            flt_img(a,b,1) = tmp_y(3);
            flt_img(a,b,2) = tmp_y(4);
            flt_img(a,b,3) = tmp_y(5);
        end
        if p == 1
            flt_img(a,b) = tmp_y(3);
        end
        
    end
   
    if p == 3
        if exist('applycform','file')
            flt_img = applycform(flt_img,makecform('lab2srgb'));
        else
            flt_img = colorspace('RGB<-Lab',flt_img);
        end
    end
end

%主函数
img = imread('cameraman.jpg');
T = img;
%img = img/255;
%flt_img = uint8(msflt(img,8,4));
flt_img = msflt(img,8,4);
figure;
imshow(img);
figure;
%imshow(flt_img);
imshow(mat2gray(flt_img));