Using Harris Corner Detector on video

edit

I am trying to use Harris Corner Detector on video based on the example given here. Here is my code:

#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include <iostream>
#include <string>

cv::Mat src, src_gray;
cv::Mat dst_norm, dst_norm_scaled;

void harris_corner(int thresh, void*)
{
    int block_size = 2;
    int aperture_size = 3;
    double k = 0.04;
    cv::Mat dst = cv::Mat::zeros(src.size(), CV_32FC1); 
    //cv::Mat dst_norm;
    cv::cornerHarris(src_gray, dst, block_size, aperture_size, k);
    cv::normalize(dst, dst_norm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    //cv::convertScaleAbs(dst_norm, dst_norm_scaled);

    //cv::imshow("Result", dst_norm_scaled);
    return;  
}

int main()
{
    cv::VideoCapture cap(0);

    const std::string win0 = "Source Video";
    //const std::string win1 = "Harris Corner Detector";
    if(!cap.isOpened())
    {
        std::cout << "Error with the video source\n";
    }
    cv::namedWindow(win0);
    int thresh = 230;
    int max_thresh = 255;
    cv::createTrackbar("Threshold: ", win0, &thresh, max_thresh, harris_corner);
    while(1)
    {
        cap >> src;
        //cv::imshow(win0, src);
        cv::cvtColor(src, src_gray, cv::COLOR_BGR2GRAY);
        harris_corner(thresh, 0);
        for(int i = 0; i < dst_norm.rows; i++)
        {
            for(int j = 0; j < dst_norm.cols; j++)
            {
                if((int) dst_norm.at<float>(i, j) > thresh)
                {
                    cv::circle(src, cv::Point(j, i), 3, cv::Scalar(0, 0, 255), 2, 8, 0);    
                }
            }
        }
        cv::imshow(win0, src);
        if(cv::waitKey(10) == 27)
        {
            break;
        }
    }

    return 0;
}

If I keep the variable thresh at a high value and if the scene does not change, the code gives decent performance as shown in image below:

However, if I change the scene (or move the camera) or reduce the thresh, the video display gets stuck or goes completely red as shown below:

I believe that this is an issue with the speed of the code, especially this part:

    for(int i = 0; i < dst_norm.rows; i++)
    {
        for(int j = 0; j < dst_norm.cols; j++)
        {
            if((int) dst_norm.at<float>(i, j) > thresh)
            {
                cv::circle(src, cv::Point(j, i), 3, cv::Scalar(0, 0, 255), 2, 8, 0);    
            }
        }
    }

How can I optimize the code and resolve this issue? Are there any other errors in the code preventing the Harris Corner Detector from working properly?