{
} program p; { Wire Position Sensor Calibration Fitting Program Copyright (C) 2009 Kevan Hashemi, Open Source Instruments Inc. This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. calib_calc_4.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 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. VERSION NOTES: Here we have moved the simplex fitting routines out of the main program and into our utils unit. We call the routines from the main program and pass the camera parameters as generic vertices of a simplex in the camera parameter space. We add calibration measurements for two other cameras. These measurements have fewer points (11 instead of 22), so we must change the num_points parameter to match the data when we switch between cameras. } uses utils,bcam,wps; const num_points=22; edge_direction=0; camera_num=1; random_scale=1; num_parameters=9; max_num_shrinks=5; y_ref=1.220;{mm} var edge_str,wire_str,coord_str:long_string; wires:array [1..num_points] of wps_wire_type; edges:array [1..num_points] of wps_edge_type; { Create a random disturbance scaled by random_scale. } function disturb:real; begin disturb:=random_scale*(random_0_to_1-0.5); end; { We obtain the calibration error by comparing each wire position to the line implied by each image. We use the camera calibration constants to generate the lines from the images. We involve the rotation of the wire images in the error calculation by generating two lines for each wire image: one from a point in the image towards the top of the CCD and another from a point towards the bottom of the CCD. The y_shift parameter tells us how much to move up and down the image to choose the two points. } function error(v:simplex_vertex_type):real; const y_shift=0.500;{mm} var i:integer; sum:real; p:xy_point_type; c:wps_camera_type; begin c.pivot.x:=v[1]; c.pivot.y:=v[2]; c.pivot.z:=v[3]; c.sensor.x:=v[4]; c.sensor.y:=v[5]; c.sensor.z:=v[6]; c.rot.x:=v[7]; c.rot.y:=v[8]; c.rot.z:=v[9]; sum:=0; for i:=1 to num_points do begin p:=edges[i].position; p.y:=p.y-y_shift; p.x:=p.x-sin(edges[i].rotation)*y_shift/cos(edges[i].rotation); sum:=sum+sqr(xyz_length(wps_ray_error(p,edge_direction,wires[i],c))); p:=edges[i].position; p.y:=p.y+y_shift; p.x:=p.x+sin(edges[i].rotation)*y_shift/cos(edges[i].rotation); sum:=sum+sqr(xyz_length(wps_ray_error(p,edge_direction,wires[i],c))); end; error:=sqrt(sum/num_points/2); end; var mount:kinematic_mount_type; i,j:integer; s:short_string; simplex:simplex_type(num_parameters); camera:wps_camera_type; done:boolean; begin fsd:=3; fsr:=6; edge_str:=' {Q0129} 1234885361 2934.05 -13.69 3276.06 -14.95 1994.10 -21.24 2318.97 -22.41 21.799 22.019 22.385 21.889 1234885500 2549.20 -14.02 2909.73 -13.55 1543.98 -12.55 1892.11 -19.10 21.801 22.039 22.418 21.892 1234885685 2111.71 -15.00 2495.05 -14.20 1034.33 -9.70 1405.20 -11.27 21.816 22.059 22.398 21.892 1234885813 1608.62 -15.95 2020.58 -14.97 452.01 -12.42 851.46 -11.33 21.819 22.065 22.422 21.901 1234886087 2549.51 -13.76 2881.42 -13.80 2356.15 -22.10 2689.03 -21.02 21.859 22.115 22.441 21.907 1234886246 2148.71 -15.09 2500.40 -13.91 1923.55 -20.06 2275.84 -22.02 21.864 22.109 22.422 21.912 1234886383 1693.38 -15.33 2068.27 -15.18 1428.54 -12.32 1807.99 -17.93 21.863 22.107 22.428 21.925 1234886516 1167.96 -17.32 1572.55 -15.71 861.71 -10.64 1268.70 -11.96 21.861 22.101 22.459 21.926 1234886808 2179.93 -14.87 2504.07 -14.09 2733.03 -21.15 3074.45 -21.54 21.896 22.166 22.485 21.949 1234886929 1764.52 -15.16 2108.16 -14.97 2315.22 -21.33 2676.24 -21.00 21.896 22.162 22.471 21.942 1234887043 1290.83 -16.93 1659.02 -15.43 1839.79 -18.45 2224.08 -20.71 21.897 22.171 22.509 21.967 1234887176 746.67 -18.35 1143.60 -17.15 1290.62 -12.37 1706.58 -16.75 21.904 22.166 22.507 21.970 1234887518 1398.03 -16.56 1736.75 -15.21 2721.54 -20.90 3092.94 -21.56 21.923 22.207 22.531 21.999 1234887648 908.76 -18.48 1271.38 -16.81 2263.88 -20.47 2659.11 -20.88 21.927 22.217 22.567 22.013 1234887917 2076.48 -65.76 2399.13 -66.37 2826.76 -73.11 3171.32 -75.10 21.946 22.232 22.514 22.021 1234888032 1657.52 -65.40 2000.54 -65.60 2412.08 -72.53 2776.21 -73.21 21.955 22.241 22.541 22.037 1234888146 1180.58 -66.08 1548.14 -65.33 1941.56 -69.29 2328.61 -71.85 21.957 22.229 22.505 22.033 1234888270 632.85 -66.30 1028.66 -66.00 1397.04 -62.54 1815.55 -66.74 21.964 22.211 22.540 22.046 1234888522 2816.12 -66.72 3156.45 -68.82 2081.20 -71.72 2409.01 -73.58 21.969 22.245 22.531 22.052 1234888637 2427.59 -66.31 2786.84 -66.75 1636.20 -64.79 1985.91 -70.06 21.977 22.251 22.547 22.064 1234888754 1986.31 -65.58 2368.22 -66.56 1128.66 -57.60 1502.78 -61.44 21.985 22.265 22.561 22.075 1234888946 1477.91 -65.65 1889.21 -65.57 549.55 -58.54 952.57 -57.83 21.985 22.258 22.586 22.082 {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 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, point 9 removed} {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 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:=' {Q0129} 112.7316 1.6410 47.8177 0.99955292 0.00517089 -0.02944856 112.7552 -2.3570 47.8605 0.99955319 0.00515180 -0.02944291 112.7786 -6.3575 47.9030 0.99955307 0.00514537 -0.02944800 112.8027 -10.3584 47.9460 0.99955306 0.00514469 -0.02944845 113.2292 1.6149 45.7948 0.99956393 0.00498845 -0.02910442 113.2538 -2.3829 45.8371 0.99956375 0.00497408 -0.02911304 113.2768 -6.3824 45.8802 0.99956400 0.00497486 -0.02910419 113.3009 -10.3895 45.9237 0.99956431 0.00495967 -0.02909616 111.8521 1.5741 43.8275 0.99955477 0.00534741 -0.02935432 111.8772 -2.4177 43.8690 0.99955442 0.00535400 -0.02936472 111.9007 -6.4205 43.9109 0.99955442 0.00534440 -0.02936649 111.9240 -10.4237 43.9553 0.99955486 0.00533120 -0.02935393 111.4718 -2.4291 41.8915 0.99955380 0.00525392 -0.02940417 111.4956 -6.4283 41.9348 0.99955389 0.00524710 -0.02940225 112.5377 1.6027 43.4520 0.99616920 0.00446006 -0.08733286 112.5614 -2.3946 43.4938 0.99616862 0.00445624 -0.08733968 112.5850 -6.3948 43.5366 0.99616933 0.00445213 -0.08733184 112.6091 -10.4014 43.5802 0.99616914 0.00444179 -0.08733452 113.0247 1.6448 47.4074 0.99616911 0.00440340 -0.08733675 113.0496 -2.3548 47.4517 0.99616908 0.00437278 -0.08733870 113.0734 -6.3546 47.4938 0.99616864 0.00437163 -0.08734380 113.0964 -10.3593 47.5386 0.99616965 0.00435273 -0.08733317 {Q0130, point 9 removed.} {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 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, point 9 removed} {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 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:=' {Q0129} 118.0705 -43.1922 -18.7357 45.1522 -64.3781 -17.6579 44.9529 -22.3919 -18.1081 {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} '; { Read the mount coordinates from coord_str. We use the mount to convert between global and wps coordinates. } mount:=read_kinematic_mount(coord_str); { Read all the wire positions, edge positions, and image rotations out of the wire and edge strings. If we're going to use only the left or right edges, we read only those. If we are going to use the centers, we obtain the image centers by taking the average of the left and right edges. } for i:=1 to num_points 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 edges[i] do begin position.y:=y_ref; 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; side:=edge_direction; case side of +1:begin s:=read_word(edge_str); position.x:=read_real(edge_str)/1000; rotation:=read_real(edge_str)/1000; end; 0:begin s:=read_word(edge_str); position.x:=read_real(edge_str)/1000; rotation:=read_real(edge_str)/1000; position.x:=(position.x+read_real(edge_str)/1000)/2; rotation:=(rotation+read_real(edge_str)/1000)/2; end; -1:begin s:=read_word(edge_str); s:=read_word(edge_str); s:=read_word(edge_str); position.x:=read_real(edge_str)/1000; rotation:=read_real(edge_str)/1000; end; end; end; delete(edge_str,1,pos(chr(10),edge_str)); end; { Report the camera number and the choice of edges for the calculation. } case edge_direction of +1:writeln('Camera ',camera_num,' using ',num_points:1,' left edges.'); 0:writeln('Camera ',camera_num,' using ',num_points:1,' center lines.'); -1:writeln('Camera ',camera_num,' using ',num_points:1,' right edges.'); end; { Start with a nominal set of calibration constants, disturbed by a random amount in all parameters. } camera:=nominal_wps_camera(camera_num); writeln('nominal: ',string_from_wps_camera(camera)); with camera do begin with pivot do begin x:=x+disturb; y:=y+disturb; z:=z+disturb; end; with sensor do begin x:=x+disturb; y:=y+disturb; z:=z+disturb; end; with rot do begin x:=x+disturb/10; y:=y+disturb/10; z:=z+disturb/10; end; end; writeln('disturbed: ',string_from_wps_camera(camera)); { Calculate and display the error corresponding to each image edge and its cmm-measured wire position. Here we use the point on each edge at y=y_ref. } for i:=1 to num_points do begin write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction)); write(' ',string_from_xyz(wps_ray_error(edges[i].position,edge_direction,wires[i],camera))); writeln; end; { Construct the fitting simplex, using our starting calibration as the first vertex. } with simplex,camera do begin vertices[1,1]:=pivot.x; vertices[1,2]:=pivot.y; vertices[1,3]:=pivot.z; vertices[1,4]:=sensor.x; vertices[1,5]:=sensor.y; vertices[1,6]:=sensor.z; vertices[1,7]:=rot.x; vertices[1,8]:=rot.y; vertices[1,9]:=rot.z; construct_size:=random_scale/10; done_counter:=0; max_done_counter:=5; end; simplex_construct(simplex,error); { Run the simplex fit until we reach convergance, as determined by the simplex_step routine itself. } done:=false; i:=0; while not done do begin simplex_step(simplex,error); done:=(simplex.done_counter>=simplex.max_done_counter); inc(i); if (i mod 100 = 0) or done then begin with simplex,camera do begin pivot.x:=vertices[1,1]; pivot.y:=vertices[1,2]; pivot.z:=vertices[1,3]; sensor.x:=vertices[1,4]; sensor.y:=vertices[1,5]; sensor.z:=vertices[1,6]; rot.x:=vertices[1,7]; rot.y:=vertices[1,8]; rot.z:=vertices[1,9]; end; writeln(i:5,' ', string_from_wps_camera(camera),' ', xyz_separation(camera.sensor,camera.pivot):fsr:fsd,' ', error(simplex.vertices[1]):fsr:fsd,' ', simplex_size(simplex):fsr); end; end; { Calculate and display the errors again. } for i:=1 to num_points do begin write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction)); write(' ',string_from_xyz(wps_ray_error(edges[i].position,edge_direction,wires[i],camera))); writeln; end; end. { NOTE: The numbers are: iterations, pivot.x, y, z (mm), sensor.x, y, z (mm), rot.x, y, z (rad) error (um), simplex volume (mm^n). Q0129_1, Centers. 4887 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1.573 0.033 -0.551 0.003 2.9e-70 Q0129_1, Left Edges. 4794 -7.180 89.867 -3.545 -17.398 95.946 -3.843 -1.573 0.039 -0.533 0.003 3.6e-71 Q0129_1, Right Edges. 4951 -6.985 89.759 -3.634 -17.134 95.795 -3.944 -1.573 0.028 -0.569 0.004 1.2e-69 Q0129_2, Centers. 4827 -6.936 37.604 -4.029 -17.316 31.139 -4.009 1.569 0.063 0.536 0.005 5.7e-73 Q0129_2, Left Edges. 6014 -7.097 37.486 -3.963 -17.504 30.999 -3.926 1.568 0.067 0.524 0.005 4.5e-72 Q0129_2, Right Edges. 5441 -6.845 37.673 -4.094 -17.201 31.229 -4.092 1.570 0.059 0.547 0.006 7.2e-71 Q0130_1, Centers 11 points. 8027 -5.028 88.682 -1.814 -15.723 94.959 -1.344 -1.567 -0.035 -0.510 0.006 1.2e-69 6113 -5.028 88.682 -1.814 -15.723 94.959 -1.344 -1.567 -0.035 -0.510 0.006 4.6e-69 6472 -5.028 88.682 -1.814 -15.723 94.959 -1.344 -1.567 -0.035 -0.510 0.006 2.3e-69 6313 -4.998 88.666 -1.833 -15.685 94.938 -1.366 -1.567 -0.035 -0.511 0.006 1.2e-69 5662 -5.028 88.682 -1.814 -15.723 94.959 -1.344 -1.567 -0.035 -0.510 0.006 7.4e-68 Q0130_2, Centers 11 points. 5768 -2.081 39.930 -6.049 -11.844 34.235 -6.762 1.568 -0.000 0.652 0.006 1.8e-71 6169 -2.081 39.930 -6.049 -11.844 34.235 -6.762 1.568 -0.000 0.652 0.006 9.1e-72 4640 -2.081 39.930 -6.049 -11.844 34.235 -6.762 1.568 -0.000 0.652 0.006 3.6e-71 NOTE: The numbers are: edge, camera, pivot.x, y, z (mm), sensor.x, y, z (mm), rot.x, y, z (mrad), pivot-ccd (mm), error (um). We displace the original camer by 1 mm and 100 mrad. We have 22 points for Q0129, 11 points for Q0130 and Q0131. We are fitting 9 parameters. C Q0129_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853 0.003 C Q0129_2 -6.936 37.604 -4.029 -17.316 31.139 -4.009 1569.173 62.729 535.665 12.229 0.005 C Q0130_1 -5.062 88.701 -1.794 -15.766 94.982 -1.320 -1567.197 -35.028 -508.767 12.419 0.006 C Q0130_2 -2.081 39.930 -6.049 -11.844 34.235 -6.762 1568.043 -0.144 652.383 11.325 0.006 L Q0131_1 -3.871 88.557 -4.489 -13.947 95.017 -4.568 -1575.956 -1.148 -588.152 11.969 0.005 C Q0131_2 -2.660 40.571 -4.675 -12.279 35.009 -5.323 1584.284 38.425 660.036 11.130 0.002 }