| | | 1 | | using System; |
| | | 2 | | using MathNet.Numerics.LinearAlgebra; |
| | | 3 | | |
| | | 4 | | namespace ReFlex.Core.Calibration.Components |
| | | 5 | | { |
| | | 6 | | public class GradientProcessor : DepthProcessorBase |
| | | 7 | | { |
| | | 8 | | #region Fields; |
| | | 9 | | |
| | | 10 | | // private int _threshold; |
| | | 11 | | private readonly int _skip; |
| | | 12 | | private Vector<double>[,] _lastProcessed; |
| | | 13 | | private Util.Calibration _calibration; |
| | | 14 | | |
| | 0 | 15 | | private static readonly int[] XWeights = { -1, -1, 0, 1, 1, 1, 0, -1, -2, -2, -1, 0, 1, 2, 2, 2, 1, 0, -1, -2 }; |
| | 0 | 16 | | private static readonly int[] YWeights = { 0, -1, -1, -1, 0, 1, 1, 1, 0, -1, -2, -2, -2, -1, 0, 1, 2, 2, 2, 1 }; |
| | | 17 | | |
| | | 18 | | #endregion |
| | | 19 | | |
| | | 20 | | #region constructor |
| | | 21 | | |
| | 0 | 22 | | public GradientProcessor(Util.Calibration calibration, int width, int height, int skip) : base(width, height) |
| | 0 | 23 | | { |
| | 0 | 24 | | _skip = skip; |
| | 0 | 25 | | _calibration = calibration; |
| | 0 | 26 | | } |
| | | 27 | | |
| | | 28 | | #endregion |
| | | 29 | | |
| | | 30 | | #region Implementation of Abstract Methods |
| | | 31 | | |
| | | 32 | | |
| | | 33 | | public override Vector<double>[,] Process(double[] depthValues) |
| | 0 | 34 | | { |
| | 0 | 35 | | var result = new Vector<double>[Width / _skip, Height / _skip]; |
| | 0 | 36 | | var offset = -1; |
| | | 37 | | |
| | 0 | 38 | | for (int i = 0; i < depthValues.Length; i += _skip) |
| | 0 | 39 | | { |
| | 0 | 40 | | var vec = Vector<double>.Build.Sparse(3); |
| | 0 | 41 | | var curr = depthValues[i]; |
| | | 42 | | |
| | | 43 | | // Smoothing: Can be removed ? |
| | | 44 | | |
| | 0 | 45 | | for (var j = 0; j < XWeights.Length; j++) |
| | 0 | 46 | | { |
| | 0 | 47 | | var n = i + ConvertDim21(XWeights[j], YWeights[j]); |
| | 0 | 48 | | if (n < depthValues.Length && n >= 0 && Math.Abs(depthValues[0] - MaxDepthValue) > double.Epsilon) |
| | 0 | 49 | | { |
| | 0 | 50 | | var diff = depthValues[n] - curr; |
| | 0 | 51 | | var sub = new[] { XWeights[j] * diff, YWeights[j] * diff, 0 }; |
| | 0 | 52 | | vec = vec.Subtract(Vector<double>.Build.DenseOfArray(sub)); |
| | 0 | 53 | | } |
| | | 54 | | else |
| | 0 | 55 | | { |
| | 0 | 56 | | vec[0] = 0; |
| | 0 | 57 | | vec[1] = 0; |
| | 0 | 58 | | } |
| | 0 | 59 | | } |
| | | 60 | | |
| | | 61 | | // Smoothing: end |
| | | 62 | | |
| | 0 | 63 | | if (i % (Width * _skip) == 0) |
| | 0 | 64 | | offset++; |
| | 0 | 65 | | vec = vec.Multiply(.1); |
| | | 66 | | |
| | 0 | 67 | | if (vec.Norm(2) > 3) |
| | 0 | 68 | | vec = vec.Normalize(2).Multiply(3.0); |
| | | 69 | | |
| | 0 | 70 | | if (curr > _calibration.UpperThreshold && curr < _calibration.LowerThreshold) |
| | 0 | 71 | | { |
| | 0 | 72 | | result[i % Width / _skip, offset] = Vector<double>.Build.Sparse(3); |
| | 0 | 73 | | continue; |
| | | 74 | | } |
| | | 75 | | |
| | 0 | 76 | | if (_lastProcessed == null) |
| | 0 | 77 | | { |
| | 0 | 78 | | result[i % Width / _skip, offset] = vec; |
| | 0 | 79 | | } |
| | | 80 | | else |
| | 0 | 81 | | { |
| | 0 | 82 | | var last = _lastProcessed[i % Width / _skip, offset]; |
| | 0 | 83 | | result[i % Width / _skip, offset] = InterpolateVector(vec, last, 0.7); |
| | 0 | 84 | | } |
| | 0 | 85 | | } |
| | | 86 | | |
| | 0 | 87 | | _lastProcessed = result; |
| | 0 | 88 | | return result; |
| | 0 | 89 | | } |
| | | 90 | | |
| | | 91 | | #endregion |
| | | 92 | | } |
| | | 93 | | |
| | | 94 | | } |