// --------------------------------------------------------------------------
// 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;
}
// --------------------------------------------------------------------------
// 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;
}
// Image of AR.Drone's camera
IplImage *image = ardrone.getImage();
// Filename
char filename[256];
SYSTEMTIME st;
GetLocalTime(&st);
sprintf(filename, "cam%d%02d%02d%02d%02d%02d.avi", st.wYear, st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond);
// Create a video writer
CvVideoWriter *video = cvCreateVideoWriter(filename, CV_FOURCC('D','I','B',' '), 30, cvGetSize(image));
// Main loop
while (!GetAsyncKeyState(VK_ESCAPE)) {
// Update
if (!ardrone.update()) break;
// Get an image
image = ardrone.getImage();
// Write a frame
cvWriteFrame(video, image);
// Display the image
cvShowImage("camera", image);
cvWaitKey(33);
}
// Save video
cvReleaseVideoWriter(&video);
// 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;
}
int runArdrone(void)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Battery
battery = ardrone.getBatteryPercentage();
int tmpOrder = -99;
int nbLabelPos = 8;
int occLabelPos[nbLabelPos];
list<int> listOrder(5,-1);
float vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.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;
}
// --------------------------------------------------------------------------
// 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;
// Thresholds
int minH = 0, maxH = 255;
int minS = 0, maxS = 255;
int minV = 0, maxV = 255;
// XML save data
std::string filename("thresholds.xml");
cv::FileStorage fs(filename, cv::FileStorage::READ);
// If there is a save file then read it
if (fs.isOpened()) {
maxH = fs["H_MAX"];
minH = fs["H_MIN"];
maxS = fs["S_MAX"];
minS = fs["S_MIN"];
maxV = fs["V_MAX"];
minV = fs["V_MIN"];
fs.release();
}
// 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;
// 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;
}
}
// 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;
}
// --------------------------------------------------------------------------
// 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;
}
// 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);
}
// 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;
}
// Image of AR.Drone's camera
IplImage *image = ardrone.getImage();
// Valuables for optical flow
IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
IplImage *prev = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
cvCvtColor(image, prev, CV_BGR2GRAY);
IplImage *eig_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *tmp_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *prev_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
IplImage *curr_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
CvPoint2D32f *corners1 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
CvPoint2D32f *corners2 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
// Main loop
while (!GetAsyncKeyState(VK_ESCAPE)) {
// Update
if (!ardrone.update()) break;
// Get an image
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_UP)) vx = 0.5;
if (KEY_DOWN(VK_DOWN)) vx = -0.5;
if (KEY_DOWN(VK_LEFT)) vr = 0.5;
if (KEY_DOWN(VK_RIGHT)) vr = -0.5;
if (KEY_DOWN('Q')) vz = 0.5;
if (KEY_DOWN('A')) vz = -0.5;
ardrone.move3D(vx, vy, vz, vr);
// Convert the camera image to grayscale
cvCvtColor(image, gray, CV_BGR2GRAY);
// Detect features
int corner_count = 50;
cvGoodFeaturesToTrack(prev, eig_img, tmp_img, corners1, &corner_count, 0.1, 5.0, NULL);
// Corner detected
if (corner_count > 0) {
char *status = (char*)malloc(corner_count * sizeof(char));
// Calicurate optical flows
CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.3);
cvCalcOpticalFlowPyrLK(prev, gray, prev_pyramid, curr_pyramid, corners1, corners2, corner_count, cvSize(10, 10), 3, status, NULL, criteria, 0);
// Drow the optical flows
for (int i = 0; i < corner_count; i++) {
cvCircle(image, cvPointFrom32f(corners1[i]), 1, CV_RGB (255, 0, 0));
if (status[i]) cvLine(image, cvPointFrom32f(corners1[i]), cvPointFrom32f(corners2[i]), CV_RGB (0, 0, 255), 1, CV_AA, 0);
}
free(status);
}
// Save the last frame
cvCopy(gray, prev);
// Display the image
cvShowImage("camera", image);
cvWaitKey(1);
}
// Release the images
cvReleaseImage(&gray);
cvReleaseImage(&prev);
cvReleaseImage(&eig_img);
cvReleaseImage(&tmp_img);
cvReleaseImage(&prev_pyramid);
cvReleaseImage(&curr_pyramid);
free(corners1);
free(corners2);
// See you
ardrone.close();
return 0;
}
int main(int argc, char **argv)
{
//int i;
//static IplImage *src_img = 0, *src_gray = 0;
CascadeClassifier face_cascade;
//OK 2014.02.14 精度は荒いが速度はよい ※速度重視
face_cascade.load("..\\..\\data\\haarcascade_frontalface_alt2.xml");
//setup image files used in the capture process
Mat captureFrame;
Mat grayscaleFrame;
static pLeapData pLeapData;
pLeapData.init();
static bool mLeapnot = false;
static bool mTakOffFlag = false;
static bool mSendCommandflag = false;
static int mSendCommandcounter = 0;
static int mSoundCommandcounter = 0;
static int mSoundCommandOKcounter = 20;
float pitch = 0; //前p：-0.5 後p： 0.9
float yaw = 0; //左y：-1.0 右y： 0.7
float roll = 0; //左R： 0.8 右R：-1.0
float pitch_pre = 0; //前p：-0.5 後p： 0.9
float yaw_pre = 0; //左y：-1.0 右y： 0.7
float roll_pre = 0; //左R： 0.8 右R：-1.0
float PosX = 0; //左右 左 -150 〜 右 150
float PosY = 0; //上下昇降 下 50 〜 上 300
float PosZ = 0; //前後 手前-100 〜 奥 100
float PosX_pre = 0; //左右 左 -150 〜 右 150
float PosY_pre = 0; //上下昇降 下 50 〜 上 300
float PosZ_pre = 0; //前後 手前-100 〜 奥 100
float Para_pre = 0.80f; //
float Para_cur = 0.2f; //
Leap::Frame frame; // controller is a Leap::Controller object
Leap::HandList hands;
Leap::Hand firstHand;
//double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
int mbatValue = 0;
//マウスイベント用
//http://ameblo.jp/banquet-of-merry-widow/entry-11101618791.html
MouseParam mparam;
mparam.x = 0; mparam.y = 0; mparam.event = 0; mparam.flags = 0;
//ウインドウへコールバック関数とコールバック関数からイベント情報を受け取る変数を渡す。
//setMouseCallback( wname, &mfunc, &mparam );
// AR.Drone class
// ARDrone ardrone;
if(mNonDronDebug == true)
{
}else
{
// Initialize
//if (!ardrone.open()) {
if ( initdrone(&ardrone) == -1) {
printf("Failed to initialize.\n");
return -1;
}
}
#ifdef MCISOUND
PlayWaveSound();
#endif
// 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("* 'F' --mFaceDetectMode：スイッチ *\n");
printf("* 'L' --LeapMode：スイッチ *\n");
printf("* *\n");
printf("***************************************\n\n");
//
//2014.01.15 add
Leap::Controller leapController;
// Get an image
static IplImage *image;
//ardrone.setCamera(0);
ardrone.setCamera(1);//下面カメラ指定
//顔検出後の枠用
CvPoint pt1;
pt1.x = 100;
pt1.y = 100;
CvScalar rcolor;
rcolor = CV_RGB( 128, 80, 128);
//ウィンドウの表示
cvNamedWindow ("FaceDetectW", CV_WINDOW_AUTOSIZE);
cvNamedWindow ("camera", CV_WINDOW_AUTOSIZE);
//ウインドウへコールバック関数とコールバック関数からイベント情報を受け取る変数を渡す。
cvSetMouseCallback( "camera", &mMouseEventfunc, &mparam );
time_t now = time(NULL);
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
std::vector<Rect> faces;
face_cascade.detectMultiScale(grayscaleFrame, faces, 1.2, 4, CV_HAAR_FIND_BIGGEST_OBJECT|CV_HAAR_SCALE_IMAGE, Size(30,30));
//printf("FaceNum:%02d\n",faces.size());
// (5)検出された全ての顔位置に，四角を描画する
Point pt1;
Point pt2;
Point cPt1;//Center Mark
int mFaceHeight=0;
int mFaceWidth=0;
//複数検出の場合は、最大のものをTrackingする。
for(int i = 0; i < (signed)faces.size(); i++)
{
if(i==0)
{
pt1.x = faces[i].x + faces[i].width;
pt1.y = faces[i].y + faces[i].height;
mFaceHeight = faces[i].height;
mFaceWidth = faces[i].width;
pt2.x = faces[i].x ;
pt2.y = faces[i].y ;
cPt1.x = faces[i].x + faces[i].width/2;
cPt1.y = faces[i].y + faces[i].height/2;
}else
{
//最大の検出対象の値をキープ
if(faces[i-1].height < faces[i].height)
{
pt1.x = faces[i].x + faces[i].width;
pt1.y = faces[i].y + faces[i].height;
mFaceHeight = faces[i].height;
mFaceWidth = faces[i].width;
pt2.x = faces[i].x;
pt2.y = faces[i].y;
cPt1.x = faces[i].x + faces[i].width/2;
cPt1.y = faces[i].y + faces[i].height/2;
}
}
}
//printf("FaceNum:%02d",faces.size());
if(faces.size() > 0)
{
//顔検出した場合の処理
mFaceLostFlag = false;
rectangle(captureFrameMat, pt1, pt2, cvScalar(0, 255, 0, 0), 1, 8, 0);
//Center Mark
circle (captureFrameMat,cPt1,5,rcolor,-2);
//double vx=0.0, vy=0.0, vr=0.0, vz=0.0;
if((cPt1.x > 0)&&(cPt1.x < 200)){
vr = 1.0;
}
if((cPt1.x > 280)&&(cPt1.x <350)){
vr = 1.0;
}
if((cPt1.x > 450)&&(cPt1.x < 520)){
vr = -1.0;
}
if((cPt1.x > 600)&&(cPt1.x < 800)){
vr = -1.0;
}
if((cPt1.y > 0)&&(cPt1.y < 350)){
//vz = 0.75;
}else if((cPt1.y > 400)&&(cPt1.y < 600)){
//vz = -0.75;
}
if((mFaceHeight > 1)&&(mFaceHeight < 200)){
//vx = 0.75;
//vz = 0.75;
}else if((mFaceHeight > 300)&&(mFaceHeight < 600)){
//vx = -0.75;
//vz = -0.75;
}
if(!mNonDronRDebug)
{
if((!ardrone.onGround())&&(mArDroneCommandFlag == false))
{
//time_t now = time(NULL);
//struct tm *pnow = localtime(&now);
//printf("FT:%02d:%02d:%02d X:%03d Y:%03d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f FH:%03d\n",pnow->tm_hour,pnow->tm_min,pnow->tm_sec, cPt1.x,cPt1.y, vx, vy, vz, vr, mFaceHeight);
//ardrone.move3D(vx, vy, vz, vr);
//msleep(30);
}
}
}else
{
//Face Lostモード
if(mFaceLostFlag == false)
{
mFaceLostFlag = true;
if(!mNonDronRDebug)
{
if (!ardrone.onGround())
{
if(mArDroneCommandFlag == false)
{
//ardrone.move3D(0.0, 0.0, 0.0, 0.0);
//printf(" X:%03d Y:%03d vx:%02d vy:%02d vz:%02d vr:%02d FH:%03d\n", 0, 0, 0, 0, 0, 0, 0);
printf("Face Lostモード\n");
//msleep(100);
}
}
}
}
}
//2014.02.22
// height value enable 150 - 400
// x 150 - 600 center:400
// y 150 - 600 center:400
//printf(" x:%02d y:%02d w:%02d h:%02d",faces[0].x,faces[0].y,faces[0].width,faces[0].height);
//printf(" cx:%02d cy:%02d w:%02d h:%02d",cPt1.x,cPt1.y,faces[0].width,faces[0].height);
//printf("\n");
//
IplImage wimage = captureFrameMat;
//static IplImage wimage = grayscaleFrame;
//cvCopy( image, wimage);
image = &wimage;
}
}catch(char *e)
{
printf("%s\n",e);
}
#endif
}
//2014.03.09 add
if((mLeapnot != true)&&(pLeapData.mLeapMode == true)&&(leapController.isConnected()))
{
frame = leapController.frame(); // controller is a Leap::Controller object
hands = frame.hands();
firstHand = hands[0];
pitch_pre = pitch;
pitch = firstHand.direction().pitch();//前p：-0.5 後p： 0.9
pitch = pitch_pre*Para_pre + pitch*Para_cur; //Para_pre:0.80 Para_cur:0.20
yaw_pre = yaw; //左y：-1.0 右y： 0.7
yaw = firstHand.direction().yaw(); //左y：-1.0 右y： 0.7
yaw = yaw_pre*Para_pre + yaw*Para_cur;
roll_pre = roll; //左R： 0.8 右R：-1.0
roll = firstHand.palmNormal().roll(); //左R： 0.8 右R：-1.0
roll = roll_pre*Para_pre + roll*Para_cur;
PosX = frame.pointables().leftmost().tipPosition().x; //左右 左-150 〜 右 150
PosY = frame.pointables().leftmost().tipPosition().y; //上下昇降 下 50 〜 上 300
PosZ = frame.pointables().leftmost().tipPosition().z * (1); //前後 手前-100 〜 奥 100
if(pLeapData.mLeapDebugPrint == true){
printf("%03d XYZ:%03.02f:%03.02f:%03.02f p:%03.02f y:%03.02f r:%03.02f TF:%01i: %i\n",mSoundCommandcounter, PosX,PosY,PosZ,pitch,yaw,roll,(int)mTakOffFlag,mSendCommandcounter);
}
//LeapMotion Value set
//LeapMotionに近づけると TakeOFF
if((PosY > 50) && (PosY < 75) && (mTakOffFlag == true))
{
if(mNonDronDebug == false)
{
if (ardrone.onGround())
{
mTakOffFlag = false;
}else
{
if(mSoundCommandcounter>mSoundCommandOKcounter){
sndPlaySound("..\\..\\src\\resource\\HackathonUser1orimasu.wav", SND_ASYNC);//orimasu
mSoundCommandcounter = 0;
}
if(!mNonDronRDebug) {
ardrone.landing();
}
mTakOffFlag = false;
mSendCommandflag = true;
if(!mNonDronRDebug)
if((pLeapData.mLeapMode == true)&&(mArDroneCommandFlag == false))
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
msleep(250);
}
}
}
if((PosY > 200) && (PosY < 250) && (mTakOffFlag == false))
{
if(mNonDronDebug == false)
{
if (ardrone.onGround())
{
if(pLeapData.mLeapMode == true)
{
mSendCommandflag = true;
if(!mNonDronRDebug)
ardrone.takeoff();
msleep(250);
printf("Leap takeoff\n");
mTakOffFlag = true;
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
//msleep(50);
msleep(100);
}
}
}
}
if((pitch > -0.6) && (pitch < -0.45)){
//前進
//vx = 1.0;
}else if((pitch < 0.9)&&(pitch > 0.5)){
//back
//vx = -1.0;
}
if((roll > 0.5)&&(roll < 0.8)){
//左傾斜
//vy = 1.0;
}else if((roll < -1.0)&&(roll > -1.4)){
//右傾斜
//vy = -1.0;
}else if((vx == 0) &&(vy == 0))
{
//左向き
if((yaw < -0.5)&&(yaw > -0.8)){
vr = 1.0;
}
//右向き
if((yaw > 0.05)&&(yaw < 0.5)){
vr = -1.0;
}
}else
{
vr = 0.0;
}
if(!pLeapData.mLeapDebugPrint == true){
printf("vxyr:%02.01f %02.01f %02.01f: %02.01f %02.01f \n",vx,vy,vr,roll,roll_pre);
}
}//(mLeapnot != true)
//キーコントロール入力
// Take off / Landing
if(mNonDronDebug == false)
{
if (key == ' ')
{
if (ardrone.onGround())
{
if(!mNonDronRDebug)
ardrone.takeoff();
msleep(300);
printf("takeoff\n");
if(mArDroneCommandFlag == false)
{
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
msleep(200);
}
if(mSoundCommandcounter>mSoundCommandOKcounter)
{
sndPlaySound("..\\..\\src\\resource\\HackathonUser1tobimasu.wav", SND_ASYNC);//orimasu
mSoundCommandcounter = 0;
}
mTakOffFlag = true;
mSendCommandflag = true;
//msleep(500);
}else//
{
if(!mNonDronRDebug)// false
{
ardrone.landing();
printf("Landing\n");
msleep(500);
ardrone.move3D(0.0, 0.0, 0.0, 0.0);
msleep(200);
}
if(mSoundCommandcounter>mSoundCommandOKcounter)
{
sndPlaySound("..\\..\\src\\resource\\HackathonUser1orimasu.wav", SND_ASYNC);//orimasu
mSoundCommandcounter = 0;
}
mTakOffFlag = false;
mSendCommandflag = true;
//msleep(500);
}
}//'Space'
}
//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");
// Move
//vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
if (key == 0x260000) vx = 1.0;//Up arrow
if (key == 0x280000) vx = -1.0;//Down arrow key
if (key == 0x250000) vr = 1.0;//Left arrow key
if (key == 0x270000) vr = -1.0;//Right arrow key
if (key == 'q') vz = 1.0;
if (key == 'a') vz = -1.0;
if (key == 'r')
{
//Reset
//ardrone.emergency();
if(mNonDronDebug == true)
{
}else
{
//2014.03.09 add
vx = 0.0;
vy = 0.0;
vz = 0.0;
vr = 0.0;
//ardrone.close();
if ( initdrone(&ardrone) == -1) {
printf("Failed to initialize.\n");
return -1;
}
}
}
// 2014.03.02 add
if((key == 'f')||(key == 'F')){
mFaceDetectMode = !mFaceDetectMode;
printf("Face Mode:%02X Battery = %d%%\n",mFaceDetectMode, ardrone.getBatteryPercentage());
}
if((key == 'l')||(key == 'L')){
pLeapData.mLeapMode = !pLeapData.mLeapMode;
printf("Leap Mode:%02X Battery = %d%%\n",pLeapData.mLeapMode, ardrone.getBatteryPercentage());
}
if((key == 'v')||(key == 'V')){
printf("Btry:%d%% mSendCommandflag:%02d\n", ardrone.getBatteryPercentage(), mSendCommandflag);
}
if((key == '0')||(key == '0')){
printf("Btry:%d%% reset setFlatTrim():%02d\n", ardrone.getBatteryPercentage(), key);
ardrone.setFlatTrim();
msleep(500);
}
if(mNonDronDebug == false)
{
gotoPlaySound(vx, vy, vr, mSoundCommandcounter, mSoundCommandOKcounter);
if((!mNonDronRDebug)&&(!ardrone.onGround()))
{
if((mArDroneCommandFlag == false)&&(MouseARMode == false))
{
ardrone.move3D(vx, vy, vz, vr);
//ardrone.move3D(vx, vy, vz, vr);
//ardrone.move3D(vx, vr, 0.0, vy);
msleep(150);
time_t now = time(NULL);
struct tm *pnow = localtime(&now);
printf("KLT:%02d:%02d:%02d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f \n",pnow->tm_hour,pnow->tm_min,pnow->tm_sec, vx, vy, vz, vr);
}
}
//ardrone.move3D(0, 0, 0, 0);
}
// Change camera
if(mNonDronDebug == false)
{
static int mode = 0;
if((key == 'c')||(key == 'C'))
ardrone.setCamera(++mode%2);
//ardrone.setCamera(++mode%4);
// Display the image
//cvCircle (image,pt1,30,rcolor,2);
IplImage wGrayImage = grayscaleFrame;
cvShowImage ("FaceDetectW", &wGrayImage);
cvShowImage("camera", image);
cvMoveWindow( "FaceDetectW", 600, 0 );
cvMoveWindow( "camera", 50, 0 );
//WindowFromPoint(point(100,200));
if ((key == 's') && (ardrone.getCameraMode() == 1))
{
imgSave("..\\..\\SaveFileName.jpg", &wGrayImage);
}
}
}//while loop
// See you
if(mNonDronDebug == false)
{
ardrone.close();
cvDestroyWindow ("camera");
cvDestroyWindow ("FaceDetectW");
}
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;
const double first_area = 30000;
double rec_area = trackRect.width * trackRect.height;
vx = ka * (first_area - rec_area);
vy = 0.0;
vr = kp * (image.cols / 2 - mc.x);
vz = kp * (image.rows / 2 - mc.y);
// const double kp = 0.005;
// vx = 0.1;
// vy = 0.0;
// vz = kp * (image.rows / 2 - cy);
// vr = kp * (image.cols / 2 - cx);
}
}
// 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);
}
// 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[])
{
const std::string Training = "quad2.png";
quadrotor_matcher::image_template_gray = cv::imread(Training, 0);
if( !quadrotor_matcher::image_template_gray.data )
{
std::cout << "...Couldn't find template image." << std::endl;
return -1;
}
Draw draw;
quadrotor_affine::initialize_st();
quadrotor_job::ctrl_gain = 0.0025;
quadrotor_job::ctrl_sweetspot = 60000.0;
quadrotor_job::ctrl_gain2 = 1.55;//1.15;
pipe* stages[] = {new quadrotor_image, new quadrotor_sift, new quadrotor_matcher, new quadrotor_affine};
parallel_pipeline::pipeline quadrotor_pipeline;
quadrotor_pipeline.addStages(4, stages);
//quadrotor_pipeline.startDebugging();
//Send the template into the pipe
quadrotor_job* newjob = new quadrotor_job;
newjob->image_gray = quadrotor_matcher::image_template_gray;
quadrotor_pipeline.pushJob("Sift",newjob);
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open(/*"192.168.1.3"*/)) {
printf("Failed to initialize.\n");
return -1;
}
ardrone.setFlatTrim();
// Instructions
printf("***************************************\n");
printf("* CV Drone Tracker Program *\n");
printf("***************************************\n");
printf("* *\n");
printf("* - Controls - *\n");
printf("* 'Space' -- Takeoff/Landing *\n");
printf("* *\n");
printf("* - Others - *\n");
printf("* 'C' -- Change camera *\n");
printf("* 'Esc' -- Exit *\n");
printf("* *\n");
printf("***************************************\n\n");
{
cv::Mat image = ardrone.getImage();
quadrotor_affine::image_center_query.at<double>(0) = (double)(image.cols)/2.0;
quadrotor_affine::image_center_query.at<double>(1) = (double)(image.rows)/2.0;
quadrotor_affine::image_center_query.at<double>(2) = 1.0;
}
{
quadrotor_affine::image_center_training.at<double>(0) = (double)(quadrotor_matcher::image_template_gray.cols)/2.0;
quadrotor_affine::image_center_training.at<double>(1) = (double)(quadrotor_matcher::image_template_gray.rows)/2.0;
quadrotor_affine::image_center_training.at<double>(2) = 1.0;
}
unsigned long curTime = timeGetTime();
unsigned long lastTime = curTime;
unsigned long lastBatteryTime = curTime;
unsigned long lastControl = curTime;
int cameraMode = 0;
cv::Mat savedControl;
while (1)
{
curTime = timeGetTime();
// Battery
if(lastBatteryTime < curTime)
{
lastBatteryTime = curTime + 10000;
printf("Battery = %d%%\n", ardrone.getBatteryPercentage());
}
// Sleep and get key
int key = cv::waitKey(30);
switch(key)
{
case 0x1b:
quadrotor_pipeline.stopDebugging();
goto quit;
case ' ':
if (ardrone.onGround())
ardrone.takeoff();
else
ardrone.landing();
break;
case 'c':
ardrone.setCamera(++cameraMode%4);
break;
}
if(ardrone.willGetNewImage())
{
quadrotor_job* newjob = new quadrotor_job;
newjob->image_color = ardrone.getImage();
quadrotor_pipeline.pushJob("Image",newjob);
}
std::vector<std::shared_ptr<job_base>> finished_jobs;
quadrotor_pipeline.getFinishedJobs(finished_jobs);
if(!finished_jobs.empty())
{
std::shared_ptr<quadrotor_job> job = std::dynamic_pointer_cast<quadrotor_job, job_base>(finished_jobs.back());
if (job->good_match)
{
savedControl = job->control;
lastControl = curTime+500;
draw.drawContour(quadrotor_matcher::image_template_gray, job->image_gray, quadrotor_affine::Contour, quadrotor_matcher::keypoints_template, job->keypoints, job->Inliers_T, job->Inliers_Q, job->AT, cv::Scalar(0,255,255), "video", true);
}
else
{
//cout << "WARNING! No Affine Transformation could be computed" << endl;
draw.drawMatches(quadrotor_matcher::image_template_gray, job->image_gray, quadrotor_matcher::keypoints_template, job->keypoints, job->Initial_Matches_T, job->Initial_Matches_Q,"video",true);
}
}
if(curTime < lastControl)
{
ardrone.move3D(savedControl.at<double>(0), savedControl.at<double>(1), savedControl.at<double>(2), savedControl.at<double>(3));
}
}
// See you
quit:;
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
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,
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();
// Local movements (z, vx, vy, vz)
cv::Mat1f M = RZ * RY * RX * V * dt;
cv::Mat measurement = (cv::Mat1f(4,1) << altitude, M(0,0), M(1,0), M(2,0));
// Correction
cv::Mat1f estimated = kalman.correct(measurement);
// Position (x, y, z)
double pos[3] = {estimated(0,0), estimated(1,0), estimated(2,0)};
printf("x = %3.2fm, y = %3.2fm, z = %3.2fm\n", pos[0], pos[1], pos[2]);
// 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;
if (key == 'q') z = 1.0;
if (key == 'a') z = -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();
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;
}
// Set the 3D position of patterns
CvPoint3D32f *objects = (CvPoint3D32f*)cvAlloc(sizeof(CvPoint3D32f) * num * PAT_SIZE);
for (int i = 0; i < num; i++) {
for (int j = 0; j < PAT_ROW; j++) {
for (int k = 0; k < PAT_COL; k++) {
objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0;
}
}
}
// Create matrices
CvMat object_points, image_points, point_counts;
cvInitMatHeader(&object_points, num * PAT_SIZE, 3, CV_32FC1, objects);
cvInitMatHeader(&image_points, num * PAT_SIZE, 1, CV_32FC2, corners);
cvInitMatHeader(&point_counts, num, 1, CV_32SC1, p_count);
// Estimate intrinsic parameters and distortion coefficients
printf("Calicurating parameters...");
CvMat *intrinsic = cvCreateMat(3, 3, CV_32FC1);
CvMat *distortion = cvCreateMat(1, 4, CV_32FC1);
cvCalibrateCamera2(&object_points, &image_points, &point_counts, cvGetSize(images[0]), intrinsic, distortion);
printf("Finished !\n");
// Output a file
printf("Generating a XML file...");
CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_WRITE);
cvWrite(fs, "intrinsic", intrinsic);
cvWrite(fs, "distortion", distortion);
cvReleaseFileStorage(&fs);
printf("Finished !\n");
// Release the matrices
free(p_count);
cvFree(&corners);
cvFree(&objects);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
}
// Release the images
for (int i = 0; i < num; i++) cvReleaseImage(&images[i]);
}
// 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;
}
// Kalman filter
CvKalman *kalman = cvCreateKalman(4, 2);
// Setup
cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1.0));
cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5));
cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(0.1));
cvSetIdentity(kalman->error_cov_post, cvRealScalar(1.0));
// Linear system
kalman->DynamMatr[0] = 1.0; kalman->DynamMatr[1] = 0.0; kalman->DynamMatr[2] = 1.0; kalman->DynamMatr[3] = 0.0;
kalman->DynamMatr[4] = 0.0; kalman->DynamMatr[5] = 1.0; kalman->DynamMatr[6] = 0.0; kalman->DynamMatr[7] = 1.0;
kalman->DynamMatr[8] = 0.0; kalman->DynamMatr[9] = 0.0; kalman->DynamMatr[10] = 1.0; kalman->DynamMatr[11] = 0.0;
kalman->DynamMatr[12] = 0.0; kalman->DynamMatr[13] = 0.0; kalman->DynamMatr[14] = 0.0; kalman->DynamMatr[15] = 1.0;
// 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(1);
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) {
// Draw a contour
cvZero(binalized);
cvDrawContours(binalized, maxContour, cvScalarAll(255), cvScalarAll(255), 0, CV_FILLED);
// Calculate the moments
CvMoments moments;
cvMoments(binalized, &moments, 1);
int my = (int)(moments.m01/moments.m00);
int mx = (int)(moments.m10/moments.m00);
// Measurements
float m[] = {mx, my};
CvMat measurement = cvMat(2, 1, CV_32FC1, m);
// Correct phase
const CvMat *correction = cvKalmanCorrect(kalman, &measurement);
}
// Prediction phase
const CvMat *prediction = cvKalmanPredict(kalman);
// Display the image
cvCircle(image, cvPointFrom32f(cvPoint2D32f(prediction->data.fl[0], prediction->data.fl[1])), 10, CV_RGB(0,255,0));
cvShowImage("camera", image);
// Release the memories
cvReleaseImage(&hsv);
cvReleaseImage(&binalized);
cvReleaseMemStorage(&contourStorage);
}
// Release the kalman filter
cvReleaseKalman(&kalman);
// 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;
zbar::ImageScanner scanner;
scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
// Main loop
while (1) {
// Key input
int key = cv::waitKey(33);
if (key == 0x1b) break;
// Get an image
cv::Mat image = ardrone.getImage();
//cout << image.channels() << " channels " << endl;
cvtColor(image, image, CV_RGB2GRAY);
//cout << image.channels() << " channels efter RGB2GRAY" << endl;
cv::Mat imgout;
//cout << image.cols << "image ardrone cols" << endl;
cvtColor(image, imgout, CV_GRAY2RGB);
//cvtColor(image, imgout);
// 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) x = 1.0;
if (key == 'k' || key == CV_VK_DOWN) x = -1.0;
if (key == 'u' || key == CV_VK_LEFT) r = 1.0;
if (key == 'o' || key == CV_VK_RIGHT) r = -1.0;
if (key == 'j') y = 1.0;
if (key == 'l') y = -1.0;
if (key == 'q') z = 1.0;
if (key == 'a') z = -1.0;
ardrone.move3D(x, y, z, r);
// Change camera
static int mode = 0;
if (key == 'c') ardrone.setCamera(++mode%4);
//cv::cvtColor(image, image, CV_GRAY2RGB);
//cv::cvtColor(image, image, CV_RGB2GRAY);
int width = image.cols;
int height = image.rows;
uchar *raw = (uchar *) image.data;
// wrap image data
zbar::Image imageQR(width, height, "Y800", raw, width * height);
// scan the image for barcodes
int n = scanner.scan(imageQR);
// extract results
for (zbar::Image::SymbolIterator symbol = imageQR.symbol_begin();
symbol != imageQR.symbol_end();
++symbol) {
cout << "Inde i for loop" << endl;
vector<Point> vp;
// do something useful with results
cout << "decoded " << symbol->get_type_name()
<< " symbol \"" << symbol->get_data() << '"' << " " << endl;
int n = symbol->get_location_size();
for (int i = 0; i<n; i++) {
vp.push_back(Point(symbol->get_location_x(i), symbol->get_location_y(i)));
}
RotatedRect r = minAreaRect(vp);
Point2f pts[4];
r.points(pts);
for (int i = 0; i<4; i++) {
line(imgout, pts[i], pts[(i + 1) % 4], Scalar(255, 0, 0), 3);
}
cout << "Angle: " << r.angle << endl;
}
//if(imgout)
imshow("imgout.jpg", imgout);
//cout << imgout.cols << " cols" << imgout.rows << " rows" << endl;
// clean up
imageQR.set_data(NULL, 0);
//waitKey(); Med dette fjernet kan vi bruge Esc
// Display the image from camera
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;
}
// Image of AR.Drone's camera
IplImage *image = ardrone.getImage();
// Variables for optical flow
int corner_count = 50;
IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
IplImage *prev = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
cvCvtColor(image, prev, CV_BGR2GRAY);
IplImage *eig_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *tmp_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *prev_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
IplImage *curr_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
CvPoint2D32f *corners1 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
CvPoint2D32f *corners2 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
// Main loop
while (1) {
// Key input
int key = cvWaitKey(33);
if (key == 0x1b) break;
// Update
if (!ardrone.update()) break;
// Get an image
image = ardrone.getImage();
// Convert the camera image to grayscale
cvCvtColor(image, gray, CV_BGR2GRAY);
// Detect features
int corner_count = 50;
cvGoodFeaturesToTrack(prev, eig_img, tmp_img, corners1, &corner_count, 0.1, 5.0, NULL);
// Corner detected
if (corner_count > 0) {
char *status = (char*)malloc(corner_count * sizeof(char));
// Calicurate optical flows
CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.3);
cvCalcOpticalFlowPyrLK(prev, gray, prev_pyramid, curr_pyramid, corners1, corners2, corner_count, cvSize(10, 10), 3, status, NULL, criteria, 0);
// Drow the optical flows
for (int i = 0; i < corner_count; i++) {
cvCircle(image, cvPointFrom32f(corners1[i]), 1, CV_RGB (255, 0, 0));
if (status[i]) cvLine(image, cvPointFrom32f(corners1[i]), cvPointFrom32f(corners2[i]), CV_RGB (0, 0, 255), 1, CV_AA, 0);
}
// Release the memory
free(status);
}
// Save the last frame
cvCopy(gray, prev);
// Display the image
cvShowImage("camera", image);
}
// Release the images
cvReleaseImage(&gray);
cvReleaseImage(&prev);
cvReleaseImage(&eig_img);
cvReleaseImage(&tmp_img);
cvReleaseImage(&prev_pyramid);
cvReleaseImage(&curr_pyramid);
free(corners1);
free(corners2);
// 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;
}
// 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;
}
// --------------------------------------------------------------------------
// 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();
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
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;
}