VISJAC Image feature Jacobian J = VISJAC(UV, CAMDATA, Z) Return the image-feature Jacobian for point features whose coordinates (u,v) comprise rows of UV. Z is a vector of the depth of each corresponding point with respect to the camera. S is the pixel scale factor in m/pixel.
0001 %VISJAC Image feature Jacobian 0002 % 0003 % J = VISJAC(UV, CAMDATA, Z) 0004 % 0005 % Return the image-feature Jacobian for point features whose coordinates 0006 % (u,v) comprise rows of UV. Z is a vector of the depth of each corresponding 0007 % point with respect to the camera. S is the pixel scale factor in m/pixel. 0008 % 0009 function J = visjac(uv, camdata, z) 0010 0011 J = []; 0012 for i=1:numrows(uv), 0013 % convert pixel units to distances at the image plane 0014 0015 % now do the Jacobian proper 0016 Jp = visjac_row(camdata, uv(i,:), z); 0017 0018 % build up the image Jacobian 0019 J = [J; Jp]; 0020 end 0021 0022 function J = visjac_row(camdata, uv, z) 0023 0024 f = camdata(1); 0025 ax = camdata(2); % pixels/m 0026 ay = camdata(3); % pixels/m 0027 u0 = camdata(4); % principal point in pixels 0028 v0 = camdata(5); % principal point in pixels 0029 0030 % convert pixel units to distances at the image plane 0031 uv_p = [1/ax 0 -u0/ax; 0 1/ay -v0/ay] * [uv(:) ; 1]; 0032 u = uv_p(1); 0033 v = uv_p(2); 0034 0035 % now do the Jacobian proper 0036 J = [f/z 0 -u/z -u*v/f (f^2+u^2)/f -v 0037 0 f/z -v/z -(f^2+v^2)/f u*v/f u]; 0038 0039 % convert to pixel unit velocities 0040 J = diag([ax ay]) * J;