/Branches/v1.0/SharpMap/Utilities/LeastSquaresTransform.cs
C# | 317 lines | 189 code | 25 blank | 103 comment | 12 complexity | fb8181d9140abbf60b07dbb507eb5559 MD5 | raw file
Possible License(s): LGPL-2.1
- // Copyright 2005, 2006 - Morten Nielsen (www.iter.dk)
- //
- // This file is part of SharpMap.
- // SharpMap is free software; you can redistribute it and/or modify
- // it under the terms of the GNU Lesser General Public License as published by
- // the Free Software Foundation; either version 2 of the License, or
- // (at your option) any later version.
- //
- // SharpMap 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 Lesser General Public License for more details.
-
- // You should have received a copy of the GNU Lesser General Public License
- // along with SharpMap; if not, write to the Free Software
- // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
-
- using System;
- using System.Collections.Generic;
- using SharpMap.Geometries;
-
- namespace SharpMap.Utilities
- {
- using GeoAPI.Geometries;
-
- /// <summary>
- /// Calculates Affine and Helmert transformation using Least-Squares Regression of input and output points
- /// </summary>
- public class LeastSquaresTransform
- {
- private readonly List<Coordinate> inputs;
- private readonly List<Coordinate> outputs;
-
- /// <summary>
- /// Initialize Least Squares transformations
- /// </summary>
- public LeastSquaresTransform()
- {
- inputs = new List<Coordinate>();
- outputs = new List<Coordinate>();
- }
-
- /// <summary>
- /// Adds an input and output value pair to the collection
- /// </summary>
- /// <param name="input"></param>
- /// <param name="output"></param>
- public void AddInputOutputPoint(Coordinate input, Coordinate output)
- {
- inputs.Add(input);
- outputs.Add(output);
- }
-
- /// <summary>
- /// Removes input and output value pair at the specified index
- /// </summary>
- /// <param name="i"></param>
- public void RemoveInputOutputPointAt(int i)
- {
- inputs.RemoveAt(i);
- outputs.RemoveAt(i);
- }
-
- /// <summary>
- /// Gets the input point value at the specified index
- /// </summary>
- /// <param name="i">index</param>
- /// <returns>Input point value a index 'i'</returns>
- public Coordinate GetInputPoint(int i)
- {
- return inputs[i];
- }
-
- /// <summary>
- /// Sets the input point value at the specified index
- /// </summary>
- /// <param name="p">Point value</param>
- /// <param name="i">index</param>
- public void SetInputPointAt(Coordinate p, int i)
- {
- inputs[i] = p;
- }
-
- /// <summary>
- /// Gets the output point value at the specified index
- /// </summary>
- /// <param name="i">index</param>
- /// <returns>Output point value a index 'i'</returns>
- public Coordinate GetOutputPoint(int i)
- {
- return outputs[i];
- }
-
- /// <summary>
- /// Sets the output point value at the specified index
- /// </summary>
- /// <param name="p">Point value</param>
- /// <param name="i">index</param>
- public void SetOutputPointAt(Coordinate p, int i)
- {
- outputs[i] = p;
- }
-
- /// <summary>
- /// 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)
- /// </summary>
- /// <remarks>
- /// a,b defines scale vector 1 of coordinate system, d,e scale vector 2. c,f defines offset.
- /// <para>
- /// 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
- /// </para>
- /// <para>
- /// Transformation based on Mikhail "Introduction to Modern Photogrammetry" p. 399-300.
- /// Extended to arbitrary number of measurements by M. Nielsen
- /// </para>
- /// </remarks>
- /// <returns>Array with the six transformation parameters and sum of squared residuals: a,b,c,d,e,f,s0</returns>
- 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:
- Coordinate meanInput = new Coordinate(0, 0);
- Coordinate meanOutput = new Coordinate(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[] 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;
-
- 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;
- }
-
- //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;
- }
-
- /// <summary>
- /// Calculates the four helmert transformation parameters {a,b,c,d} and the sum of the squares of the residuals (s0)
- /// </summary>
- /// <remarks>
- /// <para>
- /// a,b defines scale vector 1 of coordinate system, d,e scale vector 2.
- /// c,f defines offset.
- /// </para>
- /// <para>
- /// 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
- /// </para>
- /// <para>This is a transformation initially based on the affine transformation but slightly simpler.</para>
- /// </remarks>
- /// <returns>Array with the four transformation parameters, and sum of squared residuals: a,b,c,d,s0</returns>
- 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:
- Coordinate meanInput = new Coordinate(0, 0);
- Coordinate meanOutput = new Coordinate(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;
-
- //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;
- }
-
- /// <summary>
- /// Creates an n x m matrix of doubles
- /// </summary>
- /// <param name="n">width of matrix</param>
- /// <param name="m">height of matrix</param>
- /// <returns>n*m matrix</returns>
- 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;
- }
- }
- }