C++ ARDrone::close方法代码示例

// --------------------------------------------------------------------------
// 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;
    }

    // Image of AR.Drone's camera
    IplImage *image = ardrone.getImage();

    // Read intrincis camera parameters
    CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_READ);
    CvMat *intrinsic = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, NULL, "intrinsic"));
    CvMat *distortion = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, NULL, "distortion"));

    // Initialize undistortion maps
    CvMat *mapx = cvCreateMat(image->height, image->width, CV_32FC1);
    CvMat *mapy = cvCreateMat(image->height, image->width, CV_32FC1);
    cvInitUndistortMap(intrinsic, distortion, mapx, mapy);

    // Main loop
    while (1) {
        // Key input
        int key = cvWaitKey(1);
        if (key == 0x1b) break;

        // Update
        if (!ardrone.update()) break;

        // Get an image
        image = ardrone.getImage();

        // Remap the image
        cvRemap(image, image, mapx, mapy);

        // Display the image
        cvShowImage("camera", image);
    }

    // Release the matrices
    cvReleaseMat(&mapx);
    cvReleaseMat(&mapy);
    cvReleaseFileStorage(&fs);

    // 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;
    }

    // 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, 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;
    }

    // Recording flag
    bool rec = false;
    printf("Press 'R' to start/stop recording.");

    // Main loop
    while (1) {
        // Key input
        int key = cvWaitKey(1);
        if (key == 0x1b) break;

        // Update
        if (!ardrone.update()) break;

        // Get an image
        IplImage *image = ardrone.getImage();

        // Video recording start / stop
        if (key == 'r') {
            rec = !rec;
            ardrone.setVideoRecord(rec);
        }

        // 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);
    }

    // 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()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Image of AR.Drone's camera
    cv::Mat image = ardrone.getImage();
    
    // Video name
    std::time_t t = std::time(NULL);
    std::tm *local = std::localtime(&t);
    std::ostringstream stream;
    stream << 1900 + local->tm_year << "-" << 1 + local->tm_mon << "-" << local->tm_mday << "-" << local->tm_hour << "-" << local->tm_min << "-" << local->tm_sec << ".avi";

    // Create a video writer
    cv::VideoWriter writer(stream.str(), cv::VideoWriter::fourcc('D', 'I', 'B', ' '), 30, cv::Size(image.cols, image.rows));

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Get an image
        image = ardrone.getImage();

        // Write a frame
        writer << image;

        // Display the image
        imshow("camera", image);
    }

    // Output the video
    writer.release();

    // See you
    ardrone.close();

    return 0;
}

int main(){
	cout<<"Hol<"<<endl;

	//Mat img=imread("/home/mottamx/Pictures/batman.jpg");
	//namedWindow("batman");
	//imshow("batman", img);
	//waitKey(1);
	cout<<"Hol<<"<<endl;
	ARDrone ardrone;
	if (!ardrone.open()){
		cout<<"error de comunicación"<<endl;
		return -1;
	}
	cout<<"Bat: "<<ardrone.getBatteryPercentage()<<endl;
	sleep(2);
	cout<<"Bat: "<<ardrone.getBatteryPercentage()<<endl;
	sleep(2);
	cout<<"Bat: "<<ardrone.getBatteryPercentage()<<endl;
	sleep(2);
	time_t start, end;
	time (&start);
	cout.flush();
	double elapsed=0;
	namedWindow("dron");
	namedWindow("dron2");
	while(elapsed<5){
		//sleep(2);
		IplImage *im=ardrone.getImage();
		Mat img = Mat(im);
		Mat img2;
		resize(img, img2, Size(), 2,2, INTER_AREA);
		resize(img, img, Size(), 2,2, INTER_LANCZOS4);
		imshow("dron", img);
		waitKey(1);
		imshow("dron2", img2);
		waitKey(1);
		time(&end);
		elapsed=difftime(end,start);
	}
	cout<<"Tiempo: "<<setprecision(3)<<elapsed<<"segundos"<<endl;
	cout<<"<dios"<<endl;

	ardrone.close();
	ardrone.emergency();
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)
{
	player1 = new XboxController(1);

    // Initialize
    /*if (!ardrone.open()) 
	{
        printf("Failed to find the AR.Drone. Please ensure that the drone is connected via WiFi.\n");
		system("Pause");
        return -1;
    }*/
	
    // Update your AR.Drone
    /*if (!ardrone.update())
	{
		printf("This AR.Drone is not up to date, please update the AR.Drone before use.\n");
		system("Pause");
        return -1;
	}*/
    // Main loop
    while (!GetAsyncKeyState(VK_ESCAPE)) 
	{
        // Get an image
        IplImage *image = ardrone.getImage();

		controls();
		display();

        // Display the image
        cvShowImage("camera", image);
        cvWaitKey(1);
    }

    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;
}

//.........这里部分代码省略.........
    while (1) {
        usleep(33000);

        imgFromKarmen = ardrone.getImage();

        battery = ardrone.getBatteryPercentage();
        tmpOrder = -99;

        for (int i = 0; i < nbLabelPos; i++)
            occLabelPos[i] = 0;

        // Key input
        //       int key = cvWaitKey(0);
        //        if (key == 0x1b) break; //TODO

        // Get an image

        // Move
        vx = 0.0;
        vy = 0.0;
        vz = 0.0;
        vr = 0.0;

        if(order == 100){
            orderName  = "Land";
            ardrone.landing();   // Land
            newOrder = false;
        }
        else{
            if(newOrder){
                newOrder = false;
                listOrder.pop_front();
                listOrder.push_back(order);
            }

            for (list<int>::iterator it=listOrder.begin(); it != listOrder.end(); ++it){
                if(*it >= 0 && *it < nbLabelPos)
                    occLabelPos[*it]++;
            }

            for (int i = 0; i < nbLabelPos; i++){
                if(occLabelPos[i] >= 3){
                    tmpOrder = i;
                    break;
                }
            }


            switch (tmpOrder) {
            case 0 :
                orderName = "Up";
                vz = 1.0;
                break;
            case 1 :
                orderName  = "Down";
                vz = -1.0;
                break;
            case 2 :
                orderName  = "Left";
                vy = 1.0;
                break;
            case 3 :
                orderName  = "Right";
                vy = -1.0;
                break;
            case 4 :
                orderName  = "Land";
                ardrone.landing();   // Land
                break;
            case 5 :
                if (ardrone.onGround()){
                    orderName  = "Take Off";
                    ardrone.takeoff();    // Take-off
                }
                break;
            case 6 :
                orderName  = "Forward";
                vx = 1.0;
                break;
            case 7 :
                orderName  = "Backward";
                vx = -1.0;
                break;
            default :
                orderName  = "Nothing";
                break;
            }

            ardrone.move3D(vx, vy, vz, vr);
            vx = 0.0;
            vy = 0.0;
            vz = 0.0;
            vr = 0.0;
        }
    }
    // 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()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Get an image
        cv::Mat image= ardrone.getImage();

        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        std::cout << "ardrone.roll  = " << roll  * RAD_TO_DEG << " [deg]" << std::endl;
        std::cout << "ardrone.pitch = " << pitch * RAD_TO_DEG << " [deg]" << std::endl;
        std::cout << "ardrone.yaw   = " << yaw   * RAD_TO_DEG << " [deg]" << std::endl;

        // Altitude
        double altitude = ardrone.getAltitude();
        std::cout << "ardrone.altitude = " << altitude << " [m]" << std::endl;

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        std::cout << "ardrone.vx = " << vx << " [m/s]" << std::endl;
        std::cout << "ardrone.vy = " << vy << " [m/s]" << std::endl;
        std::cout << "ardrone.vz = " << vz << " [m/s]" << std::endl;

        // Battery
        int battery = ardrone.getBatteryPercentage();
        std::cout << "ardrone.battery = " << battery << " [%%]" << std::endl;

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
        if (key == 0x260000) x =  1.0;
        if (key == 0x280000) x = -1.0;
        if (key == 0x250000) r =  1.0;
        if (key == 0x270000) r = -1.0;
        ardrone.move3D(x, y, z, r);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode%4);

        // Display the image
        cv::imshow("camera", image);
    }

    // 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;
    }

    // Thresholds
    int minH = 0, maxH = 255;
    int minS = 0, maxS = 255;
    int minV = 0, maxV = 255;

    // Create a window
    cv::namedWindow("binalized");
    cv::createTrackbar("H max", "binalized", &maxH, 255);
    cv::createTrackbar("H min", "binalized", &minH, 255);
    cv::createTrackbar("S max", "binalized", &maxS, 255);
    cv::createTrackbar("S min", "binalized", &minS, 255);
    cv::createTrackbar("V max", "binalized", &maxV, 255);
    cv::createTrackbar("V min", "binalized", &minV, 255);
    cv::resizeWindow("binalized", 0, 0);

    // Main loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Update
        if (!ardrone.update()) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // HSV image
        cv::Mat hsv;
        cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV_FULL);

        // Binalize
        cv::Mat binalized;
        cv::Scalar lower(minH, minS, minV);
        cv::Scalar upper(maxH, maxS, maxV);
        cv::inRange(image, lower, upper, binalized);

        // Show result
        cv::imshow("binalized", binalized);

        // De-noising
        cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
        cv::morphologyEx(binalized, binalized, cv::MORPH_CLOSE, kernel);
        //cv::imshow("morphologyEx", binalized);

        // Detect contours
        std::vector<std::vector<cv::Point>> contours;
        cv::findContours(binalized.clone(), contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);

        // Find largest contour
        int contour_index = -1;
        double max_area = 0.0;
        for (int i = 0; i < contours.size(); i++) {
            double area = fabs(cv::contourArea(contours[i]));
            if (area > max_area) {
                contour_index = i;
                max_area = area;
            }
        }

        // Object detected
        if (contour_index >= 0) {
            // Show result
            cv::Rect rect = cv::boundingRect(contours[contour_index]);
            cv::rectangle(image, rect, cv::Scalar(0,255,0));
            //cv::drawContours(image, contours, contour_index, cv::Scalar(0,255,0));
        }

        // Display the image
        cv::imshow("camera", image);
    }

    // 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()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

    // Battery
    std::cout << "Battery = " << ardrone.getBatteryPercentage() << "[%]" << std::endl;

    // Instructions
    std::cout << "***************************************" << std::endl;
    std::cout << "*       CV Drone sample program       *" << std::endl;
    std::cout << "*           - How to play -           *" << std::endl;
    std::cout << "***************************************" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Controls -                        *" << std::endl;
    std::cout << "*    'Space' -- Takeoff/Landing       *" << std::endl;
    std::cout << "*    'Up'    -- Move forward          *" << std::endl;
    std::cout << "*    'Down'  -- Move backward         *" << std::endl;
    std::cout << "*    'Left'  -- Turn left             *" << std::endl;
    std::cout << "*    'Right' -- Turn right            *" << std::endl;
    std::cout << "*    'Q'     -- Move upward           *" << std::endl;
    std::cout << "*    'A'     -- Move downward         *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "* - Others -                          *" << std::endl;
    std::cout << "*    'C'     -- Change camera         *" << std::endl;
    std::cout << "*    'Esc'   -- Exit                  *" << std::endl;
    std::cout << "*                                     *" << std::endl;
    std::cout << "***************************************" << std::endl;

    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == 'i' || key == CV_VK_UP)    vx =  1.0;
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
        if (key == 'j') vy =  1.0;
        if (key == 'l') vy = -1.0;
        if (key == 'q') vz =  1.0;
        if (key == 'a') vz = -1.0;
        ardrone.move3D(vx, vy, vz, vr);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode % 4);

        // Display the image
        cv::imshow("camera", image);
    }

    // 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;
}

//.........这里部分代码省略.........

        // Move
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == 'i' || key == CV_VK_UP)    vx =  1.0;
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
        if (key == 'j') vy =  1.0;
        if (key == 'l') vy = -1.0;
        if (key == 'q') vz =  1.0;
        if (key == 'a') vz = -1.0;

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode % 4);

        // Switch tracking ON/OFF
        static int track = 0;
        if (key == 't') track = !track;

        // Get an image
        cv::Mat image = ardrone.getImage();

        // HSV image
        cv::Mat hsv;
        cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV_FULL);

        // Binalize
        cv::Mat binalized;
        cv::Scalar lower(minH, minS, minV);
        cv::Scalar upper(maxH, maxS, maxV);
        cv::inRange(hsv, lower, upper, binalized);

        // Show result
        cv::imshow("binalized", binalized);

        // De-noising
        cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
        cv::morphologyEx(binalized, binalized, cv::MORPH_CLOSE, kernel);
        //cv::imshow("morphologyEx", binalized);

        // Detect contours
        std::vector< std::vector<cv::Point> > contours;
        cv::findContours(binalized.clone(), contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);

        // Find largest contour
        int contour_index = -1;
        double max_area = 0.0;
        for (size_t i = 0; i < contours.size(); i++) {
            double area = fabs(cv::contourArea(contours[i]));
            if (area > max_area) {
                contour_index = i;
                max_area = area;
            }
        }

        // Object detected
        if (contour_index >= 0) {
            // Moments
            cv::Moments moments = cv::moments(contours[contour_index], true);
            double marker_y = (int)(moments.m01 / moments.m00);
            double marker_x = (int)(moments.m10 / moments.m00);

            // Show result
            cv::Rect rect = cv::boundingRect(contours[contour_index]);
            cv::rectangle(image, rect, cv::Scalar(0, 255, 0));

            // Tracking
            if (track) {
                const double kp = 0.005;
                vx = 0.1;
                vy = 0.0;
                vz = kp * (binalized.rows / 2 - marker_y);
                vr = kp * (binalized.cols / 2 - marker_x);
            }
        }

        // Display the image
        cv::putText(image, (track) ? "track on" : "track off", cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, (track) ? cv::Scalar(0, 0, 255) : cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
        cv::imshow("camera", image);
        ardrone.move3D(vx, vy, vz, vr);
    }

    // Save thresholds
    fs.open(filename, cv::FileStorage::WRITE);
    if (fs.isOpened()) {
        cv::write(fs, "H_MAX", maxH);
        cv::write(fs, "H_MIN", minH);
        cv::write(fs, "S_MAX", maxS);
        cv::write(fs, "S_MIN", minS);
        cv::write(fs, "V_MAX", maxV);
        cv::write(fs, "V_MIN", minV);
        fs.release();
    }

    // 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;
    }

    // Get a image
    IplImage* image = ardrone.getImage();

    // Images
    IplImage *gray   = cvCreateImage(cvGetSize(image), image->depth, 1);
    IplImage *smooth = cvCreateImage(cvGetSize(image), image->depth, 1);
    IplImage *canny  = cvCreateImage(cvGetSize(image), image->depth, 1);

    // Canny thresholds
    int th1 = 50, th2 = 100;
    cvNamedWindow("canny");
    cvCreateTrackbar("th1", "canny", &th1, 255);
    cvCreateTrackbar("th2", "canny", &th2, 255);

    // Main loop
    while (1) {
        // Key input
        int key = cvWaitKey(1);
        if (key == 0x1b) break;

        // Update
        if (!ardrone.update()) break;

        // Get an image
        image = ardrone.getImage();

        // Convert to gray scale
        cvCvtColor(image, gray, CV_BGR2GRAY);

        // De-noising
        cvSmooth(gray, smooth, CV_GAUSSIAN, 23, 23);

        // Detect edges
        cvCanny(smooth, canny, th1, th2, 3);

        // Detect circles
        CvMemStorage *storage = cvCreateMemStorage(0);
        CvSeq *circles = cvHoughCircles(smooth, storage, CV_HOUGH_GRADIENT, 1.0, 10.0, MAX(th1,th2), 20);

        // Draw circles
        for (int i = 0; i < circles->total; i++) {
            float *p = (float*) cvGetSeqElem(circles, i);
            cvCircle(image, cvPoint(cvRound(p[0]), cvRound(p[1])), cvRound(p[2]), CV_RGB(0,255,0), 3, 8, 0);
        }

        // Release memory
        cvReleaseMemStorage(&storage);

        // Change camera
        static int mode = 0;
        if (key == 'c') ardrone.setCamera(++mode%4);

        // Display the image
        cvShowImage("camera", image);
        cvShowImage("canny", canny);
    }

    // Release memories
    cvReleaseImage(&gray);
    cvReleaseImage(&smooth);
    cvReleaseImage(&canny);

    // 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;
    }

    // Snapshots
    std::vector<cv::Mat> snapshots;

    // Key frame
    cv::Mat last = cv::Mat(ardrone.getImage(), true);

    // ORB detector/descriptor
    cv::OrbFeatureDetector detector;
    cv::OrbDescriptorExtractor extractor;

    // Main loop
    while (!GetAsyncKeyState(VK_ESCAPE)) {
        // Update
        if (!ardrone.update()) break;

        // Get an image
        cv::Mat image = cv::Mat(ardrone.getImage());

        // Detect key points
        cv::Mat descriptorsA, descriptorsB;
        std::vector<cv::KeyPoint> keypointsA, keypointsB;
        detector.detect(last, keypointsA);
        detector.detect(image, keypointsB);
        extractor.compute(last, keypointsA, descriptorsA);
        extractor.compute(image, keypointsB, descriptorsB);

        // Match key points
        std::vector<cv::DMatch> matches;
        cv::BFMatcher matcher(cv::NORM_HAMMING, true);
        matcher.match(descriptorsA, descriptorsB, matches);

        // Count matches
        int count = 0;
        for (int i = 0; i < (int)matches.size(); i++) {
            if (matches[i].queryIdx == matches[i].trainIdx) count++; // Yet, strange way
        }

        // Take a snapshot when scene was changed
        if (count == 0) {
            image.copyTo(last);
            cv::Ptr<cv::Mat> tmp(new cv::Mat());
            image.copyTo(*tmp);
            snapshots.push_back(*tmp);
        }

        // Display the image
        cv::Mat matchImage;
        cv::drawMatches(last, keypointsA, image, keypointsB, matches, matchImage, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
        cv::imshow("camera", matchImage);
        cv::waitKey(1);
    }

    // Stiching
    cv::Mat result;
    cv::Stitcher stitcher = cv::Stitcher::createDefault();
    printf("Stitching images...\n");
    if (stitcher.stitch(snapshots, result) == cv::Stitcher::OK) {
        cv::imshow("result", result);
        cv::imwrite("result.jpg", result);
        cvWaitKey(0);
    }

    // See you
    ardrone.close();

    return 0;
}