visjac.m

来自澳大利亚Qeensland大学的计算机视觉Matlab工具箱。 This Toolbox provides a number of functions that are useful in co

%VISJAC	Visual motion Jacobian
%
%	vj = visjac(C, uv, z)
%
%	Compute the visual Jacobian giving image-plane velocity in terms of
%	camera velocity.  C is a vector of camera intrinsic parameters
%
%		C = [f alphax alphay Cx Cy]
%
% REF:	A tutorial on Visual Servo Control, Hutchinson, Hager & Corke,
%	IEEE Trans. R&A, Vol 12(5), Oct, 1996, pp 651-670.
%
%	Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab


function vj = visjac(C, uv, z)

	f = C(1);
	alphax = C(2);
	alphay = C(3);
	u = uv(1);
	v = uv(2);

	vj = [
	alphax^2*f^2&alphay 0 -u*alphax*alphay ...
	  -u*v*z*alphax z*alphay*(u^2+alphax^2*f^2) -alphax^2*f*v*z
	0 alphay^2*f^2&alphax -v*alphax*alphay ...
	  -z*alphay*(v^2+alphay^2*f^2) +u*v*z*alphay alphay^2*f*u*z] / (z*alphax*alphay*f);


	  udot = (-u*omega[x]*v*alpha[x]*z+u*omega[x]*v0*alpha[x]*z+omega[y]*u*u*
alpha[y]*z-2.0*u*omega[y]*u0*alpha[y]*z-u*T[z]*alpha[y]*f*alpha[x]+u0*omega[x]*
v*alpha[x]*z-u0*omega[x]*v0*alpha[x]*z+omega[y]*u0*u0*alpha[y]*z+u0*T[z]*alpha
[y]*f*alpha[x]+pow(alpha[x],2.0)*f*f*omega[y]*alpha[y]*z-pow(alpha[x],2.0)*
omega[z]*v*f*z+pow(alpha[x],2.0)*omega[z]*v0*f*z+pow(alpha[x],2.0)*f*f*T[x]*
alpha[y])/alpha[y]/f/alpha[x]/z;

vdot = (-omega[x]*v*v*alpha[x]*z+2.0*v*omega[x]*v0*alpha[x]*z+v*omega[y]*u*
alpha[y]*z-v*omega[y]*u0*alpha[y]*z-v*T[z]*alpha[y]*f*alpha[x]-omega[x]*v0*v0*
alpha[x]*z-v0*omega[y]*u*alpha[y]*z+v0*omega[y]*u0*alpha[y]*z+v0*T[z]*alpha[y]*
f*alpha[x]+pow(alpha[y],2.0)*omega[z]*u*f*z-pow(alpha[y],2.0)*omega[z]*u0*f*z-
pow(alpha[y],2.0)*f*f*omega[x]*alpha[x]*z+pow(alpha[y],2.0)*f*f*T[y]*alpha[x])/
alpha[y]/f/alpha[x]/z;