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

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

    // Images
    std::vector<IplImage*> images;
    printf("Press space key to take a sample picture !\n");

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

        // 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, CV_CALIB_CB_ADAPTIVE_THRESH+CV_CALIB_CB_NORMALIZE_IMAGE|CV_CALIB_CB_FAST_CHECK);

        // Chessboard detected
        if (found) {
            // Draw corners
            cvDrawChessboardCorners(image, size, corners, corner_count, found);

            // If you push Space key
            if (key == ' ') {
                // Add to buffer
                images.push_back(gray);
            }
            else {
                // Release the image
                cvReleaseImage(&gray);
            }
        }
        // Failed to detect
        else {
            // Release the image
            cvReleaseImage(&gray);
        }

        // Display the image
        cvDrawText(image, cvPoint(15, 20), "NUM = %d", (int)images.size());
        cvShowImage("camera", image);
    }

    // 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 (cvAsk("Do you save the camera parameters ? (y/n)\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;
            }
//.........这里部分代码省略.........

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

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

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

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

        // Orientation
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();
        printf("ardrone.roll  = %3.2f [deg]\n", roll  * RAD_TO_DEG);
        printf("ardrone.pitch = %3.2f [deg]\n", pitch * RAD_TO_DEG);
        printf("ardrone.yaw   = %3.2f [deg]\n", yaw   * RAD_TO_DEG);

        // Altitude
        double altitude = ardrone.getAltitude();
        printf("ardrone.altitude = %3.2f [m]\n", altitude);

        // Velocity
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        printf("ardrone.vx = %3.2f [m/s]\n", vx);
        printf("ardrone.vy = %3.2f [m/s]\n", vy);
        printf("ardrone.vz = %3.2f [m/s]\n", vz);

        // Battery
        int battery = ardrone.getBatteryPercentage();
        printf("ardrone.battery = %d [%%]\n", battery);

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

//.........这里部分代码省略.........
	struct tm *pnow = localtime(&now);

    while (1) 
	{
        // Key input
        int key = cvWaitKey(33);
        //int key = cvWaitKey(15);
		if (key == 0x1b){
			break;
		}

		//2014.03.09 add
		vx = 0.0;
		vy = 0.0;
		vz = 0.0;
		vr = 0.0;

		//音声出力タイミング用ワーク
		if (mSendCommandflag == true)
		{
			if(mSendCommandcounter++ > 50)
			{
				mSendCommandflag = false;
				mSendCommandcounter = 0;
			}
		}
        // Update
		if(mNonDronDebug == false)
		{
	        if (!ardrone.update()) 
				break;

			// Get an image
			image = ardrone.getImage();
			if((mbatValue = ardrone.getBatteryPercentage()) < 30){
				printf("Battery = %d%%\n",mbatValue );

				if(mArDroneCommandFlag == false)
					ardrone.move3D(0.0, 0.0, 0.0, 0.0);
				msleep(80);
				ardrone.landing();
				printf("Landing\n");
				msleep(180);
			}
		//}
	
#ifndef FACEDETECT
		try{
			//2014.02.15　FaceDetection追加
			// (3)メモリを確保し，読み込んだ画像のグレースケール化，ヒストグラムの均一化を行う
			CvMemStorage *storage = 0;
			storage = cvCreateMemStorage (0);
			cvClearMemStorage (storage);

			//Mat captureFrame;
			//Mat grayscaleFrame;
			Mat captureFrameMat = cvarrToMat(image);
			cvtColor(captureFrameMat, grayscaleFrame, CV_BGR2GRAY);
			equalizeHist(grayscaleFrame, grayscaleFrame);
 
			//　mFaceDetectMode：Fキーにてスイッチ
			if((mFaceDetectMode == true)
				&&((ardrone.getCameraMode() == 0)||(ardrone.getCameraMode() == 2)))//正面カメラの場合に有効
			{
				// (4)物体（顔）検出
				//create a vector array to store the face found

// --------------------------------------------------------------------------
// 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
    cvNamedWindow("binalized");
    cvCreateTrackbar("H max", "binalized", &maxH, 255);
    cvCreateTrackbar("H min", "binalized", &minH, 255);
    cvCreateTrackbar("S max", "binalized", &maxS, 255);
    cvCreateTrackbar("S min", "binalized", &minS, 255);
    cvCreateTrackbar("V max", "binalized", &maxV, 255);
    cvCreateTrackbar("V min", "binalized", &minV, 255);
    cvResizeWindow("binalized", 0, 0);

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

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

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

        // HSV image
        IplImage *hsv = cvCloneImage(image);
        cvCvtColor(image, hsv, CV_RGB2HSV_FULL);

        // Binalized image
        IplImage *binalized = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);

        // Binalize
        CvScalar lower = cvScalar(minH, minS, minV);
        CvScalar upper = cvScalar(maxH, maxS, maxV);
        cvInRangeS(image, lower, upper, binalized);

        // Show result
        cvShowImage("binalized", binalized);

        // De-noising
        cvMorphologyEx(binalized, binalized, NULL, NULL, CV_MOP_CLOSE);
 
        // Detect contours
        CvSeq *contour = NULL, *maxContour = NULL;
        CvMemStorage *contourStorage = cvCreateMemStorage();
        cvFindContours(binalized, contourStorage, &contour, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

        // Find largest contour
        double max_area = 0.0;
        while (contour) {
            double area = fabs(cvContourArea(contour));
            if (area > max_area) {
                maxContour = contour;
                max_area = area;
            }
            contour = contour->h_next;
        }

        // Object detected
        if (maxContour) {
            // Show result
            CvRect rect = cvBoundingRect(maxContour);
            CvPoint minPoint, maxPoint;
            minPoint.x = rect.x;
            minPoint.y = rect.y;
            maxPoint.x = rect.x + rect.width;
            maxPoint.y = rect.y + rect.height;
            cvRectangle(image, minPoint, maxPoint, CV_RGB(0,255,0));
        }

        // Release memory
        cvReleaseMemStorage(&contourStorage);

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

        // Release images
        cvReleaseImage(&hsv);
        cvReleaseImage(&binalized);
    }

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

// --------------------------------------------------------------------------
// 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 << "*    'T'     -- Track marker          *" << 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) {
        double cx = 0;
        double cy = 0;
        cv::Rect trackRect;
        // 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);

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

        // People detect
        trackRect = ardrone.detectHuman(image);

        cx = trackRect.x + (trackRect.width / 2);
        cy = trackRect.y + (trackRect.height / 2); 
        cv::Point2f mc = cv::Point2f(cx, cy);

        //std::cout << "cx: " << cx << " cy: " << cy <<std::endl;
        cv::circle(image, mc, 5, cv::Scalar(0,0,255));
        //std::cout << "rect size: " << trackRect.width * trackRect.height << std::endl;

        // Tracking
        if (track) {
            if (cx == 0 && cy == 0)
            {
                vx = 0.0;
                vy = 0.0;
                vr = 0.0;
                vz = 0.0;
            } else {
                const double kp = 0.005;
                const double ka = 0.005;
//.........这里部分代码省略.........

// --------------------------------------------------------------------------
// 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
    printf("Battery = %d%%\n", ardrone.getBatteryPercentage());

    // Instructions
    printf("  Q - ARDRONE_ANIM_PHI_M30_DEG\n");
    printf("  A - ARDRONE_ANIM_PHI_30_DEG\n");
    printf("  Z - ARDRONE_ANIM_THETA_M30_DEG\n");
    printf("  W - ARDRONE_ANIM_THETA_30_DEG\n");
    printf("  S - ARDRONE_ANIM_THETA_20DEG_YAW_200DEG\n");
    printf("  X - ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG\n");
    printf("  E - ARDRONE_ANIM_TURNAROUND\n");
    printf("  D - ARDRONE_ANIM_TURNAROUND_GODOWN\n");
    printf("  C - ARDRONE_ANIM_YAW_SHAKE\n");
    printf("  R - ARDRONE_ANIM_YAW_DANCE\n");
    printf("  F - ARDRONE_ANIM_PHI_DANCE\n");
    printf("  V - ARDRONE_ANIM_THETA_DANCE\n");
    printf("  T - ARDRONE_ANIM_VZ_DANCE\n");
    printf("  G - ARDRONE_ANIM_WAVE\n");
    printf("  B - ARDRONE_ANIM_PHI_THETA_MIXED\n");
    printf("  Y - ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED\n");
    printf("  H - ARDRONE_ANIM_FLIP_AHEAD\n");
    printf("  N - ARDRONE_ANIM_FLIP_BEHIND\n");
    printf("  U - ARDRONE_ANIM_FLIP_LEFT\n");
    printf("  J - ARDRONE_ANIM_FLIP_RIGHT\n");

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

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

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

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

        // Flight animations
        if (key == 'q') ardrone.setAnimation(ARDRONE_ANIM_PHI_M30_DEG,             1000);
        if (key == 'a') ardrone.setAnimation(ARDRONE_ANIM_PHI_30_DEG,              1000);
        if (key == 'z') ardrone.setAnimation(ARDRONE_ANIM_THETA_M30_DEG,           1000);
        if (key == 'w') ardrone.setAnimation(ARDRONE_ANIM_THETA_30_DEG,            1000);
        if (key == 's') ardrone.setAnimation(ARDRONE_ANIM_THETA_20DEG_YAW_200DEG,  1000);
        if (key == 'x') ardrone.setAnimation(ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG, 1000);
        if (key == 'e') ardrone.setAnimation(ARDRONE_ANIM_TURNAROUND,              5000);
        if (key == 'd') ardrone.setAnimation(ARDRONE_ANIM_TURNAROUND_GODOWN,       5000);
        if (key == 'c') ardrone.setAnimation(ARDRONE_ANIM_YAW_SHAKE,               2000);
        if (key == 'r') ardrone.setAnimation(ARDRONE_ANIM_YAW_DANCE,               5000);
        if (key == 'f') ardrone.setAnimation(ARDRONE_ANIM_PHI_DANCE,               5000);
        if (key == 'v') ardrone.setAnimation(ARDRONE_ANIM_THETA_DANCE,             5000);
        if (key == 't') ardrone.setAnimation(ARDRONE_ANIM_VZ_DANCE,                5000);
        if (key == 'g') ardrone.setAnimation(ARDRONE_ANIM_WAVE,                    5000);
        if (key == 'b') ardrone.setAnimation(ARDRONE_ANIM_PHI_THETA_MIXED,         5000);
        if (key == 'y') ardrone.setAnimation(ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED,  5000);
        if (key == 'h') ardrone.setAnimation(ARDRONE_ANIM_FLIP_AHEAD,                15);
        if (key == 'n') ardrone.setAnimation(ARDRONE_ANIM_FLIP_BEHIND,               15);
        if (key == 'u') ardrone.setAnimation(ARDRONE_ANIM_FLIP_LEFT,                 15);
        if (key == 'j') ardrone.setAnimation(ARDRONE_ANIM_FLIP_RIGHT,                15);

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

    // 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 != -1) std::cout << "Key pressed: " << key << std::endl;
		
        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 == 2490368) vx = 1.0; // Key up
        if (key == 'k' || key == CV_VK_DOWN)  vx = -1.0;
		//if (key == 2621440) vx = -1.0; // Key down
        if (key == 'u' || key == CV_VK_LEFT)  vr =  1.0;
		//if (key == 2424832) vr = 1.0; // Key left
        if (key == 'o' || key == CV_VK_RIGHT) vr = -1.0;
		//if (key == 2555904) vr = -1.0; // Key right
        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(1.0, 0.0, 0.0, 0.0);
		//std::cout << "vx: " << vx << ", vy: " << vy << ", vz: " << vz << ", vr: " << vr << std::endl;
        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 loop
    while (1) {
        // Key input
        int key = cv::waitKey(33);
        if (key == 0x1b) break;

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

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

// --------------------------------------------------------------------------
// 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
    printf("Battery = %d%%\n", ardrone.getBatteryPercentage());

    // Map
    cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3);

    // Kalman filter
    cv::KalmanFilter kalman(6, 4, 0);

    // Sampling time [s]
    const double dt = 0.033;

    // Transition matrix (x, y, z, vx, vy, vz)
    cv::Mat1f F(6, 6);
    F << 1.0, 0.0, 0.0,  dt, 0.0, 0.0,
         0.0, 1.0, 0.0, 0.0,  dt, 0.0,
         0.0, 0.0, 1.0, 0.0, 0.0,  dt,
         0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
    kalman.transitionMatrix = F;

    // Measurement matrix (0, 0, z, vx, vy, vz)
    cv::Mat1f H(4, 6);
    H << 0, 0, 1, 0, 0, 0,
         0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 1;
    kalman.measurementMatrix = H;

    // Process noise covairance (x, y, z, vx, vy, vz)
    cv::Mat1f Q(6, 6);
    Q << 0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
         0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
         0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.3, 0.0, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.3, 0.0,
         0.0, 0.0, 0.0, 0.0, 0.0, 0.3;
    kalman.processNoiseCov = Q;

    // Measurement noise covariance (z, vx, vy, vz)
    cv::Mat1f R(4, 4);
    R << 0.1, 0.0, 0.00, 0.00,
         0.0, 0.1, 0.00, 0.00,
         0.0, 0.0, 0.05, 0.00,
         0.0, 0.0, 0.00, 0.05;
    kalman.measurementNoiseCov = R;

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

        // Prediction
        cv::Mat prediction = kalman.predict();

        // Altitude
        double altitude = ardrone.getAltitude();

        // Orientations
        double roll  = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw   = ardrone.getYaw();

        // Velocities
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        cv::Mat V  = (cv::Mat1f(3,1) << vx, vy, vz);

        // Rotation matrices
        cv::Mat RZ = (cv::Mat1f(3,3) <<   cos(yaw), -sin(yaw),        0.0,
                                          sin(yaw),  cos(yaw),        0.0,
                                               0.0,       0.0,        1.0);
        cv::Mat RY = (cv::Mat1f(3,3) << cos(pitch),       0.0,  sin(pitch),
                                               0.0,       1.0,        0.0,
                                       -sin(pitch),       0.0,  cos(pitch));
        cv::Mat RX = (cv::Mat1f(3,3) <<        1.0,       0.0,        0.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;

    // Map
    cv::Mat map = cv::Mat::zeros(500, 500, CV_8UC3);

    // Position matrix
    cv::Mat P = cv::Mat::zeros(3, 1, CV_64FC1);

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

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

        // Altitude
        double altitude = ardrone.getAltitude();

        // Orientations
        double roll = ardrone.getRoll();
        double pitch = ardrone.getPitch();
        double yaw = ardrone.getYaw();

        // Velocities
        double vx, vy, vz;
        double velocity = ardrone.getVelocity(&vx, &vy, &vz);
        cv::Mat V = (cv::Mat1f(3, 1) << vx, vy, vz);

        // Rotation matrices
        cv::Mat RZ = (cv::Mat1f(3, 3) << cos(yaw), -sin(yaw), 0.0,
                                         sin(yaw),  cos(yaw), 0.0,
                                              0.0,       0.0, 1.0);
        cv::Mat RY = (cv::Mat1f(3, 3) << cos(pitch), 0.0, sin(pitch),
                                                0.0, 1.0,        0.0,
                                        -sin(pitch), 0.0, cos(pitch));
        cv::Mat RX = (cv::Mat1f(3, 3) << 1.0,       0.0,        0.0,
                                         0.0, cos(roll), -sin(roll),
                                         0.0, sin(roll),  cos(roll));

        // Time [s]
        static int64 last = cv::getTickCount();
        double dt = (cv::getTickCount() - last) / cv::getTickFrequency();
        last = cv::getTickCount();

        // Dead-reckoning
        P = P + RZ * RY * RX * V * dt;

        // Position (x, y, z)
        double pos[3] = { P.at<double>(0, 0), P.at<double>(1, 0), P.at<double>(2, 0) };
        std::cout << "x = " << pos[0] << "[m], " << "y = " << pos[1] << "[m], " << "z = " << pos[2] << "[m]" << 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 == '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(x, y, z, r);

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

        // Display the image
        cv::circle(map, cv::Point(-pos[1] * 100.0 + map.cols / 2, -pos[0] * 100.0 + map.rows / 2), 2, CV_RGB(255, 0, 0));
        cv::imshow("map", map);
        cv::imshow("camera", image);
    }

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

    // Battery
    printf("Battery = %d%%\n", ardrone.getBatteryPercentage());

    // Instructions
    printf("***************************************\n");
    printf("*       CV Drone sample program       *\n");
    printf("*           - How 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("*    'Q'     -- Move upward           *\n");
    printf("*    'A'     -- Move downward         *\n");
    printf("*                                     *\n");
    printf("* - Others -                          *\n");
    printf("*    'C'     -- Change camera         *\n");
    printf("*    'Esc'   -- Exit                  *\n");
    printf("*                                     *\n");
    printf("***************************************\n\n");

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

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

        // Get an image
        IplImage *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 == 0x260000) vx =  1.0;
        if (key == 0x280000) vx = -1.0;
        if (key == 0x250000) vr =  1.0;
        if (key == 0x270000) vr = -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
        cvShowImage("camera", image);
    }

    // See you
    ardrone.close();

    return 0;
}