00001
00002
00003
00005
00007
00008
00009 #include <librealsense/rs.hpp>
00010 #include <cstdio>
00011
00012
00013 #define GLFW_INCLUDE_GLU
00014 #include <GLFW/glfw3.h>
00015
00016 double yaw, pitch, lastX, lastY; int ml;
00017 static void on_mouse_button(GLFWwindow * win, int button, int action, int mods)
00018 {
00019 if(button == GLFW_MOUSE_BUTTON_LEFT) ml = action == GLFW_PRESS;
00020 }
00021 static double clamp(double val, double lo, double hi) { return val < lo ? lo : val > hi ? hi : val; }
00022 static void on_cursor_pos(GLFWwindow * win, double x, double y)
00023 {
00024 if(ml)
00025 {
00026 yaw = clamp(yaw - (x - lastX), -120, 120);
00027 pitch = clamp(pitch + (y - lastY), -80, 80);
00028 }
00029 lastX = x;
00030 lastY = y;
00031 }
00032
00033 int main() try
00034 {
00035
00036 rs::log_to_console(rs::log_severity::warn);
00037
00038
00039
00040 rs::context ctx;
00041 printf("There are %d connected RealSense devices.\n", ctx.get_device_count());
00042 if(ctx.get_device_count() == 0) return EXIT_FAILURE;
00043
00044
00045 rs::device * dev = ctx.get_device(0);
00046 printf("\nUsing device 0, an %s\n", dev->get_name());
00047 printf(" Serial number: %s\n", dev->get_serial());
00048 printf(" Firmware version: %s\n", dev->get_firmware_version());
00049
00050
00051 dev->enable_stream(rs::stream::depth, rs::preset::best_quality);
00052 dev->enable_stream(rs::stream::color, rs::preset::best_quality);
00053 dev->start();
00054
00055
00056 glfwInit();
00057 GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #3", nullptr, nullptr);
00058 glfwSetCursorPosCallback(win, on_cursor_pos);
00059 glfwSetMouseButtonCallback(win, on_mouse_button);
00060 glfwMakeContextCurrent(win);
00061 while(!glfwWindowShouldClose(win))
00062 {
00063
00064 glfwPollEvents();
00065 dev->wait_for_frames();
00066
00067
00068 const uint16_t * depth_image = (const uint16_t *)dev->get_frame_data(rs::stream::depth);
00069 const uint8_t * color_image = (const uint8_t *)dev->get_frame_data(rs::stream::color);
00070
00071
00072 rs::intrinsics depth_intrin = dev->get_stream_intrinsics(rs::stream::depth);
00073 rs::extrinsics depth_to_color = dev->get_extrinsics(rs::stream::depth, rs::stream::color);
00074 rs::intrinsics color_intrin = dev->get_stream_intrinsics(rs::stream::color);
00075 float scale = dev->get_depth_scale();
00076
00077
00078 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00079 glMatrixMode(GL_PROJECTION);
00080 glLoadIdentity();
00081 gluPerspective(60, (float)1280/960, 0.01f, 20.0f);
00082 glMatrixMode(GL_MODELVIEW);
00083 glLoadIdentity();
00084 gluLookAt(0,0,0, 0,0,1, 0,-1,0);
00085 glTranslatef(0,0,+0.5f);
00086 glRotated(pitch, 1, 0, 0);
00087 glRotated(yaw, 0, 1, 0);
00088 glTranslatef(0,0,-0.5f);
00089
00090
00091 glPointSize(2);
00092 glEnable(GL_DEPTH_TEST);
00093 glBegin(GL_POINTS);
00094
00095 for(int dy=0; dy<depth_intrin.height; ++dy)
00096 {
00097 for(int dx=0; dx<depth_intrin.width; ++dx)
00098 {
00099
00100 uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
00101 float depth_in_meters = depth_value * scale;
00102
00103
00104 if(depth_value == 0) continue;
00105
00106
00107 rs::float2 depth_pixel = {(float)dx, (float)dy};
00108 rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
00109 rs::float3 color_point = depth_to_color.transform(depth_point);
00110 rs::float2 color_pixel = color_intrin.project(color_point);
00111
00112
00113 const int cx = (int)std::round(color_pixel.x), cy = (int)std::round(color_pixel.y);
00114 if(cx < 0 || cy < 0 || cx >= color_intrin.width || cy >= color_intrin.height)
00115 {
00116 glColor3ub(255, 255, 255);
00117 }
00118 else
00119 {
00120 glColor3ubv(color_image + (cy * color_intrin.width + cx) * 3);
00121 }
00122
00123
00124 glVertex3f(depth_point.x, depth_point.y, depth_point.z);
00125 }
00126 }
00127 glEnd();
00128
00129 glfwSwapBuffers(win);
00130 }
00131
00132 return EXIT_SUCCESS;
00133 }
00134 catch(const rs::error & e)
00135 {
00136
00137 printf("rs::error was thrown when calling %s(%s):\n", e.get_failed_function().c_str(), e.get_failed_args().c_str());
00138 printf(" %s\n", e.what());
00139 return EXIT_FAILURE;
00140 }