data.OffsetEndoX=-51.94;
data.OffsetEndoY=-2.15;
data.OffsetEndoZ=-256.40;
data.OffsetEndoPsi=0;
data.OffsetEndoTheta=0;
data.OffsetEndoPhi=0;
data.Endo_TipHomMat=HomMatrix_Euler(data.OffsetEndoX,data.OffsetEndoY,data.OffsetEndoZ,data.OffsetEndoPsi,data.OffsetEndoTheta,data.OffsetEndoPhi);
if isequal(datastr([5:11,31:37]), 'MISSINGMISSING' )
data.Q0=0;
data.Qx=0;
data.Qy=0;
data.Qz=0;
data.Tx=0;
data.Ty=0;
data.Tz=0;
data.rmsError=0;
elseif datastr(5:11)=='MISSING'
data.Q0=0;
data.Qx=0;
data.Qy=0;
data.Qz=0;
data.Tx=0;
data.Ty=0;
data.Tz=0;
data.rmsError=0;
elseif datastr(75:81)=='MISSING'
data.Q0=str2double(datastr(5:10))/10000;
data.Qx=str2double(datastr(11:16))/10000;
data.Qy=str2double(datastr(17:22))/10000;
data.Qz=str2double(datastr(23:28))/10000;
data.Tx=str2double(datastr(29:35))/100;
data.Ty=str2double(datastr(36:42))/100;
data.Tz=str2double(datastr(43:49))/100;
data.rmsError=str2double(datastr(50:55));
data.Q02=0;
data.Qx2=0;
data.Qy2=0;
data.Qz2=0;
data.Tx2=0;
data.Ty2=0;
data.Tz2=0;
else
data.Q0=str2double(datastr(5:10))/10000;
data.Qx=str2double(datastr(11:16))/10000;
data.Qy=str2double(datastr(17:22))/10000;
data.Qz=str2double(datastr(23:28))/10000;
data.Tx=str2double(datastr(29:35))/100;
data.Ty=str2double(datastr(36:42))/100;
data.Tz=str2double(datastr(43:49))/100;
end
figure(2)
quiver3(data.FinalOffsetPosition(1),data.FinalOffsetPosition(2),data.FinalOffsetPosition(3),35*data.EndoOrient(1),35*data.EndoOrient(2),35*data.EndoOrient(3),'r*');
hold on
plot3(0,0,0,'b*');
xlim([-400 400]);
ylim([-400 400]);
zlim([-400 400]);
title('Position and Orientation of Endoscope');
xlabel('x-axis');
ylabel('y-axis');
zlabel('z-axis');
drawnow
hold off