← 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/openni_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/openni_capture.cpp
 
1
#include "opencv2/videoio/videoio.hpp"
 
2
#include "opencv2/highgui/highgui.hpp"
 
3
#include "opencv2/imgproc/imgproc.hpp"
 
4
 
 
5
#include <iostream>
 
6
 
 
7
using namespace cv;
 
8
using namespace std;
 
9
 
 
10
static void help()
 
11
{
 
12
        cout << "\nThis program demonstrates usage of depth sensors (Kinect, XtionPRO,...).\n"
 
13
                        "The user gets some of the supported output images.\n"
 
14
            "\nAll supported output map types:\n"
 
15
            "1.) Data given from depth generator\n"
 
16
            "   CAP_OPENNI_DEPTH_MAP            - depth values in mm (CV_16UC1)\n"
 
17
            "   CAP_OPENNI_POINT_CLOUD_MAP      - XYZ in meters (CV_32FC3)\n"
 
18
            "   CAP_OPENNI_DISPARITY_MAP        - disparity in pixels (CV_8UC1)\n"
 
19
            "   CAP_OPENNI_DISPARITY_MAP_32F    - disparity in pixels (CV_32FC1)\n"
 
20
            "   CAP_OPENNI_VALID_DEPTH_MASK     - mask of valid pixels (not ocluded, not shaded etc.) (CV_8UC1)\n"
 
21
            "2.) Data given from RGB image generator\n"
 
22
            "   CAP_OPENNI_BGR_IMAGE            - color image (CV_8UC3)\n"
 
23
            "   CAP_OPENNI_GRAY_IMAGE           - gray image (CV_8UC1)\n"
 
24
         << endl;
 
25
}
 
26
 
 
27
static void colorizeDisparity( const Mat& gray, Mat& rgb, double maxDisp=-1.f, float S=1.f, float V=1.f )
 
28
{
 
29
    CV_Assert( !gray.empty() );
 
30
    CV_Assert( gray.type() == CV_8UC1 );
 
31
 
 
32
    if( maxDisp <= 0 )
 
33
    {
 
34
        maxDisp = 0;
 
35
        minMaxLoc( gray, 0, &maxDisp );
 
36
    }
 
37
 
 
38
    rgb.create( gray.size(), CV_8UC3 );
 
39
    rgb = Scalar::all(0);
 
40
    if( maxDisp < 1 )
 
41
        return;
 
42
 
 
43
    for( int y = 0; y < gray.rows; y++ )
 
44
    {
 
45
        for( int x = 0; x < gray.cols; x++ )
 
46
        {
 
47
            uchar d = gray.at<uchar>(y,x);
 
48
            unsigned int H = ((uchar)maxDisp - d) * 240 / (uchar)maxDisp;
 
49
 
 
50
            unsigned int hi = (H/60) % 6;
 
51
            float f = H/60.f - H/60;
 
52
            float p = V * (1 - S);
 
53
            float q = V * (1 - f * S);
 
54
            float t = V * (1 - (1 - f) * S);
 
55
 
 
56
            Point3f res;
 
57
 
 
58
            if( hi == 0 ) //R = V,  G = t,  B = p
 
59
                res = Point3f( p, t, V );
 
60
            if( hi == 1 ) // R = q, G = V,  B = p
 
61
                res = Point3f( p, V, q );
 
62
            if( hi == 2 ) // R = p, G = V,  B = t
 
63
                res = Point3f( t, V, p );
 
64
            if( hi == 3 ) // R = p, G = q,  B = V
 
65
                res = Point3f( V, q, p );
 
66
            if( hi == 4 ) // R = t, G = p,  B = V
 
67
                res = Point3f( V, p, t );
 
68
            if( hi == 5 ) // R = V, G = p,  B = q
 
69
                res = Point3f( q, p, V );
 
70
 
 
71
            uchar b = (uchar)(std::max(0.f, std::min (res.x, 1.f)) * 255.f);
 
72
            uchar g = (uchar)(std::max(0.f, std::min (res.y, 1.f)) * 255.f);
 
73
            uchar r = (uchar)(std::max(0.f, std::min (res.z, 1.f)) * 255.f);
 
74
 
 
75
            rgb.at<Point3_<uchar> >(y,x) = Point3_<uchar>(b, g, r);
 
76
        }
 
77
    }
 
78
}
 
79
 
 
80
static float getMaxDisparity( VideoCapture& capture )
 
81
{
 
82
    const int minDistance = 400; // mm
 
83
    float b = (float)capture.get( CAP_OPENNI_DEPTH_GENERATOR_BASELINE ); // mm
 
84
    float F = (float)capture.get( CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ); // pixels
 
85
    return b * F / minDistance;
 
86
}
 
87
 
 
88
static void printCommandLineParams()
 
89
{
 
90
    cout << "-cd=       Colorized disparity? (0 or 1; 1 by default) Ignored if disparity map is not selected to show." << endl;
 
91
    cout << "-fmd=      Fixed max disparity? (0 or 1; 0 by default) Ignored if disparity map is not colorized (-cd 0)." << endl;
 
92
    cout << "-mode=     image mode: resolution and fps, supported three values:  0 - CAP_OPENNI_VGA_30HZ, 1 - CAP_OPENNI_SXGA_15HZ," << endl;
 
93
    cout << "          2 - CAP_OPENNI_SXGA_30HZ (0 by default). Ignored if rgb image or gray image are not selected to show." << endl;
 
94
    cout << "-m=        Mask to set which output images are need. It is a string of size 5. Each element of this is '0' or '1' and" << endl;
 
95
    cout << "          determine: is depth map, disparity map, valid pixels mask, rgb image, gray image need or not (correspondently)?" << endl ;
 
96
    cout << "          By default -m=01010 i.e. disparity map and rgb image will be shown." << endl ;
 
97
    cout << "-r=        Filename of .oni video file. The data will grabbed from it." << endl ;
 
98
}
 
99
 
 
100
static void parseCommandLine( int argc, char* argv[], bool& isColorizeDisp, bool& isFixedMaxDisp, int& imageMode, bool retrievedImageFlags[],
 
101
                       string& filename, bool& isFileReading )
 
102
{
 
103
    filename.clear();
 
104
    cv::CommandLineParser parser(argc, argv, "{h help||}{cd|1|}{fmd|0|}{mode|0|}{m|01010|}{r||}");
 
105
    if (parser.has("h"))
 
106
    {
 
107
        help();
 
108
        printCommandLineParams();
 
109
        exit(0);
 
110
    }
 
111
    isColorizeDisp = (parser.get<int>("cd") != 0);
 
112
    isFixedMaxDisp = (parser.get<int>("fmd") != 0);
 
113
    imageMode = parser.get<int>("mode");
 
114
    int flags = parser.get<int>("m");
 
115
    isFileReading = parser.has("r");
 
116
    if (isFileReading)
 
117
        filename = parser.get<string>("r");
 
118
    if (!parser.check())
 
119
    {
 
120
        parser.printErrors();
 
121
        help();
 
122
        exit(-1);
 
123
    }
 
124
    if (flags % 100000 == 0)
 
125
    {
 
126
        cout << "No one output image is selected." << endl;
 
127
        exit(0);
 
128
    }
 
129
    for (int i = 0; i < 5; i++)
 
130
    {
 
131
        retrievedImageFlags[4 - i] = (flags % 10 != 0);
 
132
        flags /= 10;
 
133
    }
 
134
}
 
135
 
 
136
/*
 
137
 * To work with Kinect or XtionPRO the user must install OpenNI library and PrimeSensorModule for OpenNI and
 
138
 * configure OpenCV with WITH_OPENNI flag is ON (using CMake).
 
139
 */
 
140
int main( int argc, char* argv[] )
 
141
{
 
142
    bool isColorizeDisp, isFixedMaxDisp;
 
143
    int imageMode;
 
144
    bool retrievedImageFlags[5];
 
145
    string filename;
 
146
    bool isVideoReading;
 
147
    parseCommandLine( argc, argv, isColorizeDisp, isFixedMaxDisp, imageMode, retrievedImageFlags, filename, isVideoReading );
 
148
 
 
149
    cout << "Device opening ..." << endl;
 
150
    VideoCapture capture;
 
151
    if( isVideoReading )
 
152
        capture.open( filename );
 
153
    else
 
154
    {
 
155
        capture.open( CAP_OPENNI2 );
 
156
        if( !capture.isOpened() )
 
157
            capture.open( CAP_OPENNI );
 
158
    }
 
159
 
 
160
    cout << "done." << endl;
 
161
 
 
162
    if( !capture.isOpened() )
 
163
    {
 
164
        cout << "Can not open a capture object." << endl;
 
165
        return -1;
 
166
    }
 
167
 
 
168
    if( !isVideoReading )
 
169
    {
 
170
        bool modeRes=false;
 
171
        switch ( imageMode )
 
172
        {
 
173
            case 0:
 
174
                modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_VGA_30HZ );
 
175
                break;
 
176
            case 1:
 
177
                modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_SXGA_15HZ );
 
178
                break;
 
179
            case 2:
 
180
                modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_SXGA_30HZ );
 
181
                break;
 
182
                //The following modes are only supported by the Xtion Pro Live
 
183
            case 3:
 
184
                modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_QVGA_30HZ );
 
185
                break;
 
186
            case 4:
 
187
                modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_QVGA_60HZ );
 
188
                break;
 
189
            default:
 
190
                CV_Error( Error::StsBadArg, "Unsupported image mode property.\n");
 
191
        }
 
192
        if (!modeRes)
 
193
            cout << "\nThis image mode is not supported by the device, the default value (CV_CAP_OPENNI_SXGA_15HZ) will be used.\n" << endl;
 
194
    }
 
195
 
 
196
    // Print some avalible device settings.
 
197
    cout << "\nDepth generator output mode:" << endl <<
 
198
            "FRAME_WIDTH      " << capture.get( CAP_PROP_FRAME_WIDTH ) << endl <<
 
199
            "FRAME_HEIGHT     " << capture.get( CAP_PROP_FRAME_HEIGHT ) << endl <<
 
200
            "FRAME_MAX_DEPTH  " << capture.get( CAP_PROP_OPENNI_FRAME_MAX_DEPTH ) << " mm" << endl <<
 
201
            "FPS              " << capture.get( CAP_PROP_FPS ) << endl <<
 
202
            "REGISTRATION     " << capture.get( CAP_PROP_OPENNI_REGISTRATION ) << endl;
 
203
    if( capture.get( CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
 
204
    {
 
205
        cout <<
 
206
            "\nImage generator output mode:" << endl <<
 
207
            "FRAME_WIDTH   " << capture.get( CAP_OPENNI_IMAGE_GENERATOR+CAP_PROP_FRAME_WIDTH ) << endl <<
 
208
            "FRAME_HEIGHT  " << capture.get( CAP_OPENNI_IMAGE_GENERATOR+CAP_PROP_FRAME_HEIGHT ) << endl <<
 
209
            "FPS           " << capture.get( CAP_OPENNI_IMAGE_GENERATOR+CAP_PROP_FPS ) << endl;
 
210
    }
 
211
    else
 
212
    {
 
213
        cout << "\nDevice doesn't contain image generator." << endl;
 
214
        if (!retrievedImageFlags[0] && !retrievedImageFlags[1] && !retrievedImageFlags[2])
 
215
            return 0;
 
216
    }
 
217
 
 
218
    for(;;)
 
219
    {
 
220
        Mat depthMap;
 
221
        Mat validDepthMap;
 
222
        Mat disparityMap;
 
223
        Mat bgrImage;
 
224
        Mat grayImage;
 
225
 
 
226
        if( !capture.grab() )
 
227
        {
 
228
            cout << "Can not grab images." << endl;
 
229
            return -1;
 
230
        }
 
231
        else
 
232
        {
 
233
            if( retrievedImageFlags[0] && capture.retrieve( depthMap, CAP_OPENNI_DEPTH_MAP ) )
 
234
            {
 
235
                const float scaleFactor = 0.05f;
 
236
                Mat show; depthMap.convertTo( show, CV_8UC1, scaleFactor );
 
237
                imshow( "depth map", show );
 
238
            }
 
239
 
 
240
            if( retrievedImageFlags[1] && capture.retrieve( disparityMap, CAP_OPENNI_DISPARITY_MAP ) )
 
241
            {
 
242
                if( isColorizeDisp )
 
243
                {
 
244
                    Mat colorDisparityMap;
 
245
                    colorizeDisparity( disparityMap, colorDisparityMap, isFixedMaxDisp ? getMaxDisparity(capture) : -1 );
 
246
                    Mat validColorDisparityMap;
 
247
                    colorDisparityMap.copyTo( validColorDisparityMap, disparityMap != 0 );
 
248
                    imshow( "colorized disparity map", validColorDisparityMap );
 
249
                }
 
250
                else
 
251
                {
 
252
                    imshow( "original disparity map", disparityMap );
 
253
                }
 
254
            }
 
255
 
 
256
            if( retrievedImageFlags[2] && capture.retrieve( validDepthMap, CAP_OPENNI_VALID_DEPTH_MASK ) )
 
257
                imshow( "valid depth mask", validDepthMap );
 
258
 
 
259
            if( retrievedImageFlags[3] && capture.retrieve( bgrImage, CAP_OPENNI_BGR_IMAGE ) )
 
260
                imshow( "rgb image", bgrImage );
 
261
 
 
262
            if( retrievedImageFlags[4] && capture.retrieve( grayImage, CAP_OPENNI_GRAY_IMAGE ) )
 
263
                imshow( "gray image", grayImage );
 
264
        }
 
265
 
 
266
        if( waitKey( 30 ) >= 0 )
 
267
            break;
 
268
    }
 
269
 
 
270
    return 0;
 
271
}