align.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
3 
6 
7 #include "core/video.h"
8 #include "core/depth-frame.h"
10 #include "environment.h"
11 #include "align.h"
12 #include "stream.h"
13 
14 #if defined(RS2_USE_CUDA)
15 #include "proc/cuda/cuda-align.h"
16 #elif defined(__SSSE3__)
17 #include "proc/sse/sse-align.h"
18 #endif
19 
20 namespace librealsense
21 {
22  template<int N> struct bytes { uint8_t b[N]; };
23 
24  std::shared_ptr<align> align::create_align(rs2_stream align_to)
25  {
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);
30  #else
31  return std::make_shared<librealsense::align>(align_to);
32  #endif
33  }
34 
35  template<class GET_DEPTH, class TRANSFER_PIXEL>
36  void align_images(const rs2_intrinsics& depth_intrin, const rs2_extrinsics& depth_to_other,
37  const rs2_intrinsics& other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
38  {
39  // Iterate over the pixels of the depth image
40 #pragma omp parallel for schedule(dynamic)
41  for (int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
42  {
43  int depth_pixel_index = depth_y * depth_intrin.width;
44  for (int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
45  {
46  // Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
47  if (float depth = get_depth(depth_pixel_index))
48  {
49  // Map the top-left corner of the depth pixel onto the other image
50  float depth_pixel[2] = { depth_x - 0.5f, depth_y - 0.5f }, depth_point[3], other_point[3], other_pixel[2];
52  rs2_transform_point_to_point(other_point, &depth_to_other, depth_point);
53  rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
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);
56 
57  // Map the bottom-right corner of the depth pixel onto the other image
58  depth_pixel[0] = depth_x + 0.5f; depth_pixel[1] = depth_y + 0.5f;
60  rs2_transform_point_to_point(other_point, &depth_to_other, depth_point);
61  rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
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);
64 
65  if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height)
66  continue;
67 
68  // Transfer between the depth pixels and the pixels inside the rectangle on the other image
69  for (int y = other_y0; y <= other_y1; ++y)
70  {
71  for (int x = other_x0; x <= other_x1; ++x)
72  {
73  transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
74  }
75  }
76  }
77  }
78  }
79  }
80 
81  align::align(rs2_stream to_stream) : align(to_stream, "Align")
82  {}
83 
85  const rs2::video_frame& depth, const rs2::video_stream_profile& other_profile, float z_scale)
86  {
87  uint8_t * aligned_data = reinterpret_cast<uint8_t *>(const_cast<void*>(aligned.get_data()));
88  auto aligned_profile = aligned.get_profile().as<rs2::video_stream_profile>();
89  memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.get_bytes_per_pixel());
90 
91  auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();
92 
93  auto z_intrin = depth_profile.get_intrinsics();
94  auto other_intrin = other_profile.get_intrinsics();
95  auto z_to_other = depth_profile.get_extrinsics_to(other_profile);
96 
97  auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
98  auto out_z = (uint16_t *)(aligned_data);
99 
100  align_images(z_intrin, z_to_other, other_intrin,
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)
103  {
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];
107  });
108  }
109 
110  template<int N, class GET_DEPTH>
111  void align_other_to_depth_bytes( uint8_t * other_aligned_to_depth, GET_DEPTH get_depth, const rs2_intrinsics& depth_intrin, const rs2_extrinsics& depth_to_other, const rs2_intrinsics& other_intrin, const uint8_t * other_pixels)
112  {
113  auto in_other = (const bytes<N> *)(other_pixels);
114  auto out_other = (bytes<N> *)(other_aligned_to_depth);
115  align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
116  [out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
117  }
118 
119  template<class GET_DEPTH>
120  void align_other_to_depth( uint8_t * other_aligned_to_depth, GET_DEPTH get_depth, const rs2_intrinsics& depth_intrin, const rs2_extrinsics & depth_to_other, const rs2_intrinsics& other_intrin, const uint8_t * other_pixels, rs2_format other_format)
121  {
122  switch (other_format)
123  {
124  case RS2_FORMAT_Y8:
125  align_other_to_depth_bytes<1>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
126  break;
127  case RS2_FORMAT_Y16:
128  case RS2_FORMAT_Z16:
129  align_other_to_depth_bytes<2>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
130  break;
131  case RS2_FORMAT_RGB8:
132  case RS2_FORMAT_BGR8:
133  align_other_to_depth_bytes<3>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
134  break;
135  case RS2_FORMAT_RGBA8:
136  case RS2_FORMAT_BGRA8:
137  align_other_to_depth_bytes<4>(other_aligned_to_depth, get_depth, depth_intrin, depth_to_other, other_intrin, other_pixels);
138  break;
139  default:
140  assert(false); // NOTE: rs2_align_other_to_depth_bytes<2>(...) is not appropriate for RS2_FORMAT_YUYV/RS2_FORMAT_RAW10 images, no logic prevents U/V channels from being written to one another
141  }
142  }
143 
144  void align::align_other_to_z(rs2::video_frame& aligned, const rs2::video_frame& depth, const rs2::video_frame& other, float z_scale)
145  {
146  uint8_t * aligned_data = reinterpret_cast<uint8_t *>(const_cast<void*>(aligned.get_data()));
147  auto aligned_profile = aligned.get_profile().as<rs2::video_stream_profile>();
148  memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.get_bytes_per_pixel());
149 
150  auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();
152 
153  auto z_intrin = depth_profile.get_intrinsics();
154  auto other_intrin = other_profile.get_intrinsics();
155  auto z_to_other = depth_profile.get_extrinsics_to(other_profile);
156 
157  auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
158  auto other_pixels = reinterpret_cast<const uint8_t *>(other.get_data());
159 
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());
162  }
163 
164  std::shared_ptr<rs2::video_stream_profile> align::create_aligned_profile(
165  rs2::video_stream_profile& original_profile,
167  {
168  auto from_to = std::make_pair(original_profile.get()->profile, to_profile.get()->profile);
169  auto it = _align_stream_unique_ids.find(from_to);
170  if (it != _align_stream_unique_ids.end())
171  {
172  return it->second;
173  }
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))
177  {
178  if (auto to_video_profile = As<video_stream_profile_interface>(to_profile.get()->profile))
179  {
180  if (auto aligned_video_profile = As<video_stream_profile_interface>(aligned_profile->get()->profile))
181  {
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 } });
188  }
189  }
190  }
191  _align_stream_unique_ids[from_to] = aligned_profile;
192  reset_cache(original_profile.stream_type(), to_profile.stream_type());
193  return aligned_profile;
194  }
195 
197  {
198  if (!frame)
199  return false;
200 
201  auto set = frame.as<rs2::frameset>();
202  if (!set)
203  return false;
204 
205  auto profile = frame.get_profile();
206  rs2_stream stream = profile.stream_type();
208  int index = profile.stream_index();
209 
210  //process composite frame only if it contains both a depth frame and the requested texture frame
211  bool has_tex = false, has_depth = false;
212  set.foreach_rs([this, &has_tex](const rs2::frame& frame) { if (frame.get_profile().stream_type() == _to_stream_type) has_tex = true; });
213  set.foreach_rs([&has_depth](const rs2::frame& frame)
214  { if (frame.get_profile().stream_type() == RS2_STREAM_DEPTH && frame.get_profile().format() == RS2_FORMAT_Z16) has_depth = true; });
215  if (!has_tex || !has_depth)
216  return false;
217 
218  return true;
219  }
220 
222  {
224  return ext;
225  }
226 
228  {
229  rs2::frame rv;
230  auto from_bytes_per_pixel = from.get_bytes_per_pixel();
231 
232  auto from_profile = from.get_profile().as<rs2::video_stream_profile>();
234 
235  auto aligned_profile = create_aligned_profile(from_profile, to_profile);
236 
237  auto ext = select_extension(from);
238 
239  rv = source.allocate_video_frame(
240  *aligned_profile,
241  from,
242  from_bytes_per_pixel,
243  to.get_width(),
244  to.get_height(),
245  to.get_width() * from_bytes_per_pixel,
246  ext);
247  return rv;
248  }
249 
251  {
252  auto from_profile = from.get_profile().as<rs2::video_stream_profile>();
254 
255  auto aligned_profile = aligned.get_profile().as<rs2::video_stream_profile>();
256 
257  if (to_profile.stream_type() == RS2_STREAM_DEPTH)
258  {
259  align_other_to_z(aligned, to, from, _depth_scale);
260  }
261  else
262  {
263  align_z_to_other(aligned, from, to_profile, _depth_scale);
264  }
265  }
266 
268  {
269  rs2::frame rv;
270  std::vector<rs2::frame> output_frames;
271  std::vector<rs2::frame> other_frames;
272 
273  auto frames = f.as<rs2::frameset>();
274  auto depth = frames.first_or_default(RS2_STREAM_DEPTH, RS2_FORMAT_Z16).as<rs2::depth_frame>();
275 
276  _depth_scale = ((librealsense::depth_frame*)depth.get())->get_units();
277 
279  frames.foreach_rs([&other_frames](const rs2::frame& f) {if ((f.get_profile().stream_type() != RS2_STREAM_DEPTH) && f.is<rs2::video_frame>()) other_frames.push_back(f); });
280  else
281  frames.foreach_rs([this, &other_frames](const rs2::frame& f) {if (f.get_profile().stream_type() == _to_stream_type) other_frames.push_back(f); });
282 
284  {
285  for (auto from : other_frames)
286  {
287  auto aligned_frame = allocate_aligned_frame(source, from, depth);
288  align_frames(aligned_frame, from, depth);
289  output_frames.push_back(aligned_frame);
290  }
291  }
292  else
293  {
294  auto to = other_frames.front();
295  auto aligned_frame = allocate_aligned_frame(source, depth, to);
296  align_frames(aligned_frame, depth, to);
297  output_frames.push_back(aligned_frame);
298  }
299 
300  auto new_composite = source.allocate_composite_frame(std::move(output_frames));
301  return new_composite;
302  }
303 }
format
GLint GLint GLsizei GLint GLenum format
Definition: glad/glad/glad.h:1412
librealsense::align_images
void align_images(const rs2_intrinsics &depth_intrin, const rs2_extrinsics &depth_to_other, const rs2_intrinsics &other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
Definition: align.cpp:36
librealsense
Definition: algo.h:18
librealsense::align::align_z_to_other
virtual void align_z_to_other(rs2::video_frame &aligned, const rs2::video_frame &depth, const rs2::video_stream_profile &other_profile, float z_scale)
Definition: align.cpp:84
uint8_t
unsigned char uint8_t
Definition: stdint.h:78
rs2::stream_profile::format
rs2_format format() const
Definition: rs_frame.hpp:44
pyglet_pointcloud_viewer.other_format
other_format
Definition: pyglet_pointcloud_viewer.py:107
librealsense::align::_to_stream_type
rs2_stream _to_stream_type
Definition: align.h:47
RS2_FORMAT_RGBA8
@ RS2_FORMAT_RGBA8
Definition: rs_sensor.h:70
min
int min(int a, int b)
Definition: lz4s.c:73
rs2::frame
Definition: rs_frame.hpp:345
rs2::video_stream_profile::clone
stream_profile clone(rs2_stream type, int index, rs2_format format, int width, int height, const rs2_intrinsics &intr) const
Definition: rs_frame.hpp:268
rs2::frame_source
Definition: rs_processing.hpp:18
rs-imu-calibration.input
input
Definition: rs-imu-calibration.py:35
align.h
rs2::frameset
Definition: rs_frame.hpp:947
rs2_transform_point_to_point
void rs2_transform_point_to_point(float to_point[3], const rs2_extrinsics *extrin, const float from_point[3])
rs2_extrinsics
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented.
Definition: rs_sensor.h:102
rs2::stream_profile::fps
int fps() const
Definition: rs_frame.hpp:49
test-projection-from-recording.frames
frames
Definition: test-projection-from-recording.py:30
rs2::stream_profile::as
T as() const
Definition: rs_frame.hpp:103
uint16_t
unsigned short uint16_t
Definition: stdint.h:79
sse-align.h
profile::format
rs2_format format
Definition: unit-tests-common.h:61
test-projection-from-recording.depth_intrin
depth_intrin
Definition: test-projection-from-recording.py:43
rs2_extension
rs2_extension
Specifies advanced interfaces (capabilities) objects may implement.
Definition: rs_types.h:134
b
GLboolean GLboolean GLboolean b
Definition: glad/glad/glad.h:3064
rs2_intrinsics
Video stream intrinsics.
Definition: rs_types.h:58
index
GLuint index
Definition: glad/glad/glad.h:2777
video.h
RS2_FORMAT_RGB8
@ RS2_FORMAT_RGB8
Definition: rs_sensor.h:68
rs2_intrinsics::height
int height
Definition: rs_types.h:61
rs2::stream_profile::stream_type
rs2_stream stream_type() const
Definition: rs_frame.hpp:39
librealsense::bytes
Definition: align.cpp:22
librealsense::align::create_aligned_profile
std::shared_ptr< rs2::video_stream_profile > create_aligned_profile(rs2::video_stream_profile &original_profile, rs2::video_stream_profile &to_profile)
Definition: align.cpp:164
librealsense::align::_align_stream_unique_ids
std::map< std::pair< stream_profile_interface *, stream_profile_interface * >, std::shared_ptr< rs2::video_stream_profile > > _align_stream_unique_ids
Definition: align.h:48
rs2::frame::get_data
const void * get_data() const
Definition: rs_frame.hpp:547
rs2_deproject_pixel_to_point
void rs2_deproject_pixel_to_point(float point[3], const rs2_intrinsics *intrin, const float pixel[2], float depth)
test-projection-from-recording.other_point
other_point
Definition: test-projection-from-recording.py:66
RS2_FORMAT_Z16
@ RS2_FORMAT_Z16
Definition: rs_sensor.h:64
librealsense::align::_depth_scale
float _depth_scale
Definition: align.h:50
librealsense::align::process_frame
rs2::frame process_frame(const rs2::frame_source &source, const rs2::frame &f) override
Definition: align.cpp:267
librealsense::align::reset_cache
virtual void reset_cache(rs2_stream from, rs2_stream to)
Definition: align.h:29
rs2_format
rs2_format
A stream's format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:61
librealsense::align::allocate_aligned_frame
rs2::video_frame allocate_aligned_frame(const rs2::frame_source &source, const rs2::video_frame &from, const rs2::video_frame &to)
Definition: align.cpp:227
rs2::video_stream_profile
Definition: rs_frame.hpp:201
cuda-align.h
rs_sensor.hpp
f
GLdouble f
Definition: glad/glad/glad.h:1517
rs2_stream_profile::profile
librealsense::stream_profile_interface * profile
Definition: src/stream.h:23
synthetic-stream.h
depth-frame.h
librealsense::align_other_to_depth
void align_other_to_depth(uint8_t *other_aligned_to_depth, GET_DEPTH get_depth, const rs2_intrinsics &depth_intrin, const rs2_extrinsics &depth_to_other, const rs2_intrinsics &other_intrin, const uint8_t *other_pixels, rs2_format other_format)
Definition: align.cpp:120
RS2_STREAM_DEPTH
@ RS2_STREAM_DEPTH
Definition: rs_sensor.h:46
RS2_EXTENSION_VIDEO_FRAME
@ RS2_EXTENSION_VIDEO_FRAME
Definition: rs_types.h:144
rs2::video_frame::get_width
int get_width() const
Definition: rs_frame.hpp:661
librealsense::depth_frame
Definition: depth-frame.h:17
librealsense::align_other_to_depth_bytes
void align_other_to_depth_bytes(uint8_t *other_aligned_to_depth, GET_DEPTH get_depth, const rs2_intrinsics &depth_intrin, const rs2_extrinsics &depth_to_other, const rs2_intrinsics &other_intrin, const uint8_t *other_pixels)
Definition: align.cpp:111
librealsense::to_profile
stream_profile to_profile(const stream_profile_interface *sp)
Definition: src/stream.h:222
pyglet_pointcloud_viewer.other_profile
other_profile
Definition: pyglet_pointcloud_viewer.py:156
rs2::stream_profile::get
const rs2_stream_profile * get() const
Definition: rs_frame.hpp:137
source
GLsizei GLsizei GLchar * source
Definition: glad/glad/glad.h:2828
rs_processing.hpp
librealsense::stream
Definition: src/stream.h:30
librealsense::frame
Definition: frame.h:19
librealsense::align::align
align(rs2_stream to_stream)
Definition: align.cpp:81
rs2_project_point_to_pixel
void rs2_project_point_to_pixel(float pixel[2], const rs2_intrinsics *intrin, const float point[3])
assert
#define assert(condition)
Definition: lz4.c:245
RS2_EXTENSION_DEPTH_FRAME
@ RS2_EXTENSION_DEPTH_FRAME
Definition: rs_types.h:148
rs2_intrinsics::width
int width
Definition: rs_types.h:60
rs2::depth_frame
Definition: rs_frame.hpp:813
RS2_FORMAT_Y16
@ RS2_FORMAT_Y16
Definition: rs_sensor.h:73
librealsense::align::align_other_to_z
virtual void align_other_to_z(rs2::video_frame &aligned, const rs2::video_frame &depth, const rs2::video_frame &other, float z_scale)
Definition: align.cpp:144
rs2::frame::get_profile
stream_profile get_profile() const
Definition: rs_frame.hpp:559
librealsense::align::align_frames
void align_frames(rs2::video_frame &aligned, const rs2::video_frame &from, const rs2::video_frame &to)
Definition: align.cpp:250
librealsense::align
Definition: align.h:14
environment.h
RS2_FORMAT_Y8
@ RS2_FORMAT_Y8
Definition: rs_sensor.h:72
librealsense::align::select_extension
virtual rs2_extension select_extension(const rs2::frame &input)
Definition: align.cpp:221
rs2::video_frame
Definition: rs_frame.hpp:638
librealsense::align::create_align
static std::shared_ptr< align > create_align(rs2_stream align_to)
Definition: align.cpp:24
test-projection-from-recording.depth_pixel
list depth_pixel
Definition: test-projection-from-recording.py:60
rs2::video_frame::get_bytes_per_pixel
int get_bytes_per_pixel() const
Definition: rs_frame.hpp:709
x
GLdouble x
Definition: glad/glad/glad.h:2279
librealsense::align::should_process
bool should_process(const rs2::frame &frame) override
Definition: align.cpp:196
test-projection-from-recording.depth_profile
depth_profile
Definition: test-projection-from-recording.py:27
rs2::stream_profile::stream_index
int stream_index() const
Definition: rs_frame.hpp:34
RS2_FORMAT_BGRA8
@ RS2_FORMAT_BGRA8
Definition: rs_sensor.h:71
align-depth2color.align_to
align_to
Definition: align-depth2color.py:56
it
static auto it
Definition: openvino-face-detection.cpp:375
rs2_stream
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:43
RS2_FORMAT_BGR8
@ RS2_FORMAT_BGR8
Definition: rs_sensor.h:69
profile
Definition: unit-tests-common.h:58
rs2::video_frame::get_height
int get_height() const
Definition: rs_frame.hpp:673
depth
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glad/glad/glad.h:2398
y
GLint y
Definition: glad/glad/glad.h:1397


librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Fri Aug 2 2024 08:30:00