00001 #include <librealsense/rs.hpp>
00002 #include "example.hpp"
00003
00004 #include <cstdarg>
00005 #include <thread>
00006 #include <iostream>
00007 #include <algorithm>
00008 #include <iomanip>
00009 #include <mutex>
00010 #include "concurrency.hpp"
00011 #include <atomic>
00012 #include <map>
00013
00014
00015 #pragma comment(lib, "opengl32.lib")
00016
00017 template<typename T> inline T MIN(T x, T y) { return x < y ? x : y; }
00018 template<typename T> inline T MAX(T x, T y) { return x > y ? x : y; }
00019
00020 struct int2 { int x, y; };
00021 struct rect
00022 {
00023 int x0, y0, x1, y1;
00024 bool contains(const int2 & p) const { return x0 <= p.x && y0 <= p.y && p.x < x1 && p.y < y1; }
00025 rect shrink(int amt) const { return{ x0 + amt, y0 + amt, x1 - amt, y1 - amt }; }
00026 };
00027 struct color { float r, g, b; };
00028
00029 std::string find_and_replace(std::string source, std::string const& find, std::string const& replace)
00030 {
00031 for (std::string::size_type i = 0; (i = source.find(find, i)) != std::string::npos;)
00032 {
00033 source.replace(i, find.length(), replace);
00034 i += replace.length();
00035 }
00036 return source;
00037 }
00038
00039 struct gui
00040 {
00041 int2 cursor, clicked_offset, scroll_vec;
00042 bool click, mouse_down;
00043 int clicked_id;
00044
00045 gui() : scroll_vec({ 0, 0 }), click(), mouse_down(), clicked_id() {}
00046
00047 void label(const int2 & p, const color& c, const char * format, ...)
00048 {
00049 va_list args;
00050 va_start(args, format);
00051 char buffer[2048];
00052 vsnprintf(buffer, sizeof(buffer), format, args);
00053 va_end(args);
00054
00055 glColor3f(c.r, c.g, c.b);
00056 draw_text(p.x, p.y, buffer);
00057 }
00058
00059
00060
00061
00062 void option_label(const int2& p, const color& c, rs::device& dev, rs::option opt, double max_width, bool enabled, double* value = nullptr)
00063 {
00064 auto name = find_and_replace(rs_option_to_string((rs_option)opt), "_", " ");
00065 std::string s(name);
00066
00067 auto size = name.size();
00068 while (size > 0 && stb_easy_font_width((char*)s.c_str()) > max_width)
00069 {
00070 s = name.substr(0, size--) + "...";
00071 }
00072
00073
00074 color newC = c;
00075 #define STRING_CASE(S, C) std::string S = #S; if (s.compare(0, S.length(), S) == 0) { newC = C; s = find_and_replace(s, S + " ", ""); }
00076 color color1 = { 0.6f, 1.0f, 1.0f };
00077 color color2 = { 1.0f, 0.6f, 1.0f };
00078 color color3 = { 1.0f, 1.0f, 0.6f };
00079 color color4 = { 1.0f, 0.6f, 0.6f };
00080 color color5 = { 0.6f, 0.6f, 1.0f };
00081 color color6 = { 0.6f, 1.0f, 0.6f };
00082 STRING_CASE(ZR300, color1)
00083 STRING_CASE(F200, color2)
00084 STRING_CASE(SR300, color3)
00085 STRING_CASE(R200, color4)
00086 STRING_CASE(FISHEYE, color5)
00087 STRING_CASE(COLOR, color6)
00088 if (!enabled) newC = { 0.5f, 0.5f, 0.5f };
00089
00090 auto w = stb_easy_font_width((char*)s.c_str());
00091 label(p, newC, s.c_str());
00092
00093 if (value)
00094 {
00095 std::stringstream sstream;
00096 sstream << ": " << *value;
00097 int2 newP{ p.x + w, p.y };
00098 label(newP, c, sstream.str().c_str());
00099 }
00100
00101 rect bbox{ p.x - 15, p.y - 10, p.x + w + 10, p.y + 5 };
00102 if (bbox.contains(cursor))
00103 {
00104 std::string hint = dev.get_option_description(opt);
00105 auto hint_w = stb_easy_font_width((char*)hint.c_str());
00106 fill_rect({ cursor.x - hint_w - 7, cursor.y + 5, cursor.x + 7, cursor.y - 17 }, { 1.0f, 1.0f, 1.0f });
00107 fill_rect({ cursor.x - hint_w - 6, cursor.y + 4, cursor.x + 6, cursor.y - 16 }, { 0.0f, 0.0f, 0.0f });
00108 label({ cursor.x - hint_w, cursor.y - 2 }, { 1.f, 1.f, 1.f }, hint.c_str());
00109 }
00110 }
00111
00112 void fill_rect(const rect & r, const color & c)
00113 {
00114 glBegin(GL_QUADS);
00115 glColor3f(c.r, c.g, c.b);
00116 glVertex2i(r.x0, r.y0);
00117 glVertex2i(r.x0, r.y1);
00118 glVertex2i(r.x1, r.y1);
00119 glVertex2i(r.x1, r.y0);
00120 glEnd();
00121 }
00122
00123 void outline_rect(const rect & r, const color & c)
00124 {
00125 glPushAttrib(GL_ENABLE_BIT);
00126
00127 glLineStipple(1, 0xAAAA);
00128 glEnable(GL_LINE_STIPPLE);
00129
00130 glBegin(GL_LINE_STRIP);
00131 glColor3f(c.r, c.g, c.b);
00132 glVertex2i(r.x0, r.y0);
00133 glVertex2i(r.x0, r.y1);
00134 glVertex2i(r.x1, r.y1);
00135 glVertex2i(r.x1, r.y0);
00136 glVertex2i(r.x0, r.y0);
00137 glEnd();
00138
00139 glPopAttrib();
00140 }
00141
00142 bool button(const rect & r, const std::string & label)
00143 {
00144 fill_rect(r, { 1, 1, 1 });
00145 fill_rect(r.shrink(2), r.contains(cursor) ? (mouse_down ? color{ 0.3f, 0.3f, 0.3f } : color{ 0.4f, 0.4f, 0.4f }) : color{ 0.5f, 0.5f, 0.5f });
00146 glColor3f(1, 1, 1);
00147 draw_text(r.x0 + 4, r.y1 - 8, label.c_str());
00148 return click && r.contains(cursor);
00149 }
00150
00151 bool checkbox(const rect & r, bool & value)
00152 {
00153 bool changed = false;
00154 if (click && r.contains(cursor))
00155 {
00156 value = !value;
00157 changed = true;
00158 }
00159 fill_rect(r, { 1, 1, 1 });
00160 fill_rect(r.shrink(1), { 0.5, 0.5, 0.5 });
00161 if (value) fill_rect(r.shrink(3), { 1, 1, 1 });
00162 return changed;
00163 }
00164
00165 bool slider(int id, const rect & r, double min, double max, double step, double & value, bool disable_dragger = false)
00166 {
00167 bool changed = false;
00168 const int w = r.x1 - r.x0, h = r.y1 - r.y0;
00169 double p = (w - h) * (value - min) / (max - min);
00170 if (mouse_down && clicked_id == id)
00171 {
00172 p = std::max(0.0, std::min<double>(cursor.x - clicked_offset.x - r.x0, w - h));
00173 double new_value = min + p * (max - min) / (w - h);
00174 if (step) new_value = std::round((new_value - min) / step) * step + min;
00175 changed = new_value != value;
00176 value = new_value;
00177 p = (w - h) * (value - min) / (max - min);
00178 }
00179 const rect dragger = { int(r.x0 + p), int(r.y0), int(r.x0 + p + h), int(r.y1) };
00180 if (click && dragger.contains(cursor) && !disable_dragger)
00181 {
00182 clicked_offset = { cursor.x - dragger.x0, cursor.y - dragger.y0 };
00183 clicked_id = id;
00184 }
00185 fill_rect(r, { 0.5, 0.5, 0.5 });
00186
00187 if (!disable_dragger)
00188 fill_rect(dragger, { 1, 1, 1 });
00189
00190 return changed;
00191 }
00192
00193 void indicator(const rect & r, double min, double max, double value)
00194 {
00195 value = MAX(min, MIN(max, value));
00196 const int w = r.x1 - r.x0, h = r.y0 - r.y1;
00197 double p = (w)* (value - min) / (max - min);
00198 int Xdelta = 1;
00199 int Ydelta = -1;
00200 if (value == max)
00201 {
00202 Xdelta = 0;
00203 Ydelta = -2;
00204 }
00205 else if (value == min)
00206 {
00207 Xdelta = 2;
00208 Ydelta = 0;
00209 }
00210
00211 const rect dragger = { int(r.x0 + p + Xdelta), int(r.y0), int(r.x0 + p + Ydelta), int(r.y1) };
00212 fill_rect(r, { 0.5, 0.5, 0.5 });
00213 fill_rect(dragger, { 1, 1, 1 });
00214 glColor3f(1, 1, 1);
00215
00216 std::ostringstream oss;
00217 oss << std::setprecision(2) << std::fixed << min;
00218 auto delta = (min < 0) ? 40 : 22;
00219 draw_text(r.x0 - delta, r.y1 + abs(h / 2) + 3, oss.str().c_str());
00220
00221 oss.str("");
00222 oss << std::setprecision(2) << std::fixed << max;
00223 draw_text(r.x1 + 6, r.y1 + abs(h / 2) + 3, oss.str().c_str());
00224
00225 oss.str("");
00226 oss << std::setprecision(2) << std::fixed << value;
00227 draw_text(dragger.x0 - 1, dragger.y0 + 12, oss.str().c_str());
00228 }
00229
00230 void vscroll(const rect & r, int client_height, int & offset)
00231 {
00232 if (r.contains(cursor)) offset -= scroll_vec.y * 20;
00233 offset = std::min(offset, client_height - (r.y1 - r.y0));
00234 offset = std::max(offset, 0);
00235 if (client_height <= r.y1 - r.y0) return;
00236 auto bar = r; bar.x0 = bar.x1 - 10;
00237 auto dragger = bar;
00238 dragger.y0 = bar.y0 + offset * (r.y1 - r.y0) / client_height;
00239 dragger.y1 = bar.y0 + (offset + r.y1 - r.y0) * (r.y1 - r.y0) / client_height;
00240 fill_rect(bar, { 0.5, 0.5, 0.5 });
00241 fill_rect(dragger, { 1, 1, 1 });
00242 }
00243 };
00244
00245
00246 std::mutex mm_mutex;
00247 rs::motion_data m_gyro_data;
00248 rs::motion_data m_acc_data;
00249
00250 void on_motion_event(rs::motion_data entry)
00251 {
00252 std::lock_guard<std::mutex> lock(mm_mutex);
00253 if (entry.timestamp_data.source_id == RS_EVENT_IMU_ACCEL)
00254 m_acc_data = entry;
00255 if (entry.timestamp_data.source_id == RS_EVENT_IMU_GYRO)
00256 m_gyro_data = entry;
00257 }
00258
00259 void on_timestamp_event(rs::timestamp_data entry)
00260 {
00261 }
00262
00263 struct user_data
00264 {
00265 GLFWwindow* curr_window = nullptr;
00266 gui* g = nullptr;
00267 };
00268
00269 void show_message(GLFWwindow* curr_window, const std::string& title, const std::string& message)
00270 {
00271 glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);
00272 glfwWindowHint(GLFW_FLOATING, GL_TRUE);
00273
00274 int xpos, ypos;
00275 glfwGetWindowPos(curr_window, &xpos, &ypos);
00276
00277 int width, height;
00278 glfwGetWindowSize(curr_window, &width, &height);
00279
00280 int close_win_width = 550;
00281 int close_win_height = 150;
00282 auto closeWin = glfwCreateWindow(close_win_width, close_win_height, title.c_str(), nullptr, nullptr);
00283 glfwMakeContextCurrent(closeWin);
00284 glfwSetWindowPos(closeWin, xpos + width / 2 - close_win_width / 2, ypos + height / 2 - close_win_height / 2);
00285
00286 gui g;
00287
00288 user_data data;
00289 data.curr_window = curr_window;
00290 data.g = &g;
00291
00292 glfwSetWindowUserPointer(closeWin, &data);
00293 glfwSetWindowCloseCallback(closeWin, [](GLFWwindow* w) {
00294 glfwDestroyWindow(w);
00295 });
00296
00297 glfwSetCursorPosCallback(closeWin, [](GLFWwindow * w, double cx, double cy) { reinterpret_cast<user_data *>(glfwGetWindowUserPointer(w))->g->cursor = { (int)cx, (int)cy }; });
00298 glfwSetScrollCallback(closeWin, [](GLFWwindow * w, double x, double y) { reinterpret_cast<user_data *>(glfwGetWindowUserPointer(w))->g->scroll_vec = { (int)x, (int)y }; });
00299
00300 glfwSetMouseButtonCallback(closeWin, [](GLFWwindow * w, int button, int action, int mods)
00301 {
00302 auto data = reinterpret_cast<user_data *>(glfwGetWindowUserPointer(w));
00303 if (action == GLFW_PRESS)
00304 {
00305 data->g->clicked_id = 0;
00306 data->g->click = true;
00307 }
00308 data->g->mouse_down = action != GLFW_RELEASE;
00309 });
00310
00311
00312 while (!glfwWindowShouldClose(closeWin))
00313 {
00314 glfwPollEvents();
00315 int w, h;
00316 glfwGetFramebufferSize(closeWin, &w, &h);
00317 glViewport(0, 0, w, h);
00318 glClear(GL_COLOR_BUFFER_BIT);
00319
00320 glfwGetWindowSize(closeWin, &w, &h);
00321 glLoadIdentity();
00322 glOrtho(0, w, h, 0, -1, +1);
00323
00324
00325 size_t line_lenght = 80;
00326 int2 p;
00327 p.x = 20;
00328 p.y = 30;
00329 if (message.size() < line_lenght)
00330 {
00331 g.label(p, { 1, 1, 1 }, message.c_str());
00332 }
00333 else
00334 {
00335 std::vector<std::string> str_vec;
00336 std::string temp_message = message;
00337 temp_message = find_and_replace(temp_message, "\n", " ");
00338 size_t index = 0;
00339 size_t string_size = temp_message.size();
00340 while (index < string_size)
00341 {
00342 if (index + line_lenght >= string_size)
00343 {
00344 str_vec.push_back(temp_message.substr(index));
00345 break;
00346 }
00347
00348 auto curr_index = index + temp_message.substr(index, line_lenght).find_last_of(' ');
00349
00350 str_vec.push_back(temp_message.substr(index, curr_index - index));
00351 index = curr_index;
00352 }
00353
00354 for (auto& elem : str_vec)
00355 {
00356 g.label(p, { 1, 1, 1 }, elem.c_str());
00357 p.y += 15;
00358 }
00359
00360 if (p.y > 100)
00361 glfwSetWindowSize(closeWin, close_win_width, p.y + 50);
00362 }
00363
00364 if (g.button({ w / 2 - 40, h - 40, w / 2 + 40, h - 10 }, " OK"))
00365 {
00366 glfwDestroyWindow(closeWin);
00367 }
00368
00369 glfwSwapBuffers(closeWin);
00370 g.click = false;
00371 if (!g.mouse_down) g.clicked_id = 0;
00372 }
00373 glfwMakeContextCurrent(curr_window);
00374 }
00375
00376 struct option { rs::option opt; double min, max, step, value, def; bool supports; };
00377
00378 static std::map<rs::option, std::vector<rs::option>> options_dependencies =
00379 {
00380 { rs::option::color_exposure, { rs::option::color_enable_auto_exposure } },
00381 { rs::option::color_white_balance, { rs::option::color_enable_auto_white_balance } },
00382 { rs::option::r200_lr_gain, { rs::option::r200_lr_auto_exposure_enabled } },
00383 { rs::option::r200_lr_exposure, { rs::option::r200_lr_auto_exposure_enabled } },
00384 { rs::option::r200_lr_auto_exposure_enabled, { rs::option::r200_auto_exposure_mean_intensity_set_point,
00385 rs::option::r200_auto_exposure_bright_ratio_set_point,
00386 rs::option::r200_auto_exposure_kp_dark_threshold,
00387 rs::option::r200_auto_exposure_kp_gain,
00388 rs::option::r200_auto_exposure_kp_exposure,
00389 rs::option::r200_auto_exposure_bottom_edge,
00390 rs::option::r200_auto_exposure_top_edge,
00391 rs::option::r200_auto_exposure_left_edge,
00392 rs::option::r200_auto_exposure_right_edge,
00393 } },
00394 { rs::option::r200_auto_exposure_bottom_edge, { rs::option::r200_auto_exposure_top_edge } },
00395 { rs::option::r200_auto_exposure_top_edge, { rs::option::r200_auto_exposure_bottom_edge } },
00396 { rs::option::r200_auto_exposure_left_edge, { rs::option::r200_auto_exposure_right_edge } },
00397 { rs::option::r200_auto_exposure_right_edge, { rs::option::r200_auto_exposure_left_edge } },
00398 };
00399
00400 void update_related_options(rs::device& dev, rs::option opt, std::vector<option>& options)
00401 {
00402 auto it = options_dependencies.find(opt);
00403 if (it != options_dependencies.end())
00404 {
00405 for (auto& related : it->second)
00406 {
00407 auto opt_it = std::find_if(options.begin(), options.end(), [related](option o) { return related == o.opt; });
00408 if (opt_it != options.end())
00409 {
00410 try
00411 {
00412 if (!dev.supports_option(opt_it->opt)) continue;
00413 dev.get_option_range(opt_it->opt, opt_it->min, opt_it->max, opt_it->step, opt_it->def);
00414 opt_it->value = dev.get_option(opt_it->opt);
00415 }
00416 catch (...) {}
00417 }
00418 }
00419 }
00420 }
00421
00422 bool is_any_stream_enable(rs::device* dev)
00423 {
00424 bool sts = false;
00425 for (auto i = 0; i < 5; i++)
00426 {
00427 if (dev->is_stream_enabled((rs::stream)i))
00428 {
00429 sts = true;
00430 break;
00431 }
00432 }
00433
00434 return sts;
00435 }
00436
00437 bool motion_tracking_enable = true;
00438 void enable_stream(rs::device * dev, int stream, rs::format format, int w, int h, int fps, bool enable, std::stringstream& stream_name)
00439 {
00440 stream_name.str("");
00441 if (stream == RS_CAPABILITIES_MOTION_EVENTS)
00442 {
00443 stream_name << "MOTION EVENTS";
00444
00445 if (dev->is_motion_tracking_active())
00446 return;
00447
00448 if (enable)
00449 {
00450 dev->enable_motion_tracking(on_motion_event, on_timestamp_event);
00451 motion_tracking_enable = true;
00452 }
00453 else
00454 {
00455 dev->disable_motion_tracking();
00456 motion_tracking_enable = false;
00457 }
00458 }
00459 else
00460 {
00461 stream_name << rs_stream_to_string((rs_stream)stream);
00462 if (enable)
00463 {
00464 if (!dev->is_stream_enabled((rs::stream)stream))
00465 dev->enable_stream((rs::stream)stream, w, h, format, fps);
00466 }
00467 else
00468 {
00469 if (dev->is_stream_enabled((rs::stream)stream))
00470 dev->disable_stream((rs::stream)stream);
00471 }
00472 }
00473 }
00474
00475 void update_mm_data(texture_buffer* buffers, int w, int h, gui& g)
00476 {
00477 int x = w / 3 + 10;
00478 int y = 2 * h / 3 + 5;
00479 auto rect_y0_pos = y + 36;
00480 auto rect_y1_pos = y + 28;
00481 auto indicator_width = 42;
00482
00483 buffers[5].print(x, rect_y0_pos - 10, "Gyro X: ");
00484 g.indicator({ x + 100, rect_y0_pos, x + 300, rect_y1_pos }, -10, 10, m_gyro_data.axes[0]);
00485 rect_y0_pos += indicator_width;
00486 rect_y1_pos += indicator_width;
00487
00488 buffers[5].print(x, rect_y0_pos - 10, "Gyro Y: ");
00489 g.indicator({ x + 100, rect_y0_pos, x + 300, rect_y1_pos }, -10, 10, m_gyro_data.axes[1]);
00490 rect_y0_pos += indicator_width;
00491 rect_y1_pos += indicator_width;
00492
00493 buffers[5].print(x, rect_y0_pos - 10, "Gyro Z: ");
00494 g.indicator({ x + 100, rect_y0_pos, x + 300, rect_y1_pos }, -10, 10, m_gyro_data.axes[2]);
00495 rect_y0_pos += indicator_width;
00496 rect_y1_pos += indicator_width;
00497
00498 buffers[5].print(x, rect_y0_pos - 10, "Acc X: ");
00499 g.indicator({ x + 100, rect_y0_pos, x + 300, rect_y1_pos }, -10, 10, m_acc_data.axes[0]);
00500 rect_y0_pos += indicator_width;
00501 rect_y1_pos += indicator_width;
00502
00503 buffers[5].print(x, rect_y0_pos - 10, "Acc Y: ");
00504 g.indicator({ x + 100, rect_y0_pos, x + 300, rect_y1_pos }, -10, 10, m_acc_data.axes[1]);
00505 rect_y0_pos += indicator_width;
00506 rect_y1_pos += indicator_width;
00507
00508 buffers[5].print(x, rect_y0_pos - 10, "Acc Z: ");
00509 g.indicator({ x + 100, rect_y0_pos, x + 300, rect_y1_pos }, -10, 10, m_acc_data.axes[2]);
00510 }
00511
00512 double find_option_value(const std::vector<option>& options, rs::option opt)
00513 {
00514 auto it = find_if(options.begin(), options.end(), [opt](option o) { return o.opt == opt; });
00515 if (it == options.end()) return 0.0;
00516 return it->value;
00517 }
00518
00519 void draw_autoexposure_roi_boundary(rs::stream s, const std::vector<option>& options, rs::device* dev, gui& g, int x, int y, double w, double h)
00520 {
00521 if ((s == rs::stream::depth || s == rs::stream::infrared) &&
00522 find_option_value(options, rs::option::r200_lr_auto_exposure_enabled) > 0)
00523 {
00524 auto intrinsics = dev->get_stream_intrinsics(s);
00525 auto width = intrinsics.width;
00526 auto height = intrinsics.height;
00527
00528 auto left = find_option_value(options, rs::option::r200_auto_exposure_left_edge) / width;
00529 auto right = find_option_value(options, rs::option::r200_auto_exposure_right_edge) / width;
00530 auto top = find_option_value(options, rs::option::r200_auto_exposure_top_edge) / height;
00531 auto bottom = find_option_value(options, rs::option::r200_auto_exposure_bottom_edge) / height;
00532
00533 left = x + left * w;
00534 right = x + right * w;
00535 top = y + top * h;
00536 bottom = y + bottom * h;
00537
00538 g.outline_rect({ (int)left + 1, (int)top + 1, (int)right - 1, (int)bottom - 1 }, { 1.0f, 1.0f, 1.0f });
00539 g.label({ (int)left + 4, (int)bottom - 6 }, { 1.0f, 1.0f, 1.0f }, "AE ROI");
00540 }
00541 }
00542
00543 int main(int argc, char * argv[])
00544 {
00545 rs::context ctx;
00546 GLFWwindow* win = nullptr;
00547 const auto streams = (unsigned short)rs::stream::fisheye + 1;
00548 gui g = {};
00549 rs::device * dev = nullptr;
00550 std::atomic<bool> running(true);
00551 bool has_motion_module = false;
00552 single_consumer_queue<rs::frame> frames_queue[streams];
00553 std::vector<option> options;
00554 texture_buffer buffers[streams];
00555 struct resolution
00556 {
00557 int width;
00558 int height;
00559 rs::format format;
00560 };
00561 std::map<rs::stream, resolution> resolutions;
00562
00563 std::vector<int> req_fps = { 30, 30, 30, 30, 60, 30 };
00564 struct w_h { int width, height; };
00565 std::vector<rs::format> formats = { rs::format::z16, rs::format::rgb8, rs::format::y8, rs::format::y8, rs::format::raw8, rs::format::any };
00566 std::vector<w_h> wh = { { 0,0 }, { 640,480 }, { 0,0 }, { 0,0 }, { 640,480 }, {0,0}};
00567
00568 try {
00569 rs::log_to_console(rs::log_severity::warn);
00570
00571
00572 glfwInit();
00573 glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);
00574 win = glfwCreateWindow(1550, 960, "CPP Configuration Example", nullptr, nullptr);
00575
00576 glfwMakeContextCurrent(win);
00577
00578 glfwSetWindowUserPointer(win, &g);
00579 glfwSetCursorPosCallback(win, [](GLFWwindow * w, double cx, double cy) { reinterpret_cast<gui *>(glfwGetWindowUserPointer(w))->cursor = { (int)cx, (int)cy }; });
00580 glfwSetScrollCallback(win, [](GLFWwindow * w, double x, double y) { reinterpret_cast<gui *>(glfwGetWindowUserPointer(w))->scroll_vec = { (int)x, (int)y }; });
00581 glfwSetMouseButtonCallback(win, [](GLFWwindow * w, int button, int action, int mods)
00582 {
00583 auto g = reinterpret_cast<gui *>(glfwGetWindowUserPointer(w));
00584 if (action == GLFW_PRESS)
00585 {
00586 g->clicked_id = 0;
00587 g->click = true;
00588 }
00589 g->mouse_down = action != GLFW_RELEASE;
00590 });
00591
00592
00593 if (ctx.get_device_count() < 1) throw std::runtime_error("No device found. Is it plugged in?");
00594
00595 dev = ctx.get_device(0);
00596
00597 for (int stream = 0; stream < streams; stream++)
00598 {
00599 if (dev->supports((rs::capabilities)stream))
00600 {
00601 dev->enable_stream((rs::stream)stream, wh[stream].width, wh[stream].height, formats[stream], req_fps[stream]);
00602 resolutions[(rs::stream)stream] = { dev->get_stream_width((rs::stream)stream), dev->get_stream_height((rs::stream)stream), formats[stream] };
00603 }
00604 }
00605
00606 has_motion_module = dev->supports(rs::capabilities::fish_eye) || dev->supports(rs::capabilities::motion_events);
00607
00608 if (has_motion_module)
00609 {
00610 if (dev->supports(rs::capabilities::motion_events))
00611 {
00612 dev->enable_motion_tracking(on_motion_event, on_timestamp_event);
00613 }
00614
00615 glfwSetWindowSize(win, 1100, 960);
00616 }
00617
00618 for (int i = 0; i < RS_OPTION_COUNT; ++i)
00619 {
00620 option o = { (rs::option)i };
00621 try {
00622 o.supports = dev->supports_option(o.opt);
00623 if (o.supports)
00624 {
00625 dev->get_option_range(o.opt, o.min, o.max, o.step, o.def);
00626 o.value = dev->get_option(o.opt);
00627 }
00628 options.push_back(o);
00629 }
00630 catch (...) {}
00631 }
00632 }
00633 catch (const rs::error & e)
00634 {
00635 std::stringstream ss;
00636 ss << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
00637 std::cerr << ss.str();
00638 show_message(win, "Exception", ss.str());
00639 return 1;
00640 }
00641 catch (const std::exception & e)
00642 {
00643 std::cerr << e.what() << std::endl;
00644 show_message(win, "Exception", e.what());
00645 return 1;
00646 }
00647 catch (...)
00648 {
00649 std::cerr << "Unresolved error type during camera initialization" << std::endl;
00650 show_message(win, "Exception", "Unresolved error type during camera initialization");
00651 return 1;
00652 }
00653
00654 rs::format format[streams] = {};
00655 unsigned long long frame_number[streams] = {};
00656 double frame_timestamp[streams] = {};
00657 int fps[streams] = {};
00658 double dc_preset = 0, iv_preset = 0;
00659 int offset = 0, panel_height = 1;
00660 int gui_click_flag = 0;
00661
00662 while (true)
00663 {
00664 try
00665 {
00666 while (!glfwWindowShouldClose(win))
00667 {
00668 glfwPollEvents();
00669 rs::frame frame;
00670
00671 int w, h;
00672 glfwGetFramebufferSize(win, &w, &h);
00673 glViewport(0, 0, w, h);
00674 glClear(GL_COLOR_BUFFER_BIT);
00675
00676 glfwGetWindowSize(win, &w, &h);
00677 glLoadIdentity();
00678 glOrtho(0, w, h, 0, -1, +1);
00679
00680 g.vscroll({ w - 270, 0, w, h }, panel_height, offset);
00681 int y = 10 - offset;
00682
00683 if (dev->is_streaming() || dev->is_motion_tracking_active())
00684 {
00685 if (g.button({ w - 260, y, w - 20, y + 24 }, "Stop Capture"))
00686 {
00687 if (is_any_stream_enable(dev))
00688 {
00689 running = false;
00690 for (auto i = 0; i < 5; i++) frames_queue[i].clear();
00691 dev->stop();
00692 }
00693
00694 if (has_motion_module && motion_tracking_enable)
00695 {
00696 running = false;
00697 dev->stop(rs::source::motion_data);
00698 }
00699 }
00700 }
00701 else
00702 {
00703 if (g.button({ w - 260, y, w - 20, y + 24 }, "Start Capture"))
00704 {
00705 std::vector<rs::stream> supported_streams;
00706 for (int i = (int)rs::capabilities::depth; i <= (int)rs::capabilities::fish_eye; i++)
00707 if (dev->supports((rs::capabilities)i))
00708 supported_streams.push_back((rs::stream)i);
00709 for (auto & stream : supported_streams)
00710 {
00711 if (!dev->is_stream_enabled(stream)) continue;
00712 auto intrin = dev->get_stream_intrinsics(stream);
00713 std::cout << "Capturing " << stream << " at " << intrin.width << " x " << intrin.height;
00714 std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov() << ", distortion = " << intrin.model() << std::endl;
00715 }
00716
00717 if (has_motion_module && motion_tracking_enable)
00718 {
00719 running = true;
00720 dev->start(rs::source::motion_data);
00721 }
00722
00723 if (is_any_stream_enable(dev))
00724 {
00725 running = true;
00726 dev->start();
00727 }
00728 }
00729 }
00730 y += 34;
00731 if (!dev->is_streaming())
00732 {
00733 for (int i = 0; i <= RS_CAPABILITIES_MOTION_EVENTS; ++i)
00734 {
00735 auto s = (rs::stream)i;
00736 auto cap = (rs::capabilities)i;
00737 std::stringstream stream_name;
00738
00739 if (dev->supports(cap))
00740 {
00741 static bool is_callback_set = false;
00742 bool enable;
00743 if (i == RS_CAPABILITIES_MOTION_EVENTS)
00744 enable = motion_tracking_enable;
00745 else
00746 enable = dev->is_stream_enabled(s);
00747
00748 enable_stream(dev, i, formats[i], wh[i].width, wh[i].height, req_fps[i], enable, stream_name);
00749
00750 if (!is_callback_set || g.checkbox({ w - 260, y, w - 240, y + 20 }, enable))
00751 {
00752 enable_stream(dev, i, formats[i], wh[i].width, wh[i].height, req_fps[i], enable, stream_name);
00753
00754 if (enable)
00755 {
00756 static const auto max_queue_size = 2;
00757 for (auto i = 0; i < 5; i++)
00758 {
00759 dev->set_frame_callback((rs::stream)i, [dev, &buffers, &running, &frames_queue, &resolutions, i](rs::frame frame)
00760 {
00761 if (running && frames_queue[i].size() < max_queue_size)
00762 {
00763 frames_queue[i].enqueue(std::move(frame));
00764 }
00765 });
00766 }
00767 }
00768 is_callback_set = true;
00769 }
00770 g.label({ w - 234, y + 13 }, { 1, 1, 1 }, "Enable %s", stream_name.str().c_str());
00771 y += 30;
00772 }
00773 }
00774 }
00775
00776 if (dev->is_streaming() || dev->is_motion_tracking_active())
00777 {
00778 auto new_w = w + (has_motion_module ? 150 : -280);
00779
00780 int scale_factor = (has_motion_module ? 3 : 2);
00781 int fWidth = new_w / scale_factor;
00782 int fHeight = h / scale_factor;
00783
00784 static struct position { int rx, ry, rw, rh; } pos_vec[5];
00785 pos_vec[0] = position{ fWidth, 0, fWidth, fHeight };
00786 pos_vec[1] = position{ 0, 0, fWidth, fHeight };
00787 pos_vec[2] = position{ 0, fHeight, fWidth, fHeight };
00788 pos_vec[3] = position{ fWidth, fHeight, fWidth, fHeight };
00789 pos_vec[4] = position{ 0, 2 * fHeight, fWidth, fHeight };
00790 position center_position = position{ 0, 0, fWidth * 2, fHeight * 2 };
00791 position prev_pos;
00792 bool g_clicked = g.click;
00793 static int frame_clicked[5] = {};
00794
00795 for (auto i = 0; i < 5; i++)
00796 {
00797 if (!dev->is_stream_enabled((rs::stream)i))
00798 continue;
00799
00800 if (frames_queue[i].try_dequeue(&frame))
00801 {
00802 buffers[i].upload(frame);
00803 format[i] = frame.get_format();
00804 frame_number[i] = frame.get_frame_number();
00805 frame_timestamp[i] = frame.get_timestamp();
00806 fps[i] = frame.get_framerate();
00807 }
00808
00809 if (g_clicked && gui_click_flag &&
00810 g.cursor.x >= center_position.rx && g.cursor.x <= (center_position.rw + center_position.rx) &&
00811 g.cursor.y >= center_position.ry && g.cursor.y <= (center_position.rh + center_position.ry))
00812 {
00813 pos_vec[i] = prev_pos;
00814 gui_click_flag = !gui_click_flag;
00815 for (int j = 0; j < 5; ++j)
00816 frame_clicked[j] = false;
00817
00818 g_clicked = false;
00819 }
00820 else if (g_clicked && !gui_click_flag &&
00821 g.cursor.x >= pos_vec[i].rx && g.cursor.x <= (pos_vec[i].rw + pos_vec[i].rx) &&
00822 g.cursor.y >= pos_vec[i].ry && g.cursor.y <= (pos_vec[i].rh + pos_vec[i].ry))
00823 {
00824 gui_click_flag = !gui_click_flag;
00825 frame_clicked[i] = gui_click_flag;
00826 g_clicked = false;
00827 }
00828
00829 if (frame_clicked[i])
00830 {
00831 prev_pos = pos_vec[i];
00832 pos_vec[i] = center_position;
00833 buffers[i].show((rs::stream)i, format[i], fps[i], frame_number[i], frame_timestamp[i], pos_vec[i].rx, pos_vec[i].ry, pos_vec[i].rw, pos_vec[i].rh, resolutions[(rs::stream)i].width, resolutions[(rs::stream)i].height);
00834 }
00835 else if (!gui_click_flag)
00836 buffers[i].show((rs::stream)i, format[i], fps[i], frame_number[i], frame_timestamp[i], pos_vec[i].rx, pos_vec[i].ry, pos_vec[i].rw, pos_vec[i].rh, resolutions[(rs::stream)i].width, resolutions[(rs::stream)i].height);
00837
00838 if (frame_clicked[i])
00839 draw_autoexposure_roi_boundary((rs::stream)i, options, dev, g, center_position.rx, center_position.ry, center_position.rw, center_position.rh);
00840 else if (!gui_click_flag)
00841 draw_autoexposure_roi_boundary((rs::stream)i, options, dev, g, pos_vec[i].rx, pos_vec[i].ry, fWidth, fHeight);
00842 }
00843
00844 if (has_motion_module && motion_tracking_enable)
00845 {
00846 std::lock_guard<std::mutex> lock(mm_mutex);
00847 update_mm_data(buffers, new_w, h, g);
00848 }
00849 }
00850
00851 for (auto & o : options)
00852 {
00853 if (!dev->supports_option(o.opt))
00854 {
00855 o.supports = false;
00856 continue;
00857 }
00858 if (!o.supports)
00859 {
00860 try {
00861 dev->get_option_range(o.opt, o.min, o.max, o.step, o.def);
00862 o.value = dev->get_option(o.opt);
00863 }
00864 catch (...) {}
00865 o.supports = true;
00866 }
00867
00868 auto is_checkbox = (o.min == 0) && (o.max == 1) && (o.step == 1);
00869 auto is_checked = o.value > 0;
00870
00871 if (is_checkbox ?
00872 g.checkbox({ w - 260, y + 10, w - 240, y + 30 }, is_checked) :
00873 g.slider((int)o.opt + 1, { w - 260, y + 16, w - 20, y + 36 }, o.min, o.max, o.step, o.value))
00874 {
00875 if (is_checkbox) dev->set_option(o.opt, is_checked ? 1 : 0);
00876 else dev->set_option(o.opt, o.value);
00877
00878 update_related_options(*dev, o.opt, options);
00879
00880 o.value = dev->get_option(o.opt);
00881 }
00882
00883 if (is_checkbox) g.option_label({ w - 230, y + 24 }, { 1, 1, 1 }, *dev, o.opt, 210, true);
00884 else g.option_label({ w - 260, y + 12 }, { 1, 1, 1 }, *dev, o.opt, 240, true, &o.value);
00885
00886 y += 38;
00887 }
00888
00889 g.label({ w - 260, y + 12 }, { 1, 1, 1 }, "Depth control parameters preset: %g", dc_preset);
00890 if (g.slider(100, { w - 260, y + 16, w - 20, y + 36 }, 0, 5, 1, dc_preset)) rs_apply_depth_control_preset((rs_device *)dev, static_cast<int>(dc_preset));
00891 y += 38;
00892 g.label({ w - 260, y + 12 }, { 1, 1, 1 }, "IVCAM options preset: %g", iv_preset);
00893 if (g.slider(101, { w - 260, y + 16, w - 20, y + 36 }, 0, 10, 1, iv_preset)) rs_apply_ivcam_preset((rs_device *)dev, static_cast<rs_ivcam_preset>((int)iv_preset));
00894 y += 38;
00895
00896 panel_height = y + 10 + offset;
00897
00898 glfwSwapBuffers(win);
00899 g.scroll_vec = { 0, 0 };
00900 g.click = false;
00901 if (!g.mouse_down) g.clicked_id = 0;
00902 }
00903
00904 running = false;
00905
00906 for (auto i = 0; i < streams; i++) frames_queue[i].clear();
00907
00908 if (dev->is_streaming())
00909 dev->stop();
00910
00911 for (auto i = 0; i < streams; i++)
00912 {
00913 if (dev->is_stream_enabled((rs::stream)i))
00914 dev->disable_stream((rs::stream)i);
00915 }
00916
00917 glfwTerminate();
00918 return 0;
00919 }
00920 catch (const rs::error & e)
00921 {
00922 std::stringstream ss;
00923 ss << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
00924 std::cerr << ss.str();
00925 show_message(win, "Exception", ss.str());
00926 }
00927 catch (const std::exception & e)
00928 {
00929 std::cerr << e.what() << std::endl;
00930 show_message(win, "Exception", e.what());
00931 }
00932 }
00933 }