칼만필터를 이용해서 얼굴 중심점 찾는 코딩을 해봤는데
필터를 이용해 원을 그리면 원의 반경이 점점 커지더라고요.
그래서 그냥 원의 반경을 고정시켜놓고 하였습니다.
#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;
}
