(* ::Package:: *)

BeginPackage["ParticleTrackingRadialCenter`"]


(* ::Section:: *)
(*Usage Definitions*)


RadialCenter::usage =
        "Determines the center of a 2D intensity distribution using the method given in Parthasarathy R. Nature Methods(2012)

		{{x0, y0},{sigma,sigma}} = RadialCenter[Positions,Intensities];

		Inputs
			Positions: A list of x-y coordinate pairs that coorespond to the list of intensity values in Intensities.  It is a R x 2 array, where R, the number
			of rows, is equal to M*N, the dimensions of the rectangular array for which the center is being calculated 

			Intensities:  A list of grayscale intensity values that coorespond to the x-y coordinate pairs in Positions.  It is an R x 1 array, where R, the number
			of rows, is equal to M*N, the dimensions of the rectangular array for which the center is being calculated

		Outputs
			{x0, y0}: The calculated center of radial symmetry

			{sigma, sigma}:  The approximate width of the object.  Both values are the same, but it's listed twice for consistency with other code."


Begin["`Private`"]


(* ::Section:: *)
(*Object Identification Functions*)


(*Finds the center of a 2D intensity distribution using the algorithm given in Parthasarathy R. Nature Methods(2012)*)
RadialCenter[Positions_,Intensities_]:=Module[
(*Initialize variables*)
(*Positions and Intensities are lists of x-y coordinates and grayscale intensities of an M x N array.  Positions is an R x 2 list, where R, the number of rows, is equal to
M*N, or the total number of pixels.  Intensities is an R x 1 list, where each row contains an intensity value that corresponds to the x-y coordinates given in Positions.  In the
variable initialization, Intensities is reshaped using Partition, into an M x N list.  The unusual form of the inputs was necessary to make this function work within a larger
code structure*) 
{Int=Partition[Intensities,Last[Positions][[2]]-Positions[[1,2]]+1] ,ParticleOrigin = Positions[[1,All]],
b={},Nx={},Ny={},Xm={},Ym={},dIdu={},dIdv={},fdu={},fdv={},dImag2={},m={},NanElements={},unsmoothm={},sdI2={},xcentroid={},ycentroid={},
w={},wm2p1={},sw={},smw={},smmw={},smbw={},sbw={},det={},xc={},yc={},Isub={},Px={},Py={},xoffset={},yoffset={},r2={}},

(*Set up the grid on which the gradient is going to be calculated. If you picture the pixels as a square grid, the grid points are the inner vertices between each 
group of four pixels.  Therefore, the grid points are offset from the pixel centers by 0.5 pixels in x and y.  x coordinates are the columns and
they count left to right and y-coordinates are the rows and count top to bottom.*)
{Ny,Nx} = Dimensions[Int];
Xm = Table[Range[-(Nx-1)/2+0.5,(Nx-1)/2-0.5,1],{Ny-1}];
Ym = Transpose[Table[Range[-(Ny-1)/2+0.5,(Ny-1)/2-0.5,1],{Nx-1}]];

(*calculate the gradient in the 45 degree rotated coordinates, u and v.*)
dIdu = Int[[1;;Ny-1,2;;Nx]]-Int[[2;;Ny,1;;Nx-1]];
dIdv = Int[[1;;Ny-1,1;;Nx-1]]-Int[[2;;Ny,2;;Nx]];

(*Smoothing of the gradient using a 3x3 mean filter*)
fdu = MeanFilter[dIdu,1];
fdv = MeanFilter[dIdv,1];
dImag2 = fdu*fdu + fdv*fdv; (*gradient magnitude, squared*)

(*The slope of the gradient in x-y coordinates.  The minus sign flips the array to account for y increasing downward*)
m = -(fdv + fdu) / (fdu-fdv);

(*Checks if any values of m are ComplexInfinity or Indeterminate from 0/0 or x/0 operations and if so replaces them with the unsmoothed value.
If it is still ComplexInfinity or Indeterminate, replace those elements with 0.*)
If[MemberQ[m,ComplexInfinity|Indeterminate,Infinity],
	NanElements = Position[m,ComplexInfinity|Indeterminate];
	unsmoothm = -(dIdv + dIdu) / (dIdu-dIdv);
	ReplacePart[m,Thread[NanElements -> Extract[unsmoothm,NanElements]]];

	If[MemberQ[m,ComplexInfinity|Indeterminate,Infinity],
		NanElements = Position[m,ComplexInfinity|Indeterminate];
		ReplacePart[m,NanElements -> 0];
	];
];

(*Calculate the y-intercept of the line of slope m that goes through each grid point*)
b = Ym - m * Xm;

(*Weighting: Weight by the square of the gradient magnitude and the inverse distance to the gradient intensity centroid*)
sdI2 = Total[dImag2,Infinity];
xcentroid = Total[dImag2*Xm,Infinity]/sdI2;
ycentroid = Total[dImag2*Ym,Infinity]/sdI2;
w  = dImag2/Sqrt[(Xm-xcentroid)*(Xm-xcentroid)+(Ym-ycentroid)*(Ym-ycentroid)];

(*Least squares solution to determine the translated coordinate system origin [xc,yc]
such that lines y=mx+b have the minimal total distance^2 to the origin*)
wm2p1 = w /(m * m + 1);
sw  = Total[wm2p1,Infinity];
smmw = Total[m*m*wm2p1,Infinity];
smw  = Total[m*wm2p1,Infinity];
smbw = Total[m*b*wm2p1,Infinity];
sbw  = Total[b*wm2p1,Infinity];
det = smw*smw - smmw*sw;
xc = (smbw*sw - smw*sbw)/det;
yc = (smbw*smw - smmw*sbw)/det;

(*find the center with respect to the upper left corner of the matrix of intensities passed to RadialCenter*)
xc = xc + (Nx+1)/2.0;
yc = yc + (Ny+1)/2.0;

(*Calculate an approximate width of the object, which can be used later for other processing or selection*)
Isub = Int - Min[Int];
Px = Table[Range[1,Nx,1],{Ny}];
Py = Transpose[Table[Range[1,Ny,1],{Nx}]];
xoffset = Px - xc;
yoffset = Py - yc;
r2 = xoffset*xoffset + yoffset*yoffset;
sigma = Sqrt[Total[Isub*r2,Infinity]/Total[Isub,Infinity]]/2;

(*Find the object position with respect to the full image coordinates.  ParticleOrigin are the full frame coordinates of the upper left
corner of the intensity matrix passed to RadialCenter*)
x0 = xc + ParticleOrigin[[2]] - 1;
y0 = yc + ParticleOrigin[[1]] - 1; 

(*Return the final refined position with respect to the full image coordinates and the object width, {{xo,yo},{sigma,sigma}}*)
{{x0, y0},{sigma,sigma}}
]


(* ::Section::Closed:: *)
(*End Package*)


End[ ]


EndPackage[ ]