Index: src/Common/SharpMap/Utilities/LeastSquaresTransform.cs =================================================================== diff -u -r8f6ae890fed8e8eae3a32f9c0498a10f82e0ddf9 -r5fc71a385897af92ccb092f2f969b5709afab85a --- src/Common/SharpMap/Utilities/LeastSquaresTransform.cs (.../LeastSquaresTransform.cs) (revision 8f6ae890fed8e8eae3a32f9c0498a10f82e0ddf9) +++ src/Common/SharpMap/Utilities/LeastSquaresTransform.cs (.../LeastSquaresTransform.cs) (revision 5fc71a385897af92ccb092f2f969b5709afab85a) @@ -22,285 +22,289 @@ namespace SharpMap.Utilities { - /// - /// Calculates Affine and Helmert transformation using Least-Squares Regression of input and output points - /// - public class LeastSquaresTransform - { - private List inputs; - private List outputs; + /// + /// Calculates Affine and Helmert transformation using Least-Squares Regression of input and output points + /// + public class LeastSquaresTransform + { + private readonly List inputs; + private readonly List outputs; - /// - /// Initialize Least Squares transformations - /// - public LeastSquaresTransform() - { - inputs = new List(); - outputs = new List(); - } + /// + /// Initialize Least Squares transformations + /// + public LeastSquaresTransform() + { + inputs = new List(); + outputs = new List(); + } - /// - /// Adds an input and output value pair to the collection - /// - /// - /// - public void AddInputOutputPoint(IPoint input, IPoint output) - { - inputs.Add(input); - outputs.Add(output); - } + /// + /// Adds an input and output value pair to the collection + /// + /// + /// + public void AddInputOutputPoint(IPoint input, IPoint output) + { + inputs.Add(input); + outputs.Add(output); + } - /// - /// Removes input and output value pair at the specified index - /// - /// - public void RemoveInputOutputPointAt(int i) - { - inputs.RemoveAt(i); - outputs.RemoveAt(i); - } + /// + /// Removes input and output value pair at the specified index + /// + /// + public void RemoveInputOutputPointAt(int i) + { + inputs.RemoveAt(i); + outputs.RemoveAt(i); + } - /// - /// Gets the input point value at the specified index - /// - /// index - /// Input point value a index 'i' - public IPoint GetInputPoint(int i) - { - return inputs[i]; - } + /// + /// Gets the input point value at the specified index + /// + /// index + /// Input point value a index 'i' + public IPoint GetInputPoint(int i) + { + return inputs[i]; + } - /// - /// Sets the input point value at the specified index - /// - /// Point value - /// index - public void SetInputPointAt(IPoint p, int i) - { - inputs[i] = p; - } + /// + /// Sets the input point value at the specified index + /// + /// Point value + /// index + public void SetInputPointAt(IPoint p, int i) + { + inputs[i] = p; + } - /// - /// Gets the output point value at the specified index - /// - /// index - /// Output point value a index 'i' - public IPoint GetOutputPoint(int i) - { - return outputs[i]; - } + /// + /// Gets the output point value at the specified index + /// + /// index + /// Output point value a index 'i' + public IPoint GetOutputPoint(int i) + { + return outputs[i]; + } - /// - /// Sets the output point value at the specified index - /// - /// Point value - /// index - public void SetOutputPointAt(IPoint p, int i) - { - outputs[i] = p; - } + /// + /// Sets the output point value at the specified index + /// + /// Point value + /// index + public void SetOutputPointAt(IPoint p, int i) + { + outputs[i] = p; + } - /// - /// Return an array with the six affine transformation parameters {a,b,c,d,e,f} and the sum of the squares of the residuals (s0) - /// - /// - /// a,b defines scale vector 1 of coordinate system, d,e scale vector 2. c,f defines offset. - /// - /// Converting from input (X,Y) to output coordinate system (X',Y') is done by: - /// X' = a*X + b*Y + c, Y' = d*X + e*Y + f - /// - /// - /// Transformation based on Mikhail "Introduction to Modern Photogrammetry" p. 399-300. - /// Extended to arbitrary number of measurements by M. Nielsen - /// - /// - /// Array with the six transformation parameters and sum of squared residuals: a,b,c,d,e,f,s0 - public double[] GetAffineTransformation() - { - if(inputs.Count<3) - throw(new System.Exception("At least 3 measurements required to calculate affine transformation")); + /// + /// Return an array with the six affine transformation parameters {a,b,c,d,e,f} and the sum of the squares of the residuals (s0) + /// + /// + /// a,b defines scale vector 1 of coordinate system, d,e scale vector 2. c,f defines offset. + /// + /// Converting from input (X,Y) to output coordinate system (X',Y') is done by: + /// X' = a*X + b*Y + c, Y' = d*X + e*Y + f + /// + /// + /// Transformation based on Mikhail "Introduction to Modern Photogrammetry" p. 399-300. + /// Extended to arbitrary number of measurements by M. Nielsen + /// + /// + /// Array with the six transformation parameters and sum of squared residuals: a,b,c,d,e,f,s0 + public double[] GetAffineTransformation() + { + if (inputs.Count < 3) + { + throw (new Exception("At least 3 measurements required to calculate affine transformation")); + } - //double precision isn't always enough when transforming large numbers. - //Lets subtract some mean values and add them later again: - //Find approximate center values: - IPoint meanInput = GeometryFactory.CreatePoint(0, 0); - IPoint meanOutput = GeometryFactory.CreatePoint(0, 0); - for (int i = 0; i < inputs.Count; i++) - { - meanInput.X += inputs[i].X; - meanInput.Y += inputs[i].Y; - meanOutput.X += outputs[i].X; - meanOutput.Y += outputs[i].Y; - } - meanInput.X = Math.Round(meanInput.X / inputs.Count); - meanInput.Y = Math.Round(meanInput.Y / inputs.Count); - meanOutput.X = Math.Round(meanOutput.X / inputs.Count); - meanOutput.Y = Math.Round(meanOutput.Y / inputs.Count); + //double precision isn't always enough when transforming large numbers. + //Lets subtract some mean values and add them later again: + //Find approximate center values: + IPoint meanInput = GeometryFactory.CreatePoint(0, 0); + IPoint meanOutput = GeometryFactory.CreatePoint(0, 0); + for (int i = 0; i < inputs.Count; i++) + { + meanInput.X += inputs[i].X; + meanInput.Y += inputs[i].Y; + meanOutput.X += outputs[i].X; + meanOutput.Y += outputs[i].Y; + } + meanInput.X = Math.Round(meanInput.X/inputs.Count); + meanInput.Y = Math.Round(meanInput.Y/inputs.Count); + meanOutput.X = Math.Round(meanOutput.X/inputs.Count); + meanOutput.Y = Math.Round(meanOutput.Y/inputs.Count); - double[][] N = CreateMatrix(3,3); - //Create normal equation: transpose(B)*B - //B: matrix of calibrated values. Example of row in B: [x , y , -1] - for (int i = 0; i < inputs.Count; i++) - { - //Subtract mean values - inputs[i].X -= meanInput.X; - inputs[i].Y -= meanInput.Y; - outputs[i].X -= meanOutput.X; - outputs[i].Y -= meanOutput.Y; - //Calculate summed values - N[0][0] += Math.Pow(inputs[i].X,2); - N[0][1] += inputs[i].X*inputs[i].Y; - N[0][2] += -inputs[i].X; - N[1][1] += Math.Pow(inputs[i].Y,2); - N[1][2] += -inputs[i].Y; - } - N[2][2] = inputs.Count; + double[][] N = CreateMatrix(3, 3); + //Create normal equation: transpose(B)*B + //B: matrix of calibrated values. Example of row in B: [x , y , -1] + for (int i = 0; i < inputs.Count; i++) + { + //Subtract mean values + inputs[i].X -= meanInput.X; + inputs[i].Y -= meanInput.Y; + outputs[i].X -= meanOutput.X; + outputs[i].Y -= meanOutput.Y; + //Calculate summed values + N[0][0] += Math.Pow(inputs[i].X, 2); + N[0][1] += inputs[i].X*inputs[i].Y; + N[0][2] += -inputs[i].X; + N[1][1] += Math.Pow(inputs[i].Y, 2); + N[1][2] += -inputs[i].Y; + } + N[2][2] = inputs.Count; - double[] t1 = new double[3]; - double[] t2 = new double[3]; + double[] t1 = new double[3]; + double[] t2 = new double[3]; - for (int i = 0; i < inputs.Count; i++) - { - t1[0] += inputs[i].X * outputs[i].X; - t1[1] += inputs[i].Y * outputs[i].X; - t1[2] += -outputs[i].X; + for (int i = 0; i < inputs.Count; i++) + { + t1[0] += inputs[i].X*outputs[i].X; + t1[1] += inputs[i].Y*outputs[i].X; + t1[2] += -outputs[i].X; - t2[0] += inputs[i].X * outputs[i].Y; - t2[1] += inputs[i].Y * outputs[i].Y; - t2[2] += -outputs[i].Y; - } - double[] trans = new double[7]; - // Solve equation N = transpose(B)*t1 - double frac = 1 / (-N[0][0]*N[1][1]*N[2][2]+N[0][0]*Math.Pow(N[1][2],2)+Math.Pow(N[0][1],2)*N[2][2]-2*N[1][2]*N[0][1]*N[0][2]+N[1][1]*Math.Pow(N[0][2],2)); - trans[0] = (-N[0][1]*N[1][2]*t1[2]+N[0][1]* t1[1]*N[2][2]-N[0][2]*N[1][2]*t1[1]+N[0][2]*N[1][1]*t1[2]-t1[0]*N[1][1]*N[2][2]+t1[0]*Math.Pow(N[1][2],2)) * frac; - trans[1] = (-N[0][1]*N[0][2]*t1[2]+N[0][1]* t1[0]*N[2][2]+N[0][0]*N[1][2]*t1[2]-N[0][0]*t1[1]*N[2][2]-N[0][2]*N[1][2]*t1[0]+Math.Pow(N[0][2],2)*t1[1]) * frac; - trans[2] = -(-N[1][2]*N[0][1]*t1[0]+Math.Pow(N[0][1],2)*t1[2]+N[0][0]*N[1][2]*t1[1]-N[0][0]*N[1][1]*t1[2]-N[0][2]*N[0][1]*t1[1]+N[1][1]*N[0][2]*t1[0]) * frac; - trans[2] += - meanOutput.X + meanInput.X; - // Solve equation N = transpose(B)*t2 - trans[3] = (-N[0][1]*N[1][2]*t2[2]+N[0][1]* t2[1]*N[2][2]-N[0][2]*N[1][2]*t2[1]+N[0][2]*N[1][1]*t2[2]-t2[0]*N[1][1]*N[2][2]+t2[0]*Math.Pow(N[1][2],2)) * frac; - trans[4] = (-N[0][1]*N[0][2]*t2[2]+N[0][1]* t2[0]*N[2][2]+N[0][0]*N[1][2]*t2[2]-N[0][0]*t2[1]*N[2][2]-N[0][2]*N[1][2]*t2[0]+Math.Pow(N[0][2],2)*t2[1]) * frac; - trans[5] = -(-N[1][2]*N[0][1]*t2[0]+Math.Pow(N[0][1],2)*t2[2]+N[0][0]*N[1][2]*t2[1]-N[0][0]*N[1][1]*t2[2]-N[0][2]*N[0][1]*t2[1]+N[1][1]*N[0][2]*t2[0]) * frac; - trans[5] += - meanOutput.Y + meanInput.Y; + t2[0] += inputs[i].X*outputs[i].Y; + t2[1] += inputs[i].Y*outputs[i].Y; + t2[2] += -outputs[i].Y; + } + double[] trans = new double[7]; + // Solve equation N = transpose(B)*t1 + double frac = 1/(-N[0][0]*N[1][1]*N[2][2] + N[0][0]*Math.Pow(N[1][2], 2) + Math.Pow(N[0][1], 2)*N[2][2] - 2*N[1][2]*N[0][1]*N[0][2] + N[1][1]*Math.Pow(N[0][2], 2)); + trans[0] = (-N[0][1]*N[1][2]*t1[2] + N[0][1]*t1[1]*N[2][2] - N[0][2]*N[1][2]*t1[1] + N[0][2]*N[1][1]*t1[2] - t1[0]*N[1][1]*N[2][2] + t1[0]*Math.Pow(N[1][2], 2))*frac; + trans[1] = (-N[0][1]*N[0][2]*t1[2] + N[0][1]*t1[0]*N[2][2] + N[0][0]*N[1][2]*t1[2] - N[0][0]*t1[1]*N[2][2] - N[0][2]*N[1][2]*t1[0] + Math.Pow(N[0][2], 2)*t1[1])*frac; + trans[2] = -(-N[1][2]*N[0][1]*t1[0] + Math.Pow(N[0][1], 2)*t1[2] + N[0][0]*N[1][2]*t1[1] - N[0][0]*N[1][1]*t1[2] - N[0][2]*N[0][1]*t1[1] + N[1][1]*N[0][2]*t1[0])*frac; + trans[2] += -meanOutput.X + meanInput.X; + // Solve equation N = transpose(B)*t2 + trans[3] = (-N[0][1]*N[1][2]*t2[2] + N[0][1]*t2[1]*N[2][2] - N[0][2]*N[1][2]*t2[1] + N[0][2]*N[1][1]*t2[2] - t2[0]*N[1][1]*N[2][2] + t2[0]*Math.Pow(N[1][2], 2))*frac; + trans[4] = (-N[0][1]*N[0][2]*t2[2] + N[0][1]*t2[0]*N[2][2] + N[0][0]*N[1][2]*t2[2] - N[0][0]*t2[1]*N[2][2] - N[0][2]*N[1][2]*t2[0] + Math.Pow(N[0][2], 2)*t2[1])*frac; + trans[5] = -(-N[1][2]*N[0][1]*t2[0] + Math.Pow(N[0][1], 2)*t2[2] + N[0][0]*N[1][2]*t2[1] - N[0][0]*N[1][1]*t2[2] - N[0][2]*N[0][1]*t2[1] + N[1][1]*N[0][2]*t2[0])*frac; + trans[5] += -meanOutput.Y + meanInput.Y; - //Restore values - for (int i = 0; i < inputs.Count; i++) - { - inputs[i].X += meanInput.X; - inputs[i].Y += meanInput.Y; - outputs[i].X += meanOutput.X; - outputs[i].Y += meanOutput.Y; - } + //Restore values + for (int i = 0; i < inputs.Count; i++) + { + inputs[i].X += meanInput.X; + inputs[i].Y += meanInput.Y; + outputs[i].X += meanOutput.X; + outputs[i].Y += meanOutput.Y; + } - //Calculate s0 - double s0=0; - for (int i = 0; i < inputs.Count; i++) - { - double x = inputs[i].X * trans[0] + inputs[i].Y * trans[1] + trans[2]; - double y = inputs[i].X * trans[3] + inputs[i].Y * trans[4] + trans[5]; - s0 += Math.Pow(x-outputs[i].X,2) + Math.Pow(y-outputs[i].Y,2); - } - trans[6] = Math.Sqrt(s0) / (inputs.Count); - return trans; - } + //Calculate s0 + double s0 = 0; + for (int i = 0; i < inputs.Count; i++) + { + double x = inputs[i].X*trans[0] + inputs[i].Y*trans[1] + trans[2]; + double y = inputs[i].X*trans[3] + inputs[i].Y*trans[4] + trans[5]; + s0 += Math.Pow(x - outputs[i].X, 2) + Math.Pow(y - outputs[i].Y, 2); + } + trans[6] = Math.Sqrt(s0)/(inputs.Count); + return trans; + } - /// - /// Calculates the four helmert transformation parameters {a,b,c,d} and the sum of the squares of the residuals (s0) - /// - /// - /// - /// a,b defines scale vector 1 of coordinate system, d,e scale vector 2. - /// c,f defines offset. - /// - /// - /// Converting from input (X,Y) to output coordinate system (X',Y') is done by: - /// X' = a*X + b*Y + c, Y' = -b*X + a*Y + d - /// - /// This is a transformation initially based on the affine transformation but slightly simpler. - /// - /// Array with the four transformation parameters, and sum of squared residuals: a,b,c,d,s0 - public double[] GetHelmertTransformation() - { - if (inputs.Count < 2) - throw(new System.Exception("At least 2 measurements required to calculate helmert transformation")); + /// + /// Calculates the four helmert transformation parameters {a,b,c,d} and the sum of the squares of the residuals (s0) + /// + /// + /// + /// a,b defines scale vector 1 of coordinate system, d,e scale vector 2. + /// c,f defines offset. + /// + /// + /// Converting from input (X,Y) to output coordinate system (X',Y') is done by: + /// X' = a*X + b*Y + c, Y' = -b*X + a*Y + d + /// + /// This is a transformation initially based on the affine transformation but slightly simpler. + /// + /// Array with the four transformation parameters, and sum of squared residuals: a,b,c,d,s0 + public double[] GetHelmertTransformation() + { + if (inputs.Count < 2) + { + throw (new Exception("At least 2 measurements required to calculate helmert transformation")); + } - //double precision isn't always enough. Lets subtract some mean values and add them later again: - //Find approximate center values: - IPoint meanInput = GeometryFactory.CreatePoint(0, 0); - IPoint meanOutput = GeometryFactory.CreatePoint(0, 0); - for (int i = 0; i < inputs.Count; i++) - { - meanInput.X += inputs[i].X; - meanInput.Y += inputs[i].Y; - meanOutput.X += outputs[i].X; - meanOutput.Y += outputs[i].Y; - } - meanInput.X = Math.Round(meanInput.X / inputs.Count); - meanInput.Y = Math.Round(meanInput.Y / inputs.Count); - meanOutput.X = Math.Round(meanOutput.X / inputs.Count); - meanOutput.Y = Math.Round(meanOutput.Y / inputs.Count); + //double precision isn't always enough. Lets subtract some mean values and add them later again: + //Find approximate center values: + IPoint meanInput = GeometryFactory.CreatePoint(0, 0); + IPoint meanOutput = GeometryFactory.CreatePoint(0, 0); + for (int i = 0; i < inputs.Count; i++) + { + meanInput.X += inputs[i].X; + meanInput.Y += inputs[i].Y; + meanOutput.X += outputs[i].X; + meanOutput.Y += outputs[i].Y; + } + meanInput.X = Math.Round(meanInput.X/inputs.Count); + meanInput.Y = Math.Round(meanInput.Y/inputs.Count); + meanOutput.X = Math.Round(meanOutput.X/inputs.Count); + meanOutput.Y = Math.Round(meanOutput.Y/inputs.Count); - double b00=0; - double b02=0; - double b03=0; - double[] t = new double[4]; - for (int i = 0; i < inputs.Count; i++) - { - //Subtract mean values - inputs[i].X -= meanInput.X; - inputs[i].Y -= meanInput.Y; - outputs[i].X -= meanOutput.X; - outputs[i].Y -= meanOutput.Y; - //Calculate summed values - b00 += Math.Pow(inputs[i].X,2) + Math.Pow(inputs[i].Y,2); - b02 -= inputs[i].X; - b03 -= inputs[i].Y; - t[0] += -(inputs[i].X*outputs[i].X) - (inputs[i].Y*outputs[i].Y); - t[1] += -(inputs[i].Y*outputs[i].X) + (inputs[i].X*outputs[i].Y); - t[2] += outputs[i].X; - t[3] += outputs[i].Y; - } - double frac = 1 / (-inputs.Count * b00 + Math.Pow(b02, 2) + Math.Pow(b03, 2)); - double[] result = new double[5]; - result[0] = (-inputs.Count * t[0] + b02 * t[2] + b03 * t[3]) * frac; - result[1] = (-inputs.Count * t[1] + b03 * t[2] - b02 * t[3]) * frac; - result[2] = (b02*t[0]+b03*t[1]-t[2]*b00) * frac + meanOutput.X; - result[3] = (b03*t[0]-b02*t[1]-t[3]*b00) * frac + meanOutput.Y; + double b00 = 0; + double b02 = 0; + double b03 = 0; + double[] t = new double[4]; + for (int i = 0; i < inputs.Count; i++) + { + //Subtract mean values + inputs[i].X -= meanInput.X; + inputs[i].Y -= meanInput.Y; + outputs[i].X -= meanOutput.X; + outputs[i].Y -= meanOutput.Y; + //Calculate summed values + b00 += Math.Pow(inputs[i].X, 2) + Math.Pow(inputs[i].Y, 2); + b02 -= inputs[i].X; + b03 -= inputs[i].Y; + t[0] += -(inputs[i].X*outputs[i].X) - (inputs[i].Y*outputs[i].Y); + t[1] += -(inputs[i].Y*outputs[i].X) + (inputs[i].X*outputs[i].Y); + t[2] += outputs[i].X; + t[3] += outputs[i].Y; + } + double frac = 1/(-inputs.Count*b00 + Math.Pow(b02, 2) + Math.Pow(b03, 2)); + double[] result = new double[5]; + result[0] = (-inputs.Count*t[0] + b02*t[2] + b03*t[3])*frac; + result[1] = (-inputs.Count*t[1] + b03*t[2] - b02*t[3])*frac; + result[2] = (b02*t[0] + b03*t[1] - t[2]*b00)*frac + meanOutput.X; + result[3] = (b03*t[0] - b02*t[1] - t[3]*b00)*frac + meanOutput.Y; - //Restore values - for (int i = 0; i < inputs.Count; i++) - { - inputs[i].X += meanInput.X; - inputs[i].Y += meanInput.Y; - outputs[i].X += meanOutput.X; - outputs[i].Y += meanOutput.Y; - } + //Restore values + for (int i = 0; i < inputs.Count; i++) + { + inputs[i].X += meanInput.X; + inputs[i].Y += meanInput.Y; + outputs[i].X += meanOutput.X; + outputs[i].Y += meanOutput.Y; + } - //Calculate s0 - double s0=0; - for (int i = 0; i < inputs.Count; i++) - { - double x = inputs[i].X * result[0] + inputs[i].Y * result[1] + result[2]; - double y = -inputs[i].X * result[1] + inputs[i].Y * result[0] + result[3]; - s0 += Math.Pow(x-outputs[i].X,2) + Math.Pow(y-outputs[i].Y,2); - } - result[4] = Math.Sqrt(s0) / (inputs.Count); - return result; - } + //Calculate s0 + double s0 = 0; + for (int i = 0; i < inputs.Count; i++) + { + double x = inputs[i].X*result[0] + inputs[i].Y*result[1] + result[2]; + double y = -inputs[i].X*result[1] + inputs[i].Y*result[0] + result[3]; + s0 += Math.Pow(x - outputs[i].X, 2) + Math.Pow(y - outputs[i].Y, 2); + } + result[4] = Math.Sqrt(s0)/(inputs.Count); + return result; + } - /// - /// Creates an n x m matrix of doubles - /// - /// width of matrix - /// height of matrix - /// n*m matrix - private double[][] CreateMatrix(int n, int m) - { - double[][] N = new double[n][]; - for(int i=0;i + /// Creates an n x m matrix of doubles + /// + /// width of matrix + /// height of matrix + /// n*m matrix + private double[][] CreateMatrix(int n, int m) + { + double[][] N = new double[n][]; + for (int i = 0; i < n; i++) + { + N[i] = new double[m]; + } + return N; + } + } +} \ No newline at end of file