牛顿拉普森_NR、亚像素搜索

function [u0,C] = Cal_N_R_2(Iref, Idef_0, u0, steps) %牛顿迭代的简单程序 %Idef_0 比Iref 维数大4，且两个矩阵的中心点相同 Idef_0 = double(Idef_0); Iref = double(Iref); [m n] = size(Idef_0); [m1 n1] = size(Iref); % lambda = 0.5;%%% %% %判断需要仔细写 if mod(m, 2) == 0 || mod(n, 2) == 0 || m-m1 ~= 4 || n-n1 ~= 4 %%返回错误 end % u = zeros(6,1); % if size(u0,1) == 6 % u(1:2) = u0(1:2); % elseif size(u0,2) == 6 % u(1:2) = u0(1:2)'; % elseif size(u0,1) == 2 % u(1:2) = u0; % elseif size(u0,2) == 2 % u(1:2) = u0'; % else % %%error % end % clear u0 % u0 = u; % if m-m ~= 4 || n-n1 ~= 4 % Iref = Iref((3-(m-m1)/2):(end-2+(m-m1)/2),(3-(n-n1)/2):(end-2+(n-n1)/2)); % end %% M = (m-1)/2; N = (n-1)/2; [x1,y1] = meshgrid(-N:N,-M:M); x = x1(3:end-2,3:end-2); y = y1(3:end-2,3:end-2); x0 = x1(2:end-1,2:end-1); y0 = y1(2:end-1,2:end-1); m = m - 4; n = n - 4; C = zeros(steps,1); for t = 0 : steps-1, if t == 0 u = u0(1)+u0(3)*x0+u0(4)*y0; v = u0(2)+u0(5)*x0+u0(6)*y0; else u = u1(1)+u1(3)*x0+u1(4)*y0; v = u1(2)+u1(5)*x0+u1(6)*y0; end x2 = x0-u; y2 = y0-v; %变形后位置=变形前位置+/-位移 Idef0 = interp2(x1, y1, Idef_0, x2, y2,'bicubic');%Idef是(m+2*m+2)，griddata是matlab中的插值函数。 g = Idef0(2:m+1,2:n+1); C(t+1) = 1 - sum((Iref(:) - g(:)).^2)/sum((Iref(:).^2)); if t&&(C(t+1)/C(t)<1||isnan(C(t+1))) pp = 2;%%返回结束原因 break elseif t u0 = u1; end %计算一阶导数d_Idef (m,n,6) d_Idef = diff1_2(m,n,x,y,Idef0); %计算Idef的二阶导数dd_Idef (m,n,6,6) % dd_Idef = diff2_2(m,n,x,y,Idef0); % 计算雅克比矩阵 Jac(6*1) Jac = zeros(6,1); for i =1:6 s1 = (Iref - g).*d_Idef(:,:,i); Jac(i) = sum(s1(:)); end % 计算黑松矩阵Hess(6*6) Hess = zeros(6,6); for i = 1:6 for j = 1:6 % s2 = (Iref - g).*dd_Idef(:,:,i,j)-d_Idef(:,:,i).*d_Idef(:,:,j); s2 = -d_Idef(:,:,i).*d_Idef(:,:,j); Hess(i,j) = -sum(s2(:)); end end du = Hess\Jac; du_2 = norm(du(1:2)); du_3 = norm(du(3:6)); if du_2>5e-5||du_3>10e-8 u1 = u0 - du; else pp = 1; break end end if t ==steps-1; pp = 3; end % t if pp == 1 C = C(t+1); else C = C(t); end % pp % t end %% 一阶导数 function d_Idef = diff1_2(m,n,x,y,Idef) d_Idef = zeros(m,n,6); d_Idef(:,:,1) = 1/2*(Idef(2:m+1, 3:n+2) - Idef(2:m+1, 1:n));%偏y d_Idef(:,:,2) = 1/2*(Idef(3:m+2, 2:n+1) - Idef(1:m, 2:n+1)); d_Idef(:,:,3) = d_Idef(:,:,1).*x; d_Idef(:,:,4) = d_Idef(:,:,1).*y; d_Idef(:,:,5) = d_Idef(:,:,2).*x; d_Idef(:,:,6) = d_Idef(:,:,2).*y; end %% 二阶导数 function dd_Idef = diff2_2(m,n,x,y,Idef) dd_Idef = zeros(m,n,6,6); dd_Idef(:,:,1,1) = -2* Idef(2:m+1, 2:n+1) + Idef(2:m+1, 1:n)+ Idef(2:m+1, 3:n+2); dd_Idef(:,:,1,2) = -2* Idef(2:m+1, 2:n+1) + Idef(1:m, 1:n)+ Idef(3: m+2, 3:n+2); dd_Idef(:,:,1,3) = dd_Idef(:,:,1,1).*x; dd_Idef(:,:,1,4) = dd_Idef(:,:,1,1).*y; dd_Idef(:,:,1,5) = dd_Idef(:,:,1,2).*x; dd_Idef(:,:,1,6) = dd_Idef(:,:,1,2).*y; dd_Idef(:,:,2,1) = dd_Idef(:,:,1,2); dd_Idef(:,:,2,2) = -2* Idef(2:m+1, 2:n+1) + Idef(1:m, 2:n+1)+ Idef(3:m+2, 2:n+1); dd_Idef(:,:,2,3) = dd_Idef(:,:,2,1).*x; dd_Idef(:,:,2,4) = dd_Idef(:,:,2,1).*y; dd_Idef(:,:,2,5) = dd_Idef(:,:,2,2).*x; dd_Idef(:,:,2,6) = dd_Idef(:,:,2,2).*y; dd_Idef(:,:,3,1) = dd_Idef(:,:,1,3); dd_Idef(:,:,3,2) = dd_Idef(:,:,2,3); dd_Idef(:,:,3,3) = dd_Idef(:,:,1,1).*x.*x; dd_Idef(:,:,3,4) = dd_Idef(:,:,1,1).*y.*x; dd_Idef(:,:,3,5) = dd_Idef(:,:,1,2).*x.*x; dd_Idef(:,:,3,6) = dd_Idef(:,:,1,2).*y.*x; dd_Idef(:,:,4,1) = dd_Idef(:,:,1,4); dd_Idef(:,:,4,2) = dd_Idef(:,:,2,4); dd_Idef(:,:,4,3) = dd_Idef(:,:,3,4); dd_Idef(:,:,4,4) = dd_Idef(:,:,1,1).*y.*y; dd_Idef(:,:,4,5) = dd_Idef(:,:,1,2).*x.*y; dd_Idef(:,:,4,6) = dd_Idef(:,:,1,2).*y.*y; dd_Idef(:,:,5,1) = dd_Idef(:,:,1,5); dd_Idef(:,:,5,2) = dd_Idef(:,:,2,5); dd_Idef(:,:,5,3) = dd_Idef(:,:,3,5); dd_Idef(:,:,5,4) = dd_Idef(:,:,4,5); dd_Idef(:,:,5,5) = dd_Idef(:,:,2,2).*x.*x; dd_Idef(:,:,5,6) = dd_Idef(:,:,2,2).*x.*y; dd_Idef(:,:,6,1) = dd_Idef(:,:,1,6); dd_Idef(:,:,6,2) = dd_Idef(:,:,2,6); dd_Idef(:,:,6,3) = dd_Idef(:,:,3,6); dd_Idef(:,:,6,4) = dd_Idef(:,:,4,6); dd_Idef(:,:,6,5) = dd_Idef(:,:,5,6); dd_Idef(:,:,6,6) = dd_Idef(:,:,2,2).*y.*y; end