← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/samples/cpp/intelperc_capture.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/samples/cpp/intelperc_capture.cpp
 
1
// testOpenCVCam.cpp : Defines the entry point for the console application.
 
2
//
 
3
 
 
4
#include "opencv2/videoio/videoio.hpp"
 
5
#include "opencv2/highgui/highgui.hpp"
 
6
 
 
7
#include <iostream>
 
8
 
 
9
using namespace cv;
 
10
using namespace std;
 
11
 
 
12
static bool g_printStreamSetting;
 
13
static int g_imageStreamProfileIdx;
 
14
static int g_depthStreamProfileIdx;
 
15
static bool g_irStreamShow;
 
16
static double g_imageBrightness;
 
17
static double g_imageContrast;
 
18
static bool g_printTiming;
 
19
static bool g_showClosedPoint;
 
20
 
 
21
 
 
22
static int g_closedDepthPoint[2];
 
23
 
 
24
static void printUsage(const char *arg0)
 
25
{
 
26
    const char *filename = arg0;
 
27
    while (*filename)
 
28
        filename++;
 
29
    while ((arg0 <= filename) && ('\\' != *filename) && ('/' != *filename))
 
30
        filename--;
 
31
    filename++;
 
32
 
 
33
    cout << "This program demonstrates usage of camera supported\nby Intel Perceptual computing SDK." << endl << endl;
 
34
    cout << "usage: " << filename << "[-ps] [-isp=IDX] [-dsp=IDX]\n [-ir] [-imb=VAL] [-imc=VAL]" << endl << endl;
 
35
    cout << "   -ps,            print streams setting and profiles" << endl;
 
36
    cout << "   -isp=IDX,       set profile index of the image stream" << endl;
 
37
    cout << "   -dsp=IDX,       set profile index of the depth stream" << endl;
 
38
    cout << "   -ir,            show data from IR stream" << endl;
 
39
    cout << "   -imb=VAL,       set brighness value for a image stream" << endl;
 
40
    cout << "   -imc=VAL,       set contrast value for a image stream" << endl;
 
41
    cout << "   -pts,           print frame index and frame time" << endl;
 
42
    cout << "   --show-closed,  print frame index and frame time" << endl;
 
43
    cout <<  endl;
 
44
}
 
45
 
 
46
static void parseCMDLine(int argc, char* argv[])
 
47
{
 
48
    cv::CommandLineParser parser(argc, argv,
 
49
        "{ h help | | }"
 
50
        "{ ps print-streams | | }"
 
51
        "{ isp image-stream-prof | -1 | }"
 
52
        "{ dsp depth-stream-prof | -1 | }"
 
53
        "{ir||}{imb||}{imc||}{pts||}{show-closed||}");
 
54
    if (parser.has("h"))
 
55
    {
 
56
        printUsage(argv[0]);
 
57
        exit(0);
 
58
    }
 
59
    g_printStreamSetting = parser.has("ps");
 
60
    g_imageStreamProfileIdx = parser.get<int>("isp");
 
61
    g_depthStreamProfileIdx = parser.get<int>("dsp");
 
62
    g_irStreamShow = parser.has("ir");
 
63
    if (parser.has("imb"))
 
64
        g_imageBrightness = parser.get<double>("imb");
 
65
    else
 
66
        g_imageBrightness = -DBL_MAX;
 
67
    if (parser.has("imc"))
 
68
        g_imageContrast = parser.get<double>("imc");
 
69
    else
 
70
        g_imageContrast = -DBL_MAX;
 
71
    g_printTiming = parser.has("pts");
 
72
    g_showClosedPoint = parser.has("show-closed");
 
73
    if (!parser.check())
 
74
    {
 
75
        parser.printErrors();
 
76
        exit(-1);
 
77
    }
 
78
    if (g_showClosedPoint && (-1 == g_depthStreamProfileIdx))
 
79
    {
 
80
        cerr << "For --show-closed depth profile has be selected" << endl;
 
81
        exit(-1);
 
82
    }
 
83
}
 
84
 
 
85
static void printStreamProperties(VideoCapture &capture)
 
86
{
 
87
    size_t profilesCount = (size_t)capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_INTELPERC_PROFILE_COUNT);
 
88
    cout << "Image stream." << endl;
 
89
    cout << "  Brightness = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_BRIGHTNESS) << endl;
 
90
    cout << "  Contrast = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_CONTRAST) << endl;
 
91
    cout << "  Saturation = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_SATURATION) << endl;
 
92
    cout << "  Hue = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_HUE) << endl;
 
93
    cout << "  Gamma = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_GAMMA) << endl;
 
94
    cout << "  Sharpness = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_SHARPNESS) << endl;
 
95
    cout << "  Gain = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_GAIN) << endl;
 
96
    cout << "  Backligh = " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_BACKLIGHT) << endl;
 
97
    cout << "Image streams profiles:" << endl;
 
98
    for (size_t i = 0; i < profilesCount; i++)
 
99
    {
 
100
        capture.set(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_INTELPERC_PROFILE_IDX, (double)i);
 
101
        cout << "  Profile[" << i << "]: ";
 
102
        cout << "width = " <<
 
103
            (int)capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_FRAME_WIDTH);
 
104
        cout << ", height = " <<
 
105
            (int)capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_FRAME_HEIGHT);
 
106
        cout << ", fps = " <<
 
107
            capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_FPS);
 
108
        cout << endl;
 
109
    }
 
110
 
 
111
    profilesCount = (size_t)capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_PROFILE_COUNT);
 
112
    cout << "Depth stream." << endl;
 
113
    cout << "  Low confidence value = " << capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_DEPTH_LOW_CONFIDENCE_VALUE) << endl;
 
114
    cout << "  Saturation value = " << capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE) << endl;
 
115
    cout << "  Confidence threshold = " << capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_DEPTH_CONFIDENCE_THRESHOLD) << endl;
 
116
    cout << "  Focal length = (" << capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ) << ", "
 
117
        << capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT) << ")" << endl;
 
118
    cout << "Depth streams profiles:" << endl;
 
119
    for (size_t i = 0; i < profilesCount; i++)
 
120
    {
 
121
        capture.set(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_PROFILE_IDX, (double)i);
 
122
        cout << "  Profile[" << i << "]: ";
 
123
        cout << "width = " <<
 
124
            (int)capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_FRAME_WIDTH);
 
125
        cout << ", height = " <<
 
126
            (int)capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_FRAME_HEIGHT);
 
127
        cout << ", fps = " <<
 
128
            capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_FPS);
 
129
        cout << endl;
 
130
    }
 
131
}
 
132
 
 
133
static void imshowImage(const char *winname, Mat &image, VideoCapture &capture)
 
134
{
 
135
    if (g_showClosedPoint)
 
136
    {
 
137
        Mat uvMap;
 
138
        if (capture.retrieve(uvMap, CAP_INTELPERC_UVDEPTH_MAP))
 
139
        {
 
140
            float *uvmap = (float *)uvMap.ptr() + 2 * (g_closedDepthPoint[0] * uvMap.cols + g_closedDepthPoint[1]);
 
141
            int x = (int)((*uvmap) * image.cols); uvmap++;
 
142
            int y = (int)((*uvmap) * image.rows);
 
143
 
 
144
            if ((0 <= x) && (0 <= y))
 
145
            {
 
146
                static const int pointSize = 4;
 
147
                for (int row = y; row < min(y + pointSize, image.rows); row++)
 
148
                {
 
149
                    uchar* ptrDst = image.ptr(row) + x * 3 + 2;//+2 -> Red
 
150
                    for (int col = 0; col < min(pointSize, image.cols - x); col++, ptrDst+=3)
 
151
                    {
 
152
                        *ptrDst = 255;
 
153
                    }
 
154
                }
 
155
            }
 
156
        }
 
157
    }
 
158
    imshow(winname, image);
 
159
}
 
160
static void imshowIR(const char *winname, Mat &ir)
 
161
{
 
162
    Mat image;
 
163
    if (g_showClosedPoint)
 
164
    {
 
165
        image.create(ir.rows, ir.cols, CV_8UC3);
 
166
        for (int row = 0; row < ir.rows; row++)
 
167
        {
 
168
            uchar* ptrDst = image.ptr(row);
 
169
            short* ptrSrc = (short*)ir.ptr(row);
 
170
            for (int col = 0; col < ir.cols; col++, ptrSrc++)
 
171
            {
 
172
                uchar val = (uchar) ((*ptrSrc) >> 2);
 
173
                *ptrDst = val;  ptrDst++;
 
174
                *ptrDst = val;  ptrDst++;
 
175
                *ptrDst = val;  ptrDst++;
 
176
            }
 
177
        }
 
178
 
 
179
        static const int pointSize = 4;
 
180
        for (int row = g_closedDepthPoint[0]; row < min(g_closedDepthPoint[0] + pointSize, image.rows); row++)
 
181
        {
 
182
            uchar* ptrDst = image.ptr(row) + g_closedDepthPoint[1] * 3 + 2;//+2 -> Red
 
183
            for (int col = 0; col < min(pointSize, image.cols - g_closedDepthPoint[1]); col++, ptrDst+=3)
 
184
            {
 
185
                *ptrDst = 255;
 
186
            }
 
187
        }
 
188
    }
 
189
    else
 
190
    {
 
191
        image.create(ir.rows, ir.cols, CV_8UC1);
 
192
        for (int row = 0; row < ir.rows; row++)
 
193
        {
 
194
            uchar* ptrDst = image.ptr(row);
 
195
            short* ptrSrc = (short*)ir.ptr(row);
 
196
            for (int col = 0; col < ir.cols; col++, ptrSrc++, ptrDst++)
 
197
            {
 
198
                *ptrDst = (uchar) ((*ptrSrc) >> 2);
 
199
            }
 
200
        }
 
201
    }
 
202
 
 
203
    imshow(winname, image);
 
204
}
 
205
static void imshowDepth(const char *winname, Mat &depth, VideoCapture &capture)
 
206
{
 
207
    short lowValue = (short)capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_DEPTH_LOW_CONFIDENCE_VALUE);
 
208
    short saturationValue = (short)capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE);
 
209
 
 
210
    Mat image;
 
211
    if (g_showClosedPoint)
 
212
    {
 
213
        image.create(depth.rows, depth.cols, CV_8UC3);
 
214
        for (int row = 0; row < depth.rows; row++)
 
215
        {
 
216
            uchar* ptrDst = image.ptr(row);
 
217
            short* ptrSrc = (short*)depth.ptr(row);
 
218
            for (int col = 0; col < depth.cols; col++, ptrSrc++)
 
219
            {
 
220
                if ((lowValue == (*ptrSrc)) || (saturationValue == (*ptrSrc)))
 
221
                {
 
222
                    *ptrDst = 0; ptrDst++;
 
223
                    *ptrDst = 0; ptrDst++;
 
224
                    *ptrDst = 0; ptrDst++;
 
225
                }
 
226
                else
 
227
                {
 
228
                    uchar val = (uchar) ((*ptrSrc) >> 2);
 
229
                    *ptrDst = val;  ptrDst++;
 
230
                    *ptrDst = val;  ptrDst++;
 
231
                    *ptrDst = val;  ptrDst++;
 
232
                }
 
233
            }
 
234
        }
 
235
 
 
236
        static const int pointSize = 4;
 
237
        for (int row = g_closedDepthPoint[0]; row < min(g_closedDepthPoint[0] + pointSize, image.rows); row++)
 
238
        {
 
239
            uchar* ptrDst = image.ptr(row) + g_closedDepthPoint[1] * 3 + 2;//+2 -> Red
 
240
            for (int col = 0; col < min(pointSize, image.cols - g_closedDepthPoint[1]); col++, ptrDst+=3)
 
241
            {
 
242
                *ptrDst = 255;
 
243
            }
 
244
        }
 
245
    }
 
246
    else
 
247
    {
 
248
        image.create(depth.rows, depth.cols, CV_8UC1);
 
249
        for (int row = 0; row < depth.rows; row++)
 
250
        {
 
251
            uchar* ptrDst = image.ptr(row);
 
252
            short* ptrSrc = (short*)depth.ptr(row);
 
253
            for (int col = 0; col < depth.cols; col++, ptrSrc++, ptrDst++)
 
254
            {
 
255
                if ((lowValue == (*ptrSrc)) || (saturationValue == (*ptrSrc)))
 
256
                    *ptrDst = 0;
 
257
                else
 
258
                    *ptrDst = (uchar) ((*ptrSrc) >> 2);
 
259
            }
 
260
        }
 
261
    }
 
262
    imshow(winname, image);
 
263
}
 
264
 
 
265
int main(int argc, char* argv[])
 
266
{
 
267
    parseCMDLine(argc, argv);
 
268
 
 
269
    VideoCapture capture;
 
270
    capture.open(CAP_INTELPERC);
 
271
    if (!capture.isOpened())
 
272
    {
 
273
        cerr << "Can not open a capture object." << endl;
 
274
        return -1;
 
275
    }
 
276
 
 
277
    if (g_printStreamSetting)
 
278
        printStreamProperties(capture);
 
279
 
 
280
    if (-1 != g_imageStreamProfileIdx)
 
281
    {
 
282
        if (!capture.set(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_INTELPERC_PROFILE_IDX, (double)g_imageStreamProfileIdx))
 
283
        {
 
284
            cerr << "Can not setup a image stream." << endl;
 
285
            return -1;
 
286
        }
 
287
    }
 
288
    if (-1 != g_depthStreamProfileIdx)
 
289
    {
 
290
        if (!capture.set(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_PROFILE_IDX, (double)g_depthStreamProfileIdx))
 
291
        {
 
292
            cerr << "Can not setup a depth stream." << endl;
 
293
            return -1;
 
294
        }
 
295
    }
 
296
    else if (g_irStreamShow)
 
297
    {
 
298
        if (!capture.set(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_INTELPERC_PROFILE_IDX, 0.0))
 
299
        {
 
300
            cerr << "Can not setup a IR stream." << endl;
 
301
            return -1;
 
302
        }
 
303
    }
 
304
    else
 
305
    {
 
306
        cout << "Streams not selected" << endl;
 
307
        return 0;
 
308
    }
 
309
 
 
310
    //Setup additional properies only after set profile of the stream
 
311
    if ( (-10000.0 < g_imageBrightness) && (g_imageBrightness < 10000.0))
 
312
        capture.set(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_BRIGHTNESS, g_imageBrightness);
 
313
    if ( (0 < g_imageContrast) && (g_imageContrast < 10000.0))
 
314
        capture.set(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_BRIGHTNESS, g_imageContrast);
 
315
 
 
316
    int frame = 0;
 
317
    for(;;frame++)
 
318
    {
 
319
        Mat bgrImage;
 
320
        Mat depthImage;
 
321
        Mat irImage;
 
322
 
 
323
        if (!capture.grab())
 
324
        {
 
325
            cout << "Can not grab images." << endl;
 
326
            return -1;
 
327
        }
 
328
 
 
329
        if ((-1 != g_depthStreamProfileIdx) && (capture.retrieve(depthImage, CAP_INTELPERC_DEPTH_MAP)))
 
330
        {
 
331
            if (g_showClosedPoint)
 
332
            {
 
333
                double minVal = 0.0; double maxVal = 0.0;
 
334
                minMaxIdx(depthImage, &minVal, &maxVal, g_closedDepthPoint);
 
335
            }
 
336
            imshowDepth("depth image", depthImage, capture);
 
337
        }
 
338
        if ((g_irStreamShow) && (capture.retrieve(irImage, CAP_INTELPERC_IR_MAP)))
 
339
            imshowIR("ir image", irImage);
 
340
        if ((-1 != g_imageStreamProfileIdx) && (capture.retrieve(bgrImage, CAP_INTELPERC_IMAGE)))
 
341
            imshowImage("color image", bgrImage, capture);
 
342
 
 
343
        if (g_printTiming)
 
344
        {
 
345
            cout << "Image frame: " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_POS_FRAMES)
 
346
                 << ", Depth(IR) frame: " << capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_POS_FRAMES) << endl;
 
347
            cout << "Image frame: " << capture.get(CAP_INTELPERC_IMAGE_GENERATOR | CAP_PROP_POS_MSEC)
 
348
                 << ", Depth(IR) frame: " << capture.get(CAP_INTELPERC_DEPTH_GENERATOR | CAP_PROP_POS_MSEC) << endl;
 
349
        }
 
350
        if( waitKey(30) >= 0 )
 
351
            break;
 
352
    }
 
353
 
 
354
    return 0;
 
355
}