Определение цвета семафора с помощью opencCV на малиновом PI

У меня есть проект, в котором мне нужно определить цвет семафора. Я делаю это, потому что я делаю робота, который должен остановиться на красном свете семафора. Я использую Raspberry Pi и веб-камеру.
Проблема в том, что учитывается наибольшая интенсивность света, а не интенсивность красного света светодиодов. Я использовал соответствующий шаблон turorial.Это мой код:

#include "opencv2/highgui/highgui.hpp"#include "opencv2/imgproc/imgproc.hpp"#include <iostream>
#include <stdio.h>
#include <windows.h>using namespace std;
using namespace cv;

/// Global Variables
Mat img; Mat templ; Mat result;
Mat redLight;

Mat frame;
char* image_window = "Source Image";
char* result_window = "Result window";
Point poz[2];
int match_method;
/// Function Headers
void MatchingMethod(int, void*);
void RedLight(int, void *);
int cont = 0;
/** @function main */
void pini();//pini
int main()
{
VideoCapture cap(0);
CvCapture* capture = 0;
capture = cvCaptureFromCAM(0);
if (!capture) cout << "No camera detected" << endl;
/// Load image and template

char poza[100];
redLight = imread("red.jpg", 1);
/// Create windows
namedWindow(image_window, CV_WINDOW_AUTOSIZE);
namedWindow(result_window, CV_WINDOW_AUTOSIZE);if (capture)
{
cout << "In capture ..." << endl;
for (;;)
{
Mat save_img; cap >> save_img;

if (save_img.empty())
{
std::cerr << "Something is wrong with the webcam, could not get frame." << std::endl;
}
// Save the frame into a file
imwrite("camera.jpg", save_img);
cont = 0;
match_method = CV_TM_SQDIFF_NORMED;
strcpy(poza, "camera.jpg");
img = imread(poza, 1);
RedLight(0, 0);
waitKey(0);
match_method = CV_TM_CCOEFF;
cont++;
RedLight(0, 0);
waitKey(0);
if (abs(poz[0].x - poz[1].x)< 30)
{
cout << "Match\n";
//pini() apel;
//semafor rosu
}
else
{
//nu e rosu
//forword
}
//Sleep(1000);

}
}return 0;
}
void RedLight(int, void *)
{
Mat img_display;
img.copyTo(img_display);

/// Create the result matrix
int result_cols = img.cols - templ.cols + 1;
int result_rows = img.rows - templ.rows + 1;

result.create(result_cols, result_rows, CV_32FC1);

/// Do the Matching and Normalize
matchTemplate(img, redLight, result, match_method);
normalize(result, result, 0, 1, NORM_MINMAX, -1, Mat());

/// Localizing the best match with minMaxLoc
double minVal; double maxVal; Point minLoc; Point maxLoc;
Point matchLoc;

minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, Mat());

/// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
if (match_method == CV_TM_SQDIFF || match_method == CV_TM_SQDIFF_NORMED)
{
matchLoc = minLoc;
}
else
{
matchLoc = maxLoc;
}
poz[cont] = matchLoc;
/// Show me what you got
rectangle(img_display, matchLoc, Point(matchLoc.x + redLight.cols, matchLoc.y + redLight.rows), Scalar::all(0), 2, 8, 0);
rectangle(result, matchLoc, Point(matchLoc.x + redLight.cols, matchLoc.y + redLight.rows), Scalar::all(0), 2, 8, 0);

imshow(image_window, img_display);
imshow(result_window, result);

return;

}