{
}
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 refine our simplex fitter and add the rotation of the
	wire images to the calculation of the calibration error.
}

uses
	utils,bcam,wps;
	
const
	N=22;
	edge_direction=0;
	camera_num=1;
	random_scale=1;
	num_parameters=9;
	max_num_shrinks=5;
	report=false; 
	y_ref=1.220;{mm}
	
var
	edge_str,wire_str,coord_str:long_string;
	wires:array [1..N] of wps_wire_type;
	edges:array [1..N] of wps_edge_type;
	simplex:array [1..num_parameters+1] of wps_camera_type;
	errors:array [1..num_parameters+1] of real;
	done:boolean;
	num_shrinks:integer;

{
	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(c:wps_camera_type):real;
const
	y_shift=0.500;{mm}
var
	i:integer;
	sum:real;
	p:xy_point_type;
begin
	sum:=0;
	for i:=1 to N 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/N/2);
end;

{
	Add two sets of calibration parameters.
}
function sum(a,b:wps_camera_type):wps_camera_type;
var
	c:wps_camera_type;
begin
	c.pivot:=xyz_sum(a.pivot,b.pivot);
	c.sensor:=xyz_sum(a.sensor,b.sensor);
	c.rot:=xyz_sum(a.rot,b.rot);
	c.id:=a.id;
	sum:=c;
end;

{
	Scale a set of calibration parameters.
}
function scale(a:wps_camera_type;s:real):wps_camera_type;
var
	c:wps_camera_type;
begin
	c.pivot:=xyz_scale(a.pivot,s);
	c.sensor:=xyz_scale(a.sensor,s);
	c.rot:=xyz_scale(a.rot,s);
	c.id:=a.id;
	scale:=c;
end;

{
	Subtract two sets of calibration parameters.
}
function difference(a,b:wps_camera_type):wps_camera_type;
var
	c:wps_camera_type;
begin
	c.pivot:=xyz_difference(a.pivot,b.pivot);
	c.sensor:=xyz_difference(a.sensor,b.sensor);
	c.rot:=xyz_difference(a.rot,b.rot);
	c.id:=a.id;
	difference:=c;
end;

{
	Construct a new simplex with sides of length random_scale. We assume that
	the first element in the simplex is already set. We re-calculate the error
	array.
}
procedure construct_simplex;
var i:integer;
begin
	for i:=2 to num_parameters+1 do simplex[i]:=simplex[1];
	simplex[2].pivot.x:=simplex[1].pivot.x+random_scale;
	simplex[3].pivot.y:=simplex[1].pivot.y+random_scale;
	simplex[4].pivot.z:=simplex[1].pivot.z+random_scale;
	simplex[5].sensor.x:=simplex[1].sensor.x+random_scale;
	simplex[6].sensor.y:=simplex[1].sensor.y+random_scale;
	simplex[7].sensor.z:=simplex[1].sensor.z+random_scale;
	simplex[8].rot.z:=simplex[1].rot.z+random_scale/10;
	simplex[9].rot.x:=simplex[1].rot.x+random_scale/10;
	simplex[10].rot.y:=simplex[1].rot.y+random_scale/10;
	for i:=1 to num_parameters+1 do errors[i]:=error(simplex[i]);
end;

{
	Here is our simplex fitting routine. It takes one simplex step, whereby a
	simplex shape in the fitting space is either reflected, extended, or
	contracted. As the fit converges, we re-construct the simplex to make sure
	we don't get stuck in a false convergance.
}
procedure simplex_step;

var 
	i,j:integer;
	swapped:boolean;
	c0,cc,ce,cr:wps_camera_type;
	e,ee:real;
	
begin
{
	Sort the simplex vertices into order of ascending error
	function.
}
	swapped:=true;
	while swapped do begin
		swapped:=false;
		for i:=1 to num_parameters do begin
			if errors[i]>errors[i+1] then begin
				e:=errors[i];
				c0:=simplex[i];
				errors[i]:=errors[i+1];
				simplex[i]:=simplex[i+1];
				errors[i+1]:=e;
				simplex[i+1]:=c0;
				swapped:=true;
			end;
		end;
	end;
{	
	Determine the center of mass of the first num_parameters vertices.
}
	c0:=simplex[1];
	for i:=1+1 to num_parameters do c0:=sum(c0,simplex[i]);
	c0:=scale(c0,1/num_parameters);
{
	Reflect the worste vertex through the center of mass of the others.
}
	cr:=sum(c0,scale(difference(c0,simplex[num_parameters+1]),1));
	e:=error(cr);
{
	If the error on this new vertex is somewhere between that of the the first
	num_parameters vertices, we keep it in place of the worste vertex.
}
	if (e>=errors[1]) and (e=errors[num_parameters]) then begin
		cc:=sum(simplex[num_parameters+1],scale(difference(c0,simplex[num_parameters+1]),0.5));
		ee:=error(cc);
		if ee<=errors[num_parameters+1] then begin
			simplex[num_parameters+1]:=cc;
			errors[num_parameters+1]:=ee;
			if report then write('c');
		end else begin
			inc(num_shrinks);
			if num_shrinks>max_num_shrinks then done:=true;
			if not done then begin
				if random_0_to_1<0 then begin
					for i:=2 to num_parameters+1 do begin
						simplex[i]:=sum(simplex[1],scale(difference(simplex[i],simplex[1]),0.5));
						errors[i]:=error(simplex[i]);
					end;
					if report then write('s');
				end else begin
					construct_simplex;
					if report then write('x');
				end;
			end;
		end;
	end;
{
	Sort the vertices in order of ascending error again.
}
	swapped:=true;
	while swapped do begin
		swapped:=false;
		for i:=1 to num_parameters do begin
			if errors[i]>errors[i+1] then begin
				e:=errors[i];
				c0:=simplex[i];
				errors[i]:=errors[i+1];
				simplex[i]:=simplex[i+1];
				errors[i+1]:=e;
				simplex[i+1]:=c0;
				swapped:=true;
			end;
		end;
	end;
end;

{
	simplex_volume returns the volume of the current simplex.
}
function simplex_volume:real;
var
	M:matrix_ptr;
	j:integer;
	c:wps_camera_type;
begin
	M:=new_matrix(num_parameters,num_parameters);
	for j:=1 to num_parameters do begin
		c:=difference(simplex[j+1],simplex[j]);
		M^[j,1]:=c.pivot.x;
		M^[j,2]:=c.pivot.y;
		M^[j,3]:=c.pivot.z;
		M^[j,4]:=c.sensor.x;
		M^[j,5]:=c.sensor.y;
		M^[j,6]:=c.sensor.z;
		M^[j,7]:=c.rot.z;
		M^[j,8]:=c.rot.x;
		M^[j,9]:=c.rot.y;
	end;
	simplex_volume:=matrix_determinant(M);
	dispose_matrix(M);
end;

var 
	mount:kinematic_mount_type;
	i:integer;
	s:short_string;
	
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 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 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 ',N:1,' left edges.');
		0:writeln('Camera ',camera_num,' using ',N:1,' center lines.');
		-1:writeln('Camera ',camera_num,' using ',N:1,' right edges.');
	end;
{
	Start with a nominal set of calibration constants, disturbed by a random
	amount in all parameters.
}
	simplex[1]:=nominal_wps_camera(camera_num);
	writeln('nominal:   ',string_from_wps_camera(simplex[1]));
	with simplex[1] 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(simplex[1]));
{
	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 N 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],simplex[1])));
		writeln;
	end;
{
	Construct the fitting simplex, using our starting calibration as the first
	vertex.
}
	construct_simplex;	
{
	Run the simplex fit until we reach convergance, as determined by the
	simplex_step routine itself.
}
	done:=false;
	num_shrinks:=0;
	i:=0;
	while not done do begin
		simplex_step;
		inc(i);
		if (i mod 100 = 0) or done then begin
			if report then write(crlf);
			writeln(i:5,' ',string_from_wps_camera(simplex[1]),' ',
				xyz_length(xyz_difference(simplex[1].pivot,simplex[1].sensor)):fsr:fsd,' ',
				error(simplex[1]):fsr:fsd,' ',simplex_volume:fsr);
		end;
	end;
{
	Calculate and display the errors again.
}	
	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(edges[i].position,edge_direction,wires[i],simplex[1])));
		writeln;
	end;
end.

{
NOTE: The first number on a line is the number of iterations at which the simplex fitting routine 
converged. 

Q0129_1: Centers, fit pivot xyz, sensor xyz, and rotation x,y,z. We start with random disturbance of
+-5 mm and 500 mrad from the nominal camera parameters. We have max_num_shrinks = 1. The initial
simplex has sides 1 mm and 100 mrad long. The final number on the line is the simplex volume at the
end of the fit. The following lists ten consecutive applications of the simplex fit.
 1909 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -5.0e-54
 1953 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  8.0e-53
 1590 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -5.0e-54
 1263 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  2.5e-54
 1807 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.401 11.852  0.003  5.0e-54
 1669 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  8.0e-53
  995 WPS1_A_1 -6.984 89.743 -3.308 -17.155 95.791 -3.547 -1570.796 0.000 -552.323 11.835  0.003 -6.1e-58
 1269 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -8.0e-53
 2087 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -1.3e-51
 1953 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -4.0e-53

Q0129_1: Centers, fit pivot xyz, sensor xyz, and rotation x,y,z. We now have max_num_shrinks=5 and
we choose at random to re-construct the simplex instead of shrink it. We hope this will stop the
routine from getting stuck. Here are the results of twenty runs. We start with +-5 mm +-500 mrad
random disturbance of the nominal parameters. The fit converges 20 times out of 20.
 2616 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  1.9e-59
 2983 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.401 11.852  0.003  1.2e-63
 2500 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.401 11.852  0.003  1.9e-59
 5818 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -6.2e-55
 3252 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -3.1e-55
 2352 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -4.9e-57
 3517 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.401 11.852  0.003 -1.2e-54
 1835 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -1.8e-65
 2763 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  2.4e-57
 4677 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -1.6e-55
 2914 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  4.9e-57
 2387 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  6.1e-58
 2434 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  1.5e-64
 2911 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -1.6e-55
 1531 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.401 11.852  0.003 -7.2e-65
 3139 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  2.5e-54
 3804 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -7.8e-56
 4068 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003 -1.9e-59
 2297 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  4.7e-60
 2265 WPS1_A_1 -7.035 89.782 -3.538 -17.219 95.838 -3.830 -1570.796 0.000 -551.402 11.852  0.003  3.0e-61

Q0129_1: Centers, fit all nine parameters after adding sensitivity to rotation to the error
calculation. We shift each wire center up and down by 0.5 mm on the CCD and displace using the image
rotation, and calculate the error at each of these two points instead of just at the original point.
When we see the first failure to converge, we increase max_num_shrinks from 5 to 10 and continue. We
are starting from constants that are up to 5 mm and 500 mrad away from the final values, at random. The
final failure we see is because the routine has turned the CCD around the y-axis by over 90 degrees.
 4704 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -5.4e-73
 4063 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  1.1e-72
 6757 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -3.4e-74
 5803 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  6.7e-74
 2112 WPS1_A_1 -5.833 89.079 -3.246 -15.729 94.967 -3.470 -1573.904 33.209 -571.588 11.517  0.004 -1.5e-87
 5811 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  1.3e-82
 6073 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  1.7e-74
 9050 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -2.7e-73
 7313 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  6.6e-77
 5512 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -3.1e-83
 3585 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -3.1e-83
10405 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -1.1e-72
 6867 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  2.6e-76
 9219 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -1.6e-77
 8583 WPS1_A_1 -7.133 89.752 -2.139 -17.362 95.813 -2.090 1505.215 -25.127 2597.062 11.890  0.107 -2.8e-67

Q0129_1: Centers, fitting all parameters, starting with +-5 mm and +-500 mrad displacements. We no longer
shrink the simplex. We always re-construct it when we reach the shrink step, and we count re-constructions.
We see occasions where the rotation of the sensor settles upon an equivalent combination with larger
rotations. We obtain exactly the same errors at each of the 22 points using the equivalent rotations, so
we conclude that they are indeed equivalent: the sensor is in the same orientation.
 5551 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -5.5e-70
 7538 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -4714.859 9391.642 2590.227 11.853  0.003  3.5e-71
 7129 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  8.6e-72
 6392 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  5.5e-70

Q0129_1: Centers, fitting all parameters, starting with +-1 mm and +-100 mrad displacements. We have
max_num_shrinks = 5.
 4995 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  1.2e-72
 6042 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -1.5e-70
 4551 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  5.9e-70
 4804 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -3.7e-71
 4704 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -1.9e-71
 5432 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  1.5e-70
 5820 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003 -4.7e-69
 4887 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  5.9e-70
 5558 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  9.3e-72
 4246 WPS1_A_1 -7.042 89.787 -3.590 -17.226 95.844 -3.894 -1573.266 33.136 -551.365 11.853  0.003  3.0e-70

Q0129_1: Left Edges.
 4331 WPS1_A_1 -7.180 89.867 -3.545 -17.398 95.946 -3.843 -1572.698 38.660 -533.280 11.893  0.003 -2.4e-69
 5508 WPS1_A_1 -7.180 89.867 -3.545 -17.398 95.946 -3.843 -1572.698 38.660 -533.280 11.893  0.003 -2.3e-72

Q0129_1: Right Edges.
 4583 WPS1_A_1 -6.985 89.759 -3.634 -17.134 95.795 -3.944 -1573.439 27.636 -569.372 11.813  0.004  9.3e-72
 5286 WPS1_A_1 -6.985 89.759 -3.634 -17.134 95.795 -3.944 -1573.439 27.636 -569.372 11.813  0.004 -1.9e-71
 4622 WPS1_A_1 -6.985 89.759 -3.634 -17.134 95.795 -3.944 -1573.439 27.636 -569.372 11.813  0.004  2.3e-72

Q0129_2: Centers.
 5333 WPS1_A_2 -6.936 37.604 -4.029 -17.316 31.139 -4.009 1569.173 62.729 535.665 12.229  0.005 -1.2e-72
 5462 WPS1_A_2 -6.936 37.604 -4.029 -17.316 31.139 -4.009 1569.173 62.729 535.665 12.229  0.005  3.7e-71

Q0129_2: Left Edges.
 5241 WPS1_A_2 -7.096 37.487 -3.963 -17.504 31.000 -3.926 1568.053 66.902 523.649 12.264  0.005 -1.9e-71
 5210 WPS1_A_2 -7.096 37.487 -3.963 -17.504 31.000 -3.926 1568.053 66.902 523.649 12.264  0.005 -1.9e-71

Q0129_2: Right Edges.
 4842 WPS1_A_2 -6.845 37.673 -4.094 -17.201 31.229 -4.092 1570.069 58.633 547.455 12.197  0.006 -1.5e-70
 5126 WPS1_A_2 -6.845 37.673 -4.094 -17.201 31.229 -4.092 1570.069 58.633 547.455 12.197  0.006 -1.9e-71


}