jacprojrt.m

sparse bundle ajustment的源码

function jrt=jacprojRT(rt, xyz, a)
% symbolic projection function Jacobian
  a1=a(1); a2=a(2); a3=a(3); a4=a(4); %a5=a(5);
  qr2=rt(1); qr3=rt(2); qr4=rt(3);
  t1=rt(4); t2=rt(5); t3=rt(6);
  X=xyz(1); Y=xyz(2); Z=xyz(3);

  qr1=sqrt(1-(qr2^2+qr3^2+qr4^2));
  
  jrt=[(-(2/qr1*X*qr2*qr3+(-2/qr1*Y*qr2-2*Z)*qr2+2*qr1*Y+2*qr4*X-2*qr2*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z))*a3+(-(-2/qr1*X*qr2*qr4+(2/qr1*Z*qr2-2*Y)*qr2+2*qr3*X-2*qr1*Z-2*qr2*Y)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+((2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr2*Z+(4*qr2^2-2+2*qr3^2+2*qr4^2)/qr1*Y+(-2*qr3*qr2-2*qr4*qr1)/qr1*X)*t2)*a2+(-(2/qr1*Y*qr2*qr4-2/qr1*Z*qr2*qr3+2*qr3*Y+2*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(X+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr2*Z+(4*qr2^2-2+2*qr3^2+2*qr4^2)/qr1*Y+(-2*qr3*qr2-2*qr4*qr1)/qr1*X)*t1)*a1,...
  (-((2/qr1*X*qr3-2*Z)*qr3-2*qr1*X+2*qr4*Y-2*qr3*Z-2/qr1*Y*qr3*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2))*a3+(-(-2/qr1*X*qr3*qr4+2*qr4*Z+(2*X+2/qr1*Z*qr3)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+((2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr3*Z+(2*qr3*qr2-2*qr4*qr1)/qr1*Y+(-4*qr3^2+2-2*qr2^2-2*qr4^2)/qr1*X)*t2)*a2+(-(2/qr1*Y*qr3*qr4+(-2*X-2/qr1*Z*qr3)*qr3-2*qr3*X+2*qr1*Z+2*qr2*Y)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(X+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr3*Z+(2*qr3*qr2-2*qr4*qr1)/qr1*Y+(-4*qr3^2+2-2*qr2^2-2*qr4^2)/qr1*X)*t1)*a1,...
  (-((2/qr1*X*qr4+2*Y)*qr3+(-2/qr1*Y*qr4+2*X)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2))*a3+(-((-2/qr1*X*qr4-2*Y)*qr4+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Z*qr4*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+((2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*((2*qr4*qr2-2*qr1*qr3)/qr1*Y+(-2*qr4*qr3-2*qr1*qr2)/qr1*X)*t2)*a2+(-((-2*X+2/qr1*Y*qr4)*qr4-2*qr4*X-2*qr1*Y-2/qr1*Z*qr4*qr3+2*qr2*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(X+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*((2*qr4*qr2-2*qr1*qr3)/qr1*Y+(-2*qr4*qr3-2*qr1*qr2)/qr1*X)*t1)*a1,...
  -a1/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2),...
  -a2/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2),...
  (-1/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2)*a3-(t2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z+(2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*a2-(X+t1+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*a1,...
  ((2/qr1*X*qr2*qr4+(-2/qr1*Z*qr2+2*Y)*qr2-2*qr3*X+2*qr1*Z+2*qr2*Y)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-((-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr2*Z+(4*qr2^2-2+2*qr3^2+2*qr4^2)/qr1*Y+(-2*qr3*qr2-2*qr4*qr1)/qr1*X)*t2)*a4,...
  ((2/qr1*X*qr3*qr4-2*qr4*Z+(-2*X-2/qr1*Z*qr3)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-((-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr3*Z+(2*qr3*qr2-2*qr4*qr1)/qr1*Y+(-4*qr3^2+2-2*qr2^2-2*qr4^2)/qr1*X)*t2)*a4,...
  (((2/qr1*X*qr4+2*Y)*qr4-2*qr1*X+2*qr4*Y-2*qr3*Z-2/qr1*Z*qr4*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-((-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*((2*qr4*qr2-2*qr1*qr3)/qr1*Y+(-2*qr4*qr3-2*qr1*qr2)/qr1*X)*t2)*a4,...
  0,...
  -a4/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2),...
  (-t2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z+(-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*a4];