public ImageSensorData Measure() { var tmpLocation = GetCameraLocation(_robot); var result = new KinectData(_settings.VerticalResolution, _settings.HorisontalResolution); var horisontalAngle = -_settings.HorisontalViewAngle / 2.0; var verticalAngle = -_settings.VerticalViewAngle / 2.0; for (int i = 0; i < _settings.VerticalResolution; i++) { Frame3D mediateDirection = SensorRotation.VerticalFrameRotation(tmpLocation, -verticalAngle); horisontalAngle = -_settings.HorisontalViewAngle / 2.0; for (int j = 0; j < _settings.HorisontalResolution; j++) { Frame3D direction = SensorRotation.HorisontalFrameRotation(mediateDirection, horisontalAngle); Ray ray = new Ray(tmpLocation.ToPoint3D(), SensorRotation.GetFrontDirection(direction)); var dist = double.PositiveInfinity; foreach (var body in _worldRoot.GetSubtreeChildrenFirst()) { if (_settings.Exclude.All(a => a != body)) { var inter = Intersector.Intersect(body, ray); dist = Math.Min(dist, inter); } } result.Depth[i, j] = dist; //verticalAngle += _settings.VStep; horisontalAngle += _settings.HStep; } verticalAngle += _settings.VStep; } return(new ImageSensorData(result.GetBytes())); }
public ImageSensorData Measure() { var tmpLocation = GetCameraLocation(_robot); var result = new KinectData(_settings.VerticalResolution, _settings.HorisontalResolution); var horisontalAngle = -_settings.HorisontalViewAngle/2.0; var verticalAngle = -_settings.VerticalViewAngle/2.0; for (int i = 0; i < _settings.VerticalResolution; i++) { Frame3D mediateDirection = SensorRotation.VerticalFrameRotation(tmpLocation, -verticalAngle); horisontalAngle = -_settings.HorisontalViewAngle/2.0; for (int j = 0; j < _settings.HorisontalResolution; j++) { Frame3D direction = SensorRotation.HorisontalFrameRotation(mediateDirection, horisontalAngle); Ray ray = new Ray(tmpLocation.ToPoint3D(), SensorRotation.GetFrontDirection(direction)); var dist = double.PositiveInfinity; foreach (var body in _worldRoot.GetSubtreeChildrenFirst()) if (_settings.Exclude.All(a => a != body)) { var inter = Intersector.Intersect(body, ray); dist = Math.Min(dist, inter); } result.Depth[i, j] = dist; //verticalAngle += _settings.VStep; horisontalAngle += _settings.HStep; } verticalAngle += _settings.VStep; } return new ImageSensorData(result.GetBytes()); }