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