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
help()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
colorizeDisparity(const Mat & gray,Mat & rgb,double maxDisp=-1.f,float S=1.f,float V=1.f)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
getMaxDisparity(VideoCapture & capture)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
printCommandLineParams()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
parseCommandLine(int argc,char * argv[],bool & isColorizeDisp,bool & isFixedMaxDisp,int & imageMode,bool retrievedImageFlags[],string & filename,bool & isFileReading)100 static void parseCommandLine( int argc, char* argv[], bool& isColorizeDisp, bool& isFixedMaxDisp, int& imageMode, bool retrievedImageFlags[],
101 string& filename, bool& isFileReading )
102 {
103 // set defaut values
104 isColorizeDisp = true;
105 isFixedMaxDisp = false;
106 imageMode = 0;
107
108 retrievedImageFlags[0] = false;
109 retrievedImageFlags[1] = true;
110 retrievedImageFlags[2] = false;
111 retrievedImageFlags[3] = true;
112 retrievedImageFlags[4] = false;
113
114 filename.clear();
115 isFileReading = false;
116
117 if( argc == 1 )
118 {
119 help();
120 }
121 else
122 {
123 for( int i = 1; i < argc; i++ )
124 {
125 if( !strcmp( argv[i], "--help" ) || !strcmp( argv[i], "-h" ) )
126 {
127 printCommandLineParams();
128 exit(0);
129 }
130 else if( !strcmp( argv[i], "-cd" ) )
131 {
132 isColorizeDisp = atoi(argv[++i]) == 0 ? false : true;
133 }
134 else if( !strcmp( argv[i], "-fmd" ) )
135 {
136 isFixedMaxDisp = atoi(argv[++i]) == 0 ? false : true;
137 }
138 else if( !strcmp( argv[i], "-mode" ) )
139 {
140 imageMode = atoi(argv[++i]);
141 }
142 else if( !strcmp( argv[i], "-m" ) )
143 {
144 string mask( argv[++i] );
145 if( mask.size() != 5)
146 CV_Error( Error::StsBadArg, "Incorrect length of -m argument string" );
147 int val = atoi(mask.c_str());
148
149 int l = 100000, r = 10000, sum = 0;
150 for( int j = 0; j < 5; j++ )
151 {
152 retrievedImageFlags[j] = ((val % l) / r ) == 0 ? false : true;
153 l /= 10; r /= 10;
154 if( retrievedImageFlags[j] ) sum++;
155 }
156
157 if( sum == 0 )
158 {
159 cout << "No one output image is selected." << endl;
160 exit(0);
161 }
162 }
163 else if( !strcmp( argv[i], "-r" ) )
164 {
165 filename = argv[++i];
166 isFileReading = true;
167 }
168 else
169 {
170 cout << "Unsupported command line argument: " << argv[i] << "." << endl;
171 exit(-1);
172 }
173 }
174 }
175 }
176
177 /*
178 * To work with Kinect or XtionPRO the user must install OpenNI library and PrimeSensorModule for OpenNI and
179 * configure OpenCV with WITH_OPENNI flag is ON (using CMake).
180 */
main(int argc,char * argv[])181 int main( int argc, char* argv[] )
182 {
183 bool isColorizeDisp, isFixedMaxDisp;
184 int imageMode;
185 bool retrievedImageFlags[5];
186 string filename;
187 bool isVideoReading;
188 parseCommandLine( argc, argv, isColorizeDisp, isFixedMaxDisp, imageMode, retrievedImageFlags, filename, isVideoReading );
189
190 cout << "Device opening ..." << endl;
191 VideoCapture capture;
192 if( isVideoReading )
193 capture.open( filename );
194 else
195 {
196 capture.open( CAP_OPENNI2 );
197 if( !capture.isOpened() )
198 capture.open( CAP_OPENNI );
199 }
200
201 cout << "done." << endl;
202
203 if( !capture.isOpened() )
204 {
205 cout << "Can not open a capture object." << endl;
206 return -1;
207 }
208
209 if( !isVideoReading )
210 {
211 bool modeRes=false;
212 switch ( imageMode )
213 {
214 case 0:
215 modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_VGA_30HZ );
216 break;
217 case 1:
218 modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_SXGA_15HZ );
219 break;
220 case 2:
221 modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_SXGA_30HZ );
222 break;
223 //The following modes are only supported by the Xtion Pro Live
224 case 3:
225 modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_QVGA_30HZ );
226 break;
227 case 4:
228 modeRes = capture.set( CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CAP_OPENNI_QVGA_60HZ );
229 break;
230 default:
231 CV_Error( Error::StsBadArg, "Unsupported image mode property.\n");
232 }
233 if (!modeRes)
234 cout << "\nThis image mode is not supported by the device, the default value (CV_CAP_OPENNI_SXGA_15HZ) will be used.\n" << endl;
235 }
236
237 // Print some avalible device settings.
238 cout << "\nDepth generator output mode:" << endl <<
239 "FRAME_WIDTH " << capture.get( CAP_PROP_FRAME_WIDTH ) << endl <<
240 "FRAME_HEIGHT " << capture.get( CAP_PROP_FRAME_HEIGHT ) << endl <<
241 "FRAME_MAX_DEPTH " << capture.get( CAP_PROP_OPENNI_FRAME_MAX_DEPTH ) << " mm" << endl <<
242 "FPS " << capture.get( CAP_PROP_FPS ) << endl <<
243 "REGISTRATION " << capture.get( CAP_PROP_OPENNI_REGISTRATION ) << endl;
244 if( capture.get( CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
245 {
246 cout <<
247 "\nImage generator output mode:" << endl <<
248 "FRAME_WIDTH " << capture.get( CAP_OPENNI_IMAGE_GENERATOR+CAP_PROP_FRAME_WIDTH ) << endl <<
249 "FRAME_HEIGHT " << capture.get( CAP_OPENNI_IMAGE_GENERATOR+CAP_PROP_FRAME_HEIGHT ) << endl <<
250 "FPS " << capture.get( CAP_OPENNI_IMAGE_GENERATOR+CAP_PROP_FPS ) << endl;
251 }
252 else
253 {
254 cout << "\nDevice doesn't contain image generator." << endl;
255 if (!retrievedImageFlags[0] && !retrievedImageFlags[1] && !retrievedImageFlags[2])
256 return 0;
257 }
258
259 for(;;)
260 {
261 Mat depthMap;
262 Mat validDepthMap;
263 Mat disparityMap;
264 Mat bgrImage;
265 Mat grayImage;
266
267 if( !capture.grab() )
268 {
269 cout << "Can not grab images." << endl;
270 return -1;
271 }
272 else
273 {
274 if( retrievedImageFlags[0] && capture.retrieve( depthMap, CAP_OPENNI_DEPTH_MAP ) )
275 {
276 const float scaleFactor = 0.05f;
277 Mat show; depthMap.convertTo( show, CV_8UC1, scaleFactor );
278 imshow( "depth map", show );
279 }
280
281 if( retrievedImageFlags[1] && capture.retrieve( disparityMap, CAP_OPENNI_DISPARITY_MAP ) )
282 {
283 if( isColorizeDisp )
284 {
285 Mat colorDisparityMap;
286 colorizeDisparity( disparityMap, colorDisparityMap, isFixedMaxDisp ? getMaxDisparity(capture) : -1 );
287 Mat validColorDisparityMap;
288 colorDisparityMap.copyTo( validColorDisparityMap, disparityMap != 0 );
289 imshow( "colorized disparity map", validColorDisparityMap );
290 }
291 else
292 {
293 imshow( "original disparity map", disparityMap );
294 }
295 }
296
297 if( retrievedImageFlags[2] && capture.retrieve( validDepthMap, CAP_OPENNI_VALID_DEPTH_MASK ) )
298 imshow( "valid depth mask", validDepthMap );
299
300 if( retrievedImageFlags[3] && capture.retrieve( bgrImage, CAP_OPENNI_BGR_IMAGE ) )
301 imshow( "rgb image", bgrImage );
302
303 if( retrievedImageFlags[4] && capture.retrieve( grayImage, CAP_OPENNI_GRAY_IMAGE ) )
304 imshow( "gray image", grayImage );
305 }
306
307 if( waitKey( 30 ) >= 0 )
308 break;
309 }
310
311 return 0;
312 }
313