C:/Documents and Settings/student/My Documents/robo_magellan/visual_studio_(system_controller)/system_controller/VisionSystem/VisionData.cs

Go to the documentation of this file.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace VisionSystem {
        public class VisionData {

                Bitmap _filteredCone;
                public Bitmap FilteredCone {
                        get { return _filteredCone; }
                }

                Mitov.VideoLab.VideoBuffer _buffer;
                public Mitov.VideoLab.VideoBuffer Buffer {
                        get { return _buffer; }
                }
                const byte red = 224, grn = 48, blu = 42;

                byte _redT = 50;
                public byte BluT {
                        get { return _bluT; }
                }

                byte _grnT = 50;
                public byte GrnT {
                        get { return _grnT; }
                }

                byte _bluT = 50;
                public byte RedT {
                        get { return _redT; }
                }

                Point _top, _left, _right;
                public Point Right {
                        get { return _right; }
                }

                public Point Left {
                        get { return _left; }
                }

                public Point Top {
                        get { return _top; }
                }


                public int DeltaX {
                        get { return _right.X - _left.X; }
                }

                Color _orange;
                public Color Orange {
                        get { return _orange; }
                }

                int _width = 320, _height = 240;
                int _orangePixelCount = 0;

                string _status = "ok";
                public string Status {
                        get { return _status; }
                }

                Color[,] _orngPoints;
                public Color[,] OrngPoints {
                        get { return _orngPoints; }
                }

                int _scaleFactor = 0;
                public int ScaleFactor {
                        get { return _scaleFactor; }
                }

                double _leftSlope = 0;
                public double LeftSlope {
                        get { return _leftSlope; }
                }

                double _rightSlope = 0;
                public double RightSlope {
                        get { return _rightSlope; }
                }

                bool _coneAquired = false;
                public bool ConeAquired {
                        get { return _coneAquired; }
                }

                int _turnMagnitude = 0;
                public int TurnMagnitude {
                        get { return _turnMagnitude; }
                }

                string _turnDirection = "null";
                public string TurnDirection {
                        get { return _turnDirection; }
                }

                public VisionData(ref Bitmap filtered) {
                        _filteredCone = filtered;
                        _width = _filteredCone.Width;
                        _height = _filteredCone.Height;
                        _orngPoints = new Color[_width, _height];
                        _orange = Color.FromArgb(red, grn, blu);
                        _top = new Point(0, 0);
                        _left = new Point(0, 0);
                        _right = new Point(0, 0);
                }

                public void processData(Mitov.VideoLab.VideoBuffer buffer) {
                        _buffer = buffer;
                        lookForOrange();
                        if (_orangePixelCount > 50) {
                                determineIsCone();
                                if (_coneAquired) {
                                        _turnMagnitude = _width / 2 - _top.X;
                                        if (_width / 2 - _top.X > 0) _turnDirection = "left";
                                        else if (_width / 2 - _top.X < 0) _turnDirection = "right";
                                        else _turnDirection = "null";
                                }
                        } else _coneAquired = false;
                }

                public void lookForOrange() {
                        _orngPoints = new Color[_width, _height];
                        _orangePixelCount = 0;
                        for (int j = 0; j < _buffer.GetHeight(); ++j) {
                                for (int i = 0; i < _buffer.GetWidth(); ++i) {
                                        if (Math.Abs(_buffer.GetRed(i, j) - _orange.R) < _redT
                                                && Math.Abs(_buffer.GetGreen(i, j) - _orange.G) < _grnT
                                                && Math.Abs(_buffer.GetBlue(i, j) - _orange.B) < _bluT) {
                                                //store pixel into cone pixel array
                                                _orngPoints[i, j] = Color.FromArgb(_buffer.GetRed(i, j),
                                                        _buffer.GetGreen(i, j), _buffer.GetBlue(i, j));
                                                ++_orangePixelCount;
                                        }
                                }
                        }
                }


                Graphics g;
                SolidBrush sBrush = new SolidBrush(Color.White);
                Pen pen = new Pen(Color.Green, 2);

                public void drawOrange() {
                        g = Graphics.FromImage(_filteredCone);
                        g.FillRectangle(sBrush, 0, 0, _width, _height);
                        for (int j = 0; j < _height; ++j) {
                                for (int i = 0; i < _width; ++i) {
                                        if (!_orngPoints[i, j].IsEmpty)
                                                _filteredCone.SetPixel(i, j, _orngPoints[i, j]);
                                }
                        }
                        //draw green lines
                        g.DrawLine(pen, _top, _left);
                        g.DrawLine(pen, _top, _right);
                }


                bool isPointy;

                private void determineIsCone() {
                        _coneAquired = false;
                        //calculate the slope from the top of the cone to the bottm
                        _top.X = 0; _top.Y = 0;
                        _left.X = 0; _left.Y = 0;
                        _right.X = 0; _right.Y = 0;
                        //finds the top of the cone (minimize y)
                        for (int j = 0; j < _height; ++j) {
                                for (int i = 0; i < _width; ++i) {
                                        if (!_orngPoints[i, j].IsEmpty) {
                                                _top.X = i; _top.Y = j + _scaleFactor; break;
                                        }
                                }
                                if (_top.X != 0 && _top.Y != 0) break;
                        }
                        //find the bottom left and right of the cone, (maximize y first)
                        for (int j = _height - 1; j > 0; --j) {
                                for (int i = 0; i < _width; ++i) {
                                        if (!_orngPoints[i, j].IsEmpty) {
                                                _left.Y = j - _scaleFactor; break;
                                        }
                                }
                                if (_left.Y != 0) break;
                        }
                        //find bottom left (minimize x)
                        for (int i = 0; i < _width; i++) {
                                if (!_orngPoints[i, _left.Y].IsEmpty) {
                                        _left.X = i; break;
                                }
                        }
                        //find bottom right (maximize x)
                        for (int i = _width - 1; i > 0; --i) {
                                if (!_orngPoints[i, _left.Y].IsEmpty) {
                                        _right.X = i; _right.Y = _left.Y; break;
                                }
                        }
                        _scaleFactor = (int)Math.Round(Math.Sqrt(Math.Pow(_left.X - _top.X, 2)
                                + Math.Pow(_left.Y - _top.Y, 2)) / 10.0, 0);
                        isPointy = true;
                        //check if cone is pointy
                        for (int i = _top.X + (_right.X - _left.X / 2); i < _width; i++) {
                                int j = _top.X - i;
                                if (!_orngPoints[i, _top.Y].IsEmpty) {
                                        isPointy = false; break;
                                }
                                if (_top.X - i > 0) {
                                        if (!_orngPoints[j, _top.Y].IsEmpty) {
                                                isPointy = false; break;
                                        }
                                }
                        } if (isPointy) {
                                if (_left.X != _top.X) //check for divide by zero
                                        _leftSlope = (double)(_left.Y - _top.Y)
                                                / (double)(_left.X - _top.X);
                                else _leftSlope = double.MaxValue;
                                if (_right.X != _top.X) //check for divide by zero
                                        _rightSlope = (double)(_right.Y - _top.Y)
                                                / (double)(_right.X - _top.X);
                                else _rightSlope = double.MaxValue;
                                if (Math.Abs(_leftSlope + 5) < 3
                                        || Math.Abs(_rightSlope - 5) < 3)
                                        _coneAquired = true;
                        }
                }

                public void setColor(Color c) {
                        _orange = c;
                }

                public void setFilter(byte redT, byte grnT, byte bluT) {
                        _redT = redT;
                        _grnT = grnT;
                        _bluT = bluT;
                }

                int _brightness = 0;
                public int Brightness {
                        get { return _brightness; }
                        set { _brightness = value; }
                }

                public void calibrateCamera(Mitov.VideoLab.VideoBuffer buffer) {
                        int red = 0, green = 0, blue = 0;
                        int pixelCount = buffer.GetHeight() * buffer.GetWidth();
                        for (int j = 0; j < buffer.GetHeight(); ++j) {
                                for (int i = 0; i < buffer.GetWidth(); ++i) {
                                        red += buffer.GetRed(i, j);
                                        green += buffer.GetGreen(i, j);
                                        blue += buffer.GetBlue(i, j);
                                }
                        }
                        red /= pixelCount;
                        green /= pixelCount;
                        blue /= pixelCount;
                        _brightness = (red + green + blue) / 3;
                }

        }
}

---