stream.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 #include "stream.h"
5 #include "sync.h" // For frame_archive
6 #include "image.h" // For image alignment, rectification, and deprojection routines
7 #include <algorithm> // For sort
8 #include <tuple> // For make_tuple
9 
10 using namespace rsimpl;
11 
13 {
15  {
16  throw std::runtime_error(to_string() << "The extrinsic from " << get_stream_type() << " to " << other.get_stream_type() << " is not valid");
17  }
18 
19  auto& r = dynamic_cast<const stream_interface&>(other);
20  auto from = get_pose(), to = r.get_pose();
21  if(from == to) return {{1,0,0,0,1,0,0,0,1},{0,0,0}};
22  auto transform = inverse(from) * to;
23  rs_extrinsics extrin;
24  (float3x3 &)extrin.rotation = transform.orientation;
25  (float3 &)extrin.translation = transform.position;
26  return extrin;
27 }
28 
30 {
31  for(auto & subdevice_mode : config.info.subdevice_modes)
32  {
33  for(auto pad_crop : subdevice_mode.pad_crop_options)
34  {
35  for(auto & unpacker : subdevice_mode.pf.unpackers)
36  {
37  auto selection = subdevice_mode_selection(subdevice_mode, pad_crop, (int)(&unpacker - subdevice_mode.pf.unpackers.data()));
38  if(selection.provides_stream(stream)) modes.push_back(selection);
39  }
40  }
41  }
42 
43  auto get_tuple = [stream](const subdevice_mode_selection & selection)
44  {
45  return std::make_tuple(-selection.get_width(), -selection.get_height(), -selection.get_framerate(), selection.get_format(stream));
46  };
47 
48  std::sort(begin(modes), end(modes), [get_tuple](const subdevice_mode_selection & a, const subdevice_mode_selection & b) { return get_tuple(a) < get_tuple(b); });
49  auto it = std::unique(begin(modes), end(modes), [get_tuple](const subdevice_mode_selection & a, const subdevice_mode_selection & b) { return get_tuple(a) == get_tuple(b); });
50  if(it != end(modes)) modes.erase(it, end(modes));
51 }
52 
53 void native_stream::get_mode(int mode, int * w, int * h, rs_format * f, int * fps) const
54 {
55  auto & selection = modes[mode];
56  if(w) *w = selection.get_width();
57  if(h) *h = selection.get_height();
58  if(f) *f = selection.get_format(stream);
59  if(fps) *fps = selection.get_framerate();
60 }
61 
63 {
64  return (archive && archive->is_stream_enabled(stream)) || config.requests[stream].enabled;
65 }
66 
68 {
69  if(archive && archive->is_stream_enabled(stream)) return archive->get_mode(stream);
71  {
72  for(auto subdevice_mode : config.select_modes())
73  {
74  if(subdevice_mode.provides_stream(stream)) return subdevice_mode;
75  }
76  throw std::logic_error("no mode found"); // Should never happen, select_modes should throw if no mode can be found
77  }
78  throw std::runtime_error(to_string() << "stream not enabled: " << stream);
79 }
80 
82 {
84  {
85  LOG_ERROR("The intrinsic of " << get_stream_type() << " is not valid");
86  }
87  const auto m = get_mode();
88  return pad_crop_intrinsics(m.mode.native_intrinsics, m.pad_crop);
89 }
90 
92 {
94  {
95  throw std::runtime_error(to_string() << "The intrinsic of " << get_stream_type() << " is not valid");
96  }
97  const auto m = get_mode();
98  if(m.mode.rect_modes.empty()) return get_intrinsics();
99  return pad_crop_intrinsics(m.mode.rect_modes[0], m.pad_crop);
100 }
101 
103 {
104  if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
105  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
106  return archive->get_frame_metadata(stream, frame_metadata);
107 }
108 
110 {
111  if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
112  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
113  return archive->supports_frame_metadata(stream, frame_metadata);
114 }
115 
116 unsigned long long native_stream::get_frame_number() const
117 {
118  if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
119  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
120  return archive->get_frame_number(stream);
121 }
122 
124 {
125  if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
126  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
127  return archive->get_frame_timestamp(stream);
128 }
129 
131 {
132  if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
133  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
134  return archive->get_frame_system_time(stream);
135 }
136 
137 const uint8_t * native_stream::get_frame_data() const
138 {
139  if(!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
140  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
141  return (const uint8_t *) archive->get_frame_data(stream);
142 }
143 
145 {
146  if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
147  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
148  return archive->get_frame_stride(stream);
149 }
150 
152 {
153  if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
154  if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
155  return archive->get_frame_bpp(stream);
156 }
157 
158 const uint8_t * point_stream::get_frame_data() const
159 {
160  if(image.empty() || number != get_frame_number())
161  {
163 
164  if(source.get_format() == RS_FORMAT_Z16)
165  {
166  deproject_z(reinterpret_cast<float *>(image.data()), get_intrinsics(), reinterpret_cast<const uint16_t *>(source.get_frame_data()), get_depth_scale());
167  }
168  else if(source.get_format() == RS_FORMAT_DISPARITY16)
169  {
170  deproject_disparity(reinterpret_cast<float *>(image.data()), get_intrinsics(), reinterpret_cast<const uint16_t *>(source.get_frame_data()), get_depth_scale());
171  }
172  else assert(false && "Cannot deproject image from a non-depth format");
173 
174  number = get_frame_number();
175  }
176  return image.data();
177 }
178 
179 const uint8_t * rectified_stream::get_frame_data() const
180 {
181  // If source image is already rectified, just return it without doing any work
182  if(get_pose() == source.get_pose() && get_intrinsics() == source.get_intrinsics()) return source.get_frame_data();
183 
184  if(image.empty() || number != get_frame_number())
185  {
188  rectify_image(image.data(), table, source.get_frame_data(), get_format());
189  number = get_frame_number();
190  }
191  return image.data();
192 }
193 
194 const uint8_t * aligned_stream::get_frame_data() const
195 {
196  if(image.empty() || number != get_frame_number())
197  {
199  memset(image.data(), from.get_format() == RS_FORMAT_DISPARITY16 ? 0xFF : 0x00, image.size());
200  if(from.get_format() == RS_FORMAT_Z16)
201  {
202  align_z_to_other(image.data(), (const uint16_t *)from.get_frame_data(), from.get_depth_scale(), from.get_intrinsics(), from.get_extrinsics_to(to), to.get_intrinsics());
203  }
204  else if(from.get_format() == RS_FORMAT_DISPARITY16)
205  {
206  align_disparity_to_other(image.data(), (const uint16_t *)from.get_frame_data(), from.get_depth_scale(), from.get_intrinsics(), from.get_extrinsics_to(to), to.get_intrinsics());
207  }
208  else if(to.get_format() == RS_FORMAT_Z16)
209  {
210  align_other_to_z(image.data(), (const uint16_t *)to.get_frame_data(), to.get_depth_scale(), to.get_intrinsics(), to.get_extrinsics_to(from), from.get_intrinsics(), from.get_frame_data(), from.get_format());
211  }
212  else if(to.get_format() == RS_FORMAT_DISPARITY16)
213  {
214  align_other_to_disparity(image.data(), (const uint16_t *)to.get_frame_data(), to.get_depth_scale(), to.get_intrinsics(), to.get_extrinsics_to(from), from.get_intrinsics(), from.get_frame_data(), from.get_format());
215  }
216  else assert(false && "Cannot align two images if neither have depth data");
217  number = get_frame_number();
218  }
219  return image.data();
220 }
void align_other_to_z(byte *other_aligned_to_z, const uint16_t *z_pixels, float z_scale, const rs_intrinsics &z_intrin, const rs_extrinsics &z_to_other, const rs_intrinsics &other_intrin, const byte *other_pixels, rs_format other_format)
Definition: image.cpp:594
const uint8_t * get_frame_data() const override
Definition: stream.cpp:137
rs_intrinsics get_intrinsics() const override
Definition: stream.cpp:81
const uint8_t * get_frame_data() const override
Definition: stream.cpp:179
virtual rs_extrinsics get_extrinsics_to(const rs_stream_interface &other) const override
Definition: stream.cpp:12
const rs_stream stream
Definition: stream.h:24
virtual rsimpl::pose get_pose() const =0
frame_metadata
Types of value provided from the device with each frame.
Definition: rs.hpp:160
GLint GLint GLsizei GLsizei height
Definition: glext.h:112
const GLfloat * m
Definition: glext.h:6461
native_pixel_format pf
Definition: types.h:158
GLenum GLenum GLsizei void * table
Definition: glext.h:2715
std::vector< int > pad_crop_options
Definition: types.h:162
void align_disparity_to_other(byte *disparity_aligned_to_other, const uint16_t *disparity_pixels, float disparity_scale, const rs_intrinsics &disparity_intrin, const rs_extrinsics &disparity_to_other, const rs_intrinsics &other_intrin)
Definition: image.cpp:560
GLenum GLenum GLsizei void * image
Definition: glext.h:2716
bool validate_intrinsics(rs_stream stream) const
Definition: types.cpp:725
double get_frame_timestamp() const override
Definition: stream.cpp:123
Definition: archive.h:12
rs_intrinsics pad_crop_intrinsics(const rs_intrinsics &i, int pad_crop)
Definition: types.h:477
subdevice_mode_selection get_mode() const
Definition: stream.cpp:67
GLuint GLuint stream
Definition: glext.h:1774
GLfloat GLfloat GLfloat GLfloat h
Definition: glext.h:1944
void config(uvc::device &device, uint8_t gyro_bw, uint8_t gyro_range, uint8_t accel_bw, uint8_t accel_range, uint32_t time_seed)
std::vector< subdevice_mode > subdevice_modes
Definition: types.h:273
void deproject_disparity(float *points, const rs_intrinsics &disparity_intrin, const uint16_t *disparity_pixels, float disparity_scale)
Definition: image.cpp:506
std::shared_ptr< syncronizing_archive > archive
Definition: stream.h:38
double get_frame_metadata(rs_frame_metadata frame_metadata) const override
Definition: stream.cpp:102
float translation[3]
Definition: rs.h:335
std::vector< subdevice_mode_selection > modes
Definition: stream.h:37
virtual rs_stream get_stream_type() const override
Definition: stream.h:22
bool supports_frame_metadata(rs_frame_metadata frame_metadata) const override
Definition: stream.cpp:109
GLuint GLuint end
Definition: glext.h:111
stream_request requests[RS_STREAM_NATIVE_COUNT]
Definition: types.h:451
GLfloat f
Definition: glext.h:1868
virtual rs_stream get_stream_type() const =0
void deproject_z(float *points, const rs_intrinsics &z_intrin, const uint16_t *z_pixels, float z_scale)
Definition: image.cpp:501
rs_format get_format() const override
Definition: stream.h:51
rs_format
Formats: defines how each stream can be encoded.
Definition: rs.h:53
rs_intrinsics get_rectified_intrinsics() const override
Definition: stream.cpp:91
long long get_frame_system_time() const override
Definition: stream.cpp:130
native_stream(device_config &config, rs_stream stream, calibration_validator in_validator)
Definition: stream.cpp:29
bool is_enabled() const override
Definition: stream.cpp:62
GLboolean GLboolean GLboolean GLboolean a
Definition: glext.h:1104
GLenum mode
Definition: glext.h:1117
const device_config & config
Definition: stream.h:35
std::vector< int > compute_rectification_table(const rs_intrinsics &rect_intrin, const rs_extrinsics &rect_to_unrect, const rs_intrinsics &unrect_intrin)
Definition: image.cpp:608
unsigned long long get_frame_number() const override
Definition: stream.cpp:116
const static_device_info info
Definition: types.h:450
std::vector< pixel_format_unpacker > unpackers
Definition: types.h:144
Video stream intrinsics.
Definition: rs.h:300
int get_frame_bpp() const override
Definition: stream.cpp:151
GLboolean GLboolean GLboolean b
Definition: glext.h:1104
const unsigned char * get_frame_data() const override
Definition: stream.cpp:194
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
Definition: rs.h:332
void align_z_to_other(byte *z_aligned_to_other, const uint16_t *z_pixels, float z_scale, const rs_intrinsics &z_intrin, const rs_extrinsics &z_to_other, const rs_intrinsics &other_intrin)
Definition: image.cpp:552
void rectify_image(uint8_t *rect_pixels, const std::vector< int > &rectification_table, const uint8_t *unrect_pixels, rs_format format)
Definition: image.cpp:622
float rotation[9]
Definition: rs.h:334
pose inverse(const pose &a)
Definition: types.h:123
rs_stream
Streams are different types of data provided by RealSense devices.
Definition: rs.h:33
calibration_validator validator
Definition: stream.h:27
pose get_pose() const override
Definition: stream.h:42
GLint GLint GLsizei width
Definition: glext.h:112
GLsizei GLsizei GLchar * source
Definition: glext.h:672
size_t get_image_size(int width, int height, rs_format format)
Definition: image.cpp:22
float get_depth_scale() const override
Definition: stream.h:43
int get_frame_stride() const override
Definition: stream.cpp:144
const uint8_t * get_frame_data() const override
Definition: stream.cpp:158
rs_frame_metadata
Types of value provided from the device with each frame.
Definition: rs.h:203
void align_other_to_disparity(byte *other_aligned_to_disparity, const uint16_t *disparity_pixels, float disparity_scale, const rs_intrinsics &disparity_intrin, const rs_extrinsics &disparity_to_other, const rs_intrinsics &other_intrin, const byte *other_pixels, rs_format other_format)
Definition: image.cpp:599
GLuint GLenum GLenum transform
Definition: glext.h:10445
GLdouble GLdouble GLdouble r
Definition: glext.h:247
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:726
std::vector< subdevice_mode_selection > select_modes(const stream_request(&requests)[RS_STREAM_NATIVE_COUNT]) const
Definition: types.cpp:599
#define LOG_ERROR(...)
Definition: types.h:80
bool validate_extrinsics(rs_stream from_stream, rs_stream to_stream) const
Definition: types.cpp:721


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Fri Mar 13 2020 03:16:17