c-tutorial-3-pointcloud.c
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 /***************************************************\
00005 * librealsense tutorial #3 - Point cloud generation *
00006 \***************************************************/
00007 
00008 /* First include the librealsense C header file */
00009 #include <librealsense/rs.h>
00010 #include <stdlib.h>
00011 #include <stdint.h>
00012 #include <stdio.h>
00013 #include <math.h>
00014 
00015 /* Additionall,y include the librealsense utilities header file */
00016 #include <librealsense/rsutil.h>
00017 
00018 /* Also include GLFW to allow for graphical display */
00019 #define GLFW_INCLUDE_GLU
00020 #include <GLFW/glfw3.h>
00021 
00022 /* Function calls to librealsense may raise errors of type rs_error */
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     /* Turn on logging. We can separately enable logging to console or to file, and use different severity filters for each. */
00054     rs_log_to_console(RS_LOG_SEVERITY_WARN, &e);
00055     check_error();
00056     /*rs_log_to_file(RS_LOG_SEVERITY_DEBUG, "librealsense.log", &e);
00057     check_error();*/
00058 
00059     /* Create a context object. This object owns the handles to all connected realsense devices. */
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     /* This tutorial will access only a single device, but it is trivial to extend to multiple devices */
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     /* Configure depth and color to run with the device's preferred settings */
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     /* Open a GLFW window to display our output */
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         /* Wait for new frame data */
00093         glfwPollEvents();
00094         rs_wait_for_frames(dev, &e);
00095         check_error();
00096 
00097         /* Retrieve our images */
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         /* Retrieve camera parameters for mapping between depth and color */
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         /* Set up a perspective transform in a space that we can rotate by clicking and dragging the mouse */
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         /* We will render our depth data as a set of points in 3D space */
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                 /* Retrieve the 16-bit depth value and map it into a depth in meters */
00139                 uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
00140                 float depth_in_meters = depth_value * scale;
00141 
00142                 /* Skip over pixels with a depth value of zero, which is used to indicate no data */
00143                 if(depth_value == 0) continue;
00144 
00145                 /* Map from pixel coordinates in the depth image to pixel coordinates in the color image */
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                 /* Use the color from the nearest color pixel, or pure white if this point falls outside the color image */
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                 /* Emit a vertex at the 3D location of this depth pixel */
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 }


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