cpp-stride.cpp
Go to the documentation of this file.
00001 // License: Apache 2.0. See LICENSE file in root directory.
00002 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
00003 
00005 // librealsense Multi-threading Demo 4 - low latency with callbacks and zero copy   //
00007 
00008 #include <librealsense/rs.hpp>
00009 #include <cstdio>
00010 #include <atomic>
00011 #include <map>
00012 #include <GLFW/glfw3.h>
00013 
00014 #include "concurrency.hpp"
00015 #include "example.hpp"
00016 #include <iostream>
00017 
00018 int main() try
00019 {
00020     rs::context ctx;
00021     printf("There are %d connected RealSense devices.\n", ctx.get_device_count());
00022     if (ctx.get_device_count() == 0) return EXIT_FAILURE;
00023 
00024     rs::device * dev = ctx.get_device(0);
00025     printf("\nUsing device 0, an %s\n", dev->get_name());
00026     printf("    Serial number: %s\n", dev->get_serial());
00027     printf("    Firmware version: %s\n", dev->get_firmware_version());
00028 
00029     const auto streams = 4;
00030     std::vector<uint16_t> supported_streams = { (uint16_t)rs::stream::depth, (uint16_t)rs::stream::color, (uint16_t)rs::stream::infrared};
00031     const size_t max_queue_size = 1; // To minimize latency prefer frame drops
00032     single_consumer_queue<rs::frame> frames_queue[streams];
00033     texture_buffer buffers[streams];
00034     std::atomic<bool> running(true);
00035 
00036     struct resolution
00037     {
00038         int width;
00039         int height;
00040         rs::format format;
00041     };
00042     std::map<rs::stream, resolution> resolutions;
00043 
00044     if(dev->supports(rs::capabilities::infrared2))
00045             supported_streams.push_back((uint16_t)rs::stream::infrared2);
00046 
00047     for (auto i : supported_streams)
00048     {
00049         dev->set_frame_callback((rs::stream)i, [dev, &running, &frames_queue, &resolutions, i, max_queue_size](rs::frame frame)
00050         {
00051             if (running && frames_queue[i].size() <= max_queue_size) frames_queue[i].enqueue(std::move(frame));
00052         });
00053     }
00054 
00055     dev->enable_stream(rs::stream::depth, 0, 0, rs::format::z16, 60, rs::output_buffer_format::native);
00056     dev->enable_stream(rs::stream::color, 640, 480, rs::format::rgb8, 60, rs::output_buffer_format::native);
00057     dev->enable_stream(rs::stream::infrared, 0, 0, rs::format::y8, 60, rs::output_buffer_format::native);
00058     if (dev->supports(rs::capabilities::infrared2))
00059         dev->enable_stream(rs::stream::infrared2, 0, 0, rs::format::y8, 60, rs::output_buffer_format::native);
00060 
00061     resolutions[rs::stream::depth] = { dev->get_stream_width(rs::stream::depth), dev->get_stream_height(rs::stream::depth), rs::format::z16 };
00062     resolutions[rs::stream::color] = { dev->get_stream_width(rs::stream::color), dev->get_stream_height(rs::stream::color), rs::format::rgb8 };
00063     resolutions[rs::stream::infrared] = { dev->get_stream_width(rs::stream::infrared), dev->get_stream_height(rs::stream::infrared), rs::format::y8 };
00064     if(dev->supports(rs::capabilities::infrared2))
00065         resolutions[rs::stream::infrared2] = { dev->get_stream_width(rs::stream::infrared2), dev->get_stream_height(rs::stream::infrared2), rs::format::y8 };
00066 
00067     glfwInit();
00068 
00069     auto max_aspect_ratio = 0.0f;
00070     for (auto i : supported_streams)
00071     {
00072         auto aspect_ratio = static_cast<float>(resolutions[static_cast<rs::stream>(i)].height) / static_cast<float>(resolutions[static_cast<rs::stream>(i)].width);
00073         if (max_aspect_ratio < aspect_ratio)
00074             max_aspect_ratio = aspect_ratio;
00075     };
00076 
00077     auto win = glfwCreateWindow(1100, int(1100 * max_aspect_ratio), "librealsense - stride", nullptr, nullptr);
00078     glfwMakeContextCurrent(win);
00079 
00080     dev->start();
00081 
00082     while (!glfwWindowShouldClose(win))
00083     {
00084         glfwPollEvents();
00085         rs::frame frame;
00086 
00087         int w, h;
00088         glfwGetFramebufferSize(win, &w, &h);
00089         glViewport(0, 0, w, h);
00090         glClear(GL_COLOR_BUFFER_BIT);
00091 
00092         glfwGetWindowSize(win, &w, &h);
00093         glLoadIdentity();
00094         glOrtho(0, w, h, 0, -1, +1);
00095 
00096         for (auto i = 0; i < streams; i++)
00097         {
00098             if(!dev->supports(rs::capabilities(i))) continue;
00099 
00100             auto res = resolutions[(rs::stream)i];
00101 
00102             if (frames_queue[i].try_dequeue(&frame))
00103             {
00104                 buffers[i].upload(frame);
00105             }
00106 
00107             auto x = (i % 2) * (w / 2);
00108             auto y = (i / 2) * (h / 2);
00109             buffers[i].show(x, y, w / 2, h / 2, res.width, res.height);
00110         }
00111 
00112         glfwSwapBuffers(win);
00113     }
00114 
00115     running = false;
00116 
00117     for (auto i : supported_streams) frames_queue[i].clear();
00118 
00119     dev->stop();
00120 
00121     for (auto i : supported_streams)
00122     {
00123         if (dev->is_stream_enabled((rs::stream)i))
00124             dev->disable_stream((rs::stream)i);
00125     }
00126 
00127     return EXIT_SUCCESS;
00128 }
00129 catch (const rs::error & e)
00130 {
00131     printf("rs::error was thrown when calling %s(%s):\n", e.get_failed_function().c_str(), e.get_failed_args().c_str());
00132     printf("    %s\n", e.what());
00133     return EXIT_FAILURE;
00134 }


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