void RealSenseDevice::setDepthBuffer() { rs_error * e = 0; int next_buffer = (depth_index + 1) % 2; const uint16_t* depthPtr = (const uint16_t*)rs_get_frame_data(dev, depthStream, &e); const size_t depthBufferSize = 640*480; memcpy(depth_buffer[depth_index],depthPtr,depthBufferSize * sizeof(uint16_t)); // uint16_t rescale = 1.f / depthScale; for (size_t i = 0; i < depthBufferSize;i++) { // depth_buffer[depth_index][i] = 1 / (depth_buffer[depth_index][i] + 1); // depth_buffer[depth_index][i] -= 1; } const unsigned char* rgbPtr = (const unsigned char*)rs_get_frame_data(dev, colorStream, &e); memcpy(rgb_buffer,rgbPtr,640*480*3); depth_index = next_buffer; gotDepth = true; }
char * getRealsenseDepthPixels(int devID) { return rs_get_frame_data(device[devID].dev, device[devID].depthStreamToUse , &e); }
char * getRealsenseColorPixels(int devID) { return rs_get_frame_data(device[devID].dev, device[devID].colorStreamToUse, &e); }
int main() { /* Turn on logging. We can separately enable logging to console or to file, and use different severity filters for each. */ rs_log_to_console(RS_LOG_SEVERITY_WARN, &e); check_error(); /*rs_log_to_file(RS_LOG_SEVERITY_DEBUG, "librealsense.log", &e); check_error();*/ /* Create a context object. This object owns the handles to all connected realsense devices. */ rs_context * ctx = rs_create_context(RS_API_VERSION, &e); check_error(); printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e)); check_error(); if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE; /* This tutorial will access only a single device, but it is trivial to extend to multiple devices */ rs_device * dev = rs_get_device(ctx, 0, &e); check_error(); printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e)); check_error(); printf(" Serial number: %s\n", rs_get_device_serial(dev, &e)); check_error(); printf(" Firmware version: %s\n", rs_get_device_firmware_version(dev, &e)); check_error(); /* Configure depth and color to run with the device's preferred settings */ rs_enable_stream_preset(dev, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &e); check_error(); rs_enable_stream_preset(dev, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &e); check_error(); rs_start_device(dev, RS_SOURCE_VIDEO, &e); check_error(); /* Open a GLFW window to display our output */ glfwInit(); GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #3", NULL, NULL); glfwSetCursorPosCallback(win, on_cursor_pos); glfwSetMouseButtonCallback(win, on_mouse_button); glfwMakeContextCurrent(win); while(!glfwWindowShouldClose(win)) { /* Wait for new frame data */ glfwPollEvents(); rs_wait_for_frames(dev, &e); check_error(); /* Retrieve our images */ const uint16_t * depth_image = (const uint16_t *)rs_get_frame_data(dev, RS_STREAM_DEPTH, &e); check_error(); const uint8_t * color_image = (const uint8_t *)rs_get_frame_data(dev, RS_STREAM_COLOR, &e); check_error(); /* Retrieve camera parameters for mapping between depth and color */ rs_intrinsics depth_intrin, color_intrin; rs_extrinsics depth_to_color; rs_get_stream_intrinsics(dev, RS_STREAM_DEPTH, &depth_intrin, &e); check_error(); rs_get_device_extrinsics(dev, RS_STREAM_DEPTH, RS_STREAM_COLOR, &depth_to_color, &e); check_error(); rs_get_stream_intrinsics(dev, RS_STREAM_COLOR, &color_intrin, &e); check_error(); float scale = rs_get_device_depth_scale(dev, &e); check_error(); /* Set up a perspective transform in a space that we can rotate by clicking and dragging the mouse */ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(60, (float)1280/960, 0.01f, 20.0f); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(0,0,0, 0,0,1, 0,-1,0); glTranslatef(0,0,+0.5f); glRotated(pitch, 1, 0, 0); glRotated(yaw, 0, 1, 0); glTranslatef(0,0,-0.5f); /* We will render our depth data as a set of points in 3D space */ glPointSize(2); glEnable(GL_DEPTH_TEST); glBegin(GL_POINTS); int dx, dy; for(dy=0; dy<depth_intrin.height; ++dy) { for(dx=0; dx<depth_intrin.width; ++dx) { /* Retrieve the 16-bit depth value and map it into a depth in meters */ uint16_t depth_value = depth_image[dy * depth_intrin.width + dx]; float depth_in_meters = depth_value * scale; /* Skip over pixels with a depth value of zero, which is used to indicate no data */ if(depth_value == 0) continue; /* Map from pixel coordinates in the depth image to pixel coordinates in the color image */ float depth_pixel[2] = {(float)dx, (float)dy}; float depth_point[3], color_point[3], color_pixel[2]; rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth_in_meters); rs_transform_point_to_point(color_point, &depth_to_color, depth_point); rs_project_point_to_pixel(color_pixel, &color_intrin, color_point); /* Use the color from the nearest color pixel, or pure white if this point falls outside the color image */ const int cx = (int)roundf(color_pixel[0]), cy = (int)roundf(color_pixel[1]); if(cx < 0 || cy < 0 || cx >= color_intrin.width || cy >= color_intrin.height) { glColor3ub(255, 255, 255); } else { glColor3ubv(color_image + (cy * color_intrin.width + cx) * 3); } /* Emit a vertex at the 3D location of this depth pixel */ glVertex3f(depth_point[0], depth_point[1], depth_point[2]); } } glEnd(); glfwSwapBuffers(win); } return EXIT_SUCCESS; }
int main() { /* Create a context object. This object owns the handles to all connected realsense devices. */ rs_context * ctx = rs_create_context(RS_API_VERSION, &e); check_error(); printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e)); check_error(); if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE; /* This tutorial will access only a single device, but it is trivial to extend to multiple devices */ rs_device * dev = rs_get_device(ctx, 0, &e); check_error(); printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e)); check_error(); printf(" Serial number: %s\n", rs_get_device_serial(dev, &e)); check_error(); printf(" Firmware version: %s\n", rs_get_device_firmware_version(dev, &e)); check_error(); /* Configure depth to run at VGA resolution at 30 frames per second */ rs_enable_stream(dev, RS_STREAM_DEPTH, 640, 480, RS_FORMAT_Z16, 30, &e); check_error(); rs_start_device(dev, &e); check_error(); /* Determine depth value corresponding to one meter */ const uint16_t one_meter = (uint16_t)(1.0f / rs_get_device_depth_scale(dev, &e)); check_error(); while(1) { /* This call waits until a new coherent set of frames is available on a device Calls to get_frame_data(...) and get_frame_timestamp(...) on a device will return stable values until wait_for_frames(...) is called */ rs_wait_for_frames(dev, &e); /* Retrieve depth data, which was previously configured as a 640 x 480 image of 16-bit depth values */ const uint16_t * depth_frame = (const uint16_t *)(rs_get_frame_data(dev, RS_STREAM_DEPTH, &e)); /* Print a simple text-based representation of the image, by breaking it into 10x20 pixel regions and and approximating the coverage of pixels within one meter */ char buffer[(640/10+1)*(480/20)+1]; char * out = buffer; int coverage[64] = {0}, x,y,i; for(y=0; y<480; ++y) { for(x=0; x<640; ++x) { int depth = *depth_frame++; if(depth > 0 && depth < one_meter) ++coverage[x/10]; } if(y%20 == 19) { for(i=0; i<64; ++i) { *out++ = " .:nhBXWW"[coverage[i]/25]; coverage[i] = 0; } *out++ = '\n'; } } *out++ = 0; printf("\n%s", buffer); } return EXIT_SUCCESS; }
Frame CameraWrapper::record() { // clear buffer rs_wait_for_frames(dev, &e); check_error(); std::vector<cv::Mat> depthstack; std::vector<cv::Mat> colorstack; std::vector<cv::Mat> irstack; // record a stack of frames for(int frame = 0; frame < framesToRecord; frame++) { // wait for frames rs_wait_for_frames(dev, &e); check_error(); // get data const uint16_t * depth_image = (const uint16_t *)rs_get_frame_data(dev, RS_STREAM_DEPTH, &e); check_error(); const uint8_t * color_image = (const uint8_t *)rs_get_frame_data(dev, RS_STREAM_COLOR, &e); check_error(); const uint8_t * ir_image = (const uint8_t *)rs_get_frame_data(dev, RS_STREAM_INFRARED, &e); check_error(); // convert depth to meters and float cv::Mat depthframe = cv::Mat(depth_intrin.height, depth_intrin.width, CV_32F); int dx, dy; for(dy=0; dy<depth_intrin.height; ++dy) { for(dx=0; dx<depth_intrin.width; ++dx) { /* Retrieve the 16-bit depth value and map it into a depth in meters */ uint16_t depth_value = depth_image[dy * depth_intrin.width + dx]; float depth_in_meters = depth_value * depthScale; depthframe.at<float>(dy, dx) = depth_in_meters; } } depthstack.push_back(depthframe); // color cv::Mat colorframeRGB = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, cv::Scalar(0)); memcpy(colorframeRGB.ptr(),color_image,color_intrin.height*color_intrin.width*3); // f200 gives RGB image, openCV uses BGR - so convert cv::Mat colorframeBGR; cv::cvtColor(colorframeRGB, colorframeBGR, CV_RGB2BGR); colorstack.push_back(colorframeBGR); // ir cv::Mat irframe = cv::Mat(ir_intrin.height, ir_intrin.width, CV_8UC1, cv::Scalar(0)); memcpy(irframe.ptr(),ir_image,ir_intrin.height*ir_intrin.width); irstack.push_back(irframe); } // create Frame object Frame frame1; convertIntrinsicToOpenCV(depth_intrin, frame1.cameraMatrix_depth, frame1.coefficients_depth); convertIntrinsicToOpenCV(color_intrin, frame1.cameraMatrix_color, frame1.coefficients_color); convertIntrinsicToOpenCV(ir_intrin, frame1.cameraMatrix_ir, frame1.coefficients_ir); Eigen::Matrix3f R(depth_to_color.rotation); Eigen::Vector3f T(depth_to_color.translation); Eigen::Matrix4f m1 = Eigen::Affine3f(Eigen::Translation3f(T)).matrix(); Eigen::Matrix4f m2 = Eigen::Affine3f(R).matrix(); Eigen::Matrix4f dtc = m1*m2; Eigen::Affine3f mpose (dtc); frame1.depth_to_color_transform = mpose; // call averager // DESIGN add interface // limit to 4 frames // average RGB images int framesToRecordRGB; if (framesToRecord > 4) framesToRecordRGB = 4; else framesToRecordRGB = framesToRecord; frame1.processedImageRGB = cv::Mat::zeros(color_intrin.height, color_intrin.width, CV_8UC3); for (int frame = 0; frame < framesToRecordRGB; frame++) { frame1.processedImageRGB = frame1.processedImageRGB + (1.0/framesToRecordRGB)*colorstack[frame]; } // average IR frame1.processedImageIR = cv::Mat::zeros(ir_intrin.height, ir_intrin.width, CV_8UC1); for (int frame = 0; frame < framesToRecordRGB; frame++) { frame1.processedImageIR = frame1.processedImageIR + (1.0/framesToRecordRGB)*irstack[frame]; } // average depth, calculate variance and zeroes cv::Mat belief(depth_intrin.height, depth_intrin.width, CV_8U, cv::Scalar::all(0)); cv::Mat processedImageDepth(depth_intrin.height, depth_intrin.width, CV_32F, cv::Scalar::all(0)); for (int y = 0; y < depth_intrin.height; y++) { for (int x = 0; x < depth_intrin.width; x++) { double tempresult_depth = 0; int depth_zeroes = 0; std::vector<float> var_values; for(int frame = 0; frame < depthstack.size(); frame++) { if(depthstack[frame].at<float>(y,x) == 0.0) { depth_zeroes++; }else { tempresult_depth += (float)depthstack[frame].at<float>(y,x); // increase values by big factor because of value range var_values.push_back(depthstack[frame].at<float>(y,x)*10000); } } // calculate average double var = calculateVariance(var_values); // limit variance for display reasons if (var > 255.0) var = 255.0; // reduce belief if pixel has zero value i.e. no data int zeroPenaltyFactor = 255/depthstack.size(); int beliefPx = 255-((int)var + (depth_zeroes * zeroPenaltyFactor)); if (beliefPx < 0) beliefPx = 0; processedImageDepth.at<float>(y,x) = tempresult_depth/(depthstack.size()-depth_zeroes); // zeroes: (255-((255/stack.size())*depth_zeroes) belief.at<uchar>(y,x) = beliefPx; } } frame1.belief = belief; frame1.processedImageDepth = processedImageDepth; //frame1.processedImageDepth = depthstack[0]; return frame1; }
int main() { /* Create a context object. This object owns the handles to all connected realsense devices. */ rs_context * ctx = rs_create_context(RS_API_VERSION, &e); check_error(); printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e)); check_error(); if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE; /* This tutorial will access only a single device, but it is trivial to extend to multiple devices */ rs_device * dev = rs_get_device(ctx, 0, &e); check_error(); printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e)); check_error(); printf(" Serial number: %s\n", rs_get_device_serial(dev, &e)); check_error(); printf(" Firmware version: %s\n", rs_get_device_firmware_version(dev, &e)); check_error(); /* Configure all streams to run at VGA resolution at 60 frames per second */ rs_enable_stream(dev, RS_STREAM_DEPTH, 640, 480, RS_FORMAT_Z16, 60, &e); check_error(); rs_enable_stream(dev, RS_STREAM_COLOR, 640, 480, RS_FORMAT_RGB8, 60, &e); check_error(); rs_enable_stream(dev, RS_STREAM_INFRARED, 640, 480, RS_FORMAT_Y8, 60, &e); check_error(); rs_enable_stream(dev, RS_STREAM_INFRARED2, 640, 480, RS_FORMAT_Y8, 60, NULL); /* Pass NULL to ignore errors */ rs_start_device(dev, &e); check_error(); /* Open a GLFW window to display our output */ glfwInit(); GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #2", NULL, NULL); glfwMakeContextCurrent(win); while(!glfwWindowShouldClose(win)) { /* Wait for new frame data */ glfwPollEvents(); rs_wait_for_frames(dev, &e); check_error(); glClear(GL_COLOR_BUFFER_BIT); glPixelZoom(1, -1); /* Display depth data by linearly mapping depth between 0 and 2 meters to the red channel */ glRasterPos2f(-1, 1); glPixelTransferf(GL_RED_SCALE, 0xFFFF * rs_get_device_depth_scale(dev, &e) / 2.0f); check_error(); glDrawPixels(640, 480, GL_RED, GL_UNSIGNED_SHORT, rs_get_frame_data(dev, RS_STREAM_DEPTH, &e)); check_error(); glPixelTransferf(GL_RED_SCALE, 1.0f); /* Display color image as RGB triples */ glRasterPos2f(0, 1); glDrawPixels(640, 480, GL_RGB, GL_UNSIGNED_BYTE, rs_get_frame_data(dev, RS_STREAM_COLOR, &e)); check_error(); /* Display infrared image by mapping IR intensity to visible luminance */ glRasterPos2f(-1, 0); glDrawPixels(640, 480, GL_LUMINANCE, GL_UNSIGNED_BYTE, rs_get_frame_data(dev, RS_STREAM_INFRARED, &e)); check_error(); /* Display second infrared image by mapping IR intensity to visible luminance */ if(rs_is_stream_enabled(dev, RS_STREAM_INFRARED2, NULL)) { glRasterPos2f(0, 0); glDrawPixels(640, 480, GL_LUMINANCE, GL_UNSIGNED_BYTE, rs_get_frame_data(dev, RS_STREAM_INFRARED2, &e)); } glfwSwapBuffers(win); } return EXIT_SUCCESS; }