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 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); } }
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; }
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]; }); }