Changeset 1080 for trunk/src/phys_XYZ.m

Timestamp:

Apr 17, 2020, 5:58:49 PM (4 years ago)

Author:

sommeria

Message:

ima2temperature.m added and various bug repairs

File:

• trunk/src/phys_XYZ.m (modified) (2 diffs)

trunk/src/phys_XYZ.m

@@ -86 +86 @@
     Ty=Calib.Tx_Ty_Tz(2);
     Tz=Calib.Tx_Ty_Tz(3);
-    f=Calib.fx_fy(1);%dpy=1; sx=1
-    %dpx=Calib.fx_fy(2)/Calib.fx_fy(1);
     Dx=R(5)*R(7)-R(4)*R(8);
     Dy=R(1)*R(8)-R(2)*R(7);
@@ -101 +99 @@
     A21=-R(7)*Ty+R(4)*Tz+Z21*Z0virt;
     A22=-R(1)*Tz+R(7)*Tx+Z22*Z0virt;
-    %     X0=Calib.fx_fy(1)*(R(5)*Tx-R(2)*Ty+Zx0*Z0virt);
-    %     Y0=Calib.fx_fy(2)*(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt);
     X0=(R(5)*Tx-R(2)*Ty+Zx0*Z0virt);
     Y0=(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt);
     %px to camera:
-    %     Xd=dpx*(X-Calib.Cx_Cy(1)); % sensor coordinates
-    %     Yd=(Y-Calib.Cx_Cy(2));
     Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates
     Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2);
-    dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);%/(f*f); %distortion factor
-    Xu=Xd./dist_fact;%undistorted sensor coordinates
-    Yu=Yd./dist_fact;
+    dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);% distortion factor, first approximation Xu,Yu=Xd,Yd
+    test=0;
+    niter=0;
+    while test==0 && niter<10
+        dist_fact_old=dist_fact;     
+        Xu=Xd./dist_fact;%undistorted sensor coordinates, second iteration
+        Yu=Yd./dist_fact;
+        dist_fact=1+Calib.kc*(Xu.*Xu+Yu.*Yu);% distortion factor,next approximation
+        test=max(max(abs(dist_fact-dist_fact_old)))<0.00001; % reducing the relative error to 10^-5 forthe inversion of the quadraticcorrection
+        niter=niter+1;
+    end
     denom=Dx*Xu+Dy*Yu+D0;
     Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates