{
} 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 fit the calibration parameters of both WPS-cameras simultaneously so as to minimize the error beetween the WPS-measured wire positions and the CMM-measured wire positions. For each wire position, we calculate the error at two different z-positions. } uses utils,bcam,wps; const num_points=22; edge_direction=0; random_scale=1; num_parameters=18; y_ref=1.220;{mm} z_ref=-4;{mm} ray_error=false; var edge_str,wire_str,coord_str:long_string; wires:array [1..num_points] of xyz_line_type; edges_1,edges_2: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 z_1=z_ref-2; z_2=z_ref+2; y_shift=0.5; var i:integer; sum:real; p_1,p_2:xy_point_type; ep_1,ep_2:xyz_point_type; c_1,c_2:wps_camera_type; w:wps_wire_type; begin i:=1; with c_1 do begin pivot.x:=v[i];inc(i); pivot.y:=v[i];inc(i); pivot.z:=v[i];inc(i); sensor.x:=v[i];inc(i); sensor.y:=v[i];inc(i); sensor.z:=v[i];inc(i); rot.x:=v[i];inc(i); rot.y:=v[i];inc(i); rot.z:=v[i];inc(i); end; with c_2 do begin pivot.x:=v[i];inc(i); pivot.y:=v[i];inc(i); pivot.z:=v[i];inc(i); sensor.x:=v[i];inc(i); sensor.y:=v[i];inc(i); sensor.z:=v[i];inc(i); rot.x:=v[i];inc(i); rot.y:=v[i];inc(i); rot.z:=v[i];inc(i); end; sum:=0; if ray_error then begin for i:=1 to num_points do begin with w do begin position:=wires[i].point; direction:=wires[i].direction; radius:=1/16*25.4/2; {one sixteenth inch steel pin} end; p_1:=edges_1[i].position; p_1.y:=p_1.y-y_shift; p_1.x:=p_1.x-sin(edges_1[i].rotation)*y_shift/cos(edges_1[i].rotation); sum:=sum+sqr(xyz_length(wps_ray_error(p_1,edge_direction,w,c_1))); p_1:=edges_1[i].position; p_1.y:=p_1.y+y_shift; p_1.x:=p_1.x+sin(edges_1[i].rotation)*y_shift/cos(edges_1[i].rotation); sum:=sum+sqr(xyz_length(wps_ray_error(p_1,edge_direction,w,c_1))); p_2:=edges_2[i].position; p_2.y:=p_2.y-y_shift; p_2.x:=p_2.x-sin(edges_2[i].rotation)*y_shift/cos(edges_2[i].rotation); sum:=sum+sqr(xyz_length(wps_ray_error(p_2,edge_direction,w,c_2))); p_2:=edges_2[i].position; p_2.y:=p_2.y+y_shift; p_2.x:=p_2.x+sin(edges_2[i].rotation)*y_shift/cos(edges_2[i].rotation); sum:=sum+sqr(xyz_length(wps_ray_error(p_2,edge_direction,w,c_2))); end; error:=sqrt(sum/num_points/4); end else begin for i:=1 to num_points do begin ep_1:=wps_error( edges_1[i].position,edges_2[i].position, edges_1[i].rotation,edges_2[i].rotation, c_1,c_2,wires[i],z_1); ep_2:=wps_error( edges_1[i].position,edges_2[i].position, edges_1[i].rotation,edges_2[i].rotation, c_1,c_2,wires[i],z_2); sum:=sum+sqr(xyz_length(ep_1))+sqr(xyz_length(ep_2)); end; error:=sqrt(sum/num_points/2); end; end; var mount:kinematic_mount_type; i,j:integer; s:short_string; simplex:simplex_type(num_parameters); camera_1,camera_2:wps_camera_type; done:boolean; plane:xyz_plane_type; point:xyz_point_type; 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 '; 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 '; coord_str:=' {Q0129} 118.0705 -43.1922 -18.7357 45.1522 -64.3781 -17.6579 44.9529 -22.3919 -18.1081 '; { 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 point:=wps_from_global_point(read_xyz(wire_str),mount); direction:=wps_from_global_vector(read_xyz(wire_str),mount); end; with edges_1[i] do begin position.y:=y_ref; side:=edge_direction; s:=read_word(edge_str); case side of +1:begin position.x:=read_real(edge_str)/1000; rotation:=read_real(edge_str)/1000; s:=read_word(edge_str); s:=read_word(edge_str); end; 0:begin 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); position.x:=read_real(edge_str)/1000; rotation:=read_real(edge_str)/1000; end; end; end; with edges_2[i] do begin position.y:=y_ref; side:=edge_direction; case side of +1:begin position.x:=read_real(edge_str)/1000; rotation:=read_real(edge_str)/1000; s:=read_word(edge_str); s:=read_word(edge_str); end; 0:begin 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); 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; { Start with a nominal set of calibration constants, disturbed by a random amount in all parameters. } camera_1:=nominal_wps_camera(1); writeln('nominal: ',string_from_wps_camera(camera_1)); with camera_1 do begin id:="C1"; 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_1)); camera_2:=nominal_wps_camera(2); writeln('nominal: ',string_from_wps_camera(camera_2)); with camera_2 do begin id:="C2"; 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_2)); { Construct the fitting simplex, using our starting calibration as the first vertex. } j:=1; with simplex,camera_1 do begin vertices[1,j]:=pivot.x;inc(j); vertices[1,j]:=pivot.y;inc(j); vertices[1,j]:=pivot.z;inc(j); vertices[1,j]:=sensor.x;inc(j); vertices[1,j]:=sensor.y;inc(j); vertices[1,j]:=sensor.z;inc(j); vertices[1,j]:=rot.x;inc(j); vertices[1,j]:=rot.y;inc(j); vertices[1,j]:=rot.z;inc(j); end; with simplex,camera_2 do begin vertices[1,j]:=pivot.x;inc(j); vertices[1,j]:=pivot.y;inc(j); vertices[1,j]:=pivot.z;inc(j); vertices[1,j]:=sensor.x;inc(j); vertices[1,j]:=sensor.y;inc(j); vertices[1,j]:=sensor.z;inc(j); vertices[1,j]:=rot.x;inc(j); vertices[1,j]:=rot.y;inc(j); vertices[1,j]:=rot.z;inc(j); end; with simplex do begin construct_size:=random_scale/100; done_counter:=0; max_done_counter:=10; 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 j:=1; with simplex,camera_1 do begin pivot.x:=vertices[1,j];inc(j); pivot.y:=vertices[1,j];inc(j); pivot.z:=vertices[1,j];inc(j); sensor.x:=vertices[1,j];inc(j); sensor.y:=vertices[1,j];inc(j); sensor.z:=vertices[1,j];inc(j); rot.x:=vertices[1,j];inc(j); rot.y:=vertices[1,j];inc(j); rot.z:=vertices[1,j];inc(j); end; with simplex,camera_2 do begin pivot.x:=vertices[1,j];inc(j); pivot.y:=vertices[1,j];inc(j); pivot.z:=vertices[1,j];inc(j); sensor.x:=vertices[1,j];inc(j); sensor.y:=vertices[1,j];inc(j); sensor.z:=vertices[1,j];inc(j); rot.x:=vertices[1,j];inc(j); rot.y:=vertices[1,j];inc(j); rot.z:=vertices[1,j];inc(j); end; writeln(i:5,' ',error(simplex.vertices[1]):fsr:fsd); writeln(string_from_wps_camera(camera_1)); writeln(string_from_wps_camera(camera_2)); end; end; { Calculate and display the error for each wire position at the z_ref plane. } with plane.point do begin x:=0;y:=0;z:=z_ref; end; with plane.normal do begin x:=0;y:=0;z:=1; end; for i:=1 to num_points do begin point:=xyz_line_plane_intersection(wires[i],plane); writeln(string_from_xyz(point),' ', string_from_xyz(wps_error( edges_1[i].position,edges_2[i].position, edges_1[i].rotation,edges_2[i].rotation, camera_1,camera_2,wires[i],z_ref))); end; end. { Each 10000 iterations takes a minute. After ten minutes we would stop the fit. We give the iterations, the final error, and the constants for both cameras. !ray_error 65039 0.008 C1 -6.091 89.225 -3.146 -16.045 95.146 -3.350 -1574.263 38.681 -574.408 C2 -6.424 37.887 -3.647 -16.683 31.491 -3.541 1570.017 60.475 547.514 !ray_error 112600 0.008 C1 -5.313 88.777 -3.060 -15.077 94.589 -3.243 -1574.742 38.744 -587.368 C2 -6.519 37.813 -3.447 -16.798 31.400 -3.295 1570.079 60.769 548.864 !ray_error 115132 0.007 C1 -5.778 89.034 -2.879 -15.658 94.910 -3.022 -1574.327 38.785 -576.398 C2 -7.583 37.211 -3.807 -18.117 30.653 -3.735 1568.876 63.101 528.729 !ray_error 85300 0.0092 C1 -5.704 88.933 -1.331 -15.554 94.778 -1.121 -1574.609 37.708 -586.086 C2 -6.110 37.941 -1.405 -16.288 31.562 -0.778 1570.579 61.413 556.900 Here we have fitting of the two cameras separately, with the error calculated from ray to wire instead of ray intersection to wire. The fit has not converged after 42400 iterations. The fitted parameters are divided into two independent groups of nine. ray_error 42400 0.004 C1 -6.043 89.217 -3.647 -15.989 95.138 -3.962 -1573.793 33.317 -567.650 C2 -6.801 37.670 -3.754 -17.149 31.221 -3.672 1569.319 62.490 538.083 !ray_error 47700 0.009 C1 -4.929 88.529 -2.452 -14.601 94.281 -2.496 -1575.049 39.954 -593.565 C2 -6.417 37.850 -3.014 -16.671 31.446 -2.761 1570.272 60.608 551.804 !ray_error 131555 0.008 C1 -5.1187 88.6741 -3.2767 -14.8402 94.4628 -3.5084 -1574.709 38.823 -586.605 C2 -7.8989 37.0283 -3.8923 -18.5087 30.4272 -3.8397 1568.668 63.823 525.656 }