| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Linq; |
| | | 4 | | using NLog; |
| | | 5 | | using ReFlex.Core.Common.Components; |
| | | 6 | | using ReFlex.Core.Common.Util; |
| | | 7 | | using ReFlex.Core.Networking.Util; |
| | | 8 | | using ReFlex.Core.Tracking.Interfaces; |
| | | 9 | | using ReFlex.Core.Tracking.Util; |
| | | 10 | | |
| | | 11 | | namespace ReFlex.Sensor.EmulatorModule |
| | | 12 | | { |
| | | 13 | | public class ImageBasedEmulatorCamera : IDepthCamera |
| | | 14 | | { |
| | | 15 | | #region Fields |
| | | 16 | | |
| | 0 | 17 | | private static readonly Logger Logger = LogManager.GetCurrentClassLogger(); |
| | | 18 | | private readonly string _address; |
| | | 19 | | |
| | | 20 | | private bool _isStarted; |
| | | 21 | | private StreamParameter _selectedConfig; |
| | | 22 | | private EmulatedPointCloud _pointCloud; |
| | | 23 | | |
| | | 24 | | #endregion |
| | | 25 | | #region Properties |
| | | 26 | | |
| | 0 | 27 | | public string Id => _address; |
| | | 28 | | |
| | 0 | 29 | | public string ModelDescription => $"Image-Based Emulator [{_address}]"; |
| | | 30 | | |
| | 0 | 31 | | public CameraType CameraType => CameraType.Emulator; |
| | | 32 | | |
| | | 33 | | public DepthCameraState State |
| | | 34 | | { |
| | | 35 | | get |
| | 0 | 36 | | { |
| | 0 | 37 | | return _isStarted ? DepthCameraState.Streaming : DepthCameraState.Connected; |
| | 0 | 38 | | } |
| | | 39 | | } |
| | | 40 | | |
| | 0 | 41 | | public StreamParameter StreamParameter { get; private set; } |
| | | 42 | | |
| | | 43 | | #endregion |
| | | 44 | | |
| | | 45 | | #region Events |
| | | 46 | | |
| | | 47 | | public event EventHandler<DepthCameraState> StateChanged; |
| | | 48 | | public event EventHandler<DepthCameraFrame> FrameReady; |
| | | 49 | | public event EventHandler<ImageByteArray> DepthImageReady; |
| | | 50 | | |
| | | 51 | | #endregion |
| | | 52 | | |
| | | 53 | | #region Constructor |
| | | 54 | | |
| | 0 | 55 | | public ImageBasedEmulatorCamera(string address) |
| | 0 | 56 | | { |
| | 0 | 57 | | _address = address; |
| | 0 | 58 | | } |
| | | 59 | | |
| | | 60 | | #endregion |
| | | 61 | | |
| | | 62 | | public void EnableStream(StreamParameter parameter) |
| | 0 | 63 | | { |
| | 0 | 64 | | _selectedConfig = GetPossibleConfigurations().FirstOrDefault(item => item.Width == parameter.Width && item.H |
| | | 65 | | |
| | 0 | 66 | | var param = new EmulatorParameters |
| | 0 | 67 | | { |
| | 0 | 68 | | HeightInMeters = 1.6f, |
| | 0 | 69 | | WidthInMeters = 2.4f, |
| | 0 | 70 | | MaxDepthInMeter = 2.1f, |
| | 0 | 71 | | MinDepthInMeter = 0.8f, |
| | 0 | 72 | | PlaneDistanceInMeter = 1.45f, |
| | 0 | 73 | | Radius = (int)(parameter.Width * 0.4) |
| | 0 | 74 | | }; |
| | | 75 | | |
| | 0 | 76 | | StreamParameter = parameter; |
| | | 77 | | |
| | 0 | 78 | | _pointCloud = new EmulatedPointCloud(param); |
| | 0 | 79 | | _pointCloud.InitializePointCloud(parameter); |
| | 0 | 80 | | } |
| | | 81 | | |
| | | 82 | | public IList<StreamParameter> GetPossibleConfigurations() |
| | 0 | 83 | | { |
| | 0 | 84 | | var configs = new List<StreamParameter> |
| | 0 | 85 | | { |
| | 0 | 86 | | new StreamParameter(50, 50, 30), |
| | 0 | 87 | | new StreamParameter(64, 36, 30), |
| | 0 | 88 | | new StreamParameter(64, 48, 30), |
| | 0 | 89 | | new StreamParameter(100, 100, 30), |
| | 0 | 90 | | new StreamParameter(128, 72, 30), |
| | 0 | 91 | | new StreamParameter(128, 96, 30), |
| | 0 | 92 | | new StreamParameter(640, 480, 30), |
| | 0 | 93 | | new StreamParameter(800, 600, 30), |
| | 0 | 94 | | new StreamParameter(1920, 1080, 30), |
| | 0 | 95 | | new StreamParameter(2560, 1440, 30) |
| | 0 | 96 | | }; |
| | | 97 | | |
| | 0 | 98 | | return configs; |
| | 0 | 99 | | } |
| | | 100 | | |
| | | 101 | | public void Initialize() |
| | 0 | 102 | | { |
| | 0 | 103 | | throw new NotImplementedException(); |
| | | 104 | | } |
| | | 105 | | |
| | | 106 | | public void StartStream() |
| | 0 | 107 | | { |
| | 0 | 108 | | _isStarted = true; |
| | 0 | 109 | | StateChanged?.Invoke(this, State); |
| | 0 | 110 | | } |
| | | 111 | | |
| | | 112 | | public void StopStream() |
| | 0 | 113 | | { |
| | 0 | 114 | | _isStarted = false; |
| | 0 | 115 | | StateChanged?.Invoke(this, State); |
| | 0 | 116 | | } |
| | | 117 | | |
| | | 118 | | public void Update(byte[] data) |
| | 0 | 119 | | { |
| | 0 | 120 | | if (_isStarted && _selectedConfig != null) |
| | 0 | 121 | | { |
| | 0 | 122 | | var args = new ImageByteArray( |
| | 0 | 123 | | data, |
| | 0 | 124 | | _selectedConfig.Width, |
| | 0 | 125 | | _selectedConfig.Height, |
| | 0 | 126 | | 3, |
| | 0 | 127 | | 1); |
| | | 128 | | |
| | 0 | 129 | | _pointCloud.Reset(); |
| | 0 | 130 | | _pointCloud.UpdateFromGreyScaleImage(args); |
| | | 131 | | |
| | 0 | 132 | | DepthImageReady?.Invoke(this, args); |
| | 0 | 133 | | FrameReady?.Invoke(this, new DepthCameraFrame { Depth = _pointCloud.Points }); |
| | 0 | 134 | | } |
| | | 135 | | |
| | 0 | 136 | | } |
| | | 137 | | } |
| | | 138 | | } |