ctx.grids = 3xN matrix with ones in bottom row ctx.pix = 3xN matrix with ones in bottom row
0001 function [res, resnorm] = estimateAngles(angle0, x, ctx) 0002 % ctx.grids = 3xN matrix with ones in bottom row 0003 % ctx.pix = 3xN matrix with ones in bottom row 0004 [res, resnorm] = lsqnonlin(@error_func,angle0,[],[],[],x,ctx); 0005 0006 % function err = error_func(angle, x, ctx) 0007 % % angle = [psi, phi, theta] 0008 % % x = [x, y, z, v, u, s, alpha, beta, gamma, f] 0009 % 0010 % % p = projFrom( [ x(1:10) angle x(11)] ); 0011 % p = projFrom( [ x angle] ); 0012 % vu = p * ctx.grids; 0013 % vu = vu ./ vu([3 3 3],:); 0014 % if(rand<0.02) 0015 % clf; 0016 % plot(vu(1,:),vu(2,:),'.'); 0017 % hold on; 0018 % plot(ctx.pix(1,:),ctx.pix(2,:),'r+'); 0019 % drawnow; 0020 % pause(0.1); 0021 % end 0022 % err = reshape(ctx.pix - vu,[3*size(ctx.grids,2) 1]); 0023 0024 function err = error_func(angle, x, ctx) 0025 % angle = [phi, theta] 0026 % x = [x, y, z, v, u, s, alpha, beta, gamma, f, psi] 0027 0028 % p = projFrom( [ x(1:10) angle x(11)] ); 0029 p = projFrom( [ x(1:11) angle] ); 0030 vu = p * ctx.grids; 0031 vu = vu ./ vu([3 3 3],:); 0032 if(rand<0.02) 0033 clf; 0034 plot(vu(1,:),vu(2,:),'.'); 0035 hold on; 0036 plot(ctx.pix(1,:),ctx.pix(2,:),'r+'); 0037 drawnow; 0038 pause(0.1); 0039 end 0040 err = reshape(ctx.pix - vu,[3*size(ctx.grids,2) 1]);