00001
00002
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
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)
00145 {
00146
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 }