directional edge detection

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>
#include <vector>

using namespace cv;
using namespace std;

RNG rng(12345);

Mat mat2gray(const cv::Mat& src)
{
    Mat dst;
    normalize(src, dst, 0.0, 255.0, cv::NORM_MINMAX, CV_8U);

    return dst;
}

Mat oriEdge(Mat mag, Mat ori, double magThres, double oriThres)
{
    Mat Edge = Mat::zeros(ori.size(), CV_8UC1);

    double tol=0.1;
    uchar val;

    for (int i=0; i<Edge.rows; i++)
        for (int j=0; j<Edge.cols; j++)
        {
            if ((abs(ori.at<float>(i,j)-oriThres)<tol)&&(mag.at<float>(i,j)>=magThres))
            {
                Edge.at<uchar>(i,j)=255;
            }
        }

    return Edge;
}

Mat orientationMap(const cv::Mat& mag, const cv::Mat& ori, double thresh = 1.0)
{
    Mat oriMap = Mat::zeros(ori.size(), CV_8UC3);
    Vec3b red(0, 0, 255);
    Vec3b cyan(255, 255, 0);
    Vec3b green(0, 255, 0);
    Vec3b yellow(0, 255, 255);
    for(int i = 0; i < mag.rows*mag.cols; i++)
    {
        float* magPixel = reinterpret_cast<float*>(mag.data + i*sizeof(float));
        if(*magPixel > thresh)
        {
            float* oriPixel = reinterpret_cast<float*>(ori.data + i*sizeof(float));
            Vec3b* mapPixel = reinterpret_cast<Vec3b*>(oriMap.data + i*3*sizeof(char));
            if(*oriPixel < 90.0)
                *mapPixel = red;
            else if(*oriPixel >= 90.0 && *oriPixel < 180.0)
                *mapPixel = cyan;
            else if(*oriPixel >= 180.0 && *oriPixel < 270.0)
                *mapPixel = green;
            else if(*oriPixel >= 270.0 && *oriPixel < 360.0)
                *mapPixel = yellow;
        }
    }

    return oriMap;
}



int main(int argc, char* argv[])
{

    VideoCapture cap(0); // open the default camera
    if(!cap.isOpened())  // check if we succeeded
        return -1;

//  Mat image = Mat::zeros(Size(320, 240), CV_8UC1);
    Mat image;
    Mat edges, Sx, Sy, mag, ori, oriMap;
    Mat oriEdImg, linkedImg;
    //namedWindow("edges",1);

    // contour vars
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;

    for(;;)
    {
        Mat frame;
        cap >> frame; // get a new frame from camera
        cvtColor(frame, image, CV_BGR2GRAY);


        //Mat image = Mat::zeros(Size(320, 240), CV_8UC1);
        //circle(image, Point(160, 120), 80, Scalar(255, 255, 255), -1, CV_AA);

        imshow("original", image);

        //Mat Sx;
        Sobel(image, Sx, CV_32F, 1, 0, 3);

        //Mat Sy;
        Sobel(image, Sy, CV_32F, 0, 1, 3);

        //Mat mag, ori;
        magnitude(Sx, Sy, mag);
        phase(Sx, Sy, ori, false);

        //Mat oriMap = orientationMap(mag, ori, 1.0);
        //oriMap = orientationMap(mag, ori, 1.0);

        oriEdImg = oriEdge(mag,ori,150,CV_PI/4);

        //linkedImg = linkedEdge(oriEdImg);
        // find contours instead
        /// Find contours RISK HERE!!!
        findContours( oriEdImg, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );

        /// Draw contours
        linkedImg = Mat::zeros( oriEdImg.size(), CV_8UC3 );
        for( int i = 0; i< contours.size(); i++ )
        {
            //Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
            Scalar color = Scalar(255,255,255);
            drawContours( linkedImg, contours, i, color, 2, 8, hierarchy, 0, Point() );
        }

        imshow("magnitude", mat2gray(mag));
        imshow("orientation", mat2gray(ori));
        imshow("Oriented Edge", oriEdImg);
        imshow("Linked Edge", linkedImg);


        //imshow("orientation map", oriMap);
        //waitKey();


        // to smooth out noise that affects edge detection
        edges = image;
        GaussianBlur(edges, edges, Size(7,7), 1.5, 1.5);

        //Canny(edges, edges, 0, 200, 3);
        Canny(edges, edges, 0, 150, 3);
        imshow("edges", edges);
        if(waitKey(30) >= 0) break;
    }
    // the camera will be deinitialized automatically in VideoCapture destructor
    //return 0;


    return 0;
}