// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description : This is the entry point of the program.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv) {
// Initialize
// If drone is not connected, initialise webcam
if (!ardrone.open()) {
printf("Drone failed to connect.\n");
isDroneConnected = false;
}
else isDroneConnected = true;
//// DEBUGGING
//isDroneConnected = false;
quitProgram = false;
int patternCount = 0;
visiblePattern = 0;
//lastVisiblePattern = 0;
absoluteControl = false;
vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
points = 0, pSaved = 0, cSaved = 0;
peoplePicked = false, cratePicked = false;
gameTimeStart = cvGetTickCount();
gameTimeLeft = 0;
gameOn = false;
// Loading patterns
LoadPattern(filename1, patternLibrary, patternCount);
LoadPattern(filename2, patternLibrary, patternCount);
LoadPattern(filename3, patternLibrary, patternCount);
LoadPattern(filename4, patternLibrary, patternCount);
LoadPattern(filename5, patternLibrary, patternCount);
LoadPattern(filename6, patternLibrary, patternCount);
LoadPattern(filename7, patternLibrary, patternCount);
LoadPattern(filename8, patternLibrary, patternCount);
LoadPattern(filename9, patternLibrary, patternCount);
LoadPattern(filename10, patternLibrary, patternCount);
LoadPattern(filename11, patternLibrary, patternCount);
cout << patternCount << " patterns are loaded." << endl;
// Pattern detector arguments
int norm_pattern_size = PAT_SIZE;
double fixed_thresh = 40;
double adapt_thresh = 5;//non-used with FIXED_THRESHOLD mode
int adapt_block_size = 45;//non-used with FIXED_THRESHOLD mode
double confidenceThreshold = 0.35;
int mode = 2;//1:FIXED_THRESHOLD, 2: ADAPTIVE_THRESHOLD
// Set to vertical drone camera
ardrone.setCamera(1);
// Initialise PatternDetector
PatternDetector myDetector(fixed_thresh, adapt_thresh, adapt_block_size, confidenceThreshold, norm_pattern_size, mode);
vector<Point2f> coordinates;
Point2f point;
coordinates.push_back(point);
coordinates.push_back(point);
coordinates.push_back(point);
coordinates.push_back(point);
coordinates.push_back(point);
for (int i = 0; i <= patternCount; i++) {
// Initialise each pattern coordinates
patternsCoordinates.push_back(coordinates);
//// Initialise each pattern timer
//int timer;
//timers.push_back(timer);
}
// Start main loop
while (1) {
// check to terminate program
if (quitProgram) break;
// Update
if (!ardrone.update()) break;
// OpenCV image
IplImage *img;
// Check which image source to feed to OpenCV
if (isDroneConnected) {
// Get drone image
img = ardrone.getImage();
}
else {
// Capture webcam feed
webcamCapture = cvCaptureFromCAM(0);
// Get webcam image
img = cvQueryFrame(webcamCapture);
}
// Create image matrix
Mat imgMat = Mat(img);
// Render the HUD
Mat result;
if (isDroneConnected)
result = HUD(imgMat, WIDTH, HEIGHT);
else
result = HUD(imgMat, WIDTH, WEBCAM_HEIGHT);
// Create a buffer of the image
Mat buffer;
result.copyTo(buffer);
// Run the detector
myDetector.detect(imgMat, cameraMatrix, distortions, patternLibrary, detectedPattern);
// Augment the input frame (and print out the properties of pattern if you want)
if (detectedPattern.size()) {
for (unsigned int i = 0; i < detectedPattern.size(); i++) {
// Get pattern id
int id = detectedPattern[i].id;
//// Start current pattern seen timer
//int now = cvGetTickCount();
//int passedSinceSeen = ((now - timers[detectedPattern[i].id]) / (cvGetTickFrequency() * 1000)) / 1000;
//// Only set visible pattern if detected a pattern for more than 1 second
//if (passedSinceSeen > SEEN_TIMER) SetVisiblePattern(detectedPattern[i].id);
SetVisiblePattern(id);
// Drawing
detectedPattern.at(i).draw(buffer, cameraMatrix, distortions);
// Get pattern corner and centre coordinates
Point2f ul, ur, lr, ll, centre;
detectedPattern.at(i).getCoordinates(ul, ur, lr, ll, centre, cameraMatrix, distortions);
// Store coordinates
patternsCoordinates[id][0] = ul;
patternsCoordinates[id][1] = ur;
patternsCoordinates[id][2] = lr;
patternsCoordinates[id][3] = ll;
patternsCoordinates[id][4] = centre;
if (isDroneConnected)
CheckGamePatterns(WIDTH, HEIGHT, id);
else CheckGamePatterns(WIDTH, WEBCAM_HEIGHT, id);
}
}
else {
//for (int i = 0; i < patternCount; i++) {
// // reset pattern timers
// timers[i] = cvGetTickCount();
//}
// Reset visible pattern
visiblePattern = 0;
}
//// Get elapsed time since last saw pattern
//lastVPElapsed = cvGetTickCount();
//int elapsedTime = ((lastVPElapsed - lastVPStart) / (cvGetTickFrequency() * 1000)) / 1000;
//// Check if reset timer limit passed and reset last visible pattern
//if (elapsedTime < 300)
// if (RESET_TIMER - elapsedTime < 0) lastVisiblePattern = 0;
// Get key input
int key = cvWaitKey(33);
// Providing key controls
KeyControls(key);
// Autonomous drone control
KeepGoodAltitude();
AutoAdjustPosition(key);
// Always go back to hovering if no user input, no pattern visible and too old last visible pattern (0)
if (absoluteControl && key < 0) Hover();
else if (key < 0 && visiblePattern == 0/* && lastVisiblePattern == 0*/) Hover();
// Drone movement
ardrone.move3D(vx, vy, vz, vr);
// Check game over
if (gameTimeLeft < 0) {
gameOn = false;
peoplePicked = false;
cratePicked = false;
}
// Combine buffer with original image + opacity
double opacity = 0.4;
addWeighted(buffer, opacity, result, 1 - opacity, 0, result);
// Initialise window with OpenGL support
namedWindow("AR.Drone", WINDOW_OPENGL);
// Show the OpenCV image in a new window AR.Drone
imshow("AR.Drone", result);
// Give HighGUI time to process the draw requests
cvWaitKey(1);
// Clear pattern for next tick
detectedPattern.clear();
}
// Stop processes before quitting
Stop();
return 0;
}
IT_File* ITFile_Load( char *filename ) {
IT_File *itf;
int i;
u32 messageOffset;
u32 *instrumentOffsets;
u32 *sampleOffsets;
u32 *patternOffsets;
myfile = fopen( filename, "rb" );
itf = (IT_File*)malloc(sizeof(IT_File));
memset( itf, 0, sizeof( IT_File ) );
read32(); // IMPM
fread( itf->SongName, 1, 26, myfile );
itf->PatternHighlight = read16();
itf->OrdNum = read16();
itf->InsNum = read16();
itf->SmpNum = read16();
itf->PatNum = read16();
itf->Cwtv = read16();
itf->Cmwt = read16();
itf->Flags = read16();
itf->Special = read16();
itf->GlobalVolume = read8();
itf->MasterVolume = read8();
itf->InitialSpeed = read8();
itf->InitialTempo = read8();
itf->PanningSeparation = read8();
itf->PitchWheelDepth = read8();
itf->MessageLength = read16();
messageOffset = read32();
read32(); // reserved
for( i = 0; i < 64; i++ )
itf->InitialChannelPanning[i] = read8();
for( i = 0; i < 64; i++ )
itf->InitialChannelVolume[i] = read8();
itf->OrderList = (u8*)malloc( itf->OrdNum );
for( i = 0; i < itf->OrdNum; i++ )
itf->OrderList[i] = read8();
instrumentOffsets = (u32*)malloc( itf->InsNum * 4 );
sampleOffsets = (u32*)malloc( itf->SmpNum * 4 );
patternOffsets = (u32*)malloc( itf->PatNum * 4 );
for( i = 0; i < itf->InsNum; i++ )
instrumentOffsets[i] = read32();
for( i = 0; i < itf->SmpNum; i++ )
sampleOffsets[i] = read32();
for( i = 0; i < itf->PatNum; i++ )
patternOffsets[i] = read32();
// load instruments
itf->Instruments = (IT_Instrument*)malloc( sizeof(IT_Instrument) * itf->InsNum );
for( i = 0; i < itf->InsNum; i++ ) {
memset( itf->Instruments+i,0,sizeof( IT_Instrument ) );
if( instrumentOffsets[i] ) {
fseek( myfile, instrumentOffsets[i], SEEK_SET );
LoadInstrument( itf->Instruments+i );
}
}
// load samples
itf->Samples = (IT_Sample*)malloc( sizeof(IT_Sample) * itf->SmpNum );
for( i = 0; i < itf->SmpNum; i++ ) {
memset( itf->Samples+i, 0, sizeof( IT_Sample ) );
if( sampleOffsets[i] ) {
fseek( myfile, sampleOffsets[i], SEEK_SET );
LoadSample( itf->Samples + i );
}
}
// load patterns
itf->Patterns = (IT_Pattern*)malloc( sizeof(IT_Pattern) * itf->PatNum );
for( i = 0; i < itf->PatNum; i++ ) {
memset( itf->Patterns+i, 0, sizeof( IT_Pattern ) );
if( patternOffsets[i] ) {
fseek( myfile, patternOffsets[i], SEEK_SET );
LoadPattern( itf->Patterns + i );
}
}
// read message
if( itf->MessageLength ) {
fseek( myfile, messageOffset, SEEK_SET );
itf->Message = malloc( itf->MessageLength+1 );
itf->Message[itf->MessageLength] = 0;
fread( itf->Message, 1, itf->MessageLength, myfile );
}
return itf;
}
/*
* OpenGL (GLUT) calls this routine to display the scene
*/
void display()
{
// Wait for first reshape
if (H==0) return;
// Set initial pattern
if (N==0)
{
// Clear screen and set color
glClearColor(0,0,0,0);
glClear(GL_COLOR_BUFFER_BIT);
glColor4f(1,0,0,1);
// Draw pattern from file
LoadPattern(file);
}
//
// Compute next generation
//
else
{
// Set shader
glUseProgram(shader);
// Set offsets
int id = glGetUniformLocation(shader,"dX");
if (id>=0) glUniform1f(id,dX);
id = glGetUniformLocation(shader,"dY");
if (id>=0) glUniform1f(id,dY);
id = glGetUniformLocation(shader,"img");
if (id>=0) glUniform1i(id,0);
// Copy original scene to texture
glBindTexture(GL_TEXTURE_2D,img);
glCopyTexImage2D(GL_TEXTURE_2D,0,GL_RGBA8,0,0,W,H,0);
// Redraw the texture
glClear(GL_COLOR_BUFFER_BIT);
glEnable(GL_TEXTURE_2D);
glBegin(GL_QUADS);
glTexCoord2f(0,0); glVertex2f(-1,-1);
glTexCoord2f(0,1); glVertex2f(-1,+1);
glTexCoord2f(1,1); glVertex2f(+1,+1);
glTexCoord2f(1,0); glVertex2f(+1,-1);
glEnd();
glDisable(GL_TEXTURE_2D);
// Shader off
glUseProgram(0);
}
// Lock alpha since to not interfere with game
glColorMask(1,1,1,0);
// Display parameters
glColor4f(1,1,0,0);
glWindowPos2i(5,5);
if (warn) Print("Pattern too large for screen ");
if (move) Print("FPS=%d ",FramesPerSecond());
Print("Generation=%d",N);
glColorMask(1,1,1,1);
// Render the scene and make it visible
ErrCheck("display");
glFlush();
glutSwapBuffers();
// Increment generations
N++;
}
