// VideoTool.cpp : Defines the entry point for the console application.
/* This version has been heavily modified by Victoria to detect artificial landmarks for
* the project.
*/
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <tchar.h>
#include <math.h>
// TODO: reference additional headers your program requires here
#include "cv.h"
#include "highgui.h"
// ARGH! why is this necessary ??
#define _NO_HMC_WINSOCK_
#include "SmallSocket.h"
#undef _NO_HMC_WINSOCK_
using namespace std;
// these are the two functions you'll need to write
void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v );
void processLandmarks( IplImage* frame, IplImage* f_p );
void setGlobals(); // the code appears at the very bottom of this file
char wndname[] = "VideoTool";
char wndname2[] = "Undistorted image";
char wndname3[] = "Painted image";
// here, static refers to the fact that these variables
// are local to this file (they're not available in other files,
// which is safer because it allows reuse of their names)
static CvSize imageSize;
static CvPoint old_click_pt;
static CvPoint new_click_pt;
static double framenum;
// global points for drawing boxes, defining loops, etc.
static CvPoint pt1;
static CvPoint pt2;
static CvPoint pt3;
static CvPoint pt4;
static CvPoint pt5;
static CvPoint pt6;
static CvPoint pt7;
// global calibration data
static float* M;
static float* D;
// more global variables
// these are simply recording all the different states you
// can put the program in via the keyboard
static bool bmpRecording = false;
static bool capturing = true;
static bool getImageFromFile = false;
static bool getImageAgainFromFile = false;
static bool getImage0FromVideoBMPs = false;
static bool saveNextImage = false;
static bool quittingTime = false;
static bool detectLandmarks = true;
static bool showing_savedImage = false;
static char* filename = new char[150];
static char* filename2 = new char[150];
static int frameNumber = 0;
static int keyCode = -1;
// pointers to images in memory
static IplImage *frame;
static IplImage* undis_frame;
static IplImage* undis_frame_painted;
static CvArr* undisMap;
// globals for tuning parameters by keyboard...
static int blueValue = 255;
/*
static int redHueHighSatLowerBound = 320;
static int redHueHighSatUpperBound = 25;
static int redHueLowSatLowerBound = 320;
static int redHueLowSatUpperBound = 25;
*/
enum colors{INVALID, WHITE, RED, ORANGE, GREEN, BLUE, BLACK};
struct landmark{
int color1;
int color2;
int color3;
int markNumber;
};
struct landmark* landmarks[7];
static int rvals[7] = {0, 255, 255, 220, 0, 0, 0};
static int gvals[7] = {0, 255, 0, 140, 255, 0, 0};
static int bvals[7] = {0, 255, 0, 90, 0, 255, 0};
static int hueLowerBounds[7] = {0, 30, 320, 10, 70, 200, 0};
// N, ?, 320, 0, 70, ?, ?
static int hueUpperBounds[7] = {0, 100, 15, 30, 160, 240, 360};
// N, ?, 25, 30, 160, 0, 0
static bool hueWraps[7] = {false, false, true, false, false, false, false};
//static int redHueLowerBound = 320;
//static int redHueUpperBound = 25;
//static int numInCenter = 7;
//static int orangeHueLowerBound = 0;
//static int orangeHueUpperBound = 30;
//static int greenHueLowerBound = 70;
//static int greenHueUpperBound = 160;
static double satLowerBounds[7] = {0.0, 0.0, 0.50, 0.50, 0.30, 0.20, 0.0};
// NNN, ???, ????, 0.30, 0.30, ????, ???
//static double satLowerBound = 0.30;
static double satUpperBounds[7] = {0.0, 0.30, 1.0, 1.0, 1.0, 1.0, 1.0};
static double valLowerBounds[7] = {0.0, 0.40, 0.10, 0.10, 0.10, 0.10, 0.0};
//static double valLowerBound = 0.10;
static double valUpperBounds[7] = {0.0, 1.0, 0.99, 1.0, 0.99, 0.99, 0.3};
//static double valUpperBound = 0.99;
// Definitions for the minimum sizes of landmarks
/* for a vertical line to count as being "through" a landmark,
it must be at least minVertLen long, with at least minPercentColor
of the pixels in said line being the appropriate color. The next
appropriate color must appear before maxSectionSep pixels, and meet
the same requirements. Likewise for the third section of the landmark. */
static int minVertLen = 5;
static int minHorizLen = 50;
static int maxSectionSep = 15;
static double minPercentColor = 0.90;
// Information to report back to our master control when it asks over the socket
static bool isLandmark = false;
static int whichLandmark = 0;
static int distLandmark = 0;
bool isColor(int color, double h, double s, double v)
{
//double lowerbound = (redHueHighSatLowerBound - redHueLowSatLowerBound) * val + redHueLowSatLowerBound;
//double upperbound = (redHueHighSatUpperBound - redHueLowSatUpperBound) * val + redHueLowSatUpperBound;
/*cout<<"Checking for "<<color<<endl;
cout<<"hue is "<<h<<" should be between "<<hueLowerBounds[color]<<" and "<<hueUpperBounds[color]<<endl;
cout<<"sat is "<<s<<" should be between "<<satLowerBounds[color]<<" and "<<satUpperBounds[color]<<endl;
cout<<"val is "<<v<<" should be between "<<valLowerBounds[color]<<" and "<<valUpperBounds[color]<<endl;
*/
if ( (( !hueWraps[color] && h > hueLowerBounds[color] && h < hueUpperBounds[color]) ||
((hueWraps[color] && (h > hueLowerBounds[color] || h < hueUpperBounds[color])))) &&
(s > satLowerBounds[color] && s < satUpperBounds[color]) &&
(v > valLowerBounds[color] && v < valUpperBounds[color]))
return true;
return false;
}
// handle mouse clicks here
void mouse_callback(int event, int x, int y, int flags)
{
if (event == CV_EVENT_LBUTTONDOWN)
{
// cout << "(x,y) = (" << x << "," << y << ")" << endl;
// reset old_click_pt
old_click_pt.x = new_click_pt.x;
old_click_pt.y = new_click_pt.y;
// get new click point -- note the coordinate change in y
new_click_pt.x = x; // coming in from the window system
new_click_pt.y = imageSize.height-y; // window system and images have different y axes
}
}
// print pixel information
void pixelInformation(IplImage* f)
{
if (f == NULL) return;
// see if things have changed...
if (new_click_pt.x != old_click_pt.x || new_click_pt.y != old_click_pt.y) // has it changed?
{
// draw a line from old to new
// cvLine(frame,old_click_pt,new_click_pt,CV_RGB(255,0,0),/*thickness*/1,/*connectivity*/8);
// set the old = the new
old_click_pt.x = new_click_pt.x;
old_click_pt.y = new_click_pt.y;
// print the RGB values and coordinates of the newly clicked point:
// note that blue is #0, green is #1, and red is #2 !!
/* note that this uses the undistorted frame right here... */
// this is insane!!!
// see the processRed function for an explanation
// of why we need "realy"
int realy = new_click_pt.y;
if (getImageAgainFromFile || getImageFromFile)
{
realy = imageSize.height - realy;
}
uchar* pixel = cvPtr2D(f,realy,new_click_pt.x);
cout << "(x,y) = (" << new_click_pt.x << "," << new_click_pt.y << ") with (r,g,b) = ("
<< (int)(pixel[2]) << "," << (int)(pixel[1]) << "," << (int)(pixel[0]) << ")";
double hue,sat,val;
RGBtoHSV((double)pixel[2],(double)pixel[1],(double)pixel[0],&hue,&sat,&val);
cout << " and (h,s,v) = (" << hue << "," << sat << "," << val << ")" << endl;
}
}
// the thread for the server
DWORD WINAPI ThreadFunc( LPVOID lpParam )
{
char recvbuf[80];
char sendbuf[80];
string t("HI");
SmallSocket SS(SmallSocket::SERVER);
sprintf( recvbuf, "Server Started", *(DWORD*)lpParam );
//MessageBox( NULL, szMsg, "ThreadFunc", MB_OK );
while (true)
{
SS.receive(recvbuf,80);
cout << "Received: " << recvbuf << endl;
if (recvbuf[0] == 'q') {
SS.sendout("q",2);
// set a variable that tells it to quit...
quittingTime = true;
// cvReleaseCapture( &capture );
// cvDestroyWindow("result");
break;
}
if (recvbuf[0] == 'e') {
// just an example of passing in more information
// and using sscanf to extract it
// sscanf(recvbuf,"q %lf %lf %lf ",&savedXforLog,&savedYforLog,&savedTforLog);
//
//the connection requires that something come back...
// note that we're sending two zeros every time here...
sprintf(sendbuf, "%d %d", 0, 0);
SS.sendout(sendbuf,80);
}
if (recvbuf[0] == 'l') {
// just an example of passing in more information
// and using sscanf to extract it
// sscanf(recvbuf,"q %lf %lf %lf ",&savedXforLog,&savedYforLog,&savedTforLog);
//
//the connection requires that something come back...
// note that we're sending two zeros every time here...
sprintf(sendbuf, "%d %d %d \n", isLandmark, whichLandmark, distLandmark);
for(int i = 0; i<80; i++)
{
if(sendbuf[i] == '\0')
{
break;
}
}
SS.sendout(sendbuf,i);
}
// pause for a bit...
Sleep(10);
}
return 0;
}
int main( int argc, char** argv )
{
// in case you want to log things...
//ofstream logfile("logfile_out.txt");
//ifstream loginfile("logfile_in.txt");
// set up the globals...
setGlobals();
// set up the landmarks which are valid
for (int i = 0; i<7; ++i)
{
landmarks[i] = new struct landmark;
}
landmarks[0]->color1 = ORANGE;
landmarks[0]->color2 = GREEN;
landmarks[0]->color3 = ORANGE;
landmarks[0]->markNumber = 0;
landmarks[1]->color1 = BLUE;
landmarks[1]->color2 = RED;
landmarks[1]->color3 = BLUE;
landmarks[1]->markNumber = 1;
landmarks[2]->color1 = RED;
landmarks[2]->color2 = BLUE;
landmarks[2]->color3 = RED;
landmarks[2]->markNumber = 2;
landmarks[3]->color1 = GREEN;
landmarks[3]->color2 = BLUE;
landmarks[3]->color3 = GREEN;
landmarks[3]->markNumber = 3;
landmarks[4]->color1 = BLUE;
landmarks[4]->color2 = GREEN;
landmarks[4]->color3 = BLUE;
landmarks[4]->markNumber = 4;
landmarks[5]->color1 = INVALID;
landmarks[5]->color2 = INVALID;
landmarks[5]->color3 = INVALID;
landmarks[5]->markNumber = 5;
landmarks[6]->color1 = INVALID;
landmarks[6]->color2 = INVALID;
landmarks[6]->color3 = INVALID;
landmarks[6]->markNumber = 6;
// start the server's thread
// make sure that all imageprocessing is in main's thread
DWORD dwThreadId, dwThrdParam = 1;
HANDLE hThread;
char szMsg[80];
hThread = CreateThread(
NULL, // default security attributes
0, // use default stack size
ThreadFunc, // thread function
&dwThrdParam, // argument to thread function
0, // use default creation flags
&dwThreadId); // returns the thread identifier
// Check the return value for success.
if (hThread == NULL)
{
sprintf( szMsg, "CreateThread failed." );
MessageBox( NULL, szMsg, "main", MB_OK );
}
/*
* set up the video windows
*/
cvNamedWindow( wndname, CV_WINDOW_AUTOSIZE );
cvNamedWindow( wndname2, CV_WINDOW_AUTOSIZE );
cvNamedWindow( wndname3, CV_WINDOW_AUTOSIZE );
/*
* set up the mouse input
*/
//cvSetMouseCallback( wndname, mouse_callback );
cvSetMouseCallback( wndname2, mouse_callback );
/*
* more camera-specific stuff
*/
CvCapture* capture = 0;
capture = cvCaptureFromCAM(0);
// this is allocating the undis_frame once (it will get written each time)
// get one frame and then copy it...
if (cvGrabFrame(capture))
{
frame = cvRetrieveFrame( capture );
undis_frame = cvCloneImage(frame);
undis_frame_painted = cvCloneImage(frame);
// now let's set up the undistortion parameters
// note that the docs say 3*wider, but that did not work
// 3*wider _and_ 3*higher worked...
undisMap = cvCreateMat( imageSize.width*3, imageSize.height*3, CV_32SC3 );
// need to release this at the end of the program...
cvUnDistortInit( frame, undisMap, M, D, 1 );
}
else
{
cout << "No camera??" << endl;
capturing = false;
}
// main loop
while (!quittingTime)
{
if (capturing)
{
if( !cvGrabFrame( capture )) break;
frame = cvRetrieveFrame( capture );
if( !frame ) break;
// cvUnDistortOnce( frame, undis_frame, M, D, 1 ); // might take some extra time...
cvUnDistort( frame, undis_frame, undisMap, 1 );
cvReleaseImage(&undis_frame_painted);
undis_frame_painted = cvCloneImage(undis_frame);
}
else if (getImageFromFile || getImageAgainFromFile)
{
if (getImageFromFile) {
// ask for a frame filename
int i;
// should factor this path out!!
if (getImage0FromVideoBMPs) {
i = 0;
getImage0FromVideoBMPs = false;
}
else {
cout << "What image number (in C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs): ";
cin >> i;
}
sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",i);
// we probably need to release the images returned by cvLoadImage somewhere...
//frame = cvLoadImage(filename2);
//undis_frame = cvCloneImage(frame);
cvReleaseImage(&undis_frame);
undis_frame = cvLoadImage(filename2);
cvReleaseImage(&undis_frame_painted);
undis_frame_painted = cvLoadImage(filename2);
if (undis_frame == NULL) {
cout << "Could not find that image.\n";
continue;
}
getImageFromFile = false;
getImageAgainFromFile = true;
}
else if (getImageAgainFromFile) {
cvReleaseImage(&undis_frame_painted);
undis_frame_painted = cvCloneImage(undis_frame);
}
}
else
{
;
}
// click only on undis_image
pixelInformation(undis_frame_painted);
// save the images to a file, if desired
if (bmpRecording == true && capturing == true)
{
sprintf(filename,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\BMPs\\image%05d.bmp",frameNumber);
cout << "Captured single image (dist): " << filename << endl;
cvSaveImage(filename,frame);
sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\BMPsUnd\\image%05d.bmp",frameNumber);
cout << "Captured single image (undist): " << filename2 << endl;
cvSaveImage(filename2,undis_frame);
++frameNumber;
//saveNextImage = false;
//if (getImageFromFile) {
// bmpRecording = false;
//}
}
// here we can add graphics to each frame
// in order that they show up on the screen
// we also decide what to display in this routine
if (detectLandmarks == true)
{
processLandmarks(undis_frame,undis_frame_painted);
}
/*
* display everything in the windows
*/
cvShowImage(wndname, frame);
cvShowImage(wndname2, undis_frame);
cvShowImage(wndname3, undis_frame_painted);
//handle key presses
/*
* quits
*/
if( (keyCode = cvWaitKey( 10 )) == ((int)'q') ) {
break; // quit everything
}
/*
* we use = and - to increase and decrease blueValue
*/
else if (keyCode == (int)'=') {
if (blueValue < 251) blueValue += 5;
}
else if (keyCode == (int)'-') {
if (blueValue > 4) blueValue -= 5;
}
/*
* we use n and m to increase and decrease redLowerBound,
* and , and . to increase and decrease redUpperBound.
*
else if (keyCode == (int)'n') {
if (redHueLowSatLowerBound - 1 > redHueLowSatUpperBound) --redHueLowSatLowerBound;
cout << "Low Saturation Red is [" << redHueLowSatLowerBound << "..." << redHueLowSatUpperBound << "]" << endl;
}
else if (keyCode == (int)'m') {
if (redHueLowSatLowerBound + 1 <= 360) ++redHueLowSatLowerBound;
cout << "Low Saturation [" << redHueLowSatLowerBound << "..." << redHueLowSatUpperBound << "]" << endl;
}
else if (keyCode == (int)',') {
if (redHueLowSatUpperBound - 1 >= 0) --redHueLowSatUpperBound;
cout << "Low Saturation [" << redHueLowSatLowerBound << "..." << redHueLowSatUpperBound << "]" << endl;
}
else if (keyCode == (int)'.') {
if (redHueLowSatUpperBound + 1 < redHueLowSatLowerBound) ++redHueLowSatUpperBound;
cout << "Low Saturation [" << redHueLowSatLowerBound << "..." << redHueLowSatUpperBound << "]" << endl;
}
else if (keyCode == (int)'j') {
if (redHueHighSatLowerBound - 1 > redHueHighSatUpperBound) --redHueHighSatLowerBound;
cout << "High Saturation [" << redHueHighSatLowerBound << "..." << redHueHighSatUpperBound << "]" << endl;
}
else if (keyCode == (int)'k') {
if (redHueHighSatLowerBound + 1 <= 360) ++redHueHighSatLowerBound;
cout << "High Saturation [" << redHueHighSatLowerBound << "..." << redHueHighSatUpperBound << "]" << endl;
}
else if (keyCode == (int)'l') {
if (redHueHighSatUpperBound - 1 >= 0) --redHueHighSatUpperBound;
cout << "High Saturation [" << redHueHighSatLowerBound << "..." << redHueHighSatUpperBound << "]" << endl;
}
else if (keyCode == (int)';') {
if (redHueHighSatUpperBound + 1 < redHueHighSatLowerBound) ++redHueHighSatUpperBound;
cout << "High Saturation [" << redHueHighSatLowerBound << "..." << redHueHighSatUpperBound << "]" << endl;
}
*/
/*
* records video to bitmaps (files)
*/
else if (keyCode == (int)'f') {
if (bmpRecording) {
cout << "Stopping the writing to bitmaps\n";
bmpRecording = false;
} else {
cout << "Starting to write to bitmaps\n";
bmpRecording = true;
}
showing_savedImage = false;
}
/*
* gets image from a file # in ./VideoBMPs
*/
else if (keyCode == (int)'g') {
cout << "Getting image from file.\n";
capturing = false;
getImageFromFile = true;
getImageAgainFromFile = false;
bmpRecording = false;
saveNextImage = false;
showing_savedImage = true;
}
/*
* toggle continuous video
*/
else if (keyCode == (int)'s') {
if (capturing) {
cout << "Stopping continuous capturing.\n";
capturing = false;
getImageFromFile = false;
}
else {
cout << "Starting continuous capturing.\n";
capturing = true;
getImageFromFile = false;
getImageAgainFromFile = false;
/* reset undis_image */
// note this assumes that everything went well
// initially with the first captured frame... !!
cvReleaseImage(&undis_frame);
undis_frame = cvCloneImage(frame);
cvReleaseImage(&undis_frame_painted);
undis_frame_painted = cvCloneImage(frame);
// not to mention that these transition images have to be released!
}
showing_savedImage = false;
}
else if (keyCode == (int)'e') {
// emulate server behavior
saveNextImage = true;
showing_savedImage = false;
}
/*
* toggle finding red (and displaying)
*/
else if (keyCode == (int)'r') {
detectLandmarks = !detectLandmarks;
cout << "Toggled landmark detection. Landmark detection is ";
cout << (detectLandmarks ? "active." : "inactive") << endl;
}
/*
* the spacebar advances the image in the VideoBMPs directory
*/
else if (keyCode == (int)' ') {
if (showing_savedImage == true)
{
int i;
sscanf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",&i);
sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",++i);
cvReleaseImage(&undis_frame);
undis_frame = cvLoadImage(filename2);
cvReleaseImage(&undis_frame_painted);
undis_frame_painted = cvLoadImage(filename2);
// wrap around!
if (undis_frame == NULL)
{
i = 0;
sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",i);
cvReleaseImage(&undis_frame);
undis_frame = cvLoadImage(filename2);
cvReleaseImage(&undis_frame_painted);
undis_frame_painted = cvLoadImage(filename2);
continue;
}
getImageAgainFromFile = true;
getImageFromFile = false;
}
/*
* initial hit of the space bar...
*/
else {
cout << "No image from file is currently showing..." << endl;
cout << "Getting image 0 from VideoBMPs.\n";
capturing = false;
getImageFromFile = true;
getImageAgainFromFile = false;
getImage0FromVideoBMPs = true;
bmpRecording = false;
saveNextImage = false;
showing_savedImage = true;
}
}
}
// release things before quitting
cvReleaseCapture( &capture );
cvDestroyWindow("result");
} // end of main
// just in case you'd like to use these...
inline double mymin(double a, double b) { return a<b?a:b; }
inline double mymax(double a, double b) { return a>b?a:b; }
// read about HSV at http://en.wikipedia.org/wiki/HSV_color_space
// and http://www.cs.rit.edu/~ncs/color/t_convert.html#RGB%20to%20HSV%20&%20HSV%20to%20RGB
//
// The function below was copied directly from the second webpage cited above and tweaked only slightly.
//
void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v )
{
r = r/255;
g = g/255;
b = b/255;
double min, max, delta;
min = mymin(mymin( r, g), b );
max = mymax(mymax( r, g), b );
*v = max; // v
delta = max - min;
if( max != 0 )
*s = delta / max; // s
else {
// r = g = b = 0 // s = 0, v is undefined
*s = 0;
*h = -1;
return;
}
if( r == max )
*h = ( g - b ) / delta; // between yellow & magenta
else if( g == max )
*h = 2 + ( b - r ) / delta; // between cyan & yellow
else
*h = 4 + ( r - g ) / delta; // between magenta & cyan
*h *= 60; // degrees
if( *h < 0 )
*h += 360;
}
// here is where we seek out the red molding and indicate what we've found
// the input named "frame" is the source location of our pixels
// the "output" named f_p is the destination for drawing
void processLandmarks(IplImage* frame, IplImage* f_p)
{
// some useful local variables...
int r,g,b;
double hue,sat,val;
// pixels are handled as uchar*'s
uchar* color; // a pixel from the input, "frame"
uchar* painted; // a pixel from the output, "painted"
// this draws a rectangle from pt4 to pt5 with the color
// specified by the three RGB values in the CV_RGB macro
// the R, G, and B channels are all 0 to 255...
// see the "this is insane!" caution below in order to
// use graphics on the live video stream...
cvRectangle(f_p, pt4, pt5, CV_RGB(0,0,255), 1); // 1 == thickness
// cvLine(f_p, pt4, pt5,CV_RGB(0,255,0),1,8); // 1 == thickness, 8 = connectedness
// This loops through the pixels in the rectangle specified
// by pt4 and pt5 (see setGlobals for their corners)
int xlow = (int)mymin(pt4.x,pt5.x);
int xhi = (int)mymax(pt4.x,pt5.x);
int ylow = (int)mymin(pt4.y,pt5.y);
int yhi = (int)mymax(pt4.y,pt5.y);
// Start doing stuff here.
// Parameters.
int samplerate = 1;
double mouldingthresh = 0.2;
int numsamples = (xhi-xlow)/samplerate + 1;
int* yminvals = new int[numsamples];
int* ymaxvals = new int[numsamples];
int xentry = 0;
int numreds = 0;
// First, we create the arrays to hold pointers to the fun stuff for each landmark
int* landNumInCol = new int[3]; // the number of pixels found thus far in this part of the landmark
int* landNumInColColor = new int[3]; // the number of above which are the correct color
int* landXStart = new int[3]; // The beginning of the landmark
int* landYStart = new int[3]; // The beginning of the landmark
int* landXEnd = new int[3]; // The end of the landmark
int* landYEnd = new int[3]; // The end of the landmark
// Zero everything
for(int i = 0; i<3; ++i)
{
landNumInCol[i] = 0;
landNumInColColor[i] = 0;
landXStart[i] = 0;
landYStart[i] = 0;
landXEnd[i] = 0;
landYEnd[i] = 0;
}
int landNumSep = 0; // The number of pixels since the end of the last section
/* this can be 0, 1, 2, or 3. 0-2 are landmarks we look for, 3 means we have all three of them. */
int landSearchFor = 0;
bool inLandmark = false; // We are currently inside some section of the landmark
bool nextLandmarkFound = false; // WTF??
for (int x = xlow; x<xhi; x++) //the x direction goes left to right
{
int ymax = -1;
int ymin = yhi + 1;
for(int currMark = 0; currMark < 7; currMark++)
{
int color1 = landmarks[currMark]->color1;
int color2 = landmarks[currMark]->color2;
int color3 = landmarks[currMark]->color3;
for (int y = ylow; y<yhi; y++) //the y direction goes top to bottom
{
// this is insane!!!
// unfortunately, access to the pixels is different depending on
// the source of the pixels: static images and video reverse their
// y orientations
// we handle this here...
int realy = y;
if (getImageAgainFromFile || getImageFromFile)
{
realy = imageSize.height - realy; // reverse if static image
}
// note that we need to use "realy" in each of these cases
color = cvPtr2D(frame,realy,x); // get the pixel we care about
painted = cvPtr2D(f_p,realy,x); // get the pixel we (might) change
r = (int)(color[2]); // the red component of the pixel we care about
g = (int)(color[1]); // the green component of the pixel we care about
b = (int)(color[0]); // the blue component of the pixel we care about
// you might want to call this here!
RGBtoHSV(r,g,b,&hue,&sat,&val);
// And here we begin the great search
if (isColor(color1, hue,sat,val)) // Then we've found what might be the first part!
{
if (landSearchFor == 0) // This is the first section of the landmark
{
if(landNumInCol[landSearchFor] == 0)
{
/* We need to mark this as the starting point. */
landXStart[landSearchFor] = x;
landYStart[landSearchFor] = y;
inLandmark = true;
}
++landNumInCol[landSearchFor];
++landNumInColColor[landSearchFor];
landNumSep = 0;
}
else if(landSearchFor == 1 && inLandmark)
{
++landNumInCol[landSearchFor];
}
else if(!inLandmark)
{
++landNumSep;
}
else if (color1 != color3 && landSearchFor == 2 && inLandmark)
{
++landNumInCol[landSearchFor];
}
}
if (isColor(color2, hue,sat,val))
{
if (landSearchFor == 1)
{
if(landNumInCol[landSearchFor] == 0)
{
/* We need to mark this as the starting point. */
landXStart[landSearchFor] = x;
landYStart[landSearchFor] = y;
inLandmark = true;
}
++landNumInCol[landSearchFor];
++landNumInColColor[landSearchFor];
landNumSep = 0;
}
if (landSearchFor == 0 && inLandmark)
{
if(landNumInCol[0] > minVertLen)
{
++landSearchFor;
/* We need to mark this as the starting point. */
landXStart[landSearchFor] = x;
landYStart[landSearchFor] = y;
inLandmark = true;
++landNumInCol[landSearchFor];
++landNumInColColor[landSearchFor];
}
else
{
++landNumInCol[landSearchFor];
}
}
else if ((landSearchFor == 2) && inLandmark)
{
/* looking for color 2 now, and this isn't it. Note that and move on */
++landNumInCol[landSearchFor];
}
else if (!inLandmark)
{
landNumSep++;
}
}
if (isColor(color3, hue,sat,val))
{
if (landSearchFor == 2)
{
if(landNumInCol[landSearchFor] == 0)
{
/* We need to mark this as the starting point. */
landXStart[landSearchFor] = x;
landYStart[landSearchFor] = y;
inLandmark = true;
}
++landNumInCol[landSearchFor];
++landNumInColColor[landSearchFor];
landNumSep = 0;
}
if (landSearchFor == 1 && inLandmark)
{
if(landNumInCol[1] > minVertLen)
{
++landSearchFor;
/* We need to mark this as the starting point. */
landXStart[landSearchFor] = x;
landYStart[landSearchFor] = y;
inLandmark = true;
++landNumInCol[landSearchFor];
++landNumInColColor[landSearchFor];
}
else
{
++landNumInCol[landSearchFor];
}
}
if (!inLandmark)
{
++landNumSep;
}
if(color1 != color3 && landSearchFor == 0 && inLandmark)
{
++landNumInCol[landSearchFor];
}
}
if (!( isColor(color1, hue,sat,val) || isColor(color2, hue,sat,val) || isColor(color3, hue,sat,val)))/*neither orange nor green */
{
if(inLandmark)
{
++landNumInCol[landSearchFor];
}
else
{
++landNumSep;
}
}
if (currMark == 6) // all done with this row
{
for(int i = 1; i<7; ++i)
{
if(isColor(i, hue, sat, val))
{
painted[0] = bvals[i];
painted[1] = gvals[i];
painted[2] = rvals[i];
}
}
}
/* Now, we check to see if we got thrown out of a landmark this time, or if other fun stuff has happened */
if(inLandmark)
{
if((((double)landNumInColColor[landSearchFor] / landNumInCol[landSearchFor]) < minPercentColor) && landNumInCol[landSearchFor] > 2)
{
if(landNumInCol[landSearchFor] > minVertLen)
{
landXEnd[landSearchFor] = x;
landYEnd[landSearchFor] = y;
landSearchFor++;
inLandmark = false;
}
else
{
/* no landmarky goodness here. Try again starting here. */
for(int i = 0; i<3; ++i)
{
landNumInCol[i] = 0;
landNumInColColor[i] = 0;
landXStart[i] = 0;
landYStart[i] = 0;
landXEnd[i] = 0;
landYEnd[i] = 0;
}
landNumSep = 0;
landSearchFor = 0;
inLandmark = false;
}
}
}
else if(landSearchFor != 3)
{
if(landNumSep > maxSectionSep)
{
/* no landmarky goodness here. Try again starting here. */
for(int i = 0; i<3; ++i)
{
landNumInCol[i] = 0;
landNumInColColor[i] = 0;
landXStart[i] = 0;
landYStart[i] = 0;
landXEnd[i] = 0;
landYEnd[i] = 0;
}
landNumSep = 0;
landSearchFor = 0;
inLandmark = false;
}
}
}
if(landSearchFor == 2)
{
if(landNumInCol[landSearchFor] > minVertLen)
{
landXEnd[landSearchFor] = x;
landYEnd[landSearchFor] = y;
landSearchFor++;
inLandmark = false;
}
else
{
/* no landmarky goodness here. Try again starting here. */
for(int i = 0; i<3; ++i)
{
landNumInCol[i] = 0;
landNumInColColor[i] = 0;
landXStart[i] = 0;
landYStart[i] = 0;
landXEnd[i] = 0;
landYEnd[i] = 0;
}
landNumSep = 0;
landSearchFor = 0;
inLandmark = false;
}
}
if(landSearchFor !=3)
{
/* no landmarky goodness here. Try again starting here. */
for(int i = 0; i<3; ++i)
{
landNumInCol[i] = 0;
landNumInColColor[i] = 0;
landXStart[i] = 0;
landYStart[i] = 0;
landXEnd[i] = 0;
landYEnd[i] = 0;
}
landNumSep = 0;
landSearchFor = 0;
inLandmark = false;
nextLandmarkFound = false;
isLandmark = false;
whichLandmark = 0;
distLandmark = 0;
}
else
{
isLandmark = true;
whichLandmark = currMark;
distLandmark = landYEnd[2]-landYStart[2];
/* we found a landmark. Draw a pretty line where we think it runs from/to */
CvPoint start, end;
start.x = landXStart[0];
start.y = landYStart[0];
end.x = landXEnd[2];
end.y = landYEnd[2];
cvLine(f_p, start, end,CV_RGB(255,128,255),2,8); // 1 == thickness, 8 = connectedness
cout<<"Found landmark "<<currMark<<endl;
return;
}
}
}
cout<<"No landmark found. Last values were: "<<endl;
cout<<"landSearchFor = "<<landSearchFor<<endl;
cout<<"landNumInColColor = "<<landNumInColColor[0]<<", "<<landNumInColColor[1]<<", "<<landNumInColColor[2]<<endl;
cout<<"landYStart[0] = "<<landYStart[0]<<endl;
}
void setGlobals()
{
// mouse click locations (old and new, to tell if we've
// clicked on a new point
old_click_pt.x = 0;
old_click_pt.y = 0;
new_click_pt.x = 0;
new_click_pt.y = 0;
// in theory these should get deleted at the end of the program, too...
// this is the calibration matrix of the camera
M = new float[9];
M[0]= 393.1788940f; M[1]= 0.0000000f; M[2]= 162.8373871f;
M[3]= 0.0000000f; M[4]= 395.7017517f; M[5]= 121.8078156f;
M[6]= 0.0000000f; M[7]= 0.0000000f; M[8]= 1.0000000f;
// these are the distortion parameters of the camera
D = new float[4];
D[0]=-0.358375f;
D[1]=0.168058f;
D[2]=-0.006497f;
D[3]=0.001283f;
// theseimage sizes are hard-coded, but should probably be obtained for each
// image individually -- however, with our cameras and images, this is safe.
imageSize.width = 320; imageSize.height = 240;
// set up some global points for drawing, etc.
int margin = 12;
pt1.x = margin; pt1.y = margin;
pt2.x = imageSize.width-margin; pt2.y = imageSize.height-margin;
pt3.x = imageSize.width/2;
pt3.y = imageSize.height/2;
pt4.x = 80;
pt4.y = 12;
pt5.x = 240;
pt5.y = 228;
pt6.x = 92;
pt6.y = 24;
pt7.x = 228;
pt7.y = 216;
}
syntax highlighted by Code2HTML, v. 0.9.1