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