1
$\begingroup$

I am trying to develop a line following algorithm where a drone will detect a bounding box and follow what is inside the bounding box.

I am filtering all the colors to only see the color white. Once that color is detected, I want the drone to go in a straight line from one end of the box to the other. Maybe it is easier using the draw line function on openCV but I am not sure. Any way, my biggest problem is telling the drone to follow the color or in other words the detected object.

I am using this repository from GitHub. Anyway this the code I have so far, and it only follows items that are moving. I need to follow an object that is stationary if that is possible.

#include "ardrone/ardrone.h"

int main(int argc, char *argv[]) {
    // AR.Drone class
    ARDrone ardrone;

    // Initialize

    if (!ardrone.open()) {
        std::cout << "Failed to initialize." << std::endl;
        return -1;
    }

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

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

    char textBuffer[80];
    cv::Scalar green = CV_RGB(0,255,0);
    float speed = 0.0;
    bool learnMode = false;

    // Main loop

    while (1) {

        // Key input

        int key = cv::waitKey(33);
        if (key == 0x1b) 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(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);

            // Measurements

            cv::Mat measurement = (cv::Mat1f(2, 1) << marker_x, marker_y);

            // Correction

            cv::Mat estimated = kalman.correct(measurement);

            // Show result

            cv::Rect rect = cv::boundingRect(contours[contour_index]);
            cv::rectangle(image, rect, cv::Scalar(0, 255, 0));
        }

        // Prediction

        cv::Mat1f prediction = kalman.predict();
        int radius = 1e+3 * kalman.errorCovPre.at<float>(0, 0);

        // Calculate object heading fraction

        float heading = -((image.cols/2)-prediction(0, 0))/(image.cols/2);
        sprintf(textBuffer, "heading = %+3.2f", heading);
        putText(image, textBuffer, cvPoint(30,30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

        // Show predicted position

        cv::circle(image, cv::Point(prediction(0, 0), prediction(0, 1)), radius, green, 2);

        //Speed

        if ((key >= '0') && (key <= '9')) {
            speed = (key-'0')*0.1;
            //printf("speed = %3.2f\n", speed);
        }

        sprintf(textBuffer, "speed = %3.2f", speed);
        putText(image, textBuffer, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

        // Drone control

        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;

        // Auto-follow

        vx = speed;
        vr = -heading;

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

    // See you

    ardrone.close();

    return 0;
}

I also tried using PID's like so.

        // Find largest contour
        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;
            }
        }

        // A marker detected
        if (contour_index >= 0) {
            // Moments
            cv::Moments moments = cv::moments(contours[contour_index], true);
            marker.y = (int)(moments.m01 / moments.m00);
            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));
            //cv::drawContours(image, contours, contour_index, cv::Scalar(0,255,0));
        }

        // Take off / Landing 
        if (key == ' ') {
            if (ardrone.onGround()) ardrone.takeoff();
            else                    ardrone.landing();
        }

        // Move using keyboard
        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;

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

        // Marker tracking
        if (track) {
            // PID gains
            const double kp = 0.001;
            const double ki = 0.000;
            const double kd = 0.000;

            // Errors
            double error_x = (binalized.rows / 2 - marker.y);   // Error front/back
            double error_y = (binalized.cols / 2 - marker.x);   // Error left/right

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

            // Integral terms
            static double integral_x = 0.0, integral_y = 0.0;
            if (dt > 0.1) {
                // Reset
                integral_x = 0.0;
                integral_y = 0.0;
            }
            integral_x += error_x * dt;
            integral_y += error_y * dt;

            // Derivative terms
            static double previous_error_x = 0.0, previous_error_y = 0.0;
            if (dt > 0.1) {
                // Reset
                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;

            // Command velocities
            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;
        }

        // Move
        ardrone.move3D(vx, vy, vz, vr);
| improve this question | | | | |
$\endgroup$