Beispiel #1
0
        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()));
        }
Beispiel #2
0
        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());
        }