visionloc/example_libvisionloc.cc at master · noether/visionloc · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#include <unistd.h>
#include <iostream>
#include <visionloc.hh>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

struct cam_properties {
    int id;
    int width;
    int height;

    std::string window;
    cv::Mat frame;
};

int main(int argc, char* argv[])
{
    if (argc != 2) {
        std::cerr << "Usage example_libvisionloc num_markers" << std::endl;
        return -1;
    }

    int nm = std::atoi(argv[1]);

    std::vector<Marker> markers;
    int tag;

    start_visionloc();

    std::vector<int> cam_ids = get_active_cameras();

    int num_cameras = cam_ids.size();
    struct cam_properties * cams = new struct cam_properties[num_cameras];

    std::cout << num_cameras << " camera(s)" << std::endl;
    for (int n = 0; n < num_cameras; n++) {
        cams[n].id = cam_ids[n];
        cams[n].width = get_cam_width(cams[n].id);
        cams[n].height = get_cam_height(cams[n].id);

        std::ostringstream oss;
        oss << "Cam" << cams[n].id;
        cams[n].window = oss.str();

        cv::namedWindow(cams[n].window, CV_WINDOW_AUTOSIZE);

        std::cout << "  Cam " << cams[n].id  << ": " <<
            cams[n].width << "x" <<
            cams[n].height << std::endl;
        set_expected_num_of_markers(cams[n].id, nm);
    }

    int i;
    for(i = 0; i < 1000; i++) {
        for (int n = 0; n < num_cameras; n++) {
            cv::Mat *grayframe = get_frame_from_camera(cams[n].id);
            cvtColor(*grayframe, cams[n].frame, CV_GRAY2RGB);
            delete grayframe;

            markers = get_markers_from_camera(cams[n].id);
            tag = get_tag(cams[n].id);
            std::cout << "Num of markers recognized on camera " << cams[n].id << ": " << markers.size();
            if (markers.size())
                std::cout << ", tag " << tag;
            std::cout << std::endl;
            for (std::vector<Marker>::iterator itm = markers.begin();
                    itm != markers.end(); ++itm)
            {
                std::cout << "    Marker " << (int)(itm->id) << " at ("
                    << itm->wc_corner_posX <<", "
                    << itm->wc_corner_posY <<")"
                    << " with world heading " << itm->wc_heading << std::endl;
                std::ostringstream oss;
                oss << (int)(itm->id);

                cv::putText(cams[n].frame,
                    oss.str(),
                    cv::Point(itm->cam_center_posX, cams[n].height - itm->cam_center_posY),
                    cv::FONT_HERSHEY_PLAIN,
                    5,
                    cv::Scalar(0,255,0),
                    5);

            }
            cv::imshow(cams[n].window, cams[n].frame);
        }

        if (cv::waitKey(30) == 27){
            break;
        }
        sleep(1);
    }

    stop_visionloc();
    delete cams;
}