Home > manopt > manifolds > essential > privateessential > essential_distMinAnglePair_test.m

essential_distMinAnglePair_test

PURPOSE ^

SYNOPSIS ^

function essential_distMinAnglePair_test

DESCRIPTION ^

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Fri 08-Sep-2017 12:43:19 by m2html © 2005