| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.IO; |
| | | 4 | | using System.Xml.Serialization; |
| | | 5 | | using MathNet.Numerics.LinearAlgebra; |
| | | 6 | | |
| | | 7 | | namespace ReFlex.Core.Calibration.Components |
| | | 8 | | { |
| | | 9 | | public class Calibrator |
| | | 10 | | { |
| | | 11 | | public const string DefaultCalibrationFile = "DefaultCalibrationFile.xml"; |
| | | 12 | | |
| | | 13 | | #region Fields |
| | | 14 | | |
| | | 15 | | private int _width; |
| | | 16 | | private int _height; |
| | | 17 | | private int _startX; |
| | | 18 | | private int _startY; |
| | | 19 | | |
| | 0 | 20 | | private readonly float[,] _unitMat4X4 = { |
| | 0 | 21 | | { 1.0f, 0.0f, 0.0f, 0.0f }, |
| | 0 | 22 | | { 0.0f, 1.0f, 0.0f, 0.0f }, |
| | 0 | 23 | | { 0.0f, 0.0f, 1.0f, 0.0f }, |
| | 0 | 24 | | { 0.0f, 0.0f, 0.0f, 1.0f } |
| | 0 | 25 | | }; |
| | | 26 | | |
| | | 27 | | private Matrix<float> _calibrationMatrix; |
| | | 28 | | private readonly Matrix<float> _unitMatrix; |
| | | 29 | | |
| | | 30 | | #endregion |
| | | 31 | | |
| | | 32 | | #region Properties |
| | | 33 | | |
| | 0 | 34 | | public Matrix<float> CalibrationMatrix => _calibrationMatrix ?? _unitMatrix; |
| | | 35 | | |
| | 0 | 36 | | public Util.Calibration CalibrationValues { get; private set; } = new Util.Calibration |
| | 0 | 37 | | { |
| | 0 | 38 | | SourceValues = new List<int[]>(), |
| | 0 | 39 | | TargetValues = new List<int[]>(), |
| | 0 | 40 | | UpperThreshold = 1010, |
| | 0 | 41 | | LowerThreshold = 960, |
| | 0 | 42 | | LastUpdated = new List<DateTime>() |
| | 0 | 43 | | }; |
| | | 44 | | |
| | 0 | 45 | | public int CalibrationStage { get; private set; } |
| | | 46 | | |
| | | 47 | | public int Width |
| | | 48 | | { |
| | 0 | 49 | | get => _width; |
| | | 50 | | private set |
| | 0 | 51 | | { |
| | 0 | 52 | | if (_width == value) |
| | 0 | 53 | | return; |
| | 0 | 54 | | _width = value; |
| | 0 | 55 | | UpdateSource(); |
| | 0 | 56 | | } |
| | | 57 | | } |
| | | 58 | | |
| | | 59 | | public int Height |
| | | 60 | | { |
| | 0 | 61 | | get => _height; |
| | | 62 | | private set |
| | 0 | 63 | | { |
| | 0 | 64 | | if (_height == value) |
| | 0 | 65 | | return; |
| | 0 | 66 | | _height = value; |
| | 0 | 67 | | UpdateSource(); |
| | 0 | 68 | | } |
| | | 69 | | } |
| | | 70 | | |
| | | 71 | | public int StartX |
| | | 72 | | { |
| | 0 | 73 | | get => _startX; |
| | 0 | 74 | | private set { |
| | 0 | 75 | | if (_startX == value) |
| | 0 | 76 | | return; |
| | 0 | 77 | | _startX = value; |
| | 0 | 78 | | UpdateSource(); |
| | 0 | 79 | | } |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | public int StartY |
| | | 83 | | { |
| | 0 | 84 | | get => _startY; |
| | | 85 | | private set |
| | 0 | 86 | | { |
| | 0 | 87 | | if (_startY == value) |
| | 0 | 88 | | return; |
| | 0 | 89 | | _startY = value; |
| | 0 | 90 | | UpdateSource(); |
| | 0 | 91 | | } |
| | | 92 | | } |
| | | 93 | | |
| | | 94 | | #endregion |
| | | 95 | | |
| | | 96 | | #region Events |
| | | 97 | | |
| | | 98 | | public EventHandler<Matrix<float>> CalibrationUpdated; |
| | | 99 | | public EventHandler<Matrix<float>> CalibrationFinished; |
| | | 100 | | public EventHandler<Matrix<float>> CalibrationLoaded; |
| | | 101 | | |
| | | 102 | | |
| | | 103 | | #endregion |
| | | 104 | | |
| | | 105 | | #region Constructor |
| | | 106 | | |
| | 0 | 107 | | public Calibrator() : this(500, 500, 10, 100) |
| | 0 | 108 | | { |
| | | 109 | | |
| | 0 | 110 | | } |
| | | 111 | | |
| | 0 | 112 | | public Calibrator(int width, int height, int startX, int startY) |
| | 0 | 113 | | { |
| | 0 | 114 | | _width = width; |
| | 0 | 115 | | _height = height; |
| | 0 | 116 | | _startX = startX; |
| | 0 | 117 | | _startY = startY; |
| | | 118 | | |
| | 0 | 119 | | _unitMatrix = Matrix<float>.Build.DenseOfArray(_unitMat4X4); |
| | | 120 | | |
| | 0 | 121 | | UpdateSource(); |
| | 0 | 122 | | } |
| | | 123 | | |
| | | 124 | | #endregion |
| | | 125 | | |
| | | 126 | | #region Public Methods |
| | | 127 | | |
| | | 128 | | public void SetFrameSize(int width, int height, int top, int left) |
| | 0 | 129 | | { |
| | 0 | 130 | | _startX = left; |
| | 0 | 131 | | _startY = top; |
| | 0 | 132 | | _width = width; |
| | 0 | 133 | | _height = height; |
| | | 134 | | |
| | 0 | 135 | | UpdateSource(); |
| | 0 | 136 | | } |
| | | 137 | | |
| | | 138 | | public void UpdateSource() |
| | 0 | 139 | | { |
| | 0 | 140 | | int[] init1 = { Width / 8 * 5 + StartX, Height / 3 + StartY, -1 }; |
| | 0 | 141 | | int[] init2 = { Width / 8 * 5 + StartX, Height / 3 * 2 + StartY, -1 }; |
| | 0 | 142 | | int[] init3 = { Width / 16 * 5 + StartX, Height / 3 * 2 + StartY, -1 }; |
| | | 143 | | |
| | 0 | 144 | | CalibrationValues.SourceValues.Clear(); |
| | | 145 | | |
| | 0 | 146 | | CalibrationValues.SourceValues.Add(init1); |
| | 0 | 147 | | CalibrationValues.SourceValues.Add(init2); |
| | 0 | 148 | | CalibrationValues.SourceValues.Add(init3); |
| | | 149 | | |
| | 0 | 150 | | if (CalibrationValues.TargetValues.Count == 0) |
| | 0 | 151 | | { |
| | 0 | 152 | | CalibrationValues.TargetValues.Add(init1); |
| | 0 | 153 | | CalibrationValues.TargetValues.Add(init2); |
| | 0 | 154 | | CalibrationValues.TargetValues.Add(init3); |
| | | 155 | | |
| | 0 | 156 | | CalibrationValues.LastUpdated.Add(DateTime.Now); |
| | 0 | 157 | | CalibrationValues.LastUpdated.Add(DateTime.Now); |
| | 0 | 158 | | CalibrationValues.LastUpdated.Add(DateTime.Now); |
| | 0 | 159 | | } |
| | | 160 | | |
| | 0 | 161 | | CalibrationUpdated?.Invoke(this, CalibrationMatrix); |
| | 0 | 162 | | } |
| | | 163 | | |
| | | 164 | | public void UpdateTargetValue(int idx, int x, int y, int id) |
| | 0 | 165 | | { |
| | 0 | 166 | | if (CalibrationValues.TargetValues.Count <= idx) |
| | 0 | 167 | | return; |
| | | 168 | | |
| | 0 | 169 | | CalibrationValues.TargetValues[idx] = new[] { x, y, id }; |
| | | 170 | | |
| | 0 | 171 | | if (CalibrationValues.LastUpdated.Count > idx) |
| | 0 | 172 | | { |
| | 0 | 173 | | var lastUpdate = CalibrationValues.LastUpdated[idx]; |
| | | 174 | | |
| | 0 | 175 | | if (DateTime.Now - lastUpdate < TimeSpan.FromMilliseconds(500)) |
| | 0 | 176 | | return; |
| | 0 | 177 | | } |
| | | 178 | | |
| | 0 | 179 | | if (CalibrationValues.TargetValues.Count == 3) |
| | 0 | 180 | | { |
| | 0 | 181 | | _calibrationMatrix = GenerateMappingMatrix(); |
| | 0 | 182 | | if (CalibrationValues.LastUpdated.Count > idx) |
| | 0 | 183 | | CalibrationValues.LastUpdated[idx] = DateTime.Now; |
| | 0 | 184 | | Save(); |
| | 0 | 185 | | CalibrationFinished?.Invoke(this, CalibrationMatrix); |
| | 0 | 186 | | } |
| | 0 | 187 | | } |
| | | 188 | | |
| | | 189 | | public void AddTargetValue(int x, int y, int id) |
| | 0 | 190 | | { |
| | 0 | 191 | | if (CalibrationStage >= 0 && CalibrationStage < 3) |
| | 0 | 192 | | { |
| | 0 | 193 | | CalibrationValues.TargetValues.Add(new[] { x, y, id }); |
| | 0 | 194 | | CalibrationValues.LastUpdated.Add(DateTime.Now); |
| | 0 | 195 | | CalibrationStage++; |
| | 0 | 196 | | CalibrationUpdated?.Invoke(this, CalibrationMatrix); |
| | 0 | 197 | | } |
| | | 198 | | |
| | 0 | 199 | | if (CalibrationStage != 3) |
| | 0 | 200 | | return; |
| | | 201 | | |
| | 0 | 202 | | ComputeTransformation(); |
| | 0 | 203 | | CalibrationStage++; |
| | 0 | 204 | | Save(); |
| | 0 | 205 | | } |
| | | 206 | | |
| | | 207 | | public void ComputeTransformation() |
| | 0 | 208 | | { |
| | 0 | 209 | | _calibrationMatrix = GenerateMappingMatrix(); |
| | | 210 | | |
| | 0 | 211 | | ValidateCalibrationMatrix(); |
| | | 212 | | |
| | 0 | 213 | | CalibrationFinished?.Invoke(this, CalibrationMatrix); |
| | 0 | 214 | | } |
| | | 215 | | |
| | | 216 | | public void Reset() |
| | 0 | 217 | | { |
| | 0 | 218 | | _calibrationMatrix = null; |
| | 0 | 219 | | CalibrationStage = 0; |
| | 0 | 220 | | CalibrationValues.TargetValues.Clear(); |
| | 0 | 221 | | CalibrationValues.LastUpdated.Clear(); |
| | 0 | 222 | | CalibrationUpdated?.Invoke(this, CalibrationMatrix); |
| | 0 | 223 | | } |
| | | 224 | | |
| | | 225 | | [Obsolete] |
| | | 226 | | public void Load(string xmlSerializedText) |
| | 0 | 227 | | { |
| | 0 | 228 | | var ser = new XmlSerializer(typeof(Util.Calibration)); |
| | 0 | 229 | | using (var text = new StringReader(xmlSerializedText)) |
| | 0 | 230 | | { |
| | 0 | 231 | | var result = (Util.Calibration)ser.Deserialize(text); |
| | 0 | 232 | | SetCalibrationValues(result); |
| | 0 | 233 | | } |
| | 0 | 234 | | } |
| | | 235 | | |
| | | 236 | | public void Load(Util.Calibration calibration) |
| | 0 | 237 | | { |
| | 0 | 238 | | SetCalibrationValues(calibration); |
| | 0 | 239 | | } |
| | | 240 | | |
| | | 241 | | private void SetCalibrationValues(Util.Calibration calibration) |
| | 0 | 242 | | { |
| | | 243 | | try |
| | 0 | 244 | | { |
| | | 245 | | |
| | 0 | 246 | | CalibrationValues = new Util.Calibration |
| | 0 | 247 | | { |
| | 0 | 248 | | LastUpdated = calibration.LastUpdated ?? new List<DateTime>(), |
| | 0 | 249 | | UpperThreshold = calibration.UpperThreshold, |
| | 0 | 250 | | LowerThreshold = calibration.LowerThreshold, |
| | 0 | 251 | | SourceValues = CalibrationValues.SourceValues ?? calibration.SourceValues, |
| | 0 | 252 | | TargetValues = calibration.TargetValues ?? new List<int[]>() |
| | 0 | 253 | | }; |
| | | 254 | | |
| | 0 | 255 | | _calibrationMatrix = GenerateMappingMatrix(); |
| | 0 | 256 | | CalibrationLoaded?.Invoke(this, CalibrationMatrix); |
| | | 257 | | |
| | 0 | 258 | | } |
| | 0 | 259 | | catch (Exception exc) |
| | 0 | 260 | | { |
| | 0 | 261 | | Console.Write(exc.Message); |
| | 0 | 262 | | } |
| | 0 | 263 | | } |
| | | 264 | | |
| | | 265 | | [Obsolete] |
| | | 266 | | public string Save(string fileName = DefaultCalibrationFile) |
| | 0 | 267 | | { |
| | 0 | 268 | | var ser = new XmlSerializer(typeof(Util.Calibration)); |
| | 0 | 269 | | using (var text = new StringWriter()) |
| | 0 | 270 | | { |
| | | 271 | | try |
| | 0 | 272 | | { |
| | 0 | 273 | | ser.Serialize(text, CalibrationValues); |
| | 0 | 274 | | } |
| | 0 | 275 | | catch (Exception exc) |
| | 0 | 276 | | { |
| | 0 | 277 | | Console.Write(exc.Message); |
| | 0 | 278 | | } |
| | | 279 | | |
| | 0 | 280 | | return text.ToString(); |
| | | 281 | | } |
| | 0 | 282 | | } |
| | | 283 | | |
| | | 284 | | #endregion |
| | | 285 | | |
| | | 286 | | #region Auxiliary Methods |
| | | 287 | | |
| | | 288 | | private Matrix<float> GenerateMappingMatrix() |
| | 0 | 289 | | { |
| | 0 | 290 | | if (CalibrationValues.SourceValues == null |
| | 0 | 291 | | || CalibrationValues.SourceValues.Count != 3 |
| | 0 | 292 | | || CalibrationValues.TargetValues == null |
| | 0 | 293 | | || CalibrationValues.TargetValues.Count != 3) |
| | 0 | 294 | | return _unitMatrix; |
| | | 295 | | |
| | 0 | 296 | | float[,] oPoints = { |
| | 0 | 297 | | { CalibrationValues.SourceValues[0][0], CalibrationValues.SourceValues[1][0], CalibrationValues.SourceVa |
| | 0 | 298 | | { CalibrationValues.SourceValues[0][1], CalibrationValues.SourceValues[1][1], CalibrationValues.SourceVa |
| | 0 | 299 | | { 1.0f, 1.0f, 1.0f, 0.0f }, |
| | 0 | 300 | | { 0.0f, 0.0f, 0.0f, 1.0f } |
| | 0 | 301 | | }; |
| | | 302 | | |
| | 0 | 303 | | float[,] nPoints = { |
| | 0 | 304 | | { CalibrationValues.TargetValues[0][0], CalibrationValues.TargetValues[1][0], CalibrationValues.TargetVa |
| | 0 | 305 | | { CalibrationValues.TargetValues[0][1], CalibrationValues.TargetValues[1][1], CalibrationValues.TargetVa |
| | 0 | 306 | | { 1.0f, 1.0f, 1.0f, 0.0f }, |
| | 0 | 307 | | { 0.0f, 0.0f, 0.0f, 1.0f } |
| | 0 | 308 | | }; |
| | | 309 | | |
| | 0 | 310 | | var oPointsMat = Matrix<float>.Build.DenseOfArray(oPoints); |
| | 0 | 311 | | var nPointsMat = Matrix<float>.Build.DenseOfArray(nPoints); |
| | | 312 | | |
| | 0 | 313 | | return nPointsMat.Multiply(oPointsMat.Inverse()).Inverse(); |
| | 0 | 314 | | } |
| | | 315 | | |
| | | 316 | | private void ValidateCalibrationMatrix() |
| | 0 | 317 | | { |
| | 0 | 318 | | var isValid = _calibrationMatrix.ForAll(val => !float.IsInfinity(val) && !float.IsNaN(val) && val >= float.M |
| | 0 | 319 | | if (!isValid) |
| | 0 | 320 | | _calibrationMatrix = _unitMatrix; |
| | 0 | 321 | | } |
| | | 322 | | |
| | | 323 | | #endregion |
| | | 324 | | } |
| | | 325 | | |
| | | 326 | | } |