void align::align_other_to_z(byte* aligned_data, const rs2::video_frame& depth, const rs2::video_frame& other, float z_scale)
{
auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();
auto other_profile = other.get_profile().as<rs2::video_stream_profile>();
auto z_intrin = depth_profile.get_intrinsics();
auto other_intrin = other_profile.get_intrinsics();
auto z_to_other = depth_profile.get_extrinsics_to(other_profile);
auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
auto other_pixels = reinterpret_cast<const byte*>(other.get_data());
align_other_to_depth(aligned_data, [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
z_intrin, z_to_other, other_intrin, other_pixels, other_profile.format());
}
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);
}
}
}
}
void align::align_z_to_other(byte* aligned_data, const rs2::video_frame& depth, const rs2::video_stream_profile& other_profile, float z_scale)
{
auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();
auto z_intrin = depth_profile.get_intrinsics();
auto other_intrin = other_profile.get_intrinsics();
auto z_to_other = depth_profile.get_extrinsics_to(other_profile);
auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
auto out_z = (uint16_t *)(aligned_data);
align_images(z_intrin, z_to_other, other_intrin,
[z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
[out_z, z_pixels](int z_pixel_index, int other_pixel_index)
{
out_z[other_pixel_index] = out_z[other_pixel_index] ?
std::min((int)out_z[other_pixel_index], (int)z_pixels[z_pixel_index]) :
z_pixels[z_pixel_index];
});
}
void align::align_frames(const rs2::video_frame& aligned, const rs2::video_frame& from, const rs2::video_frame& to)
{
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 = aligned.get_profile().as<rs2::video_stream_profile>();
byte* aligned_data = reinterpret_cast<byte*>(const_cast<void*>(aligned.get_data()));
memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.get_bytes_per_pixel());
if (to_profile.stream_type() == RS2_STREAM_DEPTH)
{
align_other_to_z(aligned_data, to, from, _depth_scale);
}
else
{
align_z_to_other(aligned_data, from, to_profile, _depth_scale);
}
}