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

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

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

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

저주파 통과 필터를 적용하게 되면 이전의 데이터 평균 값에 비해

최근의 데이터에 더 높은 가중치를 둘 수 있습니다.

const int ECHO=9;
const int TRIG=10;

#define SIZE 50

int buffer[SIZE];

float duration, Cm;
float sum;
float distance;
float preDistance;
float Cm1, duration1;
float alpha=0.7;

void setup() {
  Serial.begin(9600);
  Serial.flush();
  pinMode(TRIG,OUTPUT);
  pinMode(ECHO,INPUT);
  digitalWrite(TRIG,LOW);

  Cm=duration/(29.1*2);
  sum=0;

  for(int i=0; i<=SIZE-1; i++)
  {
    digitalWrite(TRIG,HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG,LOW);
    duration=pulseIn(ECHO,HIGH);
    buffer[i]=Cm;
    if(Cm>400) Cm=400;
    else if(Cm<2) Cm=2;
    sum+=buffer[i];
  }
  preDistance=sum/SIZE;
}

void loop() {
  digitalWrite(TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG,LOW);
  duration1=pulseIn(ECHO,HIGH);
  Cm1=duration1/58.8;

  distance=alpha*preDistance+(1-alpha)*Cm1;

  if(distance>400) distance=400;
  else if(distance<2) distance=2;

  Serial.println((int)distance);
  delay(5);

  preDistance=distance;
}

업로드 한 후 결과를 살펴보면 중간에 생기던 잡음이 제거되며 또한 물체와 센서 사이의 거리가 갑자기 바뀌더라도 정확하게 측정될 것입니다.

'칼만필터의 이해'라는 책에서 제시하고 있는 '이동평균 필터'의 개념과 Matlab 코드를 응용하여 아두이노 방식으로 적용해보았습니다.

이동평균 필터는 일정한 갯수의 데이터를 모아서 평균을 낸 값을 출력하는 방식입니다.

const int ECHO=9;
const int TRIG=10;

#define SIZE 50

int buffer[SIZE];
int Cm1, duration1;

long duration, Cm;

float sum;
float distance;

void setup() {
  Serial.begin(9600);
  Serial.flush();
  pinMode(TRIG,OUTPUT);
  pinMode(ECHO,INPUT);
  digitalWrite(TRIG,LOW);

  Cm=duration/(29.1*2);
  sum=0;

  for(int i=0; i<=SIZE-1; i++)
  {
    digitalWrite(TRIG,HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG,LOW);
    duration=pulseIn(ECHO,HIGH);
    buffer[i]=Cm;
    if(Cm>400) Cm=400;
    else if(Cm<2) Cm=2;
    sum+=buffer[i];
  }
}

void loop() {
  digitalWrite(TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG,LOW);
  duration1=pulseIn(ECHO,HIGH);
  Cm1=duration1/58.8;
  sum-=buffer[0];

  for(int i=0; i<SIZE-1; i++)
  {
    buffer[i]=buffer[i+1];
  }

  buffer[SIZE-1]=Cm1;
  sum+=buffer[SIZE-1];
  distance=sum/SIZE;

  if(distance>400) distance=400;
  else if(distance<2) distance=2;

  Serial.println((int)distance);
  delay(10);
}

위 코드의 setup 부분에서 배열을 채워놓고 loop로 들어가는 이유는 배열을 모두 0으로 채워놓고 시작하게 되면 초기에 오차가 크게 나타나는 현상을 피할 수 있기 때문입니다.

이동평균 필터는 배열에 저장되는 모든 데이터를 동일한 가중치를 놓고 평균을 구하기 때문에 최근 값이랑 이전 값이 차이가 나더라도 출력되는 값에서 이를 민첩하게 반영하지 못합니다. 따라서 이동평균필터는 데이터 간의 변화가 클 때에는 사용하기에 적합하지 않습니다.

아두이노를 이용해서 가장 손쉽게 사용해볼 수 있는 센서는 초음파센서(HC-SR04)라고 생각합니다.

이 센서는 초음파를 보내고 받을 때 까지의 시간을 측정합니다.

따라서 이 센서의 기본적인 반환값은 측정물체에 의해 반사된 초음파가 다시 센서까지 도달하는데 걸리는 시간이라고 할 수 있습니다.

​그러므로 초음파 센서를 사용하여 물체까지 거리를 구하기 위해서는 반환값을 적절히 변형해야 합니다.

데이터 시트에서 제시한 초음파의 속도는 340m/s이므로 1cm이동하는데 대략 29.4microsec가 걸린다는 것을 알 수 있습니다.