CameraK4A.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25  */
26 
27 #include <rtabmap/core/camera/CameraK4A.h>
28 #include <rtabmap/utilite/UTimer.h>
29 #include <rtabmap/utilite/UThreadC.h>
30 #include <rtabmap/core/util2d.h>
31 #include <rtabmap/core/Compression.h>
32 #include <opencv2/imgproc/types_c.h>
33 
34 #ifdef RTABMAP_K4A
35 #include <k4a/k4a.h>
36 #include <k4arecord/playback.h>
37 #endif
38 
39 namespace rtabmap
40 {
41 
42 bool CameraK4A::available()
43 {
44 #ifdef RTABMAP_K4A
45  return true;
46 #else
47  return false;
48 #endif
49 }
50 
51 CameraK4A::CameraK4A(
52  int deviceId,
53  float imageRate,
54  const Transform & localTransform) :
55  Camera(imageRate, localTransform)
56 #ifdef RTABMAP_K4A
57  ,
58  deviceHandle_(NULL),
59  config_(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
60  transformationHandle_(NULL),
61  captureHandle_(NULL),
62  playbackHandle_(NULL),
63  deviceId_(deviceId),
64  rgb_resolution_(0),
65  framerate_(2),
66  depth_resolution_(2),
67  ir_(false),
68  previousStamp_(0.0)
69 #endif
70 {
71 }
72 
73 CameraK4A::CameraK4A(
74  const std::string & fileName,
75  float imageRate,
76  const Transform & localTransform) :
77  Camera(imageRate, localTransform)
78 #ifdef RTABMAP_K4A
79  ,
80  deviceHandle_(NULL),
81  transformationHandle_(NULL),
82  captureHandle_(NULL),
83  playbackHandle_(NULL),
84  deviceId_(-1),
85  fileName_(fileName),
86  rgb_resolution_(0),
87  framerate_(2),
88  depth_resolution_(2),
89  ir_(false),
90  previousStamp_(0.0)
91 #endif
92 {
93 }
94 
95 CameraK4A::~CameraK4A()
96 {
97  close();
98 }
99 
100 void CameraK4A::close()
101 {
102 #ifdef RTABMAP_K4A
103  if (playbackHandle_ != NULL)
104  {
105  k4a_playback_close((k4a_playback_t)playbackHandle_);
106  playbackHandle_ = NULL;
107  }
108  else if (deviceHandle_ != NULL)
109  {
110  k4a_device_stop_imu(deviceHandle_);
111 
112  k4a_device_stop_cameras(deviceHandle_);
113  k4a_device_close(deviceHandle_);
114  deviceHandle_ = NULL;
115  config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
116  }
117 
118  if (transformationHandle_ != NULL)
119  {
120  k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
121  transformationHandle_ = NULL;
122  }
123 #endif
124 }
125 
126 void CameraK4A::setPreferences(int rgb_resolution, int framerate, int depth_resolution)
127 {
128 #ifdef RTABMAP_K4A
129  rgb_resolution_ = rgb_resolution;
130  framerate_ = framerate;
131  depth_resolution_ = depth_resolution;
132  UINFO("setPreferences(): %i %i %i", rgb_resolution, framerate, depth_resolution);
133 #endif
134 }
135 
136 void CameraK4A::setIRDepthFormat(bool enabled)
137 {
138 #ifdef RTABMAP_K4A
139  ir_ = enabled;
140 #endif
141 }
142 
143 bool CameraK4A::init(const std::string & calibrationFolder, const std::string & cameraName)
144 {
145 #ifdef RTABMAP_K4A
146 
147  if (!fileName_.empty())
148  {
149  close();
150 
151  if (k4a_playback_open(fileName_.c_str(), (k4a_playback_t*)&playbackHandle_) != K4A_RESULT_SUCCEEDED)
152  {
153  UERROR("Failed to open recording \"%s\"", fileName_.c_str());
154  return false;
155  }
156 
157  uint64_t recording_length = k4a_playback_get_last_timestamp_usec((k4a_playback_t)playbackHandle_);
158  UINFO("Recording is %lld seconds long", recording_length / 1000000);
159 
160  if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration_))
161  {
162  UERROR("Failed to get calibration");
163  close();
164  return false;
165  }
166 
167  }
168  else if (deviceId_ >= 0)
169  {
170  if(deviceHandle_!=NULL)
171  {
172  this->close();
173  }
174 
175  switch(rgb_resolution_)
176  {
177  case 0: config_.color_resolution = K4A_COLOR_RESOLUTION_720P; break;
178  case 1: config_.color_resolution = K4A_COLOR_RESOLUTION_1080P; break;
179  case 2: config_.color_resolution = K4A_COLOR_RESOLUTION_1440P; break;
180  case 3: config_.color_resolution = K4A_COLOR_RESOLUTION_1536P; break;
181  case 4: config_.color_resolution = K4A_COLOR_RESOLUTION_2160P; break;
182  case 5:
183  default: config_.color_resolution = K4A_COLOR_RESOLUTION_3072P; break;
184  }
185 
186  switch(framerate_)
187  {
188  case 0: config_.camera_fps = K4A_FRAMES_PER_SECOND_5; break;
189  case 1: config_.camera_fps = K4A_FRAMES_PER_SECOND_15; break;
190  case 2:
191  default: config_.camera_fps = K4A_FRAMES_PER_SECOND_30; break;
192  }
193 
194  switch(depth_resolution_)
195  {
196  case 0: config_.depth_mode = K4A_DEPTH_MODE_NFOV_2X2BINNED; break;
197  case 1: config_.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED; break;
198  case 2: config_.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED; break;
199  case 3:
200  default: config_.depth_mode = K4A_DEPTH_MODE_WFOV_UNBINNED; break;
201  }
202 
203  // This is fixed for now
204  config_.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
205 
206  int device_count = k4a_device_get_installed_count();
207 
208  if (device_count == 0)
209  {
210  UERROR("No k4a devices attached!");
211  return false;
212  }
213  else if(deviceId_ > device_count)
214  {
215  UERROR("Cannot select device %d, only %d devices detected.", deviceId_, device_count);
216  }
217 
218  UINFO("CameraK4A found %d k4a device(s) attached", device_count);
219 
220  // Open the first plugged in Kinect device
221  if (K4A_FAILED(k4a_device_open(deviceId_, &deviceHandle_)))
222  {
223  UERROR("Failed to open k4a device!");
224  return false;
225  }
226 
227  // Get the size of the serial number
228  size_t serial_size = 0;
229  k4a_device_get_serialnum(deviceHandle_, NULL, &serial_size);
230 
231  // Allocate memory for the serial, then acquire it
232  char *serial = (char*)(malloc(serial_size));
233  k4a_device_get_serialnum(deviceHandle_, serial, &serial_size);
234  serial_number_.assign(serial, serial_size);
235  free(serial);
236 
237  UINFO("Opened K4A device: %s", serial_number_.c_str());
238 
239  // Start the camera with the given configuration
240  if (K4A_FAILED(k4a_device_start_cameras(deviceHandle_, &config_)))
241  {
242  UERROR("Failed to start cameras!");
243  close();
244  return false;
245  }
246 
247  UINFO("K4A camera started successfully");
248 
249  if (K4A_FAILED(k4a_device_get_calibration(deviceHandle_, config_.depth_mode, config_.color_resolution, &calibration_)))
250  {
251  UERROR("k4a_device_get_calibration() failed!");
252  close();
253  return false;
254  }
255  }
256  else
257  {
258  UERROR("k4a_device_get_calibration() no file and no valid device id!");
259  return false;
260  }
261 
262  if (ir_)
263  {
264  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
265  K.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fx;
266  K.at<double>(1,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fy;
267  K.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cx;
268  K.at<double>(1,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cy;
269  cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
270  D.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k1;
271  D.at<double>(0,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k2;
272  D.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p1;
273  D.at<double>(0,3) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p2;
274  D.at<double>(0,4) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k3;
275  D.at<double>(0,5) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k4;
276  D.at<double>(0,6) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k5;
277  D.at<double>(0,7) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k6;
278  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
279  cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
280  P.at<double>(0,0) = K.at<double>(0,0);
281  P.at<double>(1,1) = K.at<double>(1,1);
282  P.at<double>(0,2) = K.at<double>(0,2);
283  P.at<double>(1,2) = K.at<double>(1,2);
284  model_ = CameraModel(
285  "k4a_ir",
286  cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height),
287  K,D,R,P,
288  this->getLocalTransform());
289  UASSERT(model_.isValidForRectification());
290  model_.initRectificationMap();
291  }
292  else
293  {
294  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
295  K.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.fx;
296  K.at<double>(1,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.fy;
297  K.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cx;
298  K.at<double>(1,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cy;
299  cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
300  D.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.k1;
301  D.at<double>(0,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.k2;
302  D.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.p1;
303  D.at<double>(0,3) = calibration_.color_camera_calibration.intrinsics.parameters.param.p2;
304  D.at<double>(0,4) = calibration_.color_camera_calibration.intrinsics.parameters.param.k3;
305  D.at<double>(0,5) = calibration_.color_camera_calibration.intrinsics.parameters.param.k4;
306  D.at<double>(0,6) = calibration_.color_camera_calibration.intrinsics.parameters.param.k5;
307  D.at<double>(0,7) = calibration_.color_camera_calibration.intrinsics.parameters.param.k6;
308  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
309  cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
310  P.at<double>(0,0) = K.at<double>(0,0);
311  P.at<double>(1,1) = K.at<double>(1,1);
312  P.at<double>(0,2) = K.at<double>(0,2);
313  P.at<double>(1,2) = K.at<double>(1,2);
314  model_ = CameraModel(
315  "k4a_color",
316  cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height),
317  K,D,R,P,
318  this->getLocalTransform());
319  }
320 
321  if (ULogger::level() <= ULogger::kInfo)
322  {
323  UINFO("K4A calibration:");
324  std::cout << model_ << std::endl;
325  }
326 
327  transformationHandle_ = k4a_transformation_create(&calibration_);
328 
329  // Get imu transform
330  k4a_calibration_extrinsics_t* imu_extrinsics;
331  if(ir_)
332  {
333  imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
334  }
335  else
336  {
337  imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
338  }
339  imuLocalTransform_ = Transform(
340  imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
341  imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
342  imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
343 
344  UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
345  UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
346  imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
347  UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
348 
349 
350  // Start playback or camera
351  if (!fileName_.empty())
352  {
353  k4a_record_configuration_t config;
354  if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
355  {
356  UERROR("Failed to getting recording configuration");
357  close();
358  return false;
359  }
360  }
361  else
362  {
363  if (K4A_FAILED(k4a_device_start_imu(deviceHandle_)))
364  {
365  UERROR("Failed to start K4A IMU");
366  close();
367  return false;
368  }
369 
370  UINFO("K4a IMU started successfully");
371 
372  // Get an initial capture to put the camera in the right state
373  if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE))
374  {
375  k4a_capture_release(captureHandle_);
376  return true;
377  }
378 
379  close();
380  return false;
381  }
382  return true;
383 #else
384  UERROR("CameraK4A: RTAB-Map is not built with Kinect for Azure SDK support!");
385  return false;
386 #endif
387 }
388 
389 bool CameraK4A::isCalibrated() const
390 {
391  return true;
392 }
393 
394 std::string CameraK4A::getSerial() const
395 {
396 #ifdef RTABMAP_K4A
397  if(!fileName_.empty())
398  {
399  return fileName_;
400  }
401  return(serial_number_);
402 #else
403  return "";
404 #endif
405 }
406 
407 SensorData CameraK4A::captureImage(CameraInfo * info)
408 {
409  SensorData data;
410 
411 #ifdef RTABMAP_K4A
412 
413  k4a_image_t ir_image_ = NULL;
414  k4a_image_t rgb_image_ = NULL;
415  k4a_imu_sample_t imu_sample_;
416 
417  double t = UTimer::now();
418 
419  bool captured = false;
420  if(playbackHandle_)
421  {
422  k4a_stream_result_t result = K4A_STREAM_RESULT_FAILED;
423  while((UTimer::now()-t < 0.1) &&
424  (K4A_STREAM_RESULT_SUCCEEDED != (result=k4a_playback_get_next_capture(playbackHandle_, &captureHandle_)) ||
425  ((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
426  {
427  k4a_capture_release(captureHandle_);
428  // the first frame may be null, just retry for 1 second
429  }
430  if (result == K4A_STREAM_RESULT_EOF)
431  {
432  // End of file reached
433  UINFO("End of file reached");
434  }
435  else if (result == K4A_STREAM_RESULT_FAILED)
436  {
437  UERROR("Failed to read entire recording");
438  }
439  captured = result == K4A_STREAM_RESULT_SUCCEEDED;
440  }
441  else // device
442  {
443  k4a_wait_result_t result = K4A_WAIT_RESULT_FAILED;
444  while((UTimer::now()-t < 5.0) &&
445  (K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE)) ||
446  ((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
447  {
448  k4a_capture_release(captureHandle_);
449  // the first frame may be null, just retry for 5 seconds
450  }
451  captured = result == K4A_WAIT_RESULT_SUCCEEDED;
452  }
453 
454  if (captured && (rgb_image_!=NULL || ir_image_!=NULL))
455  {
456  cv::Mat bgrCV;
457  cv::Mat depthCV;
458  IMU imu;
459 
460  if (ir_image_ != NULL)
461  {
462  // Convert IR image
463  cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
464  k4a_image_get_width_pixels(ir_image_),
465  CV_16UC1,
466  (void*)k4a_image_get_buffer(ir_image_));
467 
468  bgrCV16.convertTo(bgrCV, CV_8U);
469 
470  bgrCV = model_.rectifyImage(bgrCV);
471 
472  // Release the image
473  k4a_image_release(ir_image_);
474  }
475  else
476  {
477  // Convert RGB image
478  if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
479  {
480  bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
481  CV_8UC1,
482  (void*)k4a_image_get_buffer(rgb_image_)));
483  }
484  else
485  {
486  cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
487  k4a_image_get_width_pixels(rgb_image_),
488  CV_8UC4,
489  (void*)k4a_image_get_buffer(rgb_image_));
490 
491  cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
492  }
493 
494  // Release the image
495  k4a_image_release(rgb_image_);
496  }
497 
498  double stamp = UTimer::now();
499  if(!bgrCV.empty())
500  {
501  // Retrieve depth image from capture
502  k4a_image_t depth_image_ = k4a_capture_get_depth_image(captureHandle_);
503 
504  if (depth_image_ != NULL)
505  {
506  stamp = ((double)k4a_image_get_timestamp_usec(depth_image_)) / 1000000;
507 
508  if (ir_)
509  {
510  depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
511  k4a_image_get_width_pixels(depth_image_),
512  CV_16UC1,
513  (void*)k4a_image_get_buffer(depth_image_));
514 
515  depthCV = model_.rectifyDepth(depthCV);
516  }
517  else
518  {
519  k4a_image_t transformedDepth = NULL;
520  if (k4a_image_create(k4a_image_get_format(depth_image_),
521  bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
522  {
523  if(k4a_transformation_depth_image_to_color_camera(transformationHandle_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
524  {
525  depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
526  k4a_image_get_width_pixels(transformedDepth),
527  CV_16UC1,
528  (void*)k4a_image_get_buffer(transformedDepth)).clone();
529  }
530  else
531  {
532  UERROR("K4A failed to register depth image");
533  }
534 
535  k4a_image_release(transformedDepth);
536  }
537  else
538  {
539  UERROR("K4A failed to allocate registered depth image");
540  }
541  }
542  k4a_image_release(depth_image_);
543  }
544  }
545 
546  k4a_capture_release(captureHandle_);
547 
548  if(playbackHandle_)
549  {
550  // Get IMU sample, clear buffer
551  // FIXME: not tested, uncomment when tested.
552  k4a_playback_seek_timestamp(playbackHandle_, stamp* 1000000+1, K4A_PLAYBACK_SEEK_BEGIN);
553  if(K4A_STREAM_RESULT_SUCCEEDED == k4a_playback_get_previous_imu_sample(playbackHandle_, &imu_sample_))
554  {
555  double stmp = ((double)imu_sample_.acc_timestamp_usec) / 1000000;
556  imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
557  cv::Mat::eye(3, 3, CV_64FC1),
558  cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
559  cv::Mat::eye(3, 3, CV_64FC1),
560  imuLocalTransform_);
561  }
562  else
563  {
564  UWARN("IMU data NULL");
565  }
566  }
567  else
568  {
569  // Get IMU sample, clear buffer
570  if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(deviceHandle_, &imu_sample_, 60))
571  {
572  imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
573  cv::Mat::eye(3, 3, CV_64FC1),
574  cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
575  cv::Mat::eye(3, 3, CV_64FC1),
576  imuLocalTransform_);
577  }
578  else
579  {
580  UERROR("IMU data NULL");
581  }
582  }
583 
584  // Relay the data to rtabmap
585  if (!bgrCV.empty() && !depthCV.empty())
586  {
587  data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
588  if(!imu.empty())
589  {
590  data.setIMU(imu);
591  }
592 
593  // Frame rate
594  if (playbackHandle_ && this->getImageRate() < 0.0f)
595  {
596  if (stamp == 0)
597  {
598  UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
599  }
600  else if (previousStamp_ > 0)
601  {
602  float ratio = -this->getImageRate();
603  int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
604  if (sleepTime > 10000)
605  {
606  UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
607  sleepTime / 1000, previousStamp_, stamp);
608  sleepTime = 10000;
609  }
610  if (sleepTime > 2)
611  {
612  uSleep(sleepTime - 2);
613  }
614 
615  // Add precision at the cost of a small overhead
616  while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
617  {
618  //
619  }
620 
621  double slept = timer_.getElapsedTime();
622  timer_.start();
623  UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
624  }
625  previousStamp_ = stamp;
626  }
627  }
628  }
629 #else
630  UERROR("CameraK4A: RTAB-Map is not built with Kinect for Azure SDK support!");
631 #endif
632  return data;
633 }
634 
635 } // namespace rtabmap
NULL
#define NULL
rtabmap::CameraK4A::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraK4A.cpp:407
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:63
f
f
rtabmap::CameraK4A::~CameraK4A
virtual ~CameraK4A()
Definition: CameraK4A.cpp:95
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:64
config
config
rtabmap::CameraK4A::isCalibrated
virtual bool isCalibrated() const
Definition: CameraK4A.cpp:389
UASSERT
#define UASSERT(condition)
glm::uint64_t
detail::uint64 uint64_t
Definition: fwd.hpp:920
rtabmap::CameraK4A::setIRDepthFormat
void setIRDepthFormat(bool enabled)
Definition: CameraK4A.cpp:136
rtabmap::CameraK4A::setPreferences
void setPreferences(int rgb_resolution, int framerate, int depth_resolution)
Definition: CameraK4A.cpp:126
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
rtabmap::CameraK4A::CameraK4A
CameraK4A(int deviceId=0, float imageRate=0.0f, const Transform &localTransform=CameraModel::opticalRotation())
Definition: CameraK4A.cpp:51
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:63
rtabmap::Camera::getNextSeqID
int getNextSeqID()
Definition: Camera.h:84
rtabmap::CameraK4A::getSerial
virtual std::string getSerial() const
Definition: CameraK4A.cpp:394
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
UWARN
#define UWARN(...)
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraK4A::available
static bool available()
Definition: CameraK4A.cpp:42
rtabmap::SensorData::setIMU
void setIMU(const IMU &imu)
Definition: SensorData.h:268
rtabmap::CameraK4A::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraK4A.cpp:143
rtabmap::uncompressImage
cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat &bytes)
Definition: Compression.cpp:130
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58