forked from IntelRealSense/librealsense
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrs-multicam.cpp
206 lines (183 loc) · 6.54 KB
/
rs-multicam.cpp
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
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include "example.hpp" // Include short list of convenience functions for rendering
#include <string>
#include <map>
#include <algorithm>
#include <mutex> // std::mutex, std::lock_guard
#include <cmath> // std::ceil
const std::string no_camera_message = "No camera connected, please connect 1 or more";
const std::string platform_camera_name = "Platform Camera";
class device_container
{
// Helper struct per pipeline
struct view_port
{
std::map<int, rs2::frame> frames_per_stream;
rs2::colorizer colorize_frame;
texture tex;
rs2::pipeline pipe;
rs2::pipeline_profile profile;
};
public:
void enable_device(rs2::device dev)
{
std::string serial_number(dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
std::lock_guard<std::mutex> lock(_mutex);
if (_devices.find(serial_number) != _devices.end())
{
return; //already in
}
// Ignoring platform cameras (webcams, etc..)
if (platform_camera_name == dev.get_info(RS2_CAMERA_INFO_NAME))
{
return;
}
// Create a pipeline from the given device
rs2::pipeline p;
rs2::config c;
c.enable_device(serial_number);
// Start the pipeline with the configuration
rs2::pipeline_profile profile = p.start(c);
// Hold it internally
_devices.emplace(serial_number, view_port{ {},{},{}, p, profile });
}
void remove_devices(const rs2::event_information& info)
{
std::lock_guard<std::mutex> lock(_mutex);
// Go over the list of devices and check if it was disconnected
auto itr = _devices.begin();
while(itr != _devices.end())
{
if (info.was_removed(itr->second.profile.get_device()))
{
itr = _devices.erase(itr);
}
else
{
++itr;
}
}
}
size_t device_count()
{
std::lock_guard<std::mutex> lock(_mutex);
return _devices.size();
}
int stream_count()
{
std::lock_guard<std::mutex> lock(_mutex);
int count = 0;
for (auto&& sn_to_dev : _devices)
{
for (auto&& stream : sn_to_dev.second.frames_per_stream)
{
if (stream.second)
{
count++;
}
}
}
return count;
}
void poll_frames()
{
std::lock_guard<std::mutex> lock(_mutex);
// Go over all device
for (auto&& view : _devices)
{
// Ask each pipeline if there are new frames available
rs2::frameset frameset;
if (view.second.pipe.poll_for_frames(&frameset))
{
for (int i = 0; i < frameset.size(); i++)
{
rs2::frame new_frame = frameset[i];
int stream_id = new_frame.get_profile().unique_id();
view.second.frames_per_stream[stream_id] = view.second.colorize_frame(new_frame); //update view port with the new stream
}
}
}
}
void render_textures(int cols, int rows, float view_width, float view_height)
{
std::lock_guard<std::mutex> lock(_mutex);
int stream_no = 0;
for (auto&& view : _devices)
{
// For each device get its frames
for (auto&& id_to_frame : view.second.frames_per_stream)
{
// If the frame is available
if (id_to_frame.second)
{
view.second.tex.upload(id_to_frame.second);
}
rect frame_location{ view_width * (stream_no % cols), view_height * (stream_no / cols), view_width, view_height };
if (rs2::video_frame vid_frame = id_to_frame.second.as<rs2::video_frame>())
{
rect adjuested = frame_location.adjust_ratio({ static_cast<float>(vid_frame.get_width())
, static_cast<float>(vid_frame.get_height()) });
view.second.tex.show(adjuested);
stream_no++;
}
}
}
}
private:
std::mutex _mutex;
std::map<std::string, view_port> _devices;
};
int main(int argc, char * argv[]) try
{
// Create a simple OpenGL window for rendering:
window app(1280, 960, "CPP Multi-Camera Example");
device_container connected_devices;
rs2::context ctx; // Create librealsense context for managing devices
// Register callback for tracking which devices are currently connected
ctx.set_devices_changed_callback([&](rs2::event_information& info)
{
connected_devices.remove_devices(info);
for (auto&& dev : info.get_new_devices())
{
connected_devices.enable_device(dev);
}
});
// Initial population of the device list
for (auto&& dev : ctx.query_devices()) // Query the list of connected RealSense devices
{
connected_devices.enable_device(dev);
}
while (app) // Application still alive?
{
connected_devices.poll_frames();
auto total_number_of_streams = connected_devices.stream_count();
if (total_number_of_streams == 0)
{
draw_text(int(std::max(0.f, (app.width() / 2) - no_camera_message.length() * 3)),
int(app.height() / 2), no_camera_message.c_str());
continue;
}
if (connected_devices.device_count() == 1)
{
draw_text(0, 10, "Please connect another camera");
}
int cols = int(std::ceil(std::sqrt(total_number_of_streams)));
int rows = int(std::ceil(total_number_of_streams / static_cast<float>(cols)));
float view_width = (app.width() / cols);
float view_height = (app.height() / rows);
connected_devices.render_textures(cols, rows, view_width, view_height);
}
return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
return EXIT_FAILURE;
}
catch (const std::exception & e)
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}