cpp-config-ui.cpp
Go to the documentation of this file.
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     // extended label method for option lines
00060     // the purpose is to provide a little more visual context to the various options,
00061     // and make config-ui interface more human friendly
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), "_", " "); // replacing _ with ' ' to reduce visual clutter
00065         std::string s(name);
00066 
00067         auto size = name.size(); // align the string to max. allowed width
00068         while (size > 0 && stb_easy_font_width((char*)s.c_str()) > max_width)
00069         {
00070             s = name.substr(0, size--) + "...";
00071         }
00072 
00073         // remove option prefixes converting them to visual hints through color:
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         // if value is required, append it at the end of the string
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;       // Use camera-supported native streams
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         //rs::log_to_file(rs::log_severity::debug, "librealsense.log");
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 }


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Tue Jun 25 2019 19:54:38