有大佬有灰度梯度法亚像素位移计算的matlab程序吗？

或者我手头有一些function，帮我写个主程序也行，可提升c币报酬

function xyinput=cpcorr_graient(varargin)
[xyinput_in,xybase_in,input,base]=ParseInputs(varargin{:});
%相关系数，据实验为10时较合理
CORRSIZE=10;
%直角坐标输入
rects_input=calc_rects(xyinput_in,CORRSIZE,input);
rects_base=calc_rects(xybase_in,CORRSIZE,base);
ncp=size(xyinput_in,1);
xyinput=xyinput_in;%初始化调整后
for icp=1:ncp
    if isequal(rects_input(icp,3:4),[0 0])||isequal(rects_base(icp,3:4),[0 0])
    %靠近边缘无法调整
    continue
    end
    sub_input=imcrop(input,rects_input(icp,:));
    sub_base=imcrop(base,rects_base(icp,:));
% sub_input = imresize( sub_input ,scale,' bicubic ');
% sub_base = imresize ( sub_base ,scale,' bicubicf')；
inputsize=size(sub_input);
% 确定有限
if any(~isfinite(sub_input(:)))||any(~isfinite( sub_base(:)))
% NaN or Inf , unable to adjust
    continue
end
% 确定sub_input标准差非零
if std( sub_input(:))==0
% 标准差为0无法调整
    continue
end
norm_cross_corr=normxcorr2(sub_input,sub_base);
% 将其设置为false，使得峰值寻找程序只返回像素精确的峰值数据
%因此可以使用梯度方法来获取亚像素数据
subpixel=false;
[xpeak,ypeak,amplitude]=findpeak(norm_cross_corr,subpixel);
% eliminate any poor correlations
THRESHOLD=0.5;
if(amplitude<THRESHOLD)
continue
end
% 通过互相关修正得到的偏移量，以反映图像的缩放
corr_offset=[(( xpeak-inputsize(2))-CORRSIZE)...
(( ypeak-inputsize(l))-CORRSIZE)];
% 限制像素位移到10所以梯度法的空间,即子图像总是完全重叠基图像
if abs(corr_offset (1))>= CORRSIZE
   corr_offset(1)=0;
end
if abs(corr_offset(2))>C0RRSIZE
corr_offset(2)=0;
end
filtx =[1/12-8/12 0 8/12 -1/12];
%y方向的转置
filty =filtxf;
%用掩模卷积子图像得到偏导数近似
for i=l: size( sub_input,2)
Gx(i,:)= conv(filtx,sub_input (i,:));
end
% 同一段求导四次
Gx=Gx(3:size(Gx,1)-2,5:size(Gx,2)-4);
% 同样方法计算
for i=l: size( sub_input,1)
Gy(:,i)= conv(filty,sub_input(:,i));
end
Gy=Gy(5:size(Gy,l)-4,3: size (Gy ,2),2);
% 计算截断泰勒展开式最小二乘解的和
Gx2=sum(sum(Gx.*Gx));
Gy2=sum(sum(Gy.*Gy));
Gxy=sum(sum(Gx.*Gy));
xshift = CORRSIZE+corr_offset(l,1);
yshift = CORRSIZE+corr_offset(l,2);
% 子图像下的基图像部分，包括像素位移
sub_base_shift=sub_base(yshift+3:yshift+inputsize(1,1)-2,xshift +3: xshift ...
+ inputsize (1,2)-2);
sub_input_shift=sub_input(3:size(sub_input,1)-2,3:size(sub_input,2),2);
Gxt=sum(sum (( sub_base_shift- sub_input_shift ).*Gx));
Gyt=sum(sum (( sub_base_shift- sub_input_shift ).*Gy));
mat=[Gx2 Gxy;Gxy Gy2];
% 亚像素位移
disp=pinv(mat)*[Gxt;Gyt];
corr_offset=corr_offset +disp';
clear Gx Gy Gx Gxt Gxy Gyt xshift yshift
% 消除控制点的突变
ind=find(abs(corr_offset)>(CORRSIZE-1), 1);
if~isempty(ind)
continue
end
input_fractional_offset=xyinput(icp,:)-round(xyinput(icp,:));
base_fractional_offset=xybase_in(icp,:)- round (xybase_in(icp,:));
%调整控制点
xyinput(icp,:)=xyinput(icp,:)-input_fractional_offset-corr_offset+base_fractional_offset;
end
%
function rect=calc_rects(xy,halfwidth,img)
% 计算矩形，这样imcrop将返回中心像素内xy坐标的图像
default_width=2* halfwidth ;
default_height=default_width ;
% xy指定矩形的中心,需要左上角坐标
upperleft=round(xy)-halfwidth;
%需要修改图像边缘附近的像素
upper=upperleft(:,2);
left=upperleft(:,1);
lower=upper+default_height;
right=left + default_width;
width=default_width*ones(size(upper));
height = default_height*ones( size ( upper ));
%检查图像外的坐标边缘
[upper,height]=adjust_lo_edge(upper,height);
[dum,height]=adjust_hi_edge(lower,size(img ,1),height );
[left,width]= adjust_lo_edge(left,1,width);
[dum ,width ]=adjust_hi_edge (right,size(img,2),width);
%当小于默认大小时，将宽度和高度设置为零
iw=find(width<default_width);
ih=find(height<default_height);
idx=unique([iw;ih]);
width(idx)=0;
height(idx)=0;
rect=[left upper width height ];
%--------------------------------
function [coordinates,breadth]=adjust_lo_edge(coordinates,edge,breadth)
indx = find(coordinates<edge);
if~isempty ( indx )
breadth(indx)=breadth(indx)-abs(coordinates(indx)-edge);
coordinates(indx)=edge;
end
%--------------------------
function[coordinates,breadth]=adjust_hi_edge ( coordinates,edge,breadth )
indx = find ( coordinates > edge );
if~isempty ( indx )
breadth ( indx ) = breadth ( indx ) - abs ( coordinates ( indx )-edge);
coordinates ( indx ) = edge ;
end
%-------------------------------
function [ xyinput_in,xybase_in,input, base]= ParseInputs(varargin)
iptchecknargin(4,4,nargin,mfilename);
xyinput_in=varargin{1};
xybase_in= varargin{2};
if size ( xyinput_in,2)~=2||size(xybase_in,2)~=2
msg = sprintf('In fimction %s control point matrices must be M-by -2.',mfilename);
eid = sprintf ('Images:%s: cpMatrixMustBeMby2', mfilename );
error(eid,msg);
end
if size ( xyinput_in,1)~=size(xybase_in,1)
msg = sprintf('%s,INPUT and BASE images need same number of control points.',mfilename);
eid = sprintf('Images:%s: needSameNumOfControlPoints',mfilename);
error(eid,msg);
end
input = varargin{3};
base = varargin {4};
if ndims( input )~=2 ||ndims(base)~=2
msg = sprintf ...
('In function %s，Images must be intensity images',mfilename );
eid=sprintf('Images :%s: intensityImagesReq',mfilename);
error(eid,msg);
end
input=double(input);
base=double(base);
if any(xyinput_in (:) <0.5)||any(xyinput_in (:,1) >size (input,2)+0.5)||any(xyinput_in(:,2) >size (input ,1) +0.5)||any( xybase_in (:) <0.5)||any( xybase_in (:,1) >...
    size(base,2)+0,5)||any( xybase_in (:,2) >size (base,1) +0.5)
msg = sprintf('In function %s，Control Points must be in pixel coordinates',mfilename );
eid = sprintf('Images:%s: cpPointsMustBelnPixCoord',mfilename);
error(eid,msg);
end