VENUS ROBOTIX

#include <opencv2/core/core.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>

#include "image_processing.h"

const unsigned short width       = 640;

const unsigned short height      = 480;

const double         f_per_s     = 636;

const double         ball_radius = 10;

int main(int argc, char *argv[])

{

    cv::VideoCapture cap(2);

    cap.set(CV_CAP_PROP_FRAME_WIDTH,  width);

    cap.set(CV_CAP_PROP_FRAME_HEIGHT, height);

    if(!cap.isOpened())

    {

        return -1;

    }

    cv::namedWindow("Capture", CV_WINDOW_AUTOSIZE|CV_WINDOW_FREERATIO);

    hsv_threshold threshold = {100,  110, 200, 50};

    cv::Mat frame;

    cv::Mat binary;

    float radius = 0.0f;

    double z, x, y = 0.0;

    cv::vector<cv::Vec3f> circles;

    while(1)

    {

        cap >> frame;

        binary = extract_color(frame, threshold);         

        cv::HoughCircles(binary, circles, CV_HOUGH_GRADIENT, 1, binary.rows/8, 200, 10, 0, 0);

        if(circles.size() > 0)

        {

            cv::Point2f center(circles[0][0], circles[0][1]);

            radius = circles[0][2];

            z = ball_radius / radius * f_per_s;

            y = (center.x - width / 2) * z / f_per_s;

            x = (center.y - height / 2) * z / f_per_s;

            std::cout << "Z: " << z << " X: " << x << " Y: " << y << std::endl;

            cv::circle(frame, center, 3, cv::Scalar(0, 255, 0), -1, 8, 0);

            cv::circle(frame, center, radius, cv::Scalar(0, 0, 255), 3, 8, 0);

        }

        cv::imshow("Capture",    frame);

        if(cv::waitKey(30) == 27)

        {

            cv::destroyAllWindows();            

            break;

        }

    }

    return 0;

}