0001 function essential_distMinAnglePair_test 0002 resetRands(3) 0003 flagDegenerateCase=true; 0004 k=2; 0005 0006 e3=[0;0;1]; 0007 Q1=rot_randn([],[],2); 0008 if flagDegenerateCase 0009 Q1b=[Q1(:,:,1);Q1(:,:,2)]; 0010 Q2b=essential_randomVerticalMotion(Q1b); 0011 Q2=cat(3,Q2b(1:3,:),Q2b(4:6,:)); 0012 else 0013 Q2=rot_randn([],[],2); 0014 end 0015 Rzt=@(t) rot(t*e3); 0016 0017 Q21tk=@(t,k) Rzt(t)*essential_flipAmbiguity_R1(Q2(:,:,1),k); 0018 Q22tk=@(t,k) Rzt(t)*essential_flipAmbiguity_R2(Q2(:,:,2),k); 0019 0020 figure(1) 0021 [tMin,fMin,tBreak1,tBreak2,Q2Flip]=essential_distMinAnglePair([Q1(:,:,1);Q1(:,:,2)],[Q2(:,:,1);Q2(:,:,2)],k); 0022 tMin=modAngle(tMin); 0023 ft=@(t) (rot_dist(Q1(:,:,1),Q21tk(t,k))^2+rot_dist(Q1(:,:,2),Q22tk(t,k))^2); 0024 dft=@(t) 2*e3'*(Q1(:,:,1)*logrot(Q1(:,:,1)'*Q21tk(t,k))+Q1(:,:,2)*logrot(Q1(:,:,2)'*Q22tk(t,k))); 0025 check_der(ft,dft,'angle') 0026 hold on 0027 plot(tBreak1,ft(tBreak1),'r+') 0028 plot(tBreak2,ft(tBreak2),'g+') 0029 0030 plot(tMin,fMin,'kx','MarkerSize',20) 0031 0032 hold off