capture.cpp
Go to the documentation of this file.
1 #pragma warning (disable :4996)
2 #undef _CRT_SECURE_NO_DEPRECATE
3 #include <PS1080.h> // For XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE property
4 #include <io/capture.hpp>
5 
6 using namespace std;
7 using namespace openni;
8 
9 #define REPORT_ERROR(msg) kfusion::cuda::error ((msg), __FILE__, __LINE__)
10 
11 double
12 calculateFocalLength (int pixelWidth, double fov)
13 {
14  return pixelWidth / (2.0 * tan (fov / 2.0));
15 }
16 
18 {
19  openni::Device device;
20  openni::VideoStream depthStream;
21  openni::VideoStream colorStream;
22  openni::VideoFrameRef depthFrame;
23  openni::VideoFrameRef colorFrame;
24  char strError[1024];
25  bool has_depth;
26  bool has_image;
27 };
28 
30  : depth_focal_length_VGA (0.f)
31  , baseline (0.f)
32  , shadow_value (0)
33  , no_sample_value (0)
34  , pixelSize (0.0)
35  , max_depth (0)
36  , maxFrameIndex_(1000000)
37  , recording_(false)
38  , rec_count_(0)
39 {}
40 
41 kfusion::OpenNISource::OpenNISource(int device) {open (device); }
44 
45 void kfusion::OpenNISource::open (int device)
46 {
47  open(""); // An empty string triggers openni::ANY_DEVICE
48 }
49 
50 // uri can be a device ID or filename
51 void kfusion::OpenNISource::open(const std::string& filename)
52 {
53  isOni_ = false;
54  impl_ = cv::Ptr<Impl>( new Impl () );
55 
56  openni::Status rc;
57 
58  rc = openni::OpenNI::initialize ();
59  if (rc != openni::STATUS_OK)
60  {
61  const string error(openni::OpenNI::getExtendedError());
62  sprintf (impl_->strError, "Init failed: %s\n", error.c_str() );
63  REPORT_ERROR (impl_->strError);
64  }
65 
66  if (filename.length() > 0)
67  {
68  isOni_ = true;
69  rc = impl_->device.open( filename.c_str() );
70  }
71  else
72  rc = impl_->device.open(openni::ANY_DEVICE);
73 
74  if (rc != openni::STATUS_OK)
75  {
76  const string error(openni::OpenNI::getExtendedError());
77  sprintf (impl_->strError, "Device open failed: %s\n", error.c_str() );
78  REPORT_ERROR (impl_->strError);
79  }
80 
81  impl_->has_image = impl_->device.hasSensor(SENSOR_COLOR);
82  impl_->has_depth = impl_->device.hasSensor(SENSOR_DEPTH);
83 
84  rc = impl_->device.setDepthColorSyncEnabled(true);
85  if (rc != openni::STATUS_OK)
86  {
87  sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() );
88  REPORT_ERROR (impl_->strError);
89  }
90 
91  rc = impl_->colorStream.create(impl_->device, openni::SENSOR_COLOR);
92  if (rc != openni::STATUS_OK)
93  {
94  sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() );
95  REPORT_ERROR (impl_->strError);
96  }
97 
98  rc = impl_->depthStream.create(impl_->device, openni::SENSOR_DEPTH);
99  if (rc != openni::STATUS_OK)
100  {
101  sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() );
102  REPORT_ERROR (impl_->strError);
103  }
104 
105  if(isOni_)
106  {
107  impl_->device.getPlaybackControl()->setRepeatEnabled(false);
108  maxFrameIndex_ = impl_->device.getPlaybackControl()->getNumberOfFrames(impl_->depthStream) - 10;
109  }
110  else
111  {
112  openni::VideoMode colorMode;
113  colorMode.setResolution (1280, 1024);
114  colorMode.setFps(30);
115  colorMode.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
116 
117  openni::VideoMode depthMode;
118  depthMode.setResolution (640, 480);
119  depthMode.setFps(30);
120  //depthMode.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
121  depthMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
122 
123  rc = impl_->colorStream.setVideoMode(colorMode);
124  if (rc != openni::STATUS_OK)
125  {
126  sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() );
127  REPORT_ERROR (impl_->strError);
128  }
129 
130  rc = impl_->depthStream.setVideoMode(depthMode);
131  if (rc != openni::STATUS_OK)
132  {
133  sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() );
134  REPORT_ERROR (impl_->strError);
135  }
136  Status rc = rec_.create(string("./Captured" + std::to_string(rec_count_++) + ".oni").c_str());
137  if(rc != STATUS_OK)
138  {
139  printf ("Error creating Record object.\n");
140  return;
141  }
142  rec_.attach(impl_->depthStream);
143  rec_.attach(impl_->colorStream);
144  }
145 
146  getParams ();
147 
148  rc = impl_->colorStream.start();
149  if (rc != openni::STATUS_OK)
150  {
151  sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() );
152  REPORT_ERROR (impl_->strError);
153  }
154 
155  rc = impl_->depthStream.start();
156  if (rc != openni::STATUS_OK)
157  {
158  sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() );
159  REPORT_ERROR (impl_->strError);
160  }
161 
162 }
163 
165 {
166  double speed = impl_->device.getPlaybackControl()->getSpeed();
167  if(isOni_ && speed == 1.0)
168  {
169  impl_->device.getPlaybackControl()->setSpeed(-1.0);
170  }
171  else if (isOni_ && speed == -1.0)
172  {
173  impl_->device.getPlaybackControl()->setSpeed(1.0);
174  }
175 }
176 
177 
179 {
180  if(!isOni_ && !recording_)
181  {
182  rec_.start();
183  recording_ = true;
184  printf ("Start recording.\n");
185  }
186  else if(!isOni_ && recording_)
187  {
188  rec_.stop();
189  rec_.destroy();
190  recording_ = false;
191  Status rc = rec_.create(string("./Captured" + std::to_string(rec_count_++) + ".oni").c_str());
192  if(rc != STATUS_OK)
193  {
194  printf ("Error creating Record object.\n");
195  return;
196  }
197  rec_.attach(impl_->depthStream);
198  rec_.attach(impl_->colorStream);
199  printf ("Stop recording.\n");
200  }
201 }
202 
204 {
205  if (impl_)
206  {
207  impl_->colorStream.stop();
208  impl_->colorStream.destroy();
209 
210  impl_->depthStream.stop();
211  impl_->depthStream.destroy();
212 
213  impl_->device.close();
214  }
215  if(recording_)
216  {
217  rec_.stop();
218  rec_.destroy();
219  recording_ = false;
220  printf ("Stop recording.\n");
221  }
222 
223  impl_.release();
224  depth_focal_length_VGA = 0;
225  baseline = 0.f;
226  shadow_value = 0;
227  no_sample_value = 0;
228  pixelSize = 0.0;
229 }
230 
231 int kfusion::OpenNISource::grab(cv::Mat& depth, cv::Mat& color)
232 {
233  Status rc = STATUS_OK;
234  if (impl_->has_depth)
235  {
236  rc = impl_->depthStream.readFrame(&impl_->depthFrame);
237  if (rc != openni::STATUS_OK)
238  {
239  sprintf (impl_->strError, "Frame grab failed: %s\n", openni::OpenNI::getExtendedError() );
240  REPORT_ERROR (impl_->strError);
241  }
242 
243  const void* pDepth = impl_->depthFrame.getData();
244  int x = impl_->depthFrame.getWidth();
245  int y = impl_->depthFrame.getHeight();
246  cv::Mat(y, x, CV_16U, (void*)pDepth).copyTo(depth);
247  }
248  else
249  {
250  depth.release();
251  printf ("no depth\n");
252  }
253 
254  if (impl_->has_image)
255  {
256  rc = impl_->colorStream.readFrame(&impl_->colorFrame);
257  if (rc != openni::STATUS_OK)
258  {
259  sprintf (impl_->strError, "Frame grab failed: %s\n", openni::OpenNI::getExtendedError() );
260  REPORT_ERROR (impl_->strError);
261  }
262 
263  const void* pColor = impl_->colorFrame.getData();
264  int x = impl_->colorFrame.getWidth();
265  int y = impl_->colorFrame.getHeight();
266  cv::Mat(y, x, CV_8UC3, (void*)pColor).copyTo(color);
267  cv::cvtColor(color, color, cv::COLOR_RGB2BGR);
268  }
269  else
270  {
271  color.release();
272  printf ("no color\n");
273  }
274 
275  int frame = impl_->depthFrame.getFrameIndex();
276  if(frame > maxFrameIndex_)
277  return 2;
278 
279  return impl_->has_image || impl_->has_depth;
280 }
281 
283 {
284  openni::Status rc = STATUS_OK;
285 
286  max_depth = impl_->depthStream.getMaxPixelValue();
287 
288  double baseline_local = 0; // in mm
289  if ( impl_->depthStream.isPropertySupported (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE) )
290  {
291  rc = impl_->depthStream.getProperty (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline_local); // Device specific -- from PS1080.h
292  }
293  else
294  printf ("Baseline not available.\n");
295 
296  shadow_value = 0;
297  no_sample_value = 0;
298 
299  // baseline from cm -> mm
300  baseline = (float)(baseline_local * 10);
301 
302  //focal length
303  int width = impl_->colorStream.getVideoMode().getResolutionX();
304  float hFov = impl_->colorStream.getHorizontalFieldOfView();
305  depth_focal_length_VGA = (float) calculateFocalLength( width, hFov);
306 }
307 
309 {
310  Status rc = STATUS_OK;
311 
312  if(value) // "on"
313  {
314  rc = impl_->device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
315  if (rc != openni::STATUS_OK)
316  {
317  sprintf (impl_->strError, "Setting registration failed: %s\n", openni::OpenNI::getExtendedError() );
318  REPORT_ERROR (impl_->strError);
319  }
320  }
321  else // "off"
322  {
323  rc = impl_->device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF);
324  if (rc != openni::STATUS_OK)
325  {
326  sprintf (impl_->strError, "Setting registration failed: %s\n", openni::OpenNI::getExtendedError() );
327  REPORT_ERROR (impl_->strError);
328  }
329  }
330 
331  getParams ();
332  return rc == STATUS_OK;
333 }
kfusion::OpenNISource::release
void release()
Definition: capture.cpp:203
kfusion::OpenNISource::Impl::depthStream
openni::VideoStream depthStream
Definition: capture.cpp:20
REPORT_ERROR
#define REPORT_ERROR(msg)
Definition: capture.cpp:9
kfusion::OpenNISource::grab
int grab(cv::Mat &depth, cv::Mat &image)
Definition: capture.cpp:231
kfusion::OpenNISource::Impl::has_image
bool has_image
Definition: capture.cpp:26
kfusion::OpenNISource::Impl::device
openni::Device device
Definition: capture.cpp:19
kfusion::OpenNISource::setRegistration
bool setRegistration(bool value=false)
Definition: capture.cpp:308
kfusion::OpenNISource::Impl
Definition: capture.cpp:17
kfusion::OpenNISource::~OpenNISource
~OpenNISource()
Definition: capture.cpp:43
kfusion::OpenNISource::triggerPause
void triggerPause()
Definition: capture.cpp:164
kfusion::OpenNISource::Impl::depthFrame
openni::VideoFrameRef depthFrame
Definition: capture.cpp:22
capture.hpp
kfusion::OpenNISource::Impl::colorFrame
openni::VideoFrameRef colorFrame
Definition: capture.cpp:23
scripts.normalize_multiple.filename
filename
Definition: normalize_multiple.py:60
kfusion::OpenNISource::getParams
void getParams()
Definition: capture.cpp:282
kfusion::OpenNISource::Impl::has_depth
bool has_depth
Definition: capture.cpp:25
std
Definition: HalfEdge.hpp:124
kfusion::OpenNISource::Impl::colorStream
openni::VideoStream colorStream
Definition: capture.cpp:21
calculateFocalLength
double calculateFocalLength(int pixelWidth, double fov)
Definition: capture.cpp:12
kfusion::OpenNISource::OpenNISource
OpenNISource()
Definition: capture.cpp:29
kfusion::cuda::error
KF_EXPORTS void error(const char *error_string, const char *file, const int line, const char *func="")
Error handler. All GPU functions from this subsystem call the function to report an error....
Definition: device_memory.cpp:7
kfusion::OpenNISource::triggerRecord
void triggerRecord()
Definition: capture.cpp:178
kfusion::OpenNISource::open
void open(int device)
Definition: capture.cpp:45
lvr2::hdf5util::open
std::shared_ptr< HighFive::File > open(const std::string &filename)
Definition: Hdf5Util.cpp:202


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:22