PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusIntelRealSense.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2 Program: Plus
3 Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
4 See License.txt for details.
5 =========================================================Plus=header=end*/
6 
7 #include "PlusConfigure.h"
9 
10 // Local includes
11 #include "vtkPlusChannel.h"
12 #include "vtkPlusDataSource.h"
13 
14 // IntelRealSense includes
15 #include <rs.hpp>
16 
17 // stl includes
18 #include <vector>
19 
20 // VTK includes
21 #include <vtkImageData.h>
22 #include <vtkObjectFactory.h>
23 
24 
25 //----------------------------------------------------------------------------
27 #define REALSENSE_DEFAULT_FRAME_WIDTH 640
28 #define REALSENSE_DEFAULT_FRAME_HEIGHT 480
29 #define REALSENSE_DEFAULT_FRAME_RATE 30
30 
31 //----------------------------------------------------------------------------
32 class vtkPlusIntelRealSense::vtkInternal
33 {
34 public:
35  vtkPlusIntelRealSense * External;
36 
37  vtkInternal(vtkPlusIntelRealSense* external)
38  : External(external)
39  , Align(nullptr)
40  {
41  }
42 
43  virtual ~vtkInternal()
44  {
45  }
46 
47  struct RSFrameConfig
48  {
49  RSFrameConfig(rs2_stream st, std::string sn, int width, int height, int fr)
50  {
51  this->StreamType = st;
52  this->SourceName = sn;
53  this->Height = height;
54  this->Width = width;
55  this->FrameRate = fr;
56  }
57  rs2_stream StreamType;
58  std::string SourceName;
59  vtkPlusDataSource* Source;
60  unsigned int Height;
61  unsigned int Width;
62  unsigned int FrameRate;
63  };
64 
65  // Configuration parameters
66  bool UseRealSenseColorizer = false;
67  bool AlignDepthStream = false;
68 
69  // Frame setup for RGB & depth
70  std::vector<RSFrameConfig> VideoSources;
71 
72  PlusStatus SetDepthScaleToMm(rs2::device dev);
73  float DepthScaleToMm;
74 
75  rs2::config Config;
76  rs2::pipeline Pipe;
77  rs2::pipeline_profile Profile;
78 
79  // Alignment
80  PlusStatus SetStreamToAlign(const std::vector<rs2::stream_profile>& streams);
81  rs2_stream AlignTo;
82  rs2::align* Align; // rs2::align doesn't have a default constructor, so we must use a pointer
83 };
84 
85 //----------------------------------------------------------------------------
86 PlusStatus vtkPlusIntelRealSense::vtkInternal::SetDepthScaleToMm(rs2::device dev)
87 {
88  // Go over the device's sensors
89  for (rs2::sensor& sensor : dev.query_sensors())
90  {
91  // Check if the sensor if a depth sensor
92  if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
93  {
94  this->DepthScaleToMm = dpt.get_depth_scale();
95  return PLUS_SUCCESS;
96  }
97  }
98  LOG_ERROR("Failed to get depth scale from Intel RealSense.");
99  return PLUS_FAIL;
100 }
101 
102 //----------------------------------------------------------------------------
103 PlusStatus vtkPlusIntelRealSense::vtkInternal::SetStreamToAlign(const std::vector<rs2::stream_profile>& streams)
104 {
105  //Given a vector of streams, we try to find a depth stream and another stream to align depth with.
106  //We prioritize color streams to make the view look better.
107  //If color is not available, we take another stream that (other than depth)
108  rs2_stream align_to = RS2_STREAM_ANY;
109  bool depth_stream_found = false;
110  bool color_stream_found = false;
111  for (rs2::stream_profile sp : streams)
112  {
113  rs2_stream profile_stream = sp.stream_type();
114  if (profile_stream != RS2_STREAM_DEPTH)
115  {
116  if (!color_stream_found) //Prefer color
117  align_to = profile_stream;
118 
119  if (profile_stream == RS2_STREAM_COLOR)
120  {
121  color_stream_found = true;
122  }
123  }
124  else
125  {
126  depth_stream_found = true;
127  }
128  }
129 
130  if (!depth_stream_found)
131  {
132  LOG_ERROR("Failed to align RealSense streams. No Intel RealSense Depth stream available. Please proivde a depth stream, or set AlignDepthStream to FALSE.");
133  return PLUS_FAIL;
134  }
135 
136  if (align_to == RS2_STREAM_ANY)
137  {
138  LOG_ERROR("Failed to align RealSense streams. No Intel RealSense stream found to align with Depth. Please proivde an RGB stream, or set AlignDepthStream to FALSE");
139  return PLUS_FAIL;
140  }
141  this->AlignTo = align_to;
142  return PLUS_SUCCESS;
143 }
144 
145 //----------------------------------------------------------------------------
147  : Internal(new vtkInternal(this))
148 {
150  this->StartThreadForInternalUpdates = true;
153 }
154 
155 //----------------------------------------------------------------------------
157 {
158  delete Internal;
159  Internal = nullptr;
160 }
161 
162 //----------------------------------------------------------------------------
163 void vtkPlusIntelRealSense::PrintSelf(ostream& os, vtkIndent indent)
164 {
165  this->Superclass::PrintSelf(os, indent);
166  // TODO: Re-implement this without hard-coding
167  //os << indent << "Intel RealSense 3d Camera: D415" << std::endl;
168  //os << indent << "RgbDataSourceName: " << RgbDataSourceName << std::endl;
169  //os << indent << "DepthDataSourceName: " << DepthDataSourceName << std::endl;
170 }
171 
172 //-----------------------------------------------------------------------------
173 PlusStatus vtkPlusIntelRealSense::ReadConfiguration(vtkXMLDataElement* rootConfigElement)
174 {
175  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement);
176  XML_READ_BOOL_ATTRIBUTE_NONMEMBER_OPTIONAL(UseRealSenseColorizer, this->Internal->UseRealSenseColorizer, deviceConfig);
177  XML_READ_BOOL_ATTRIBUTE_NONMEMBER_OPTIONAL(AlignDepthStream, this->Internal->AlignDepthStream, deviceConfig);
178 
179  XML_FIND_NESTED_ELEMENT_REQUIRED(dataSourcesElement, deviceConfig, "DataSources");
180  for (int nestedElementIndex = 0; nestedElementIndex < dataSourcesElement->GetNumberOfNestedElements(); nestedElementIndex++)
181  {
182  vtkXMLDataElement* dataElement = dataSourcesElement->GetNestedElement(nestedElementIndex);
183  if (STRCASECMP(dataElement->GetName(), "DataSource") != 0)
184  {
185  // if this is not a data source element, skip it
186  continue;
187  }
188 
189  if (dataElement->GetAttribute("Type") != NULL && STRCASECMP(dataElement->GetAttribute("Type"), "Video") == 0)
190  {
191  // this is a video element
192  // get tool ID
193  const char* toolId = dataElement->GetAttribute("Id");
194  if (toolId == NULL)
195  {
196  // tool doesn't have ID needed to generate transform
197  LOG_ERROR("Failed to initialize IntelRealSense DataSource: Id is missing");
198  continue;
199  }
200 
201  // get Intel RealSense video parameters for this source
202  rs2_stream sourceType;
203  XML_READ_ENUM2_ATTRIBUTE_NONMEMBER_REQUIRED(FrameType, sourceType, dataElement, "RGB", RS2_STREAM_COLOR, "DEPTH", RS2_STREAM_DEPTH);
205  XML_READ_VECTOR_ATTRIBUTE_NONMEMBER_OPTIONAL(int, 2, FrameSize, frameSize, dataElement);
206  int frameRate = REALSENSE_DEFAULT_FRAME_RATE;
207  XML_READ_SCALAR_ATTRIBUTE_NONMEMBER_OPTIONAL(int, FrameRate, frameRate, dataElement);
208  vtkInternal::RSFrameConfig source(sourceType, toolId, frameSize[0], frameSize[1], frameRate);
209  this->Internal->VideoSources.push_back(source);
210  }
211  else
212  {
213  LOG_ERROR("DataSource with unknown Type.");
214  return PLUS_FAIL;
215  }
216  }
217  return PLUS_SUCCESS;
218 }
219 
220 //-----------------------------------------------------------------------------
221 PlusStatus vtkPlusIntelRealSense::WriteConfiguration(vtkXMLDataElement* rootConfigElement)
222 {
223  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement);
224 
225  // write bool UseRealSenseColorizer
226  if (this->Internal->UseRealSenseColorizer)
227  {
228  deviceConfig->SetAttribute("UseRealSenseColorizer", "TRUE");
229  }
230  else
231  {
232  deviceConfig->SetAttribute("UseRealSenseColorizer", "FALSE");
233  }
234 
235  // write bool AlignDepthStream
236  if (this->Internal->AlignDepthStream)
237  {
238  deviceConfig->SetAttribute("AlignDepthStream", "TRUE");
239  }
240  else
241  {
242  deviceConfig->SetAttribute("AlignDepthStream", "FALSE");
243  }
244 
245  return PLUS_SUCCESS;
246 }
247 
248 //----------------------------------------------------------------------------
250 {
251  // configure IntelRealSense streams
252  this->Internal->Config.disable_all_streams();
253  std::vector<vtkInternal::RSFrameConfig>::iterator it;
254  for (it = begin(this->Internal->VideoSources); it != end(this->Internal->VideoSources); it++)
255  {
256  rs2_format format;
257  switch (it->StreamType)
258  {
259  case RS2_STREAM_COLOR:
260  format = RS2_FORMAT_RGB8;
261  break;
262  case RS2_STREAM_DEPTH:
263  format = RS2_FORMAT_Z16;
264  break;
265  }
266 
267  // get Plus data source for this stream
268  GetVideoSource(it->SourceName.c_str(), it->Source);
269 
270  // enable stream on RealSense
271  this->Internal->Config.enable_stream(
272  it->StreamType,
273  it->Width,
274  it->Height,
275  format,
276  it->FrameRate
277  );
278  }
279 
280  // try starting pipeline to check if all stream parameters are OK
281  PlusStatus pipeStartResult = PLUS_SUCCESS;
282  try
283  {
284  this->Internal->Profile = this->Internal->Pipe.start(this->Internal->Config);
285  }
286  catch (rs2::error e)
287  {
288  LOG_ERROR("Failed to start IntelRealSense streams. Check your RealSense camera supports the stream types, resolutions & frame rates you are trying to use. RealSense API gave the error: '"
289  << e.what() << "'");
290  pipeStartResult = PLUS_FAIL;
291  }
292 
293  if (pipeStartResult == PLUS_FAIL)
294  {
295  return PLUS_FAIL;
296  }
297 
298  if (this->Internal->SetDepthScaleToMm(this->Internal->Profile.get_device()) == PLUS_FAIL)
299  {
300  return PLUS_FAIL;
301  }
302 
303  this->Internal->Pipe.stop();
304 
305  return PLUS_SUCCESS;
306 }
307 
308 //----------------------------------------------------------------------------
310 {
311  delete this->Internal->Align;
312  this->Internal->Align = nullptr;
313  return PLUS_SUCCESS;
314 }
315 
316 //----------------------------------------------------------------------------
318 {
319  this->FrameNumber = 0;
320 
321  PlusStatus pipeStartResult = PLUS_SUCCESS;
322  try
323  {
324  this->Internal->Profile = this->Internal->Pipe.start(this->Internal->Config);
325  }
326  catch (rs2::error e)
327  {
328  LOG_ERROR("Failed to start IntelRealSense streams. Check your RealSense camera supports the stream types, resolutions & frame rates you are trying to use. RealSense API gave the error: '"
329  << e.what() << "'");
330  pipeStartResult = PLUS_FAIL;
331  }
332 
333  if (pipeStartResult == PLUS_FAIL)
334  {
335  return PLUS_FAIL;
336  }
337 
338  if (this->Internal->AlignDepthStream)
339  {
340  // setup to align depth frame to color
341  if (this->Internal->SetStreamToAlign(this->Internal->Profile.get_streams()) == PLUS_FAIL)
342  {
343  return PLUS_FAIL;
344  }
345  this->Internal->Align = new rs2::align(this->Internal->AlignTo);
346  }
347 
348  return PLUS_SUCCESS;
349 }
350 
351 //----------------------------------------------------------------------------
353 {
354  this->Internal->Pipe.stop();
355  return PLUS_SUCCESS;
356 }
357 
358 //----------------------------------------------------------------------------
360 {
361  // TODO: Implement some configuration checks here
362  return PLUS_SUCCESS;
363 }
364 
365 //----------------------------------------------------------------------------
367 {
368  // wait for frame
369  rs2::frameset frames = this->Internal->Pipe.wait_for_frames();
370 
371  // if requested, align depth to color
372  if (this->Internal->AlignDepthStream)
373  {
374  frames = this->Internal->Align->process(frames);
375  }
376 
377  // forward video data to PlusDataSource
378  std::vector<vtkInternal::RSFrameConfig>::iterator it;
379  for (it = begin(this->Internal->VideoSources); it != end(this->Internal->VideoSources); it++)
380  {
381  // if source is null, return an error
382  if (it->Source == nullptr)
383  {
384  LOG_WARNING("vtkPlusIntelRealSense::InternalUpdate Unable to grab video source '" << it->SourceName << "'. Skipping frame.");
385  return PLUS_FAIL;
386  }
387 
388  // if this is the first frame, initialize the buffer
389  if (it->Source->GetNumberOfItems() == 0 && it->StreamType == RS2_STREAM_COLOR)
390  {
391  LOG_INFO("setting up color frame");
392  it->Source->SetImageType(US_IMG_RGB_COLOR);
393  it->Source->SetPixelType(VTK_UNSIGNED_CHAR);
394  it->Source->SetNumberOfScalarComponents(3);
395  it->Source->SetInputFrameSize(it->Width, it->Height, 1);
396  }
397  else if (it->Source->GetNumberOfItems() == 0 && it->StreamType == RS2_STREAM_DEPTH)
398  {
399  if (this->Internal->UseRealSenseColorizer)
400  {
401  // depth output is RGB from rs2::colorizer
402  it->Source->SetImageType(US_IMG_RGB_COLOR);
403  it->Source->SetPixelType(VTK_UNSIGNED_CHAR);
404  it->Source->SetNumberOfScalarComponents(3);
405  it->Source->SetInputFrameSize(it->Width, it->Height, 1);
406  }
407  else
408  {
409  // depth output is raw depth data
410  LOG_INFO("setting up depth frame");
411  it->Source->SetImageType(US_IMG_BRIGHTNESS);
412  it->Source->SetPixelType(VTK_TYPE_UINT16);
413  it->Source->SetNumberOfScalarComponents(1);
414  it->Source->SetInputFrameSize(it->Width, it->Height, 1);
415  }
416  }
417 
418  // get frame data
419  rs2::frame frame = frames.first(it->StreamType);
420  if (!frame)
421  {
422  LOG_ERROR("Failed to get IntelRealSense frame");
423  return PLUS_FAIL;
424  }
425 
426  // add frame to PLUS buffer
427  if (it->StreamType == RS2_STREAM_COLOR)
428  {
429  FrameSizeType frameSizeColor = { it->Width, it->Height, 1 };
430  if (it->Source->AddItem((void *)frame.get_data(), it->Source->GetInputImageOrientation(), frameSizeColor, VTK_UNSIGNED_CHAR, 3, US_IMG_RGB_COLOR, 0, this->FrameNumber) == PLUS_FAIL)
431  {
432  LOG_ERROR("vtkPlusIntelRealSense::InternalUpdate Unable to send RGB image. Skipping frame.");
433  return PLUS_FAIL;
434  }
435  }
436  else if (it->StreamType == RS2_STREAM_DEPTH)
437  {
438  FrameSizeType frameSizeDepth = { it->Width, it->Height, 1 };
439  if (this->Internal->UseRealSenseColorizer)
440  {
441  rs2::colorizer color;
442  color.set_option(RS2_OPTION_HISTOGRAM_EQUALIZATION_ENABLED, 1);
443  color.set_option(RS2_OPTION_MIN_DISTANCE, 0.6);
444  color.set_option(RS2_OPTION_MAX_DISTANCE, 1.0);
445  rs2::video_frame vfr = color.colorize(frame);
446  if (it->Source->AddItem((void *)vfr.get_data(), it->Source->GetInputImageOrientation(), frameSizeDepth, VTK_UNSIGNED_CHAR, 3, US_IMG_RGB_COLOR, 0, this->FrameNumber) == PLUS_FAIL)
447  {
448  LOG_ERROR("vtkPlusIntelRealSense::InternalUpdate Unable to send RGB image. Skipping frame.");
449  return PLUS_FAIL;
450  }
451  }
452  else
453  {
454  if (it->Source->AddItem((void *)frame.get_data(), it->Source->GetInputImageOrientation(), frameSizeDepth, VTK_TYPE_UINT16, 1, US_IMG_BRIGHTNESS, 0, this->FrameNumber) == PLUS_FAIL)
455  {
456  LOG_ERROR("vtkPlusIntelRealSense::InternalUpdate Unable to send DEPTH image. Skipping frame.");
457  return PLUS_FAIL;
458  }
459  }
460  }
461  }
462 
463  this->FrameNumber++;
464  return PLUS_SUCCESS;
465 }
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
virtual PlusStatus InternalConnect()
const char * source
Definition: phidget22.h:2461
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement)
igsioStatus PlusStatus
Definition: PlusCommon.h:40
#define REALSENSE_DEFAULT_FRAME_RATE
DataSourceContainer VideoSources
bool RequireImageOrientationInConfiguration
double AcquisitionRate
#define PLUS_FAIL
Definition: PlusCommon.h:43
Interface class to Intel RealSense cameras.
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
virtual PlusStatus NotifyConfigured()
double InternalUpdateRate
virtual PlusStatus ReadConfiguration(vtkXMLDataElement *config)
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement)
PhidgetLCD_Font int * width
Definition: phidget22.h:4275
#define REALSENSE_DEFAULT_FRAME_HEIGHT
bool StartThreadForInternalUpdates
PhidgetLCD_Font int int * height
Definition: phidget22.h:4275
virtual void PrintSelf(ostream &os, vtkIndent indent)
vtkStandardNewMacro(vtkPlusIntelRealSense)
#define REALSENSE_DEFAULT_FRAME_WIDTH
virtual PlusStatus InternalDisconnect()
PlusStatus GetVideoSource(const char *aSourceId, vtkPlusDataSource *&aVideoSource)
virtual PlusStatus InternalUpdate()
PlusStatus WriteConfiguration(vtkXMLDataElement *config)
Interface to a 3D positioning tool, video source, or generalized data stream.