4 #include "../include/librealsense2/hpp/rs_sensor.hpp"
5 #include "../include/librealsense2/hpp/rs_processing.hpp"
15 template<
int N>
struct bytes {
byte b[N]; };
17 template<
class GET_DEPTH,
class TRANSFER_PIXEL>
19 const rs2_intrinsics& other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
22 #pragma omp parallel for schedule(dynamic)
23 for (
int depth_y = 0; depth_y < depth_intrin.
height; ++depth_y)
25 int depth_pixel_index = depth_y * depth_intrin.
width;
26 for (
int depth_x = 0; depth_x < depth_intrin.
width; ++depth_x, ++depth_pixel_index)
29 if (
float depth = get_depth(depth_pixel_index))
32 float depth_pixel[2] = { depth_x - 0.5f, depth_y - 0.5f }, depth_point[3], other_point[3], other_pixel[2];
36 const int other_x0 =
static_cast<int>(other_pixel[0] + 0.5f);
37 const int other_y0 =
static_cast<int>(other_pixel[1] + 0.5f);
40 depth_pixel[0] = depth_x + 0.5f; depth_pixel[1] = depth_y + 0.5f;
44 const int other_x1 =
static_cast<int>(other_pixel[0] + 0.5f);
45 const int other_y1 =
static_cast<int>(other_pixel[1] + 0.5f);
47 if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.
width || other_y1 >= other_intrin.
height)
51 for (
int y = other_y0;
y <= other_y1; ++
y)
53 for (
int x = other_x0;
x <= other_x1; ++
x)
55 transfer_pixel(depth_pixel_index,
y * other_intrin.
width +
x);
69 byte* aligned_data =
reinterpret_cast<byte*
>(
const_cast<void*
>(aligned.
get_data()));
71 memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.
get_bytes_per_pixel());
79 auto z_pixels =
reinterpret_cast<const uint16_t*
>(
depth.get_data());
80 auto out_z = (
uint16_t *)(aligned_data);
83 [z_pixels, z_scale](
int z_pixel_index) {
return z_scale * z_pixels[z_pixel_index]; },
84 [out_z, z_pixels](
int z_pixel_index,
int other_pixel_index)
86 out_z[other_pixel_index] = out_z[other_pixel_index] ?
87 std::min((
int)out_z[other_pixel_index], (
int)z_pixels[z_pixel_index]) :
88 z_pixels[z_pixel_index];
92 template<
int N,
class GET_DEPTH>
95 auto in_other = (
const bytes<N> *)(other_pixels);
96 auto out_other = (
bytes<N> *)(other_aligned_to_depth);
97 align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
98 [out_other, in_other](
int depth_pixel_index,
int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
101 template<
class GET_DEPTH>
107 align_other_to_depth_bytes<1>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
111 align_other_to_depth_bytes<2>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
115 align_other_to_depth_bytes<3>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
119 align_other_to_depth_bytes<4>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
128 byte* aligned_data =
reinterpret_cast<byte*
>(
const_cast<void*
>(aligned.
get_data()));
130 memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.
get_bytes_per_pixel());
139 auto z_pixels =
reinterpret_cast<const uint16_t*
>(
depth.get_data());
140 auto other_pixels =
reinterpret_cast<const byte*
>(other.
get_data());
142 align_other_to_depth(aligned_data, [z_pixels, z_scale](
int z_pixel_index) {
return z_scale * z_pixels[z_pixel_index]; },
143 z_intrin, z_to_other, other_intrin, other_pixels,
other_profile.format());
156 auto aligned_profile = std::make_shared<rs2::video_stream_profile>(original_profile.
clone(original_profile.
stream_type(), original_profile.
stream_index(), original_profile.
format()));
157 aligned_profile->get()->profile->set_framerate(original_profile.
fps());
158 if (
auto original_video_profile = As<video_stream_profile_interface>(original_profile.
get()->
profile))
160 if (
auto to_video_profile = As<video_stream_profile_interface>(
to_profile.get()->profile))
162 if (
auto aligned_video_profile = As<video_stream_profile_interface>(aligned_profile->get()->profile))
164 aligned_video_profile->set_dims(to_video_profile->get_width(), to_video_profile->get_height());
165 auto aligned_intrinsics = to_video_profile->get_intrinsics();
166 aligned_intrinsics.width = to_video_profile->get_width();
167 aligned_intrinsics.height = to_video_profile->get_height();
168 aligned_video_profile->set_intrinsics([aligned_intrinsics]() {
return aligned_intrinsics; });
169 aligned_profile->register_extrinsics_to(
to_profile, { { 1,0,0,0,1,0,0,0,1 },{ 0,0,0 } });
175 return aligned_profile;
193 bool has_tex =
false, has_depth =
false;
197 if (!has_tex || !has_depth)
221 rv =
source.allocate_video_frame(
224 from_bytes_per_pixel,
252 std::vector<rs2::frame> output_frames;
253 std::vector<rs2::frame> other_frames;
267 for (
auto from : other_frames)
271 output_frames.push_back(aligned_frame);
276 auto to = other_frames.front();
279 output_frames.push_back(aligned_frame);
282 auto new_composite =
source.allocate_composite_frame(
std::move(output_frames));
283 return new_composite;