// --------------------------------------------------------------------------
// main(Number of arguments, Value of arguments)
// Description : This is the main function.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Recording flag
int rec = 0;
printf("Press 'R' to start/stop recording.");
// Main loop
while (!GetAsyncKeyState(VK_ESCAPE)) {
// Update
if (!ardrone.update()) break;
// Get an image
IplImage *image = ardrone.getImage();
// Video recording start / stop
if (KEY_PUSH('R')) {
if (rec) {
ardrone.stopVideoRecord();
rec = 0;
}
else {
ardrone.startVideoRecord();
rec = 1;
}
}
// Show recording state
if (rec) {
static CvFont font = cvFont(1.0);
cvPutText(image, "REC", cvPoint(10, 20), &font, CV_RGB(255,0,0));
}
// Display the image
cvShowImage("camera", image);
cvWaitKey(1);
}
// See you
ardrone.close();
return 0;
}
// --------------------------------------------------------------------------
// main(Number of arguments, Value of arguments)
// This is the main function.
// Return value Success:0 Error:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Main loop
while (!GetAsyncKeyState(VK_ESCAPE)) {
// Update your AR.Drone
if (!ardrone.update()) break;
// Get an image
IplImage *image = ardrone.getImage();
// Battery
printf("ardrone.battery = %d [��] (�c���%d��)\n", battery, 12*battery/100);
// Take off / Landing
if (KEY_PUSH(VK_SPACE)) {
if (ardrone.onGround()) ardrone.takeoff();
else ardrone.landing();
}
// Emergency stop
if (KEY_PUSH(VK_RETURN)) ardrone.emergency();
// AR.Drone is flying
if (!ardrone.onGround()) {
double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
// Keyboard
if (KEY_DOWN(VK_UP)) x = 0.5;
if (KEY_DOWN(VK_DOWN)) x = -0.5;
if (KEY_DOWN(VK_LEFT)) r = 0.5;
if (KEY_DOWN(VK_RIGHT)) r = -0.5;
if (KEY_DOWN('Q')) z = 0.5;
if (KEY_DOWN('A')) z = -0.5;
// Joypad
JOYINFOEX JoyInfoEx;
JoyInfoEx.dwSize = sizeof(JOYINFOEX);
JoyInfoEx.dwFlags = JOY_RETURNALL;
// Get joypad infomations
if (joyGetPosEx(0, &JoyInfoEx) == JOYERR_NOERROR) {
int y_pad = -((int)JoyInfoEx.dwXpos - 0x7FFF) / 32512.0*100.0;
int x_pad = -((int)JoyInfoEx.dwYpos - 0x7FFF) / 32512.0*100.0;
int r_pad = -((int)JoyInfoEx.dwZpos - 0x7FFF) / 32512.0*100.0;
int z_pad = ((int)JoyInfoEx.dwRpos - 0x7FFF) / 32512.0*100.0;
printf("X = %d ", x_pad);
printf("Y = %d ", y_pad);
printf("Z = %d ", z_pad);
printf("R = %d\n", r_pad);
x = 0.5 * x_pad / 100;
y = 0.5 * y_pad / 100;
z = 0.5 * z_pad / 100;
r = 0.5 * r_pad / 100;
if (JoyInfoEx.dwButtons & JOY_BUTTON1) ardrone.takeoff();
if (JoyInfoEx.dwButtons & JOY_BUTTON2) ardrone.landing();
}
// Move
ardrone.move3D(x, y, z, r);
}
// Display the image
cvShowImage("camera", image);
cvWaitKey(1);
}
// See you
ardrone.close();
return 0;
}
// --------------------------------------------------------------------------
// main(Number of arguments, Value of arguments)
// Description : This is the main function.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Images
std::vector<IplImage*> images;
printf("Press space key to take a sample picture !\n");
// Main loop
while (!GetAsyncKeyState(VK_ESCAPE)) {
// Update
if (!ardrone.update()) break;
// Get an image
IplImage *image = ardrone.getImage();
// If you push Space key,
if (KEY_PUSH(VK_SPACE)) {
// Convert the camera image to grayscale
IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
cvCvtColor(image, gray, CV_BGR2GRAY);
// Detect the chessboard
int corner_count = 0;
CvSize size = cvSize(PAT_COL, PAT_ROW);
CvPoint2D32f corners[PAT_SIZE];
int found = cvFindChessboardCorners(gray, size, corners, &corner_count);
// Success
if (found) {
// Draw corners.
cvDrawChessboardCorners(image, size, corners, corner_count, found);
// Add to buffer.
images.push_back(gray);
//Beep(3000, 100);
}
// Failed to detect
else {
// Release the image.
cvReleaseImage(&gray);
//Beep(100, 100);
}
}
// Display the image
cvDrawText(image, cvPoint(15, 20), "NUM = %d", (int)images.size());
cvShowImage("camera", image);
cvWaitKey(1);
}
// Destroy the window
cvDestroyWindow("camera");
// At least one image was taken
if (!images.empty()) {
// Total number of images.
const int num = (int)images.size();
//// For debug
//for (int i = 0; i < num; i++) {
// char name[256];
// sprintf(name, "images[%d/%d]", i+1, num);
// cvShowImage(name, images[i]);
// cvWaitKey(0);
// cvDestroyWindow(name);
//}
// Ask save parameters or not
if (ardAsk("Do you save the camera parameters ?\n")) {
// Detect coners
int *p_count = (int*)malloc(sizeof(int) * num);
CvPoint2D32f *corners = (CvPoint2D32f*)cvAlloc(sizeof(CvPoint2D32f) * num * PAT_SIZE);
for (int i = 0; i < num; i++) {
// Detect chessboard
int corner_count = 0;
CvSize size = cvSize(PAT_COL, PAT_ROW);
int found = cvFindChessboardCorners(images[i], size, &corners[i * PAT_SIZE], &corner_count);
// Convert the corners to sub-pixel
cvFindCornerSubPix(images[i], &corners[i * PAT_SIZE], corner_count, cvSize(3, 3), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
p_count[i] = corner_count;
}
// Set the 3D position of patterns
CvPoint3D32f *objects = (CvPoint3D32f*)cvAlloc(sizeof(CvPoint3D32f) * num * PAT_SIZE);
for (int i = 0; i < num; i++) {
for (int j = 0; j < PAT_ROW; j++) {
for (int k = 0; k < PAT_COL; k++) {
objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0;
}
}
}
// Create matrices
CvMat object_points, image_points, point_counts;
cvInitMatHeader(&object_points, num * PAT_SIZE, 3, CV_32FC1, objects);
cvInitMatHeader(&image_points, num * PAT_SIZE, 1, CV_32FC2, corners);
cvInitMatHeader(&point_counts, num, 1, CV_32SC1, p_count);
// Estimate intrinsic parameters and distortion coefficients
printf("Calicurating parameters...");
CvMat *intrinsic = cvCreateMat(3, 3, CV_32FC1);
CvMat *distortion = cvCreateMat(1, 4, CV_32FC1);
cvCalibrateCamera2(&object_points, &image_points, &point_counts, cvGetSize(images[0]), intrinsic, distortion);
printf("Finished !\n");
// Output a file
printf("Generating a XML file...");
CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_WRITE);
cvWrite(fs, "intrinsic", intrinsic);
cvWrite(fs, "distortion", distortion);
cvReleaseFileStorage(&fs);
printf("Finished !\n");
// Release the matrices
free(p_count);
cvFree(&corners);
cvFree(&objects);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
}
// Release the images
for (int i = 0; i < num; i++) cvReleaseImage(&images[i]);
}
// See you
ardrone.close();
return 0;
}
// --------------------------------------------------------------------------
// 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)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Battery
int battery = ardrone.getBatteryPercentage();
printf("ardrone.battery = %d [%%]\n", battery);
// Instructions
printf("***************************************\n");
printf("* CV Drone sample program *\n");
printf("* - Haw To Play - *\n");
printf("***************************************\n");
printf("* *\n");
printf("* - Controls - *\n");
printf("* 'Space' -- Takeoff/Landing *\n");
printf("* 'Up' -- Move forward *\n");
printf("* 'Down' -- Move backward *\n");
printf("* 'Left' -- Turn left *\n");
printf("* 'Right' -- Turn right *\n");
printf("* 'Shift+Up' -- Move upward *\n");
printf("* 'Shift+Down' -- Move downward *\n");
printf("* 'Shift+Left' -- Move left *\n");
printf("* 'Shift+Right' -- Move right *\n");
printf("* *\n");
printf("* - Others - *\n");
printf("* 'C' -- Change camera *\n");
printf("* 'Esc' -- Exit *\n");
printf("* *\n");
printf("***************************************\n\n");
// Main loop
while (!GetAsyncKeyState(VK_ESCAPE)) {
// Update
if (!ardrone.update()) break;
// Get an image
IplImage *image = ardrone.getImage();
// Take off / Landing
if (KEY_PUSH(VK_SPACE)) {
if (ardrone.onGround()) ardrone.takeoff();
else ardrone.landing();
}
// Move
double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
if (KEY_DOWN(VK_SHIFT)) {
if (KEY_DOWN(VK_UP)) vz = 1.0;
if (KEY_DOWN(VK_DOWN)) vz = -1.0;
if (KEY_DOWN(VK_LEFT)) vy = 1.0;
if (KEY_DOWN(VK_RIGHT)) vy = -1.0;
}
else {
if (KEY_DOWN(VK_UP)) vx = 1.0;
if (KEY_DOWN(VK_DOWN)) vx = -1.0;
if (KEY_DOWN(VK_LEFT)) vr = 1.0;
if (KEY_DOWN(VK_RIGHT)) vr = -1.0;
}
ardrone.move3D(vx, vy, vz, vr);
// Change camera
static int mode = 0;
if (KEY_PUSH('C')) ardrone.setCamera(++mode%4);
// Display the image
cvShowImage("camera", image);
cvWaitKey(1);
}
// See you
ardrone.close();
return 0;
}
// --------------------------------------------------------------------------
// main(Number of arguments, Value of arguments)
// Description : This is the main function.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Image of AR.Drone's camera
IplImage *image = ardrone.getImage();
// Valuables for optical flow
IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
IplImage *prev = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
cvCvtColor(image, prev, CV_BGR2GRAY);
IplImage *eig_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *tmp_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *prev_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
IplImage *curr_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
CvPoint2D32f *corners1 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
CvPoint2D32f *corners2 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
// Main loop
while (!GetAsyncKeyState(VK_ESCAPE)) {
// Update
if (!ardrone.update()) break;
// Get an image
image = ardrone.getImage();
// Take off / Landing
if (KEY_PUSH(VK_SPACE)) {
if (ardrone.onGround()) ardrone.takeoff();
else ardrone.landing();
}
// Move
double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
if (KEY_DOWN(VK_UP)) vx = 0.5;
if (KEY_DOWN(VK_DOWN)) vx = -0.5;
if (KEY_DOWN(VK_LEFT)) vr = 0.5;
if (KEY_DOWN(VK_RIGHT)) vr = -0.5;
if (KEY_DOWN('Q')) vz = 0.5;
if (KEY_DOWN('A')) vz = -0.5;
ardrone.move3D(vx, vy, vz, vr);
// Convert the camera image to grayscale
cvCvtColor(image, gray, CV_BGR2GRAY);
// Detect features
int corner_count = 50;
cvGoodFeaturesToTrack(prev, eig_img, tmp_img, corners1, &corner_count, 0.1, 5.0, NULL);
// Corner detected
if (corner_count > 0) {
char *status = (char*)malloc(corner_count * sizeof(char));
// Calicurate optical flows
CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.3);
cvCalcOpticalFlowPyrLK(prev, gray, prev_pyramid, curr_pyramid, corners1, corners2, corner_count, cvSize(10, 10), 3, status, NULL, criteria, 0);
// Drow the optical flows
for (int i = 0; i < corner_count; i++) {
cvCircle(image, cvPointFrom32f(corners1[i]), 1, CV_RGB (255, 0, 0));
if (status[i]) cvLine(image, cvPointFrom32f(corners1[i]), cvPointFrom32f(corners2[i]), CV_RGB (0, 0, 255), 1, CV_AA, 0);
}
free(status);
}
// Save the last frame
cvCopy(gray, prev);
// Display the image
cvShowImage("camera", image);
cvWaitKey(1);
}
// Release the images
cvReleaseImage(&gray);
cvReleaseImage(&prev);
cvReleaseImage(&eig_img);
cvReleaseImage(&tmp_img);
cvReleaseImage(&prev_pyramid);
cvReleaseImage(&curr_pyramid);
free(corners1);
free(corners2);
// See you
ardrone.close();
return 0;
}
