{
} program p; { calib_calc_2.pas calls routines in our wps.pas library to calculate the error in WPS measurement. It takes a set of WPS measurements and compares them to CMM measurements. It adjusts camera calibration constants until the root mean square camera error is close to a minimum. The program prints the CMM wire positions and the WPS three-dimensional error vector at the start of the fitting procedure. It prints the camera calibration constants and the rms error during the fitting. It prints the error vectors a second time at the end, and last of all calculates the ccd-pivot distance using the final parameters. At the end of the code are parameters we obtained with various sub-sets of the data, and by fitting various sub-sets of the parameters. Compile Instructions: Re-name this program to p.pas and compile with "make p" in LWDAQ/Build. Adjust global constants and re-compile. Input data is embedded in the code. Output is to the terminal. } uses utils,bcam,wps; const N=11; N_error=N; edge_direction=1; camera_num=2; random_scale=1; var camera:wps_camera_type; mount:kinematic_mount_type; edge_str,wire_str,coord_str:long_string; i,j:integer; wires:array [1..N] of wps_wire_type; images:array [1..N] of xy_point_type; s:short_string; num_steps:integer; function error(c:wps_camera_type):real; var i:integer; sum:real; begin sum:=0; for i:=1 to N_error do sum:=sum+sqr(xyz_length(wps_ray_error(images[i],edge_direction,wires[i],c))); error:=sqrt(sum/N); end; function step(c:wps_camera_type;d:real):wps_camera_type; var ax,ay,sum:real; cc:wps_camera_type; begin cc:=c; with cc do begin with pivot do begin x:=x+2*(random_0_to_1-0.5)*d; y:=y+2*(random_0_to_1-0.5)*d; { z:=z+2*(random_0_to_1-0.5)*d;} end; with sensor do begin x:=x+2*(random_0_to_1-0.5)*d; y:=y+2*(random_0_to_1-0.5)*d; z:=z+2*(random_0_to_1-0.5)*d; end; with rot do begin { z:=arctan((sensor.y-pivot.y)/(sensor.x-pivot.x));} { y:=arctan((sensor.z-pivot.z)/(pivot.x-sensor.x));} { x:=x+2*(random_0_to_1-0.5)*d/10;} { y:=y+2*(random_0_to_1-0.5)*d/10;} z:=z+2*(random_0_to_1-0.5)*d/10; end; end; if error(c)>error(cc) then step:=cc else step:=c; end; var a,b,c:xyz_line_type; p:xy_point_type; q:xyz_point_type; begin fsd:=3; fsr:=6; camera:=nominal_wps_camera(camera_num); with camera do begin with pivot do begin x:=x+(random_0_to_1-0.5)*random_scale; y:=y+(random_0_to_1-0.5)*random_scale; end; with sensor do begin x:=x+(random_0_to_1-0.5)*random_scale; y:=y+(random_0_to_1-0.5)*random_scale; z:=z+(random_0_to_1-0.5)*random_scale; end; end; writeln(string_from_wps_camera(camera)); edge_str:=' {Q0130} 1213110444 2848.77 7.09 3171.98 6.19 2786.24 4.80 3106.39 4.97 23.569 23.871 23.772 23.617 1213110597 2478.01 7.25 2818.34 6.27 2417.89 5.14 2755.74 4.53 23.583 23.875 23.772 23.623 1213110759 2060.03 8.27 2420.36 7.87 2004.63 5.12 2359.88 6.28 23.597 23.894 23.794 23.628 1213110906 1584.59 9.02 1969.78 7.52 1535.46 4.95 1915.77 3.85 23.621 23.923 23.806 23.642 1213111021 1037.09 9.77 1452.90 9.12 996.24 4.95 1405.37 4.34 23.622 23.918 23.807 23.641 1213111132 393.94 10.97 850.16 9.78 363.46 4.40 811.74 5.31 23.632 23.906 23.793 23.651 1213111406 2287.50 6.99 2621.03 7.65 2614.79 4.65 2954.23 5.75 23.651 23.933 23.810 23.653 1213111601 1627.34 8.82 1993.75 8.76 1985.91 4.50 2355.84 4.72 23.655 23.926 23.799 23.663 {1213111709 818.03 9.97 1229.20 9.56 1219.05 5.51 1629.61 4.84 23.661 23.930 23.801 23.668} 1213112060 1342.42 192.58 1684.48 196.03 2734.04 204.63 3108.41 209.43 23.692 23.966 23.821 23.683 1213112208 966.40 190.42 1325.30 193.53 2413.96 200.44 2804.65 204.92 23.706 23.977 23.833 23.695 1213112373 547.82 188.29 945.87 190.45 2060.79 196.07 2481.88 201.43 23.718 23.993 23.829 23.698 {Q0131, Point 1 corrected left edge} { 1213122233 3207.15 14.23 3207.15 14.23 2890.82 26.29 3207.15 25.40 23.566 23.867 23.741 23.628 1213122405 2830.11 13.34 3165.46 13.77 2539.18 26.83 2872.31 26.47 23.565 23.847 23.740 23.623 1213122539 2407.60 12.94 2761.36 12.78 2143.87 27.82 2496.53 27.21 23.580 23.873 23.749 23.631 1213122677 1929.90 12.47 2305.53 12.20 1696.11 28.97 2072.15 27.84 23.596 23.884 23.762 23.628 1213122794 1381.98 11.27 1785.82 11.49 1181.24 30.43 1586.50 28.95 23.598 23.880 23.750 23.629 1213122919 741.33 9.07 1183.85 10.55 579.06 32.09 1022.30 30.80 23.608 23.899 23.769 23.634 1213123194 2636.01 13.04 2967.15 14.20 2737.72 26.43 3076.52 25.66 23.628 23.916 23.782 23.651 1213123355 1971.85 12.09 2332.69 11.94 2138.31 27.88 2507.42 27.21 23.640 23.923 23.775 23.646 1213123478 1163.28 10.12 1565.42 10.85 1408.58 29.86 1818.23 28.84 23.648 23.932 23.778 23.654 1213124105 1697.46 181.41 2043.35 184.94 2808.67 214.36 3188.04 216.86 23.690 23.981 23.810 23.676 1213124241 1323.53 177.32 1687.26 181.15 2503.66 211.55 2899.96 214.94 23.697 23.987 23.825 23.689 1213124369 909.66 173.54 1294.94 177.07 2167.19 208.88 2582.30 212.00 23.698 23.974 23.831 23.690 } '; wire_str:=' {Q0130} 111.7920 7.7853 44.3442 0.99999324 0.00021539 -0.00367112 111.8133 3.7750 44.3878 0.99999323 0.00008940 -0.00367757 111.8351 -0.2258 44.4332 0.99999328 0.00007317 -0.00366411 111.8578 -4.2300 44.4751 0.99999326 0.00006007 -0.00367086 111.8801 -8.2284 44.5172 0.99999323 0.00005921 -0.00367798 111.9021 -12.2322 44.5619 0.99999328 0.00004369 -0.00366548 112.8199 3.7627 43.3839 0.99999335 -0.00000660 -0.00364560 112.8535 -2.2392 43.4484 0.99999335 -0.00002084 -0.00364782 {112.8535 -2.2392 43.4484 0.99999335 -0.00002084 -0.00364782 } 113.6592 -0.2862 40.5197 0.97818151 0.00219896 0.20774046 113.6759 -3.2832 40.5519 0.97818112 0.00219834 0.20774231 113.6930 -6.3024 40.5828 0.97818194 0.00219319 0.20773847 {Q0131} { 111.8021 7.7840 44.2651 0.99993495 0.00036492 0.01139999 111.8222 3.7758 44.3113 0.99993474 0.00023516 0.01142185 111.8442 -0.2246 44.3548 0.99993473 0.00022720 0.01142255 111.8664 -4.2263 44.3975 0.99993472 0.00020959 0.01142395 111.8890 -8.2264 44.4387 0.99993490 0.00021781 0.01140822 111.9103 -12.2318 44.4842 0.99993469 0.00018295 0.01142754 112.8273 3.7636 43.2978 0.99993451 0.00016862 0.01144311 112.8607 -2.2369 43.3619 0.99993456 0.00015286 0.01143879 112.8607 -2.2369 43.3619 0.99993456 0.00015286 0.01143879 113.6510 -0.2865 40.5312 0.97794118 0.00234389 0.20886730 113.6683 -3.2870 40.5635 0.97794144 0.00234277 0.20886612 113.6841 -6.2866 40.5963 0.97793874 0.00233801 0.20887879 } '; coord_str:=' {Q0130} 118.0668 -43.2519 -18.6473 45.1356 -64.4028 -17.6246 44.9564 -22.4162 -18.0720 {Q0131} { 118.0663 -43.2519 -18.6495 45.1359 -64.4029 -17.6254 44.9566 -22.4167 -18.0723 } '; mount:=read_kinematic_mount(coord_str); for i:=1 to N do begin with wires[i] do begin radius:=1/16*25.4/2; {one sixteenth inch steel pin} position:=wps_from_global_point(read_xyz(wire_str),mount); direction:=wps_from_global_vector(read_xyz(wire_str),mount); end; with images[i] do begin y:=1.220; {mm} if camera_num=2 then begin s:=read_word(edge_str);s:=read_word(edge_str); s:=read_word(edge_str);s:=read_word(edge_str); end; case edge_direction of +1:begin s:=read_word(edge_str); x:=read_real(edge_str)/1000; end; 0:begin s:=read_word(edge_str); x:=read_real(edge_str)/1000; s:=read_word(edge_str); x:=(x+read_real(edge_str)/1000)/2; end; -1:begin s:=read_word(edge_str); s:=read_word(edge_str); s:=read_word(edge_str); x:=read_real(edge_str)/1000; end; end; end; edge_str:=delete_to_mark(edge_str,chr(10)); end; case edge_direction of +1:writeln('Camera ',camera_num,' using left edges.'); 0:writeln('Camera ',camera_num,' using center line.'); -1:writeln('Camera ',camera_num,' using right edges.'); end; for i:=1 to N do begin write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction)); write(' ',string_from_xyz(wps_ray_error(images[i],edge_direction,wires[i],camera))); writeln; end; writeln(string_from_wps_camera(camera),' ', xyz_length(xyz_difference(camera.pivot,camera.sensor)):fsr:fsd,' ', error(camera):fsr:fsd); for i:=1 to 200 do begin for j:=1 to 1000 do begin camera:=step(camera,0.001); end; writeln(i:3,' ',string_from_wps_camera(camera),' ', xyz_length(xyz_difference(camera.pivot,camera.sensor)):fsr:fsd,' ', error(camera):fsr:fsd); end; for i:=1 to N do begin write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction)); write(' ',string_from_xyz(wps_ray_error(images[i],edge_direction,wires[i],camera))); writeln; end; end. { NOTE: The first number on a line is the number of iterations divided by 100k. Q0131-1. Skip points 1 and 9. With wire centers, fitting pivot x, y, sensor x, y, z and rot.z, random starting values for the fitted position parameters: +-0.5 mm. 100 WPS1_A_1 -3.670 88.223 -5.000 -13.737 94.619 -5.228 -1570.796 0.000 -600.018 11.929 0.006 100 WPS1_A_1 -3.359 88.123 -5.000 -13.331 94.485 -5.208 -1570.796 0.000 -589.225 11.830 0.006 100 WPS1_A_1 -4.161 88.575 -5.000 -14.350 95.059 -5.225 -1570.796 0.000 -579.979 12.079 0.006 100 WPS1_A_1 -3.935 88.497 -5.000 -14.040 94.944 -5.212 -1570.796 0.000 -592.025 11.989 0.005 100 WPS1_A_1 -3.982 88.420 -5.000 -14.136 94.870 -5.230 -1570.796 0.000 -589.273 12.031 0.007 100 WPS1_A_1 -3.905 88.513 -5.000 -13.989 94.956 -5.208 -1570.796 0.000 -597.021 11.968 0.005 100 WPS1_A_1 -3.903 88.329 -5.000 -14.047 94.763 -5.236 -1570.796 0.000 -591.058 12.014 0.007 100 WPS1_A_1 -3.886 88.397 -5.000 -14.007 94.835 -5.227 -1570.796 0.000 -588.820 11.998 0.006 100 WPS1_A_1 -3.476 88.040 -5.000 -13.514 94.402 -5.239 -1570.796 0.000 -605.646 11.887 0.007 200 WPS1_A_1 -3.584 88.505 -5.000 -13.529 94.909 -5.175 -1570.796 0.000 -606.290 11.831 0.003 200 WPS1_A_1 -3.784 88.620 -5.000 -13.784 95.056 -5.178 -1570.796 0.000 -600.542 11.893 0.004 200 WPS1_A_1 -3.574 88.499 -5.000 -13.517 94.902 -5.174 -1570.796 0.000 -605.987 11.827 0.003 200 WPS1_A_1 -3.593 88.514 -5.000 -13.541 94.920 -5.174 -1570.796 0.000 -604.993 11.834 0.003 200 WPS1_A_1 -3.439 88.430 -5.000 -13.343 94.812 -5.171 -1570.796 0.000 -609.380 11.784 0.003 200 WPS1_A_1 -3.776 88.645 -5.000 -13.766 95.082 -5.173 -1570.796 0.000 -599.515 11.886 0.004 200 WPS1_A_1 -3.417 88.386 -5.000 -13.323 94.763 -5.176 -1570.796 0.000 -610.756 11.783 0.003 200 WPS1_A_1 -3.987 88.754 -5.000 -14.040 95.224 -5.178 -1570.796 0.000 -593.488 11.956 0.004 200 WPS1_A_1 -3.627 88.519 -5.000 -13.587 94.929 -5.177 -1570.796 0.000 -605.338 11.846 0.003 200 WPS1_A_1 -3.793 88.617 -5.000 -13.799 95.055 -5.180 -1570.796 0.000 -599.658 11.899 0.004 1000 WPS1_A_1 -3.570 88.568 -5.000 -13.492 94.976 -5.164 -1570.796 0.000 -606.059 11.812 0.003 1000 WPS1_A_1 -3.653 88.627 -5.000 -13.595 95.049 -5.162 -1570.796 0.000 -602.571 11.836 0.003 1000 WPS1_A_1 -3.390 88.456 -5.000 -13.264 94.834 -5.162 -1570.796 0.000 -611.560 11.756 0.003 Q0131-1. Skip point 1 and 9. Use left edges. 200 WPS1_A_1 -3.872 88.542 -5.000 -13.942 94.994 -5.195 -1570.796 0.000 -574.585 11.962 0.004 200 WPS1_A_1 -3.565 88.375 -5.000 -13.551 94.781 -5.187 -1570.796 0.000 -584.397 11.865 0.004 Q0131-1. Skip point 9. Correct left edge of Point 1. Use left edges. 200 WPS1_A_1 -3.792 88.476 -5.000 -13.848 94.915 -5.199 -1570.796 0.000 -594.528 11.943 0.005 200 WPS1_A_1 -3.660 88.361 -5.000 -13.691 94.776 -5.202 -1570.796 0.000 -599.985 11.908 0.005 Q0131-2. Skip point 9. Use centers. 200 WPS1_A_2 -4.935 39.241 -5.000 -15.188 33.310 -5.677 1570.796 0.000 579.758 11.864 0.006 Q0131-2. Skip point 9. Use left edges. 200 WPS1_A_2 -4.694 39.316 -5.000 -14.891 33.400 -5.674 1570.796 0.000 565.562 11.808 0.006 Q0130-1. Skip point 9. Use left edges. 200 WPS1_A_1 -3.839 88.157 -5.000 -14.179 94.272 -5.250 -1570.796 0.000 -539.313 12.015 0.004 Q0130-2. Skip point 9. Use left edges. 200 WPS1_A_2 -4.726 38.535 -5.000 -15.252 32.428 -5.425 1570.796 0.000 537.062 12.177 0.006 }