| | 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 | | } |