Changeset 1080 for trunk/src/phys_XYZ.m
- Timestamp:
- Apr 17, 2020, 5:58:49 PM (5 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/src/phys_XYZ.m
r1078 r1080 86 86 Ty=Calib.Tx_Ty_Tz(2); 87 87 Tz=Calib.Tx_Ty_Tz(3); 88 f=Calib.fx_fy(1);%dpy=1; sx=189 %dpx=Calib.fx_fy(2)/Calib.fx_fy(1);90 88 Dx=R(5)*R(7)-R(4)*R(8); 91 89 Dy=R(1)*R(8)-R(2)*R(7); … … 101 99 A21=-R(7)*Ty+R(4)*Tz+Z21*Z0virt; 102 100 A22=-R(1)*Tz+R(7)*Tx+Z22*Z0virt; 103 % X0=Calib.fx_fy(1)*(R(5)*Tx-R(2)*Ty+Zx0*Z0virt);104 % Y0=Calib.fx_fy(2)*(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt);105 101 X0=(R(5)*Tx-R(2)*Ty+Zx0*Z0virt); 106 102 Y0=(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt); 107 103 %px to camera: 108 % Xd=dpx*(X-Calib.Cx_Cy(1)); % sensor coordinates109 % Yd=(Y-Calib.Cx_Cy(2));110 104 Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates 111 105 Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2); 112 dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);%/(f*f); %distortion factor 113 Xu=Xd./dist_fact;%undistorted sensor coordinates 114 Yu=Yd./dist_fact; 106 dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);% distortion factor, first approximation Xu,Yu=Xd,Yd 107 test=0; 108 niter=0; 109 while test==0 && niter<10 110 dist_fact_old=dist_fact; 111 Xu=Xd./dist_fact;%undistorted sensor coordinates, second iteration 112 Yu=Yd./dist_fact; 113 dist_fact=1+Calib.kc*(Xu.*Xu+Yu.*Yu);% distortion factor,next approximation 114 test=max(max(abs(dist_fact-dist_fact_old)))<0.00001; % reducing the relative error to 10^-5 forthe inversion of the quadraticcorrection 115 niter=niter+1; 116 end 115 117 denom=Dx*Xu+Dy*Yu+D0; 116 118 Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
Note: See TracChangeset
for help on using the changeset viewer.