14 #if defined(RS2_USE_CUDA)
16 #elif defined(__SSSE3__)
26 #if defined(RS2_USE_CUDA)
27 return std::make_shared<librealsense::align_cuda>(
align_to);
28 #elif defined(__SSSE3__)
29 return std::make_shared<librealsense::align_sse>(
align_to);
31 return std::make_shared<librealsense::align>(
align_to);
35 template<
class GET_DEPTH,
class TRANSFER_PIXEL>
37 const rs2_intrinsics& other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
40 #pragma omp parallel for schedule(dynamic)
41 for (
int depth_y = 0; depth_y <
depth_intrin.height; ++depth_y)
44 for (
int depth_x = 0; depth_x <
depth_intrin.width; ++depth_x, ++depth_pixel_index)
47 if (
float depth = get_depth(depth_pixel_index))
54 const int other_x0 =
static_cast<int>(other_pixel[0] + 0.5f);
55 const int other_y0 =
static_cast<int>(other_pixel[1] + 0.5f);
62 const int other_x1 =
static_cast<int>(other_pixel[0] + 0.5f);
63 const int other_y1 =
static_cast<int>(other_pixel[1] + 0.5f);
65 if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.
width || other_y1 >= other_intrin.
height)
69 for (
int y = other_y0;
y <= other_y1; ++
y)
71 for (
int x = other_x0;
x <= other_x1; ++
x)
73 transfer_pixel(depth_pixel_index,
y * other_intrin.
width +
x);
89 memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.
get_bytes_per_pixel());
97 auto z_pixels =
reinterpret_cast<const uint16_t*
>(
depth.get_data());
98 auto out_z = (
uint16_t *)(aligned_data);
101 [z_pixels, z_scale](
int z_pixel_index) {
return z_scale * z_pixels[z_pixel_index]; },
102 [out_z, z_pixels](
int z_pixel_index,
int other_pixel_index)
104 out_z[other_pixel_index] = out_z[other_pixel_index] ?
105 std::min((
int)out_z[other_pixel_index], (
int)z_pixels[z_pixel_index]) :
106 z_pixels[z_pixel_index];
110 template<
int N,
class GET_DEPTH>
113 auto in_other = (
const bytes<N> *)(other_pixels);
114 auto out_other = (
bytes<N> *)(other_aligned_to_depth);
116 [out_other, in_other](
int depth_pixel_index,
int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
119 template<
class GET_DEPTH>
125 align_other_to_depth_bytes<1>(other_aligned_to_depth, get_depth,
depth_intrin, depth_to_other, other_intrin, other_pixels);
129 align_other_to_depth_bytes<2>(other_aligned_to_depth, get_depth,
depth_intrin, depth_to_other, other_intrin, other_pixels);
133 align_other_to_depth_bytes<3>(other_aligned_to_depth, get_depth,
depth_intrin, depth_to_other, other_intrin, other_pixels);
137 align_other_to_depth_bytes<4>(other_aligned_to_depth, get_depth,
depth_intrin, depth_to_other, other_intrin, other_pixels);
148 memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.
get_bytes_per_pixel());
157 auto z_pixels =
reinterpret_cast<const uint16_t*
>(
depth.get_data());
158 auto other_pixels =
reinterpret_cast<const uint8_t *
>(other.
get_data());
160 align_other_to_depth(aligned_data, [z_pixels, z_scale](
int z_pixel_index) {
return z_scale * z_pixels[z_pixel_index]; },
161 z_intrin, z_to_other, other_intrin, other_pixels,
other_profile.format());
174 auto aligned_profile = std::make_shared<rs2::video_stream_profile>(original_profile.
clone(original_profile.
stream_type(), original_profile.
stream_index(), original_profile.
format()));
175 aligned_profile->get()->profile->set_framerate(original_profile.
fps());
176 if (
auto original_video_profile = As<video_stream_profile_interface>(original_profile.
get()->
profile))
178 if (
auto to_video_profile = As<video_stream_profile_interface>(
to_profile.get()->profile))
180 if (
auto aligned_video_profile = As<video_stream_profile_interface>(aligned_profile->get()->profile))
182 aligned_video_profile->set_dims(to_video_profile->get_width(), to_video_profile->get_height());
183 auto aligned_intrinsics = to_video_profile->get_intrinsics();
184 aligned_intrinsics.width = to_video_profile->get_width();
185 aligned_intrinsics.height = to_video_profile->get_height();
186 aligned_video_profile->set_intrinsics([aligned_intrinsics]() {
return aligned_intrinsics; });
187 aligned_profile->register_extrinsics_to(
to_profile, { { 1,0,0,0,1,0,0,0,1 },{ 0,0,0 } });
193 return aligned_profile;
211 bool has_tex =
false, has_depth =
false;
215 if (!has_tex || !has_depth)
239 rv =
source.allocate_video_frame(
242 from_bytes_per_pixel,
270 std::vector<rs2::frame> output_frames;
271 std::vector<rs2::frame> other_frames;
285 for (
auto from : other_frames)
289 output_frames.push_back(aligned_frame);
294 auto to = other_frames.front();
297 output_frames.push_back(aligned_frame);
300 auto new_composite =
source.allocate_composite_frame(std::move(output_frames));
301 return new_composite;