VS2010 C++ OpenCV Tracking Shapes in Motion not Working Correctly

edit

The below code attempts to track shapes in motion, and runs without error - however, it does not work as intended. It's purpose is to draw a perimeter around the polygons detected in the camera feed for ever frame, with the perimeter color dependent on the number of vertices (i.e. the shape). However, as depicted by the cout statements in and above the for loop in the processFrame method, no contours are ever detected by the program. Why might this be? Are there other issues hindering this code from fulfilling its objective? Any help is more than appreciated.

ALSO: threshold is commented out because it triggers a breakpoint when run.

ALSO: I'm pretty sure threshold is the reason my code doesn't work - no contours are found without thresholding, but thresholding triggers a breakpoint.

ALSO: Console window output is an infinite steam of 0, one on each line (this is the number of contours detected).

#include <sstream>
#include <string>
#include <iostream>
#include <fstream>
#include <time.h>
#include<opencv2\opencv.hpp>
#include<opencv2\highgui\highgui.hpp>

std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;

void process_contour(cv::Mat& frame, std::vector<cv::Point> const& contour)
{
    cv::Scalar TRIANGLE_COLOR(255, 0, 0);
    cv::Scalar QUADRILATERAL_COLOR(0, 255, 0);
    cv::Scalar HEPTAGON_COLOR(0, 0, 255);

    cv::Scalar colour;
    if (contour.size() == 3) {
        colour = TRIANGLE_COLOR;
    } else if (contour.size() == 4) {
        colour = QUADRILATERAL_COLOR;
    } else if (contour.size() == 7) {
        colour = HEPTAGON_COLOR;
    } else {
        return;
    }

    cv::Point const* points(&contour[0]);
    int n_points(static_cast<int>(contour.size()));

    polylines(frame, &points, &n_points, 1, true, colour, 4);
}

void process_frame(cv::Mat const& frame, cv::Mat& result_frame)
{
    cv::Mat feedGrayScale;
    cv::cvtColor(frame, feedGrayScale, cv::COLOR_BGR2GRAY);
    cv::imshow("Grayscale", feedGrayScale);
    //cv::inRange(feedGrayScale,cv::Scalar(25,130,105),cv::Scalar(40,195,150),feedGrayScale);
    frame.copyTo(result_frame);

    //thresholding the grayscale image to get better results
    //cv::threshold(feedGrayScale, feedGrayScale, 128, 255, cv::THRESH_BINARY);
    cv::findContours(feedGrayScale, contours, hierarchy, CV_RETR_TREE,CV_CHAIN_APPROX_SIMPLE );
    std::cout << contours.size() << std::endl;

    for (size_t k(0); k < contours.size(); ++k) {
        std::cout << "IT WORKS!" << std::endl;
        std::vector<cv::Point> approx_contour;
        cv::approxPolyDP(cv::Mat(contours[k]), approx_contour, 3, true);
        process_contour(result_frame, approx_contour);
    }
}

int main()
{
    cv::VideoCapture cap(0); // open the video camera no. 0

    if (!cap.isOpened())  // if not success, exit program
    {
        std::cout << "Cannot open the video cam\n";
        return -1;
    }

    cv::namedWindow("Original", CV_WINDOW_AUTOSIZE);
    cv::namedWindow("Tracked", CV_WINDOW_AUTOSIZE);

    // Process frames from the video stream...
    for(;;) {
        cv::Mat frame, result_frame;

        // read a new frame from video
        if (!cap.read(frame)) {
            std::cout << "Cannot read a frame from video stream\n";
            break;
        }
        process_frame(frame, result_frame);
        cv::imshow("Original", frame);
        cv::imshow("Tracked", result_frame);
        if (cv::waitKey(20) == 27) { // Quit on ESC
            break;
        }
    }

    return 0;
}