pointcloud.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 
4 #include "../include/librealsense2/rs.hpp"
5 #include "../include/librealsense2/rsutil.h"
6 #include "environment.h"
8 #include "proc/pointcloud.h"
9 #include "option.h"
10 #include "environment.h"
11 #include "context.h"
12 #include "../device.h"
13 #include "../stream.h"
14 #include <iostream>
15 #include "device-calibration.h"
16 
17 #ifdef RS2_USE_CUDA
19 #endif
20 #ifdef __SSSE3__
22 #endif
23 
24 namespace librealsense
25 {
26  template<class MAP_DEPTH> void deproject_depth(float * points, const rs2_intrinsics & intrin, const uint16_t * depth, MAP_DEPTH map_depth)
27  {
28  for (int y = 0; y < intrin.height; ++y)
29  {
30  for (int x = 0; x < intrin.width; ++x)
31  {
32  const float pixel[] = { (float)x, (float)y };
33  rs2_deproject_pixel_to_point(points, &intrin, pixel, map_depth(*depth++));
34  points += 3;
35  }
36  }
37  }
38 
41  {
42  auto image = output.get_vertices();
43  deproject_depth((float*)image, depth_intrinsics, (const uint16_t*)depth_frame.get_data(), [depth_scale](uint16_t z) { return depth_scale * z; });
44  return (float3*)image;
45  }
46 
47  float3 transform(const rs2_extrinsics *extrin, const float3 &point) { float3 p = {}; rs2_transform_point_to_point(&p.x, extrin, &point.x); return p; }
48  float2 project(const rs2_intrinsics *intrin, const float3 & point) { float2 pixel = {}; rs2_project_point_to_pixel(&pixel.x, intrin, &point.x); return pixel; }
49  float2 pixel_to_texcoord(const rs2_intrinsics *intrin, const float2 & pixel) { return{ pixel.x / (intrin->width), pixel.y / (intrin->height) }; }
50  float2 project_to_texcoord(const rs2_intrinsics *intrin, const float3 & point) { return pixel_to_texcoord(intrin, project(intrin, point)); }
51 
53  {
55  {
56  rs2_extrinsics ex;
59  if (environment::get_instance().get_extrinsics_graph().try_fetch_extrinsics(
60  *ds->profile, *os->profile, &ex))
61  {
62  _extrinsics = ex;
63  }
64  }
65  }
66 
68  {
69  if (!_output_stream || _depth_stream.get_profile().get() != depth.get_profile().get())
70  {
77  }
78 
79  bool found_depth_intrinsics = false;
80  bool found_depth_units = false;
81 
82  if (!_depth_intrinsics)
83  {
84  auto stream_profile = depth.get_profile();
85  if (auto video = stream_profile.as<rs2::video_stream_profile>())
86  {
87  _depth_intrinsics = video.get_intrinsics();
89  _occlusion_filter->set_depth_intrinsics(_depth_intrinsics.value());
90 
91  preprocess();
92 
93  found_depth_intrinsics = true;
94  }
95  }
96 
97  if (!_depth_units)
98  {
99  auto sensor = ((frame_interface*)depth.get())->get_sensor().get();
100  _depth_units = sensor->get_option(RS2_OPTION_DEPTH_UNITS).query();
101  found_depth_units = true;
102  }
103 
104  set_extrinsics();
105  }
106 
107  template< class callback >
109  {
110  return {
112  []( rs2_calibration_change_callback* p ) { p->release(); }
113  };
114  }
115 
117  {
119  {
121 
123  {
124  auto sensor = ((frame_interface*)other.get())->get_sensor();
125  if (sensor)
126  {
128  = std::shared_ptr< pointcloud >( this, []( pointcloud * p ) {} );
129 
130  auto dev = sensor->get_device().shared_from_this();
131  auto * d2r = dynamic_cast<calibration_change_device*>(dev.get());
132  if( d2r )
133  try
134  {
135  std::weak_ptr< pointcloud > wr{ _registered_auto_calib_cb };
136  auto fn = [=]( rs2_calibration_status status ) {
137  auto r = wr.lock();
138  if( ! r )
139  // nobody there any more!
140  return;
142  {
143  stream_profile_interface *ds = nullptr, *os = nullptr;
144  for( size_t x = 0, N = dev->get_sensors_count(); x < N; ++x )
145  {
146  sensor_interface & s = dev->get_sensor( x );
147  for( auto const & sp : s.get_active_streams() )
148  {
149  if( sp->get_stream_type() == RS2_STREAM_COLOR )
150  {
151  auto vspi = As< video_stream_profile_interface >(
152  sp.get() );
153  if( vspi )
154  {
155  os = vspi;
156  _other_intrinsics = vspi->get_intrinsics();
157  _occlusion_filter->set_texel_intrinsics(
159  }
160  }
161  else if( sp->get_stream_type() == RS2_STREAM_DEPTH )
162  {
163  ds = sp.get();
164  }
165  }
166  }
167  if( ds && os )
168  {
169  rs2_extrinsics ex;
171  .get_extrinsics_graph()
172  .try_fetch_extrinsics( *ds, *os, &ex ) )
173  _extrinsics = ex;
174  else
175  LOG_ERROR( "Failed to refresh extrinsics after calibration change" );
176  }
177  }
178  };
179 
180  d2r->register_calibration_change_callback(
182  }
183  catch( const std::bad_weak_ptr & )
184  {
185  LOG_WARNING( "Device destroyed" );
186  }
187  }
188  }
189  }
190 
192  return;
193 
194  _other_stream = other;
197 
198  if (!_other_intrinsics)
199  {
201  if (auto video = stream_profile.as<rs2::video_stream_profile>())
202  {
203  _other_intrinsics = video.get_intrinsics();
204  _occlusion_filter->set_texel_intrinsics(_other_intrinsics.value());
205  }
206  }
207 
208  set_extrinsics();
209  }
210 
212  const float3* points,
213  const unsigned int width,
214  const unsigned int height,
215  const rs2_intrinsics &other_intrinsics,
216  const rs2_extrinsics& extr,
217  float2* pixels_ptr)
218  {
219  auto tex_ptr = (float2*)output.get_texture_coordinates();
220 
221  for (unsigned int y = 0; y < height; ++y)
222  {
223  for (unsigned int x = 0; x < width; ++x)
224  {
225  if (points->z)
226  {
227  auto trans = transform(&extr, *points);
228  //auto tex_xy = project_to_texcoord(&mapped_intr, trans);
229  // Store intermediate results for poincloud filters
230  *pixels_ptr = project(&other_intrinsics, trans);
231  auto tex_xy = pixel_to_texcoord(&other_intrinsics, *pixels_ptr);
232 
233  *tex_ptr = tex_xy;
234  }
235  else
236  {
237  *tex_ptr = { 0.f, 0.f };
238  *pixels_ptr = { 0.f, 0.f };
239  }
240  ++points;
241  ++tex_ptr;
242  ++pixels_ptr;
243  }
244  }
245  }
246 
248  {
249  return source.allocate_points(_output_stream, depth);
250  }
251 
253  {
254  auto res = allocate_points(source, depth);
255  auto pframe = (librealsense::points*)(res.get());
256 
258 
259  auto vid_frame = depth.as<rs2::video_frame>();
260 
261  // Pixels calculated in the mapped texture. Used in post-processing filters
262  float2* pixels_ptr = _pixels_map.data();
263  rs2_intrinsics mapped_intr;
265  bool map_texture = false;
266  {
268  {
269  mapped_intr = *_other_intrinsics;
270  extr = *_extrinsics;
271  map_texture = true;
272  }
273  }
274 
275  if (map_texture)
276  {
277  auto height = vid_frame.get_height();
278  auto width = vid_frame.get_width();
279 
280  get_texture_map(res, points, width, height, mapped_intr, extr, pixels_ptr);
281 
282  if (run__occlusion_filter(extr))
283  {
284  if (_occlusion_filter->find_scanning_direction(extr) == vertical)
285  {
286  _occlusion_filter->set_scanning(static_cast<uint8_t>(vertical));
287  _occlusion_filter->_depth_units = _depth_units.value();
288  }
289  _occlusion_filter->process(pframe->get_vertices(), pframe->get_texture_coordinates(), _pixels_map, depth);
290  }
291  }
292  return res;
293  }
294 
296  : pointcloud("Pointcloud")
297  {}
298 
301  {
302  _occlusion_filter = std::make_shared<occlusion_filter>();
303 
304  auto occlusion_invalidation = std::make_shared<ptr_option<uint8_t>>(
306  occlusion_max - 1, 1,
308  (uint8_t*)&_occlusion_filter->_occlusion_filter,
309  "Occlusion removal");
310  occlusion_invalidation->on_set([this, occlusion_invalidation](float val)
311  {
312  if (!occlusion_invalidation->is_valid(val))
314  << "Unsupported occlusion filtering requiested " << val << " is out of range.");
315 
316  _occlusion_filter->set_mode(static_cast<uint8_t>(val));
317 
318  });
319  occlusion_invalidation->set_description(1.f, "Off");
320  occlusion_invalidation->set_description(2.f, "On");
321  register_option(RS2_OPTION_FILTER_MAGNITUDE, occlusion_invalidation);
322  }
323 
325  {
326  if (!frame)
327  return false;
328 
329  auto set = frame.as<rs2::frameset>();
330 
331  if (set)
332  {
333  //process composite frame only if it contains both a depth frame and the requested texture frame
335  return false;
336 
338  if (!tex)
339  return false;
340  auto depth = set.first_or_default(RS2_STREAM_DEPTH, RS2_FORMAT_Z16);
341  if (!depth)
342  return false;
343  }
344  else
345  {
346  auto p = frame.get_profile();
347  if (p.stream_type() == RS2_STREAM_DEPTH && p.format() == RS2_FORMAT_Z16)
348  return true;
349 
350  if (p.stream_type() == _stream_filter.stream && p.format() == _stream_filter.format && p.stream_index() == _stream_filter.index)
351  return true;
352  return false;
353 
354  //TODO: switch to this code when map_to api is removed
355  //if (_stream_filter != RS2_STREAM_ANY)
356  // return false;
357  //process single frame only if it is a depth frame
358  //if (frame.get_profile().stream_type() != RS2_STREAM_DEPTH || frame.get_profile().format() != RS2_FORMAT_Z16)
359  // return false;
360  }
361 
362  return true;
363  }
364 
366  {
367  rs2::frame rv;
368  if (auto composite = f.as<rs2::frameset>())
369  {
370  auto texture = composite.first(_stream_filter.stream);
372 
373  auto depth = composite.first(RS2_STREAM_DEPTH, RS2_FORMAT_Z16);
375  rv = process_depth_frame(source, depth);
376  }
377  else
378  {
379  if (f.is<rs2::depth_frame>())
380  {
382  rv = process_depth_frame(source, f);
383  }
385  {
387  }
388  }
389  return rv;
390  }
391 
392  std::shared_ptr<pointcloud> pointcloud::create()
393  {
394  #ifdef RS2_USE_CUDA
395  return std::make_shared<librealsense::pointcloud_cuda>();
396  #else
397  #ifdef __SSSE3__
398  return std::make_shared<librealsense::pointcloud_sse>();
399  #else
400  return std::make_shared<librealsense::pointcloud>();
401  #endif
402  #endif
403  }
404 
406  {
407  return (_occlusion_filter->active() && !_occlusion_filter->is_same_sensor(extr));
408  }
409 }
rs2::frame process_depth_frame(const rs2::frame_source &source, const rs2::depth_frame &depth)
Definition: pointcloud.cpp:252
static void rs2_transform_point_to_point(float to_point[3], const struct rs2_extrinsics *extrin, const float from_point[3])
Definition: rsutil.h:168
std::shared_ptr< occlusion_filter > _occlusion_filter
Definition: pointcloud.h:45
GLint y
rs2_frame * get() const
Definition: rs_frame.hpp:590
rs2::stream_profile _output_stream
Definition: pointcloud.h:50
GLuint const GLchar * name
bool should_process(const rs2::frame &frame) override
Definition: pointcloud.cpp:324
GLdouble s
GLfloat GLfloat p
Definition: glext.h:12687
#define LOG_WARNING(...)
Definition: src/types.h:241
void inspect_depth_frame(const rs2::frame &depth)
Definition: pointcloud.cpp:67
stream_profile get_profile() const
Definition: rs_frame.hpp:557
void inspect_other_frame(const rs2::frame &other)
Definition: pointcloud.cpp:116
stream_filter _prev_stream_filter
Definition: pointcloud.h:59
rs2_intrinsics intrin
calibration_change_callback_ptr create_calibration_change_callback_ptr(callback &&cb)
Definition: pointcloud.cpp:108
GLint GLint GLsizei GLsizei GLsizei depth
optional_value< rs2_extrinsics > _extrinsics
Definition: pointcloud.h:44
const void * get_data() const
Definition: rs_frame.hpp:545
unsigned short uint16_t
Definition: stdint.h:79
frame allocate_points(const stream_profile &profile, const frame &original) const
float2 project_to_texcoord(const rs2_intrinsics *intrin, const float3 &point)
Definition: pointcloud.cpp:50
GLdouble GLdouble z
unsigned char uint8_t
Definition: stdint.h:78
point
Definition: rmse.py:166
const texture_coordinate * get_texture_coordinates() const
Definition: rs_frame.hpp:792
The texture class.
Definition: example.hpp:402
GLenum GLenum GLsizei void * image
status
Defines return codes that SDK interfaces use. Negative values indicate errors, a zero value indicates...
virtual bool run__occlusion_filter(const rs2_extrinsics &extr)
Definition: pointcloud.cpp:405
void register_option(rs2_option id, std::shared_ptr< option > option)
Definition: options.h:86
optional_value< rs2_intrinsics > _other_intrinsics
Definition: pointcloud.h:42
frame first_or_default(rs2_stream s, rs2_format f=RS2_FORMAT_ANY) const
Definition: rs_frame.hpp:978
GLuint GLfloat * val
virtual void get_texture_map(rs2::points output, const float3 *points, const unsigned int width, const unsigned int height, const rs2_intrinsics &other_intrinsics, const rs2_extrinsics &extr, float2 *pixels_ptr)
Definition: pointcloud.cpp:211
GLdouble f
bool is() const
Definition: rs_frame.hpp:570
static std::shared_ptr< pointcloud > create()
Definition: pointcloud.cpp:392
rs2::frame _depth_stream
Definition: pointcloud.h:52
static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics *intrin, const float pixel[2], float depth)
Definition: rsutil.h:83
virtual void preprocess()
Definition: pointcloud.h:32
GLdouble GLdouble r
GLdouble x
rs2_calibration_status
Definition: rs_device.h:356
rs2::frame _other_stream
Definition: pointcloud.h:51
GLint GLsizei GLsizei height
const vertex * get_vertices() const
Definition: rs_frame.hpp:767
void deproject_depth(float *points, const rs2_intrinsics &intrin, const uint16_t *depth, MAP_DEPTH map_depth)
Definition: pointcloud.cpp:26
virtual rs2::points allocate_points(const rs2::frame_source &source, const rs2::frame &f)
Definition: pointcloud.cpp:247
def callback(frame)
Definition: t265_stereo.py:91
float2 project(const rs2_intrinsics *intrin, const float3 &point)
Definition: pointcloud.cpp:48
optional_value< float > _depth_units
Definition: pointcloud.h:43
#define LOG_ERROR(...)
Definition: src/types.h:242
int stream_index() const
Definition: rs_frame.hpp:34
std::vector< float2 > _pixels_map
Definition: pointcloud.h:48
static environment & get_instance()
librealsense::stream_profile_interface * profile
Definition: context.h:34
static const double d2r
Definition: src/types.h:58
float3 transform(const rs2_extrinsics *extrin, const float3 &point)
Definition: pointcloud.cpp:47
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
rs2_format format() const
Definition: rs_frame.hpp:44
float2 pixel_to_texcoord(const rs2_intrinsics *intrin, const float2 &pixel)
Definition: pointcloud.cpp:49
static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics *intrin, const float point[3])
Definition: rsutil.h:19
optional_value< rs2_intrinsics > _depth_intrinsics
Definition: pointcloud.h:41
rs2_extrinsics extr
Definition: test-pose.cpp:258
const rs2_stream_profile * get() const
Definition: rs_frame.hpp:137
std::pair< int, int > pixel
Definition: rs-measure.cpp:18
typename::boost::move_detail::remove_reference< T >::type && move(T &&t) BOOST_NOEXCEPT
Video stream intrinsics.
Definition: rs_types.h:58
GLsizei GLsizei GLchar * source
std::shared_ptr< pointcloud > _registered_auto_calib_cb
Definition: pointcloud.h:60
GLuint res
Definition: glext.h:8856
virtual const float3 * depth_to_points(rs2::points output, const rs2_intrinsics &depth_intrinsics, const rs2::depth_frame &depth_frame, float depth_scale)
Definition: pointcloud.cpp:39
virtual stream_profiles get_active_streams() const =0
rs2_stream stream_type() const
Definition: rs_frame.hpp:39
static const char * occlusion_invalidation
Definition: model-views.h:195
rs2::frame process_frame(const rs2::frame_source &source, const rs2::frame &f) override
Definition: pointcloud.cpp:365
GLint GLsizei width
GLdouble GLdouble GLint GLint const GLdouble * points
std::shared_ptr< rs2_calibration_change_callback > calibration_change_callback_ptr
Definition: src/types.h:1074
T as() const
Definition: rs_frame.hpp:580
std::string to_string(T value)


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:39