{
}
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

}