1 #include <opencv2/videoio/videoio.hpp>
2 #include <opencv2/highgui.hpp>
3 #include <opencv2/imgproc.hpp>
4
5 #include <list>
6 #include <iostream>
7 #include <thread>
8 #include <mutex>
9 #include <condition_variable>
10 #include <atomic>
11
12 using namespace cv;
13 using std::cout;
14 using std::cerr;
15 using std::endl;
16
17
18 // Stores frames along with their timestamps
19 struct Frame
20 {
21 int64 timestamp;
22 Mat frame;
23 };
24
main()25 int main()
26 {
27 //! [Open streams]
28 // Open depth stream
29 VideoCapture depthStream(CAP_OPENNI2_ASTRA);
30 // Open color stream
31 VideoCapture colorStream(0, CAP_V4L2);
32 //! [Open streams]
33
34 // Check that stream has opened
35 if (!colorStream.isOpened())
36 {
37 cerr << "ERROR: Unable to open color stream" << endl;
38 return 1;
39 }
40
41 // Check that stream has opened
42 if (!depthStream.isOpened())
43 {
44 cerr << "ERROR: Unable to open depth stream" << endl;
45 return 1;
46 }
47
48 //! [Setup streams]
49 // Set color and depth stream parameters
50 colorStream.set(CAP_PROP_FRAME_WIDTH, 640);
51 colorStream.set(CAP_PROP_FRAME_HEIGHT, 480);
52 depthStream.set(CAP_PROP_FRAME_WIDTH, 640);
53 depthStream.set(CAP_PROP_FRAME_HEIGHT, 480);
54 depthStream.set(CAP_PROP_OPENNI2_MIRROR, 0);
55 //! [Setup streams]
56
57 // Print color stream parameters
58 cout << "Color stream: "
59 << colorStream.get(CAP_PROP_FRAME_WIDTH) << "x" << colorStream.get(CAP_PROP_FRAME_HEIGHT)
60 << " @" << colorStream.get(CAP_PROP_FPS) << " fps" << endl;
61
62 //! [Get properties]
63 // Print depth stream parameters
64 cout << "Depth stream: "
65 << depthStream.get(CAP_PROP_FRAME_WIDTH) << "x" << depthStream.get(CAP_PROP_FRAME_HEIGHT)
66 << " @" << depthStream.get(CAP_PROP_FPS) << " fps" << endl;
67 //! [Get properties]
68
69 //! [Read streams]
70 // Create two lists to store frames
71 std::list<Frame> depthFrames, colorFrames;
72 const std::size_t maxFrames = 64;
73
74 // Synchronization objects
75 std::mutex mtx;
76 std::condition_variable dataReady;
77 std::atomic<bool> isFinish;
78
79 isFinish = false;
80
81 // Start depth reading thread
82 std::thread depthReader([&]
83 {
84 while (!isFinish)
85 {
86 // Grab and decode new frame
87 if (depthStream.grab())
88 {
89 Frame f;
90 f.timestamp = cv::getTickCount();
91 depthStream.retrieve(f.frame, CAP_OPENNI_DEPTH_MAP);
92 if (f.frame.empty())
93 {
94 cerr << "ERROR: Failed to decode frame from depth stream" << endl;
95 break;
96 }
97
98 {
99 std::lock_guard<std::mutex> lk(mtx);
100 if (depthFrames.size() >= maxFrames)
101 depthFrames.pop_front();
102 depthFrames.push_back(f);
103 }
104 dataReady.notify_one();
105 }
106 }
107 });
108
109 // Start color reading thread
110 std::thread colorReader([&]
111 {
112 while (!isFinish)
113 {
114 // Grab and decode new frame
115 if (colorStream.grab())
116 {
117 Frame f;
118 f.timestamp = cv::getTickCount();
119 colorStream.retrieve(f.frame);
120 if (f.frame.empty())
121 {
122 cerr << "ERROR: Failed to decode frame from color stream" << endl;
123 break;
124 }
125
126 {
127 std::lock_guard<std::mutex> lk(mtx);
128 if (colorFrames.size() >= maxFrames)
129 colorFrames.pop_front();
130 colorFrames.push_back(f);
131 }
132 dataReady.notify_one();
133 }
134 }
135 });
136 //! [Read streams]
137
138 //! [Pair frames]
139 // Pair depth and color frames
140 while (!isFinish)
141 {
142 std::unique_lock<std::mutex> lk(mtx);
143 while (!isFinish && (depthFrames.empty() || colorFrames.empty()))
144 dataReady.wait(lk);
145
146 while (!depthFrames.empty() && !colorFrames.empty())
147 {
148 if (!lk.owns_lock())
149 lk.lock();
150
151 // Get a frame from the list
152 Frame depthFrame = depthFrames.front();
153 int64 depthT = depthFrame.timestamp;
154
155 // Get a frame from the list
156 Frame colorFrame = colorFrames.front();
157 int64 colorT = colorFrame.timestamp;
158
159 // Half of frame period is a maximum time diff between frames
160 const int64 maxTdiff = int64(1000000000 / (2 * colorStream.get(CAP_PROP_FPS)));
161 if (depthT + maxTdiff < colorT)
162 {
163 depthFrames.pop_front();
164 continue;
165 }
166 else if (colorT + maxTdiff < depthT)
167 {
168 colorFrames.pop_front();
169 continue;
170 }
171 depthFrames.pop_front();
172 colorFrames.pop_front();
173 lk.unlock();
174
175 //! [Show frames]
176 // Show depth frame
177 Mat d8, dColor;
178 depthFrame.frame.convertTo(d8, CV_8U, 255.0 / 2500);
179 applyColorMap(d8, dColor, COLORMAP_OCEAN);
180 imshow("Depth (colored)", dColor);
181
182 // Show color frame
183 imshow("Color", colorFrame.frame);
184 //! [Show frames]
185
186 // Exit on Esc key press
187 int key = waitKey(1);
188 if (key == 27) // ESC
189 {
190 isFinish = true;
191 break;
192 }
193 }
194 }
195 //! [Pair frames]
196
197 dataReady.notify_one();
198 depthReader.join();
199 colorReader.join();
200
201 return 0;
202 }
203