Ejemplo n.º 1
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;
    }

    // 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;
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
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;
}
Ejemplo n.º 4
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;
}
Ejemplo n.º 5
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;
}