Puku's Laboratory

#include "ardrone/ardrone.h"

// --------------------------------------------------------------------------
// main(引数の数, 引数リスト)
// メイン関数
// 戻り値: 正常終了:0   エラー: -1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // AR.Droneクラス
    ARDrone ardrone;

    // 初期化
    if (!ardrone.open()) {
        std::cout << "初期化に失敗しました" << std::endl;
        return -1;
    }

    // 下カメラに切り替え
    ardrone.setCamera(1);

    // 閾値
    int minH = 0, maxH = 179;
    int minS = 0, maxS = 255;
    int minV = 0, maxV = 255;

    // XMLファイルを開く
    std::string filename("thresholds.xml");
    cv::FileStorage fs(filename, cv::FileStorage::READ);

    //ファイルが有れば閾値読み込み
    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();
    }

    // ウィンドウを生成
    cv::namedWindow("binalized");
    cv::createTrackbar("H max", "binalized", &maxH, 179);
    cv::createTrackbar("H min", "binalized", &minH, 179);
    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);

    // メインループ
    while (1) {
        // キー入力
        int key = cv::waitKey(30);
        if (key == 0x1b) break;

        // 画像を取得
        cv::Mat image = ardrone.getImage();

        // HSVに変換
        cv::Mat hsv;
        cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);

        // 2値化
        cv::Mat binalized;
        cv::Scalar lower(minH, minS, minV);
        cv::Scalar upper(maxH, maxS, maxV);
        cv::inRange(hsv, lower, upper, binalized);

        // 表示
        cv::imshow("binalized", binalized);

        // ノイズ除去
        cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
        cv::morphologyEx(binalized, binalized, cv::MORPH_CLOSE, kernel);
        //cv::imshow("morphologyEx", binalized);

        // 輪郭を検出
        std::vector<std::vector<cv::Point>> contours;
        cv::findContours(binalized.clone(), contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);

        // 一番大きい輪郭を抽出
        int contour_index = -1;
        double max_area = 0.0;
        for (int i = 0; i < (int)contours.size(); i++) {
            double area = fabs(cv::contourArea(contours[i]));
            if (area > max_area) {
                contour_index = i;
                max_area = area;
            }
        }

        // マーカ
        static cv::Point marker(binalized.cols / 2, binalized.rows / 2);

        // マーカが見つかった
        if (contour_index >= 0) {
            // 重心
            cv::Moments moments = cv::moments(contours[contour_index], true);
            marker.y = (int)(moments.m01 / moments.m00);
            marker.x = (int)(moments.m10 / moments.m00);

            // 表示
            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));
        }

        // 離陸・着陸
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // キー入力で移動
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (key == CV_VK_UP)    vx =  1.0;
        if (key == CV_VK_DOWN)  vx = -1.0;
        if (key == CV_VK_LEFT)  vr =  1.0;
        if (key == CV_VK_RIGHT) vr = -1.0;
        if (key == 'q')         vz =  1.0;
        if (key == 'a')         vz = -1.0;

        // マーカ追跡のON/OFF
        static int track = 0;
        if (key == 't') track = !track;
        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_AA);

        // マーカ追跡
        if (track) {
            // PIDゲイン
            const double kp = 0.001;
            const double ki = 0.000;
            const double kd = 0.000;

            // 誤差 (画像座標系-機体座標系の変換のため符号とXYが逆)
            double error_x = (binalized.rows / 2 - marker.y);
            double error_y = (binalized.cols / 2 - marker.x);

            // 時間 [s]
            static int64 last_t = 0.0;
            double dt = (cv::getTickCount() - last_t) / cv::getTickFrequency();
            last_t = cv::getTickCount();

            // 積分項
            static double integral_x = 0.0, integral_y = 0.0;
            if (dt > 0.1) {
                // リセット
                integral_x = 0.0;
                integral_y = 0.0;
            }
            integral_x += error_x * dt;
            integral_y += error_y * dt;

            // 微分項
            static double previous_error_x = 0.0, previous_error_y = 0.0;
            if (dt > 0.1) {
                // リセット
                previous_error_x = 0.0;
                previous_error_y = 0.0;
            }
            double derivative_x = (error_x - previous_error_x) / dt;
            double derivative_y = (error_y - previous_error_y) / dt;
            previous_error_x = error_x;
            previous_error_y = error_y;

            // 操作量
            vx = kp * error_x + ki * integral_x + kd * derivative_x;
            vy = kp * error_y + ki * integral_y + kd * derivative_y;
            vz = 0.0;
            vr = 0.0;
            std::cout << "(vx, vy)" << "(" << vx << "," << vy << ")" << std::endl;
        }

        // 移動
        ardrone.move3D(vx, vy, vz, vr);

        // 表示
        cv::imshow("camera", image);
    }

    // さようなら
    ardrone.close();

    // 閾値を保存
    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();
    }

    return 0;
}

        // マーカ追跡
        if (track) {
            // PIDゲイン
            const double kp = 0.001;
            const double ti = DBL_MAX;
            const double td = 0.000;
             :
             :
            // 操作量
            vx = kp * (error_x + integral_x/ti + td * derivative_x);
            vy = kp * (error_y + integral_y/ti + td * derivative_y);
            vz = 0.0;
            vr = 0.0;
            std::cout << "(vx, vy)" << "(" << vx << "," << vy << ")" << std::endl;
        }