void remove_background(rs2::video_frame& other_frame, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist) { const uint16_t* p_depth_frame = reinterpret_cast<const uint16_t*>(depth_frame.get_data()); uint8_t* p_other_frame = reinterpret_cast<uint8_t*>(const_cast<void*>(other_frame.get_data())); int width = other_frame.get_width(); int height = other_frame.get_height(); int other_bpp = other_frame.get_bytes_per_pixel(); #pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop for (int y = 0; y < height; y++) { auto depth_pixel_index = y * width; for (int x = 0; x < width; x++, ++depth_pixel_index) { // Get the depth value of the current pixel auto pixels_distance = depth_scale * p_depth_frame[depth_pixel_index]; // Check if the depth value is invalid (<=0) or greater than the threashold if (pixels_distance <= 0.f || pixels_distance > clipping_dist) { // Calculate the offset in other frame's buffer to current pixel auto offset = depth_pixel_index * other_bpp; // Set pixel to "background" color (0x999999) std::memset(&p_other_frame[offset], 0x99, other_bpp); } } } }
rs2::frame align::allocate_aligned_frame(const rs2::frame_source& source, const rs2::video_frame& from, const rs2::video_frame& to) { rs2::frame rv; auto from_bytes_per_pixel = from.get_bytes_per_pixel(); auto from_profile = from.get_profile().as<rs2::video_stream_profile>(); auto to_profile = to.get_profile().as<rs2::video_stream_profile>(); auto aligned_profile = create_aligned_profile(from_profile, to_profile); auto ext = from.is<rs2::depth_frame>() ? RS2_EXTENSION_DEPTH_FRAME : RS2_EXTENSION_VIDEO_FRAME; rv = source.allocate_video_frame( *aligned_profile, from, from_bytes_per_pixel, to.get_width(), to.get_height(), to.get_width() * from_bytes_per_pixel, ext); return rv; }