칼만필터를 이용해서 얼굴 중심점 찾는 코딩을 해봤는데

필터를 이용해 원을 그리면 원의 반경이 점점 커지더라고요.

그래서 그냥 원의 반경을 고정시켜놓고 하였습니다.

#include "ardrone/ardrone.h"

#include <iostream>

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

   cv::CascadeClassifier face_classifier;

   face_classifier.load("C:/Users/hayoon/Downloads/opencv/sources/data/haarcascades/haarcascade_frontalface_alt.xml");

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

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

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

   // Transition matrix (x, y, vx, vy)
   cv::Mat1f A(4, 4);
   A << 1.0, 0.0, dt, 0.0,
      0.0, 1.0, 0.0, dt,
      0.0, 0.0, 1.0, 0.0,
      0.0, 0.0, 0.0, 1.0;
   kalman.transitionMatrix = A;

   // Measurement matrix (x, y)
   cv::Mat1f H(2, 4);
   H << 1, 0, 0, 0,
      0, 1, 0, 0;
   kalman.measurementMatrix = H;

   // Process noise covariance (x, y, vx, vy)
   cv::Mat1f Q(4, 4);
   Q << 1e-5, 0.0, 0.0, 0.0,
      0.0, 1e-5, 0.0, 0.0,
      0.0, 0.0, 1e-5, 0.0,
      0.0, 0.0, 0.0, 1e-5;
   kalman.processNoiseCov = Q;

   // Measurement noise covariance (x, y)
   cv::Mat1f R(2, 2);
   R << 1e-1, 0.0,
      0.0, 1e-1;
   kalman.measurementNoiseCov = R;

   while (1) {
      bool frame_valid = true;

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

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

      cv::Mat frame = image.clone();

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

      if (frame_valid) {
         try {
            // convert captured frame to gray scale & equalize
            cv::Mat grayframe;
            cv::cvtColor(frame, grayframe, CV_BGR2GRAY);
            cv::equalizeHist(grayframe, grayframe);

            // -------------------------------------------------------------
            // face detection routine

            // a vector array to store the face found
            std::vector<cv::Rect> faces;

            face_classifier.detectMultiScale(grayframe, faces,
               1.1, // increase search scale by 10% each pass
               3,   // merge groups of three detections
               CV_HAAR_FIND_BIGGEST_OBJECT | CV_HAAR_SCALE_IMAGE,
               cv::Size(30, 30)
               );

            // -------------------------------------------------------------

            // Prediction
            cv::Mat1f prediction = kalman.predict();
            int radius = 1e+3 * kalman.errorCovPre.at<float>(0, 0);
           
            // draw the results
            for (int i = 0; i < faces.size(); i++) {
               cv::Point lb(faces[i].x + faces[i].width, faces[i].y + faces[i].height);
               cv::Point tr(faces[i].x, faces[i].y);

               cv::rectangle(frame, lb, tr, cv::Scalar(0, 255, 0), 3, 4, 0);
        
               cv::Point center(faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5);
           
               cv::circle(frame, center, 10, cv::Scalar(0, 255, 0), 2);
            }
           
            // Display the image
            //cv::imshow("camera", image);
            cv::imshow("webcam", frame);
         }
         catch (cv::Exception& e) {
            std::cerr << "Exception occurred. Ignoring frame... " << e.err << std::endl;
         }
      }
   }

    // See you
    ardrone.close();

    return 0;
}