00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <librealsense/rs.h>
00010 #include <stdlib.h>
00011 #include <stdint.h>
00012 #include <stdio.h>
00013 #include <math.h>
00014
00015
00016 #include <librealsense/rsutil.h>
00017
00018
00019 #define GLFW_INCLUDE_GLU
00020 #include <GLFW/glfw3.h>
00021
00022
00023 rs_error * e = 0;
00024 void check_error()
00025 {
00026 if(e)
00027 {
00028 printf("rs_error was raised when calling %s(%s):\n", rs_get_failed_function(e), rs_get_failed_args(e));
00029 printf(" %s\n", rs_get_error_message(e));
00030 exit(EXIT_FAILURE);
00031 }
00032 }
00033
00034 double yaw, pitch, lastX, lastY; int ml;
00035 static void on_mouse_button(GLFWwindow * win, int button, int action, int mods)
00036 {
00037 if(button == GLFW_MOUSE_BUTTON_LEFT) ml = action == GLFW_PRESS;
00038 }
00039 static double clamp(double val, double lo, double hi) { return val < lo ? lo : val > hi ? hi : val; }
00040 static void on_cursor_pos(GLFWwindow * win, double x, double y)
00041 {
00042 if(ml)
00043 {
00044 yaw = clamp(yaw - (x - lastX), -120, 120);
00045 pitch = clamp(pitch + (y - lastY), -80, 80);
00046 }
00047 lastX = x;
00048 lastY = y;
00049 }
00050
00051 int main()
00052 {
00053
00054 rs_log_to_console(RS_LOG_SEVERITY_WARN, &e);
00055 check_error();
00056
00057
00058
00059
00060 rs_context * ctx = rs_create_context(RS_API_VERSION, &e);
00061 check_error();
00062 printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e));
00063 check_error();
00064 if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE;
00065
00066
00067 rs_device * dev = rs_get_device(ctx, 0, &e);
00068 check_error();
00069 printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e));
00070 check_error();
00071 printf(" Serial number: %s\n", rs_get_device_serial(dev, &e));
00072 check_error();
00073 printf(" Firmware version: %s\n", rs_get_device_firmware_version(dev, &e));
00074 check_error();
00075
00076
00077 rs_enable_stream_preset(dev, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &e);
00078 check_error();
00079 rs_enable_stream_preset(dev, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &e);
00080 check_error();
00081 rs_start_device(dev, &e);
00082 check_error();
00083
00084
00085 glfwInit();
00086 GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #3", NULL, NULL);
00087 glfwSetCursorPosCallback(win, on_cursor_pos);
00088 glfwSetMouseButtonCallback(win, on_mouse_button);
00089 glfwMakeContextCurrent(win);
00090 while(!glfwWindowShouldClose(win))
00091 {
00092
00093 glfwPollEvents();
00094 rs_wait_for_frames(dev, &e);
00095 check_error();
00096
00097
00098 const uint16_t * depth_image = (const uint16_t *)rs_get_frame_data(dev, RS_STREAM_DEPTH, &e);
00099 check_error();
00100 const uint8_t * color_image = (const uint8_t *)rs_get_frame_data(dev, RS_STREAM_COLOR, &e);
00101 check_error();
00102
00103
00104 rs_intrinsics depth_intrin, color_intrin;
00105 rs_extrinsics depth_to_color;
00106 rs_get_stream_intrinsics(dev, RS_STREAM_DEPTH, &depth_intrin, &e);
00107 check_error();
00108 rs_get_device_extrinsics(dev, RS_STREAM_DEPTH, RS_STREAM_COLOR, &depth_to_color, &e);
00109 check_error();
00110 rs_get_stream_intrinsics(dev, RS_STREAM_COLOR, &color_intrin, &e);
00111 check_error();
00112 float scale = rs_get_device_depth_scale(dev, &e);
00113 check_error();
00114
00115
00116 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00117 glMatrixMode(GL_PROJECTION);
00118 glLoadIdentity();
00119 gluPerspective(60, (float)1280/960, 0.01f, 20.0f);
00120 glMatrixMode(GL_MODELVIEW);
00121 glLoadIdentity();
00122 gluLookAt(0,0,0, 0,0,1, 0,-1,0);
00123 glTranslatef(0,0,+0.5f);
00124 glRotated(pitch, 1, 0, 0);
00125 glRotated(yaw, 0, 1, 0);
00126 glTranslatef(0,0,-0.5f);
00127
00128
00129 glPointSize(2);
00130 glEnable(GL_DEPTH_TEST);
00131 glBegin(GL_POINTS);
00132
00133 int dx, dy;
00134 for(dy=0; dy<depth_intrin.height; ++dy)
00135 {
00136 for(dx=0; dx<depth_intrin.width; ++dx)
00137 {
00138
00139 uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
00140 float depth_in_meters = depth_value * scale;
00141
00142
00143 if(depth_value == 0) continue;
00144
00145
00146 float depth_pixel[2] = {(float)dx, (float)dy};
00147 float depth_point[3], color_point[3], color_pixel[2];
00148 rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth_in_meters);
00149 rs_transform_point_to_point(color_point, &depth_to_color, depth_point);
00150 rs_project_point_to_pixel(color_pixel, &color_intrin, color_point);
00151
00152
00153 const int cx = (int)roundf(color_pixel[0]), cy = (int)roundf(color_pixel[1]);
00154 if(cx < 0 || cy < 0 || cx >= color_intrin.width || cy >= color_intrin.height)
00155 {
00156 glColor3ub(255, 255, 255);
00157 }
00158 else
00159 {
00160 glColor3ubv(color_image + (cy * color_intrin.width + cx) * 3);
00161 }
00162
00163
00164 glVertex3f(depth_point[0], depth_point[1], depth_point[2]);
00165 }
00166 }
00167 glEnd();
00168
00169 glfwSwapBuffers(win);
00170 }
00171
00172 return EXIT_SUCCESS;
00173 }