| | 1 | | using ReFlex.Core.Common.Components; |
| | 2 | | using ReFlex.Core.Common.Interfaces; |
| | 3 | | using ReFlex.Core.Common.Util; |
| | 4 | | using ReFlex.Core.Interactivity.Components; |
| | 5 | | using ReFlex.Core.Interactivity.Interfaces; |
| | 6 | | using ReFlex.Core.Interactivity.Util; |
| | 7 | | using Math = System.Math; |
| | 8 | |
|
| | 9 | | namespace PointCloud.Benchmark.Interactivity |
| | 10 | | { |
| | 11 | | public abstract class InteractionObserverBase : IInteractionObserver, IPerformanceReporter |
| | 12 | | { |
| | 13 | | #region Fields |
| | 14 | |
|
| | 15 | | private readonly InteractionSmoothingBehaviour _smoothingBehaviour; |
| 0 | 16 | | private int _numSmoothingFrames = 5; |
| 0 | 17 | | private int _historySize = 25; |
| 0 | 18 | | private int _maxNumEmptyFramesBetween = 3; |
| 0 | 19 | | private float _touchMergeDistance2d = 64f; |
| 0 | 20 | | private float _depthScale = 100.0f; |
| | 21 | | private FilterType _filterType; |
| | 22 | | private float _maxConfidence; |
| | 23 | | private int _extremumTypeCheckRadius; |
| | 24 | | private int _extremumTypeCheckNumSamples; |
| | 25 | |
|
| 0 | 26 | | protected Tuple<int, int>[] _fixedSamples = Array.Empty<Tuple<int, int>>(); |
| 0 | 27 | | protected Tuple<int, int>[] _stochasticSamples = Array.Empty<Tuple<int, int>>(); |
| | 28 | |
|
| | 29 | | #endregion |
| | 30 | |
|
| | 31 | | #region Properties |
| | 32 | |
|
| | 33 | | /// <summary> |
| | 34 | | /// Gets or sets the distance. |
| | 35 | | /// </summary> |
| | 36 | | /// <value> |
| | 37 | | /// The distance. |
| | 38 | | /// </value> |
| 0 | 39 | | public float Distance { get; set; } |
| | 40 | |
|
| | 41 | | /// <summary> |
| | 42 | | /// Gets or sets the minimum depth. |
| | 43 | | /// </summary> |
| | 44 | | /// <value> |
| | 45 | | /// The maximum depth. |
| | 46 | | /// </value> |
| 0 | 47 | | public float MinDistance { get; set; } |
| | 48 | |
|
| | 49 | | /// <summary> |
| | 50 | | /// Gets or sets the maximum depth. |
| | 51 | | /// </summary> |
| | 52 | | /// <value> |
| | 53 | | /// The maximum depth. |
| | 54 | | /// </value> |
| 0 | 55 | | public float MaxDistance { get; set; } |
| | 56 | |
|
| | 57 | | /// <summary> |
| | 58 | | /// defines how many points should be checked to determine the type of the extremum |
| | 59 | | /// </summary> |
| | 60 | | public int ExtremumTypeCheckNumSamples |
| | 61 | | { |
| 0 | 62 | | get => _extremumTypeCheckNumSamples; |
| | 63 | | set |
| 0 | 64 | | { |
| 0 | 65 | | if (_extremumTypeCheckNumSamples == value) |
| 0 | 66 | | return; |
| 0 | 67 | | _extremumTypeCheckNumSamples = value; |
| 0 | 68 | | UpdateSamples(); |
| 0 | 69 | | } |
| | 70 | | } |
| | 71 | |
|
| | 72 | | /// <summary> |
| | 73 | | /// The pixel radius in which the points are sampled for deriving the type of the local extremum |
| | 74 | | /// </summary> |
| | 75 | | public int ExtremumTypeCheckRadius |
| | 76 | | { |
| 0 | 77 | | get => _extremumTypeCheckRadius; |
| | 78 | | set |
| 0 | 79 | | { |
| 0 | 80 | | if (_extremumTypeCheckRadius == value) |
| 0 | 81 | | return; |
| 0 | 82 | | _extremumTypeCheckRadius = value; |
| 0 | 83 | | UpdateSamples(); |
| 0 | 84 | | } |
| | 85 | | } |
| | 86 | |
|
| | 87 | | /// <summary> |
| | 88 | | /// Which ratio of "correctly aligned" points should match to distinguish between Undefined or Minimum/Maximum ? |
| | 89 | | /// </summary> |
| 0 | 90 | | public float ExtremumTypeCheckFittingPercentage { get; set; } |
| | 91 | |
|
| | 92 | | /// <summary> |
| | 93 | | /// Defines how neighboring the points are sampled for determining the type of extremum. |
| | 94 | | /// </summary> |
| 0 | 95 | | public ExtremumTypeCheckMethod ExtremumTypeCheckMethod { get; set; } |
| | 96 | |
|
| | 97 | | public int InteractionHistorySize |
| | 98 | | { |
| 0 | 99 | | get => _historySize; |
| | 100 | | set |
| 0 | 101 | | { |
| 0 | 102 | | _historySize = value; |
| 0 | 103 | | if (_smoothingBehaviour != null) |
| 0 | 104 | | { |
| 0 | 105 | | _smoothingBehaviour.NumFramesHistory = value; |
| | 106 | | // update depending smoothing/filter values |
| 0 | 107 | | NumSmoothingFrames = _numSmoothingFrames; |
| 0 | 108 | | MaxNumEmptyFramesBetween = _maxNumEmptyFramesBetween; |
| 0 | 109 | | } |
| 0 | 110 | | } |
| | 111 | | } |
| | 112 | |
|
| | 113 | | public int NumSmoothingFrames |
| | 114 | | { |
| 0 | 115 | | get => _numSmoothingFrames; |
| | 116 | | set |
| 0 | 117 | | { |
| 0 | 118 | | _numSmoothingFrames = value; |
| 0 | 119 | | if (_smoothingBehaviour != null) |
| 0 | 120 | | _smoothingBehaviour.NumFramesSmoothing = value < _smoothingBehaviour.NumFramesHistory |
| 0 | 121 | | ? _smoothingBehaviour.NumFramesHistory |
| 0 | 122 | | : value; |
| 0 | 123 | | } |
| | 124 | | } |
| | 125 | |
|
| | 126 | | // TODO: write tests for negative (!) values and values larger than history |
| | 127 | | public int MaxNumEmptyFramesBetween |
| | 128 | | { |
| 0 | 129 | | get => _maxNumEmptyFramesBetween; |
| | 130 | | set |
| 0 | 131 | | { |
| 0 | 132 | | _maxNumEmptyFramesBetween = value; |
| 0 | 133 | | if (_smoothingBehaviour != null) |
| 0 | 134 | | _smoothingBehaviour.MaxNumEmptyFramesBetween = value < _smoothingBehaviour.NumFramesHistory |
| 0 | 135 | | ? _smoothingBehaviour.NumFramesHistory |
| 0 | 136 | | : value; |
| 0 | 137 | | } |
| | 138 | | } |
| | 139 | |
|
| | 140 | | public float TouchMergeDistance2D |
| | 141 | | { |
| 0 | 142 | | get => _touchMergeDistance2d; |
| | 143 | | set |
| 0 | 144 | | { |
| 0 | 145 | | _touchMergeDistance2d = value; |
| 0 | 146 | | if (_smoothingBehaviour != null) |
| 0 | 147 | | _smoothingBehaviour.TouchMergeDistance2D = value; |
| 0 | 148 | | } |
| | 149 | | } |
| | 150 | |
|
| | 151 | | public float DepthScale |
| | 152 | | { |
| 0 | 153 | | get => _depthScale; |
| | 154 | | set |
| 0 | 155 | | { |
| 0 | 156 | | _depthScale = value; |
| 0 | 157 | | if (_smoothingBehaviour != null) |
| 0 | 158 | | _smoothingBehaviour.DepthScale = value; |
| 0 | 159 | | } |
| | 160 | | } |
| | 161 | |
|
| | 162 | | public FilterType FilterType |
| | 163 | | { |
| 0 | 164 | | get => _filterType; |
| | 165 | | set |
| 0 | 166 | | { |
| 0 | 167 | | _filterType = value; |
| 0 | 168 | | if (_smoothingBehaviour != null) |
| 0 | 169 | | _smoothingBehaviour.UpdateFilterType(_filterType); |
| 0 | 170 | | } |
| | 171 | | } |
| | 172 | |
|
| | 173 | |
|
| | 174 | | /// <summary> |
| | 175 | | /// Gets or sets the minimum angle. |
| | 176 | | /// </summary> |
| | 177 | | /// <value> |
| | 178 | | /// The minimum angle. |
| | 179 | | /// </value> |
| 0 | 180 | | public virtual float MinAngle { get; set; } |
| | 181 | |
|
| | 182 | | /// <summary> |
| | 183 | | /// Gets or sets the minimum confidence. |
| | 184 | | /// </summary> |
| | 185 | | /// <value> |
| | 186 | | /// The minimum confidence. |
| | 187 | | /// </value> |
| 0 | 188 | | public virtual float MinConfidence { get; set; } |
| | 189 | |
|
| | 190 | | /// <summary> |
| | 191 | | /// Gets or sets the maximum confidence. |
| | 192 | | /// </summary> |
| | 193 | | /// <value> |
| | 194 | | /// The maximum confidence. |
| | 195 | | /// </value> |
| | 196 | | public virtual float MaxConfidence |
| | 197 | | { |
| 0 | 198 | | get => _maxConfidence; |
| | 199 | | set |
| 0 | 200 | | { |
| 0 | 201 | | _maxConfidence = value; |
| 0 | 202 | | if (_smoothingBehaviour != null) |
| 0 | 203 | | _smoothingBehaviour.MaxConfidence = (int) MaxConfidence; |
| 0 | 204 | | } |
| | 205 | | } |
| | 206 | |
|
| 0 | 207 | | public bool MeasurePerformance { get; set; } |
| | 208 | |
|
| 0 | 209 | | protected int FrameId { get; private set; } |
| | 210 | |
|
| | 211 | | #region Abstract Members |
| | 212 | |
|
| | 213 | | public abstract ObserverType Type { get; } |
| | 214 | | public abstract PointCloud3 PointCloud { get; set; } |
| | 215 | | public abstract VectorField2 VectorField { get; set; } |
| | 216 | |
|
| | 217 | | public abstract event EventHandler<IList<Interaction>> NewInteractions; |
| | 218 | |
|
| | 219 | | public event EventHandler<PerformanceDataItem> PerformanceDataUpdated; |
| | 220 | |
|
| | 221 | | public event EventHandler<IList<InteractionFrame>> InteractionHistoryUpdated; |
| | 222 | |
|
| | 223 | | public abstract Task<ProcessingResult> Update(); |
| | 224 | |
|
| | 225 | | #endregion |
| | 226 | |
|
| | 227 | | #region Constructor |
| | 228 | |
|
| 0 | 229 | | protected InteractionObserverBase() |
| 0 | 230 | | { |
| 0 | 231 | | _smoothingBehaviour = new InteractionSmoothingBehaviour(InteractionHistorySize); |
| 0 | 232 | | } |
| | 233 | |
|
| | 234 | | #endregion |
| | 235 | |
|
| | 236 | | public void UpdateFrameId(int frameId) |
| 0 | 237 | | { |
| 0 | 238 | | FrameId = frameId; |
| 0 | 239 | | } |
| | 240 | |
|
| | 241 | | #endregion |
| | 242 | |
|
| | 243 | | protected InteractionFrame ComputeSmoothingValue(IList<Interaction> rawInteractions) |
| 0 | 244 | | { |
| 0 | 245 | | var result = _smoothingBehaviour.Update(rawInteractions); |
| 0 | 246 | | InteractionHistoryUpdated?.Invoke(this, _smoothingBehaviour.InteractionsFramesCache); |
| | 247 | |
|
| 0 | 248 | | return result; |
| 0 | 249 | | } |
| | 250 | |
|
| | 251 | | protected virtual ExtremumDescription ComputeExtremumType(Point3[][] pointsArray, int xPos, int yPos) |
| 0 | 252 | | { |
| 0 | 253 | | if (ExtremumTypeCheckMethod == ExtremumTypeCheckMethod.Global) |
| 0 | 254 | | return new ExtremumDescription |
| 0 | 255 | | { Type = ExtremumType.Maximum, NumFittingPoints = ExtremumTypeCheckNumSamples, PercentageFittingPoin |
| | 256 | |
|
| 0 | 257 | | var xMax = pointsArray.Length - 1; |
| 0 | 258 | | var yMax = xMax > 0 ? pointsArray[0].Length - 1 : 0; |
| | 259 | |
|
| | 260 | | // refZ is the absolute distance from sensor |
| 0 | 261 | | var refZ = Math.Abs(pointsArray[xPos][yPos].Z); |
| | 262 | |
|
| 0 | 263 | | var numCloser = 0; |
| 0 | 264 | | var numFarther = 0; |
| | 265 | |
|
| | 266 | | Tuple<int, int>[] samples; |
| 0 | 267 | | switch (ExtremumTypeCheckMethod) |
| | 268 | | { |
| | 269 | | case ExtremumTypeCheckMethod.StochasticStatic: |
| 0 | 270 | | samples = _stochasticSamples; |
| 0 | 271 | | break; |
| | 272 | | case ExtremumTypeCheckMethod.StochasticDynamic: |
| 0 | 273 | | samples = GenerateSamples(); |
| 0 | 274 | | break; |
| | 275 | | case ExtremumTypeCheckMethod.Global: |
| 0 | 276 | | throw new ArgumentException("invalid Method 'Global' at this point."); |
| | 277 | | case ExtremumTypeCheckMethod.FixedRadius: |
| | 278 | | default: |
| 0 | 279 | | samples = _fixedSamples; |
| 0 | 280 | | break; |
| | 281 | | } |
| | 282 | |
|
| 0 | 283 | | for (var i = 0; i < samples.Length; i++) |
| 0 | 284 | | { |
| 0 | 285 | | var sample = GetSample(pointsArray, xPos, yPos, samples, i, xMax, yMax); |
| 0 | 286 | | if (Math.Abs(sample.Z) > refZ) |
| 0 | 287 | | numFarther++; |
| | 288 | | else |
| 0 | 289 | | numCloser++; |
| 0 | 290 | | } |
| | 291 | |
|
| | 292 | |
|
| 0 | 293 | | var ratioMax = ExtremumTypeCheckNumSamples > 0 ? (float)numCloser / ExtremumTypeCheckNumSamples : 0; |
| 0 | 294 | | var ratioMin = ExtremumTypeCheckNumSamples > 0 ? (float)numFarther / ExtremumTypeCheckNumSamples : 0; |
| | 295 | |
|
| 0 | 296 | | if (numCloser > numFarther && ratioMax > ExtremumTypeCheckFittingPercentage) |
| 0 | 297 | | { |
| 0 | 298 | | return new ExtremumDescription |
| 0 | 299 | | { |
| 0 | 300 | | Type = ExtremumType.Maximum, |
| 0 | 301 | | NumFittingPoints = numCloser, |
| 0 | 302 | | PercentageFittingPoints = ratioMax |
| 0 | 303 | | }; |
| | 304 | | } |
| | 305 | |
|
| 0 | 306 | | if (numFarther > numCloser && ratioMin > ExtremumTypeCheckFittingPercentage) |
| 0 | 307 | | { |
| 0 | 308 | | return new ExtremumDescription |
| 0 | 309 | | { |
| 0 | 310 | | Type = ExtremumType.Minimum, |
| 0 | 311 | | NumFittingPoints = numFarther, |
| 0 | 312 | | PercentageFittingPoints = ratioMin |
| 0 | 313 | | }; |
| | 314 | | } |
| | 315 | |
|
| 0 | 316 | | return new ExtremumDescription |
| 0 | 317 | | { |
| 0 | 318 | | Type = ExtremumType.Undefined, |
| 0 | 319 | | NumFittingPoints = Math.Max(numCloser, numFarther), |
| 0 | 320 | | PercentageFittingPoints = Math.Max(ratioMin, ratioMax) |
| 0 | 321 | | }; |
| | 322 | |
|
| 0 | 323 | | } |
| | 324 | |
|
| | 325 | | public List<Interaction> ConvertDepthValue(List<Interaction> interactions) |
| 0 | 326 | | { |
| 0 | 327 | | var result = new List<Interaction>(); |
| | 328 | |
|
| 0 | 329 | | foreach (var item in interactions) |
| 0 | 330 | | { |
| 0 | 331 | | if (item == null) |
| 0 | 332 | | continue; |
| | 333 | |
|
| 0 | 334 | | if (item.Position.Z < Distance) |
| 0 | 335 | | { |
| 0 | 336 | | var depth = (Distance - item.Position.Z - MinDistance) / (MinDistance - MaxDistance); |
| | 337 | |
|
| 0 | 338 | | if (depth > 0) |
| 0 | 339 | | continue; |
| | 340 | |
|
| 0 | 341 | | if (depth < -1) |
| 0 | 342 | | depth = -1; |
| | 343 | |
|
| 0 | 344 | | item.Position.Z = depth; |
| 0 | 345 | | } |
| | 346 | | else |
| 0 | 347 | | { |
| 0 | 348 | | var depth = (item.Position.Z - MinDistance - Distance) / (MaxDistance - MinDistance); |
| | 349 | |
|
| 0 | 350 | | if (depth < 0) |
| 0 | 351 | | continue; |
| | 352 | |
|
| 0 | 353 | | if (depth > 1) |
| 0 | 354 | | depth = 1; |
| | 355 | |
|
| 0 | 356 | | item.Position.Z = depth; |
| 0 | 357 | | } |
| | 358 | |
|
| 0 | 359 | | result.Add(item); |
| 0 | 360 | | } |
| | 361 | |
|
| 0 | 362 | | return result; |
| 0 | 363 | | } |
| | 364 | |
|
| | 365 | | protected virtual IEnumerable<Interaction> ApplyConfidenceFilter(IEnumerable<Interaction> interactions) |
| 0 | 366 | | { |
| 0 | 367 | | return interactions.Where(item => item.Confidence > MinConfidence); |
| 0 | 368 | | } |
| | 369 | |
|
| | 370 | | protected virtual List<Interaction> ComputeExtremumType(List<Interaction> interactions, Point3[][] pointsArray) |
| 0 | 371 | | { |
| 0 | 372 | | interactions.ForEach(interaction => |
| 0 | 373 | | interaction.ExtremumDescription = ComputeExtremumType(pointsArray, (int)interaction.Position.X, (int)int |
| | 374 | |
|
| 0 | 375 | | return interactions; |
| 0 | 376 | | } |
| | 377 | |
|
| | 378 | | protected virtual void UpdatePerformanceMetrics(ProcessPerformance perfItem) |
| 0 | 379 | | { |
| 0 | 380 | | var pData = new PerformanceDataItem |
| 0 | 381 | | { |
| 0 | 382 | | FrameId = Convert.ToInt32(FrameId), |
| 0 | 383 | | FrameStart = DateTime.Now.Ticks, |
| 0 | 384 | | Process = perfItem, |
| 0 | 385 | | Stage = PerformanceDataStage.ProcessingData |
| 0 | 386 | | }; |
| | 387 | |
|
| 0 | 388 | | PerformanceDataUpdated?.Invoke(this, pData); |
| 0 | 389 | | } |
| | 390 | |
|
| | 391 | | /// <summary> |
| | 392 | | /// Calculates the average distance for the currently stored PointCloud. |
| | 393 | | /// </summary> |
| | 394 | | /// <returns></returns> |
| | 395 | | public virtual float CalculateAverageDistance() |
| 0 | 396 | | { |
| 0 | 397 | | var sum = 0f; |
| 0 | 398 | | var points = PointCloud?.AsArray(); |
| | 399 | |
|
| 0 | 400 | | if (points == null) |
| 0 | 401 | | return sum; |
| | 402 | |
|
| 0 | 403 | | var numValidSamples = 0; |
| | 404 | |
|
| 0 | 405 | | for (var i = 0; i < points.Length; ++i) |
| 0 | 406 | | { |
| 0 | 407 | | if (!points[i].IsValid || points[i].IsFiltered) |
| 0 | 408 | | continue; |
| | 409 | |
|
| 0 | 410 | | numValidSamples++; |
| 0 | 411 | | sum += points[i].Z; |
| 0 | 412 | | } |
| | 413 | |
|
| 0 | 414 | | return (float) decimal.Round((decimal) (sum / numValidSamples), 2); |
| 0 | 415 | | } |
| | 416 | |
|
| | 417 | | private static Point3 GetSample(Point3[][] pointsArray, int xPos, int yPos, Tuple<int, int>[] samples, int i, in |
| 0 | 418 | | { |
| 0 | 419 | | var xIdx = xPos + samples[i].Item1; |
| 0 | 420 | | xIdx = Math.Min(Math.Max(xIdx, 0), xMax); |
| | 421 | |
|
| 0 | 422 | | var yIdx = yPos + samples[i].Item2; |
| 0 | 423 | | yIdx = Math.Min(Math.Max(yIdx, 0), yMax); |
| 0 | 424 | | var sample = pointsArray[xIdx][yIdx]; |
| | 425 | |
|
| 0 | 426 | | return sample; |
| 0 | 427 | | } |
| | 428 | |
|
| | 429 | | private Tuple<int, int>[] GenerateSamples() |
| 0 | 430 | | { |
| 0 | 431 | | var result = new List<Tuple<int, int>>(); |
| | 432 | |
|
| 0 | 433 | | var rnd = new Random(); |
| | 434 | |
|
| 0 | 435 | | var min = (int)Math.Floor(0.5f * ExtremumTypeCheckRadius); |
| 0 | 436 | | var max = ExtremumTypeCheckRadius; |
| | 437 | |
|
| 0 | 438 | | for (var i = 0; i < ExtremumTypeCheckNumSamples; i++) |
| 0 | 439 | | { |
| 0 | 440 | | var xStochastic = rnd.Next(min, max); |
| 0 | 441 | | var yStochastic = rnd.Next(min, max); |
| | 442 | |
|
| 0 | 443 | | result.Add(new Tuple<int, int>(xStochastic, yStochastic)); |
| 0 | 444 | | } |
| | 445 | |
|
| 0 | 446 | | return result.ToArray(); |
| 0 | 447 | | } |
| | 448 | |
|
| | 449 | | private void UpdateSamples() |
| 0 | 450 | | { |
| 0 | 451 | | var samplesFixed = new List<Tuple<int, int>>(); |
| | 452 | |
|
| 0 | 453 | | for (var i = 0; i < ExtremumTypeCheckNumSamples; i++) |
| 0 | 454 | | { |
| 0 | 455 | | var p = (float)i / ExtremumTypeCheckRadius; |
| 0 | 456 | | var xFixed = (int) Math.Floor(ExtremumTypeCheckRadius * Math.Cos(p * 2 * Math.PI)); |
| 0 | 457 | | var yFixed = (int) Math.Floor(ExtremumTypeCheckRadius * Math.Sin(p * 2 * Math.PI)); |
| | 458 | |
|
| 0 | 459 | | samplesFixed.Add(new Tuple<int, int>(xFixed, yFixed)); |
| 0 | 460 | | } |
| 0 | 461 | | _fixedSamples = samplesFixed.ToArray(); |
| | 462 | |
|
| 0 | 463 | | _stochasticSamples = GenerateSamples(); |
| 0 | 464 | | } |
| | 465 | | } |
| | 466 | | } |