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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07