cpp-pointcloud.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 
00004 #include <librealsense/rs.hpp>
00005 #include "example.hpp"
00006 
00007 #include <chrono>
00008 #include <vector>
00009 #include <sstream>
00010 #include <iostream>
00011 #include <algorithm>
00012 
00013 inline void glVertex(const rs::float3 & vertex) { glVertex3fv(&vertex.x); }
00014 inline void glTexCoord(const rs::float2 & tex_coord) { glTexCoord2fv(&tex_coord.x); }
00015 
00016 struct state { double yaw, pitch, lastX, lastY; bool ml; std::vector<rs::stream> tex_streams; int index; rs::device * dev; };
00017 
00018 int main(int argc, char * argv[]) try
00019 {
00020     rs::log_to_console(rs::log_severity::warn);
00021     //rs::log_to_file(rs::log_severity::debug, "librealsense.log");
00022 
00023     rs::context ctx;
00024     if(ctx.get_device_count() == 0) throw std::runtime_error("No device detected. Is it plugged in?");
00025     rs::device & dev = *ctx.get_device(0);
00026 
00027     dev.enable_stream(rs::stream::depth, rs::preset::best_quality);
00028     dev.enable_stream(rs::stream::color, rs::preset::best_quality);
00029     dev.enable_stream(rs::stream::infrared, rs::preset::best_quality);
00030     try { dev.enable_stream(rs::stream::infrared2, rs::preset::best_quality); } catch(...) {}
00031     dev.start();
00032     
00033     state app_state = {0, 0, 0, 0, false, {rs::stream::color, rs::stream::depth, rs::stream::infrared}, 0, &dev};
00034     if(dev.is_stream_enabled(rs::stream::infrared2)) app_state.tex_streams.push_back(rs::stream::infrared2);
00035     
00036     glfwInit();
00037     std::ostringstream ss; ss << "CPP Point Cloud Example (" << dev.get_name() << ")";
00038     GLFWwindow * win = glfwCreateWindow(640, 480, ss.str().c_str(), 0, 0);
00039     glfwSetWindowUserPointer(win, &app_state);
00040         
00041     glfwSetMouseButtonCallback(win, [](GLFWwindow * win, int button, int action, int mods)
00042     {
00043         auto s = (state *)glfwGetWindowUserPointer(win);
00044         if(button == GLFW_MOUSE_BUTTON_LEFT) s->ml = action == GLFW_PRESS;
00045         if(button == GLFW_MOUSE_BUTTON_RIGHT && action == GLFW_PRESS) s->index = (s->index+1) % s->tex_streams.size();
00046     });
00047         
00048     glfwSetCursorPosCallback(win, [](GLFWwindow * win, double x, double y)
00049     {
00050         auto s = (state *)glfwGetWindowUserPointer(win);
00051         if(s->ml)
00052         {
00053             s->yaw -= (x - s->lastX);
00054             s->yaw = std::max(s->yaw, -120.0);
00055             s->yaw = std::min(s->yaw, +120.0);
00056             s->pitch += (y - s->lastY);
00057             s->pitch = std::max(s->pitch, -80.0);
00058             s->pitch = std::min(s->pitch, +80.0);
00059         }
00060         s->lastX = x;
00061         s->lastY = y;
00062     });
00063         
00064     glfwSetKeyCallback(win, [](GLFWwindow * win, int key, int scancode, int action, int mods)
00065     {
00066         auto s = (state *)glfwGetWindowUserPointer(win);
00067         if (action == GLFW_RELEASE)
00068         {
00069             if (key == GLFW_KEY_ESCAPE) glfwSetWindowShouldClose(win, 1);
00070             else if (key == GLFW_KEY_F1)
00071             {
00072                if (!s->dev->is_streaming()) s->dev->start();
00073             }
00074             else if (key == GLFW_KEY_F2)
00075             {
00076                if (s->dev->is_streaming()) s->dev->stop();
00077             }
00078         }
00079     });
00080 
00081     glfwMakeContextCurrent(win);
00082     texture_buffer tex;
00083 
00084     int frames = 0; float time = 0, fps = 0;
00085     auto t0 = std::chrono::high_resolution_clock::now();
00086     while (!glfwWindowShouldClose(win))
00087     {
00088         glfwPollEvents();
00089         if(dev.is_streaming()) dev.wait_for_frames();
00090 
00091         auto t1 = std::chrono::high_resolution_clock::now();
00092         time += std::chrono::duration<float>(t1-t0).count();
00093         t0 = t1;
00094         ++frames;
00095         if(time > 0.5f)
00096         {
00097             fps = frames / time;
00098             frames = 0;
00099             time = 0;
00100         }
00101 
00102         const rs::stream tex_stream = app_state.tex_streams[app_state.index];
00103         const rs::extrinsics extrin = dev.get_extrinsics(rs::stream::depth, tex_stream);
00104         const rs::intrinsics depth_intrin = dev.get_stream_intrinsics(rs::stream::depth);
00105         const rs::intrinsics tex_intrin = dev.get_stream_intrinsics(tex_stream);
00106         bool identical = depth_intrin == tex_intrin && extrin.is_identity();
00107       
00108         glPushAttrib(GL_ALL_ATTRIB_BITS);
00109 
00110         tex.upload(dev, tex_stream);
00111 
00112         int width, height;
00113         glfwGetFramebufferSize(win, &width, &height);
00114         glViewport(0, 0, width, height);
00115         glClearColor(52.0f/255, 72.f/255, 94.0f/255.0f, 1);
00116         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00117 
00118         glMatrixMode(GL_PROJECTION);
00119         glPushMatrix();
00120         gluPerspective(60, (float)width/height, 0.01f, 20.0f);
00121 
00122         glMatrixMode(GL_MODELVIEW);
00123         glPushMatrix();
00124         gluLookAt(0,0,0, 0,0,1, 0,-1,0);
00125 
00126         glTranslatef(0,0,+0.5f);
00127         glRotated(app_state.pitch, 1, 0, 0);
00128         glRotated(app_state.yaw, 0, 1, 0);
00129         glTranslatef(0,0,-0.5f);
00130 
00131         glPointSize((float)width/640);
00132         glEnable(GL_DEPTH_TEST);
00133         glEnable(GL_TEXTURE_2D);
00134         glBindTexture(GL_TEXTURE_2D, tex.get_gl_handle());
00135         glBegin(GL_POINTS);
00136 
00137         auto points = reinterpret_cast<const rs::float3 *>(dev.get_frame_data(rs::stream::points));
00138         auto depth = reinterpret_cast<const uint16_t *>(dev.get_frame_data(rs::stream::depth));
00139         
00140         for(int y=0; y<depth_intrin.height; ++y)
00141         {
00142             for(int x=0; x<depth_intrin.width; ++x)
00143             {
00144                 if(points->z) //if(uint16_t d = *depth++)
00145                 {
00146                     //const rs::float3 point = depth_intrin.deproject({static_cast<float>(x),static_cast<float>(y)}, d*depth_scale);
00147                     glTexCoord(identical ? tex_intrin.pixel_to_texcoord({static_cast<float>(x),static_cast<float>(y)}) : tex_intrin.project_to_texcoord(extrin.transform(*points)));
00148                     glVertex(*points);
00149                 }
00150                 ++points;
00151             }
00152         }
00153         glEnd();
00154         glPopMatrix();
00155         glMatrixMode(GL_PROJECTION);
00156         glPopMatrix();
00157         glPopAttrib();
00158 
00159         glfwGetWindowSize(win, &width, &height);
00160         glPushAttrib(GL_ALL_ATTRIB_BITS);
00161         glPushMatrix();
00162         glOrtho(0, width, height, 0, -1, +1);
00163         
00164         std::ostringstream ss; ss << dev.get_name() << " (" << app_state.tex_streams[app_state.index] << ")";
00165         draw_text((width-get_text_width(ss.str().c_str()))/2, height-20, ss.str().c_str());
00166 
00167         ss.str(""); ss << fps << " FPS";
00168         draw_text(20, 40, ss.str().c_str());
00169         glPopMatrix();
00170 
00171         glfwSwapBuffers(win);
00172     }
00173 
00174     glfwDestroyWindow(win);
00175     glfwTerminate();
00176     return EXIT_SUCCESS;
00177 }
00178 catch(const rs::error & e)
00179 {
00180     std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
00181     return EXIT_FAILURE;
00182 }
00183 catch(const std::exception & e)
00184 {
00185     std::cerr << e.what() << std::endl;
00186     return EXIT_FAILURE;
00187 }


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