MRPT  2.0.2
CKinect.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/hwdrivers/CKinect.h>
13 #include <mrpt/img/TStereoCamera.h>
14 #include <mrpt/poses/CPose3DQuat.h>
16 
17 // Universal include for all versions of OpenCV
18 #include <mrpt/3rdparty/do_opencv_includes.h>
19 
20 using namespace mrpt::hwdrivers;
21 using namespace mrpt::system;
22 using namespace mrpt::serialization;
23 using namespace mrpt::math;
24 using namespace mrpt::obs;
25 using namespace mrpt::poses;
26 using namespace std;
27 
29 
30 // Whether to profile memory allocations:
31 //#define KINECT_PROFILE_MEM_ALLOC
32 
33 #if MRPT_HAS_KINECT_FREENECT
34 #if defined(__APPLE__)
35 #include <libfreenect/libfreenect.h>
36 #else
37 #include <libfreenect.h>
38 #endif
39 #else
40 #define KINECT_W 640
41 #define KINECT_H 480
42 #endif
43 
44 #if MRPT_HAS_KINECT_FREENECT
45 // Macros to convert the opaque pointers in the class header:
46 #define f_ctx reinterpret_cast<freenect_context*>(m_f_ctx)
47 #define f_ctx_ptr reinterpret_cast<freenect_context**>(&m_f_ctx)
48 #define f_dev reinterpret_cast<freenect_device*>(m_f_dev)
49 #define f_dev_ptr reinterpret_cast<freenect_device**>(&m_f_dev)
50 #endif // MRPT_HAS_KINECT_FREENECT
51 
52 #ifdef KINECT_PROFILE_MEM_ALLOC
54 #endif
55 // int int a;
56 
58 {
59 #ifdef MRPT_KINECT_DEPTH_10BIT
60  const float k1 = 1.1863f;
61  const float k2 = 2842.5f;
62  const float k3 = 0.1236f;
63 
64  for (size_t i = 0; i < KINECT_RANGES_TABLE_LEN; i++)
65  m_range2meters[i] =
66  static_cast<uint16_t>(k3 * tanf(i / k2 + k1) / 1e-3f);
67 
68 #else
69  for (size_t i = 0; i < KINECT_RANGES_TABLE_LEN; i++)
70  m_range2meters[i] =
71  static_cast<uint16_t>(1e+3f / (i * (-0.0030711016) + 3.3309495161));
72 #endif
73 
74  // Minimum/Maximum range means error:
75  m_range2meters[0] = 0;
76  m_range2meters[KINECT_RANGES_TABLE_LEN - 1] = 0;
77 }
78 
79 /*-------------------------------------------------------------
80  ctor
81  -------------------------------------------------------------*/
83  : m_sensorPoseOnRobot(),
84  m_relativePoseIntensityWRTDepth(
85  0, -0.02, 0, -90.0_deg, 0.0_deg, -90.0_deg)
86 {
88 
89  // Get maximum range:
90  // Recall: r[Max-1] means error.
92 
93  // Default label:
94  m_sensorLabel = "KINECT";
95 
96  // =========== Default params ===========
97  // ----- RGB -----
99  640; // By default set 640x480, on connect we'll update this.
100  m_cameraParamsRGB.nrows = 480;
101 
102  m_cameraParamsRGB.cx(328.94272028759258);
103  m_cameraParamsRGB.cy(267.48068171871557);
104  m_cameraParamsRGB.fx(529.2151);
105  m_cameraParamsRGB.fy(525.5639);
106 
107  m_cameraParamsRGB.dist.fill(0);
108 
109  // ----- Depth -----
112 
113  m_cameraParamsDepth.cx(339.30781);
114  m_cameraParamsDepth.cy(242.7391);
115  m_cameraParamsDepth.fx(594.21434);
116  m_cameraParamsDepth.fy(591.04054);
117 
118  m_cameraParamsDepth.dist.fill(0);
119 
120 #if !MRPT_HAS_KINECT
122  "MRPT was compiled without support for Kinect. Please, rebuild it.");
123 #endif
124 }
125 
126 /*-------------------------------------------------------------
127  dtor
128  -------------------------------------------------------------*/
129 CKinect::~CKinect() { this->close(); }
130 /** This method can or cannot be implemented in the derived class, depending on
131  * the need for it.
132  * \exception This method must throw an exception with a descriptive message if
133  * some critical error is found.
134  */
136 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
137  * \exception This method must throw an exception with a descriptive message if
138  * some critical error is found.
139  */
141 {
142  bool thereIs, hwError;
143 
145  std::make_shared<CObservation3DRangeScan>();
146  CObservationIMU::Ptr newObs_imu = std::make_shared<CObservationIMU>();
147 
148  getNextObservation(*newObs, *newObs_imu, thereIs, hwError);
149 
150  if (hwError)
151  {
152  m_state = ssError;
153  THROW_EXCEPTION("Couldn't communicate to the Kinect sensor!");
154  }
155 
156  if (thereIs)
157  {
158  m_state = ssWorking;
159 
160  vector<CSerializable::Ptr> objs;
162  objs.push_back(newObs);
163  if (m_grab_IMU) objs.push_back(newObs_imu);
164 
165  appendObservations(objs);
166  }
167 }
168 
169 /** Loads specific configuration for the device from a given source of
170  * configuration parameters, for example, an ".ini" file, loading from the
171  * section "[iniSection]" (see config::CConfigFileBase and derived classes)
172  * \exception This method must throw an exception with a descriptive message if
173  * some critical parameter is missing or has an invalid value.
174  */
176  const mrpt::config::CConfigFileBase& configSource,
177  const std::string& iniSection)
178 {
180  configSource.read_float(iniSection, "pose_x", 0),
181  configSource.read_float(iniSection, "pose_y", 0),
182  configSource.read_float(iniSection, "pose_z", 0),
183  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
184  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
185  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
186 
188  configSource.read_bool(iniSection, "preview_window", m_preview_window);
189 
190  // "Stereo" calibration data:
191  // [<SECTION>_LEFT] // Depth
192  // ...
193  // [<SECTION>_RIGHT] // RGB
194  // ...
195  // [<SECTION>_LEFT2RIGHT_POSE]
196  // pose_quaternion = [x y z qr qx qy qz]
197 
198  const mrpt::poses::CPose3D twist(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg);
199 
201  sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we
202  // fail to load from cfg at least we
203  // have some reasonable numbers.
205  sc.rightCameraPose =
207  .asTPose();
208 
209  try
210  {
211  sc.loadFromConfigFile(iniSection, configSource);
212  }
213  catch (const std::exception& e)
214  {
215  std::cout << "[CKinect::loadConfig_sensorSpecific] Warning: Ignoring "
216  "error loading calibration parameters:\n"
217  << e.what();
218  }
223 
224  // Id:
225  m_user_device_number = configSource.read_int(
226  iniSection, "device_number", m_user_device_number);
227 
228  m_grab_image =
229  configSource.read_bool(iniSection, "grab_image", m_grab_image);
230  m_grab_depth =
231  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
233  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
234  m_grab_IMU = configSource.read_bool(iniSection, "grab_IMU", m_grab_IMU);
235 
236  m_video_channel = configSource.read_enum<TVideoChannel>(
237  iniSection, "video_channel", m_video_channel);
238 
239  {
240  std::string s = configSource.read_string(
241  iniSection, "relativePoseIntensityWRTDepth", "");
242  if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
243  }
244 
245  m_initial_tilt_angle = configSource.read_int(
246  iniSection, "initial_tilt_angle", m_initial_tilt_angle);
247 }
248 
249 bool CKinect::isOpen() const
250 {
251 #if MRPT_HAS_KINECT_FREENECT
252  return f_dev != nullptr;
253 #else
254  return false;
255 #endif
256 
257 #
258 }
259 
260 #if MRPT_HAS_KINECT_FREENECT
261 // ======== GLOBAL CALLBACK FUNCTIONS ========
262 void depth_cb(freenect_device* dev, void* v_depth, uint32_t timestamp)
263 {
264  const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
265 
266  auto* depth = reinterpret_cast<uint16_t*>(v_depth);
267 
268  auto* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
269 
270  // Update of the timestamps at the end:
271  std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
272  CObservation3DRangeScan& obs = obj->internal_latest_obs();
273 
274  obs.hasRangeImage = true;
275  obs.range_is_depth = true;
276 
277 #ifdef KINECT_PROFILE_MEM_ALLOC
278  alloc_tim.enter("depth_cb alloc");
279 #endif
280 
281  // This method will try to exploit memory pooling if possible:
282  obs.rangeImage_setSize(frMode.height, frMode.width);
283 
284 #ifdef KINECT_PROFILE_MEM_ALLOC
285  alloc_tim.leave("depth_cb alloc");
286 #endif
287 
288  const CKinect::TDepth2RangeArray& r2m = obj->getRawDepth2RangeConversion();
289  obs.rangeUnits = 1e-3f; // we use mm as units
290 
291  for (int r = 0; r < frMode.height; r++)
292  for (int c = 0; c < frMode.width; c++)
293  {
294  // For now, quickly save the depth as it comes from the sensor,
295  // it'll
296  // transformed later on in getNextObservation()
297  const uint16_t v = *depth++;
298  obs.rangeImage.coeffRef(r, c) = r2m[v & KINECT_RANGES_TABLE_MASK];
299  }
300  obj->internal_tim_latest_depth() = timestamp;
301 }
302 
303 void rgb_cb(freenect_device* dev, void* img_data, uint32_t timestamp)
304 {
305  auto* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
306  const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
307 
308  // Update of the timestamps at the end:
309  std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
310  CObservation3DRangeScan& obs = obj->internal_latest_obs();
311 
312 #ifdef KINECT_PROFILE_MEM_ALLOC
313  alloc_tim.enter("depth_rgb loadFromMemoryBuffer");
314 #endif
315 
316  obs.hasIntensityImage = true;
317  if (obj->getVideoChannel() == CKinect::VIDEO_CHANNEL_RGB)
318  {
319  // Color image: We asked for Bayer data, so we can decode it outselves
320  // here
321  // and avoid having to reorder Green<->Red channels, as would be needed
322  // with
323  // the RGB image from freenect.
324  obs.intensityImageChannel =
326  obs.intensityImage.resize(
327  frMode.width, frMode.height, mrpt::img::CH_RGB);
328 
329 #if MRPT_HAS_OPENCV
330  const cv::Mat src_img_bayer(
331  frMode.height, frMode.width, CV_8UC1, img_data, frMode.width);
332 
333  cv::Mat& dst_img_RGB = obs.intensityImage.asCvMatRef();
334 
335  // Decode Bayer image:
336  cv::cvtColor(src_img_bayer, dst_img_RGB, cv::COLOR_BayerGB2BGR);
337 #else
338  THROW_EXCEPTION("Need building with OpenCV!");
339 #endif
340  }
341  else
342  {
343  // IR data: grayscale 8bit
344  obs.intensityImageChannel = mrpt::obs::CObservation3DRangeScan::CH_IR;
345  obs.intensityImage.loadFromMemoryBuffer(
346  frMode.width, frMode.height,
347  false, // Color image?
348  reinterpret_cast<unsigned char*>(img_data));
349  }
350 
351  // obs.intensityImage.setChannelsOrder_RGB();
352 
353 #ifdef KINECT_PROFILE_MEM_ALLOC
354  alloc_tim.leave("depth_rgb loadFromMemoryBuffer");
355 #endif
356 
357  obj->internal_tim_latest_rgb() = timestamp;
358 }
359 // ======== END OF GLOBAL CALLBACK FUNCTIONS ========
360 #endif // MRPT_HAS_KINECT_FREENECT
361 
363 {
364  if (isOpen()) close();
365 
366  // Alloc memory, if this is the first time:
367  m_buf_depth.resize(640 * 480 * 3); // We'll resize this below if needed
368  m_buf_rgb.resize(640 * 480 * 3);
369 
370 #if MRPT_HAS_KINECT_FREENECT // ----> libfreenect
371  // Try to open the device:
372  if (freenect_init(f_ctx_ptr, nullptr) < 0)
373  THROW_EXCEPTION("freenect_init() failed");
374 
375  freenect_set_log_level(
376  f_ctx,
377 #ifdef _DEBUG
378  FREENECT_LOG_DEBUG
379 #else
380  FREENECT_LOG_WARNING
381 #endif
382  );
383 
384  int nr_devices = freenect_num_devices(f_ctx);
385  // printf("[CKinect] Number of devices found: %d\n", nr_devices);
386 
387  if (!nr_devices) THROW_EXCEPTION("No Kinect devices found.");
388 
389  // Open the given device number:
390  if (freenect_open_device(f_ctx, f_dev_ptr, m_user_device_number) < 0)
392  "Error opening Kinect sensor with index: %d", m_user_device_number);
393 
394  // Setup:
395  if (m_initial_tilt_angle != 360) // 360 means no motor command.
397  freenect_set_led(f_dev, LED_RED);
398  freenect_set_depth_callback(f_dev, depth_cb);
399  freenect_set_video_callback(f_dev, rgb_cb);
400 
401  // rgb or IR channel:
402  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
403  FREENECT_RESOLUTION_MEDIUM,
405  ? FREENECT_VIDEO_IR_8BIT
406  : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so
407  // we can directly decode it here
408  );
409 
410  // Switch to that video mode:
411  if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
412  THROW_EXCEPTION("Error setting Kinect video mode.");
413 
414  // Get video mode:
415  const freenect_frame_mode frMode = freenect_get_current_video_mode(f_dev);
416 
417  // Realloc mem:
418  m_buf_depth.resize(frMode.width * frMode.height * 3);
419  m_buf_rgb.resize(frMode.width * frMode.height * 3);
420 
421  // Save resolution:
422  m_cameraParamsRGB.ncols = frMode.width;
423  m_cameraParamsRGB.nrows = frMode.height;
424 
425  m_cameraParamsDepth.ncols = frMode.width;
426  m_cameraParamsDepth.nrows = frMode.height;
427 
428  freenect_set_video_buffer(f_dev, &m_buf_rgb[0]);
429  freenect_set_depth_buffer(f_dev, &m_buf_depth[0]);
430 
431  freenect_set_depth_mode(
432  f_dev, freenect_find_depth_mode(
433  FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_10BIT));
434 
435  // Set user data = pointer to "this":
436  freenect_set_user(f_dev, this);
437 
438  if (freenect_start_depth(f_dev) < 0)
439  THROW_EXCEPTION("Error starting depth streaming.");
440 
441  if (freenect_start_video(f_dev) < 0)
442  THROW_EXCEPTION("Error starting video streaming.");
443 
444 #endif // MRPT_HAS_KINECT_FREENECT
445 }
446 
448 {
449 #if MRPT_HAS_KINECT_FREENECT
450  if (f_dev)
451  {
452  freenect_stop_depth(f_dev);
453  freenect_stop_video(f_dev);
454  freenect_close_device(f_dev);
455  }
456  m_f_dev = nullptr;
457 
458  if (f_ctx) freenect_shutdown(f_ctx);
459  m_f_ctx = nullptr;
460 #endif // MRPT_HAS_KINECT_FREENECT
461 }
462 
463 /** Changes the video channel to open (RGB or IR) - you can call this method
464  before start grabbing or in the middle of streaming and the video source will
465  change on the fly.
466  Default is RGB channel.
467 */
468 void CKinect::setVideoChannel([[maybe_unused]] const TVideoChannel vch)
469 {
470 #if MRPT_HAS_KINECT_FREENECT
471  m_video_channel = vch;
472  if (!isOpen()) return; // Nothing else to do here.
473 
474  // rgb or IR channel:
475  freenect_stop_video(f_dev);
476 
477  // rgb or IR channel:
478  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
479  FREENECT_RESOLUTION_MEDIUM,
481  ? FREENECT_VIDEO_IR_8BIT
482  : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so
483  // we can directly decode it here
484  );
485 
486  // Switch to that video mode:
487  if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
488  THROW_EXCEPTION("Error setting Kinect video mode.");
489 
490  freenect_start_video(f_dev);
491 
492 #endif // MRPT_HAS_KINECT_FREENECT
493 }
494 
495 /** The main data retrieving function, to be called after calling loadConfig()
496  * and initialize().
497  * \param out_obs The output retrieved observation (only if
498  * there_is_obs=true).
499  * \param there_is_obs If set to false, there was no new observation.
500  * \param hardware_error True on hardware/comms error.
501  *
502  * \sa doProcess
503  */
505  mrpt::obs::CObservation3DRangeScan& m_out_obs, bool& there_is_obs,
506  bool& hardware_error)
507 {
508  there_is_obs = false;
509  hardware_error = false;
510 
511 #if MRPT_HAS_KINECT_FREENECT
512 
513  using namespace std::chrono_literals;
514  const auto max_wait = 40ms; // 1/25 FPS
515 
516  // Mark previous observation's timestamp as out-dated:
517  m_latest_obs.hasPoints3D = false;
518  m_latest_obs.hasRangeImage = false;
519  m_latest_obs.hasIntensityImage = false;
520  m_latest_obs.hasConfidenceImage = false;
521 
522  const TTimeStamp tim0 = mrpt::system::now();
523 
524  // Reset these timestamp flags so if they are !=0 in the next call we're
525  // sure they're new frames.
526  m_latest_obs_cs.lock();
527  m_tim_latest_rgb = 0;
528  m_tim_latest_depth = 0;
529  m_latest_obs_cs.unlock();
530 
531  while (freenect_process_events(f_ctx) >= 0 &&
532  mrpt::system::now() < (tim0 + max_wait))
533  {
534  // Got a new frame?
535  if ((!m_grab_image ||
536  m_tim_latest_rgb != 0) && // If we are NOT grabbing RGB or we are
537  // and there's a new frame...
538  (!m_grab_depth ||
539  m_tim_latest_depth != 0) // If we are NOT grabbing Depth or we are
540  // and there's a new frame...
541  )
542  {
543  // Approx: 0.5ms delay between depth frame (first) and RGB frame
544  // (second).
545  // cout << "m_tim_latest_rgb: " << m_tim_latest_rgb << "
546  // m_tim_latest_depth: "<< m_tim_latest_depth <<endl;
547  there_is_obs = true;
548  break;
549  }
550  }
551 
552  // Handle the case when there is NOT depth frames (if there's something very
553  // close blocking the IR sensor) but we have RGB:
554  if ((m_grab_image && m_tim_latest_rgb != 0) &&
555  (m_grab_depth && m_tim_latest_depth == 0))
556  {
557  // Mark the entire range data as invalid:
558  m_latest_obs.hasRangeImage = true;
559  m_latest_obs.range_is_depth = true;
560 
561  m_latest_obs_cs.lock(); // Important: if system is running slow, etc.
562  // we cannot tell for sure that the depth
563  // buffer is not beeing filled right now:
564  m_latest_obs.rangeImage.setSize(
566  m_latest_obs.rangeImage.fill(0); // "0" means: error in range
567  m_latest_obs_cs.unlock();
568  there_is_obs = true;
569  }
570 
571  if (!there_is_obs) return;
572 
573  // We DO have a fresh new observation:
574 
575  // Quick save the observation to the user's object:
576  m_latest_obs_cs.lock();
577  m_out_obs.swap(m_latest_obs);
578  m_latest_obs_cs.unlock();
579 #endif
580 
581  // Set common data into observation:
582  // --------------------------------------
583  m_out_obs.sensorLabel = m_sensorLabel;
584  m_out_obs.timestamp = mrpt::system::now();
585  m_out_obs.sensorPose = m_sensorPoseOnRobot;
587 
588  m_out_obs.cameraParams = m_cameraParamsDepth;
590 
591  // 3D point cloud:
592  if (m_out_obs.hasRangeImage && m_grab_3D_points)
593  {
594  m_out_obs.unprojectInto(m_out_obs);
595 
596  if (!m_grab_depth)
597  {
598  m_out_obs.hasRangeImage = false;
599  m_out_obs.rangeImage.resize(0, 0);
600  }
601  }
602 
603  // preview in real-time?
604  if (m_preview_window)
605  {
606  if (m_out_obs.hasRangeImage)
607  {
609  {
611  if (!m_win_range)
612  {
613  m_win_range =
614  mrpt::gui::CDisplayWindow::Create("Preview RANGE");
615  m_win_range->setPos(5, 5);
616  }
617 
618  // Normalize the image
619  mrpt::img::CImage img = m_out_obs.rangeImage_getAsImage();
620  m_win_range->showImage(img);
621  }
622  }
623  if (m_out_obs.hasIntensityImage)
624  {
626  {
628  if (!m_win_int)
629  {
630  m_win_int =
631  mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
632  m_win_int->setPos(300, 5);
633  }
634  m_win_int->showImage(m_out_obs.intensityImage);
635  }
636  }
637  }
638  else
639  {
640  if (m_win_range) m_win_range.reset();
641  if (m_win_int) m_win_int.reset();
642  }
643 }
644 
645 /* -----------------------------------------------------
646  getNextObservation (with IMU)
647 ----------------------------------------------------- */
650  mrpt::obs::CObservationIMU& out_obs_imu, bool& there_is_obs,
651  bool& hardware_error)
652 {
653  // First, try getting the RGB+Depth data:
654  getNextObservation(out_obs, there_is_obs, hardware_error);
655 
656  // If successful, fill out the accelerometer data:
657  if (there_is_obs && this->m_grab_IMU)
658  {
659  double acc_x = 0, acc_y = 0, acc_z = 0; // In m/s^2
660  bool has_good_acc = false;
661 
662 #if MRPT_HAS_KINECT_FREENECT
663  {
664  freenect_update_tilt_state(f_dev);
665  freenect_raw_tilt_state* state = freenect_get_tilt_state(f_dev);
666  if (state)
667  {
668  has_good_acc = true;
669  double lx, ly, lz;
670  freenect_get_mks_accel(state, &lx, &ly, &lz);
671 
672  // Convert to a unified coordinate system:
673  // +x: forward
674  // +y: left
675  // +z: upward
676  acc_x = -lz;
677  acc_y = -lx;
678  acc_z = -ly;
679  }
680  }
681 #endif
682 
683  // Common part for any implementation:
684  if (has_good_acc)
685  {
686  out_obs_imu.sensorLabel = out_obs.sensorLabel + "_IMU";
687  out_obs_imu.timestamp = out_obs.timestamp;
688  out_obs_imu.sensorPose = out_obs.sensorPose;
689 
690  for (auto&& i : out_obs_imu.dataIsPresent) i = false;
691 
692  out_obs_imu.dataIsPresent[IMU_X_ACC] = true;
693  out_obs_imu.dataIsPresent[IMU_Y_ACC] = true;
694  out_obs_imu.dataIsPresent[IMU_Z_ACC] = true;
695 
696  out_obs_imu.rawMeasurements[IMU_X_ACC] = acc_x;
697  out_obs_imu.rawMeasurements[IMU_Y_ACC] = acc_y;
698  out_obs_imu.rawMeasurements[IMU_Z_ACC] = acc_z;
699  }
700  }
701 }
702 
703 /* -----------------------------------------------------
704  setPathForExternalImages
705 ----------------------------------------------------- */
707  [maybe_unused]] const std::string& directory)
708 {
709  // Ignore for now. It seems performance is better grabbing everything
710  // to a single big file than creating hundreds of smaller files per
711  // second...
712  return;
713 
714  // if (!mrpt::system::createDirectory( directory ))
715  // {
716  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
717  // externally
718  // saved images: %s",directory.c_str() );
719  // }
720  // m_path_for_external_images = directory;
721 }
722 
723 /** Change tilt angle \note Sensor must be open first. */
724 void CKinect::setTiltAngleDegrees([[maybe_unused]] double angle)
725 {
726  ASSERTMSG_(isOpen(), "Sensor must be open first");
727 
728 #if MRPT_HAS_KINECT_FREENECT
729  freenect_set_tilt_degs(f_dev, angle);
730 #endif
731 }
732 
734 {
735  ASSERTMSG_(isOpen(), "Sensor must be open first");
736 
737 #if MRPT_KINECT_WITH_FREENECT
738  freenect_update_tilt_state(f_dev);
739  freenect_raw_tilt_state* ts = freenect_get_tilt_state(f_dev);
740  return freenect_get_tilt_degs(ts);
741 #else
742  return 0;
743 #endif
744 }
mrpt::img::TCamera m_cameraParamsRGB
Params for the RGB camera.
Definition: CKinect.h:512
mrpt::img::TCamera m_cameraParamsDepth
Params for the Depth camera.
Definition: CKinect.h:514
uint32_t nrows
Definition: TCamera.h:40
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
Definition: CKinect.cpp:140
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
void unprojectInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams=T3DPointsProjectionParams(), const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Unprojects the RGB+D image pair into a 3D point cloud (with color if the target map supports it) and ...
std::vector< bool > dataIsPresent
Each entry in this vector is true if the corresponding data index contains valid data (the IMU unit s...
void resize(size_t row, size_t col)
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
std::vector< double > rawMeasurements
The accelerometer and/or gyroscope measurements taken by the IMU at the given timestamp.
TVideoChannel m_video_channel
The video channel to open: RGB or IR.
Definition: CKinect.h:533
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:175
double getTiltAngleDegrees()
Definition: CKinect.cpp:733
std::string m_sensorLabel
See CGenericSensor.
mrpt::gui::CDisplayWindow::Ptr m_win_int
Definition: CKinect.h:496
size_t m_preview_window_decimation
If preview is enabled, only show 1 out of N images.
Definition: CKinect.h:494
void setTiltAngleDegrees(double angle)
Change tilt angle.
Definition: CKinect.cpp:724
double m_maxRange
Sensor max range (meters)
Definition: CKinect.h:523
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:177
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
mrpt::math::TPose3DQuat asTPose() const
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
y-axis acceleration (local/vehicle frame) (m/sec2)
STL namespace.
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation, raw gyroscope and accelerometer values), altimeters or magnetometers.
z-axis acceleration (local/vehicle frame) (m/sec2)
#define KINECT_RANGES_TABLE_LEN
Definition: CKinect.h:30
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section) override
See the class documentation at the top for expected parameters.
Definition: CKinect.cpp:175
void close()
Close the Connection to the sensor (not need to call it manually unless desired for some reason...
Definition: CKinect.cpp:447
std::vector< uint8_t > m_buf_rgb
Definition: CKinect.h:537
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define KINECT_RANGES_TABLE_MASK
Definition: CKinect.h:31
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
void appendObservations(const std::vector< mrpt::serialization::CSerializable::Ptr > &obj)
This method must be called by derived classes to enqueue a new observation in the list to be returned...
void calculate_range2meters()
Compute m_range2meters at construction.
Definition: CKinect.cpp:57
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
Definition: CKinect.h:265
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:173
constexpr double DEG2RAD(const double x)
Degrees to radians.
int m_initial_tilt_angle
Set Kinect tilt to an initial deegre (it should be take in account in the sensor pose by the user) ...
Definition: CKinect.h:520
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
TDepth2RangeArray m_range2meters
The table raw depth -> range in meters.
Definition: CKinect.h:539
This namespace contains representation of robot actions and observations.
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
Definition: CKinect.cpp:135
bool hasRangeImage
true means the field rangeImage contains valid data
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void loadFromConfigFile(const std::string &section, const mrpt::config::CConfigFileBase &cfg)
Load all the params from a config source, in the same format that used in saveToConfigFile().
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
void setPathForExternalImages(const std::string &directory) override
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
Definition: CKinect.cpp:706
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
int m_user_device_number
Number of device to open (0:first,...)
Definition: CKinect.h:526
void setVideoChannel(const TVideoChannel vch)
Changes the video channel to open (RGB or IR) - you can call this method before start grabbing or in ...
Definition: CKinect.cpp:468
void getNextObservation(mrpt::obs::CObservation3DRangeScan &out_obs, bool &there_is_obs, bool &hardware_error)
The main data retrieving function, to be called after calling loadConfig() and initialize().
Definition: CKinect.cpp:504
CKinect()
Default ctor.
Definition: CKinect.cpp:82
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:171
void enter(const std::string_view &func_name) noexcept
Start of a named section.
~CKinect() override
Default ctor.
Definition: CKinect.cpp:129
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[x y z yaw pitch roll]"...
Definition: CPose3D.cpp:764
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
size_t m_preview_decim_counter_range
Definition: CKinect.h:495
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
mrpt::poses::CPose3D sensorPose
The pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
mrpt::poses::CPose3D m_relativePoseIntensityWRTDepth
See mrpt::obs::CObservation3DRangeScan for a diagram of this pose.
Definition: CKinect.h:516
bool hasIntensityImage
true means the field intensityImage contains valid data
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:28
bool isOpen() const
Whether there is a working connection to the sensor.
Definition: CKinect.cpp:249
mrpt::img::CImage rangeImage_getAsImage(const std::optional< mrpt::img::TColormap > color=std::nullopt, const std::optional< float > normMinRange=std::nullopt, const std::optional< float > normMaxRange=std::nullopt, const std::optional< std::string > additionalLayerName=std::nullopt) const
Builds a visualization from the rangeImage.
double leave(const std::string_view &func_name) noexcept
End of a named section.
bool m_preview_window
Show preview window while grabbing.
Definition: CKinect.h:492
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:31
std::vector< uint8_t > m_buf_depth
Temporary buffers for image grabbing.
Definition: CKinect.h:537
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
uint16_t[KINECT_RANGES_TABLE_LEN] TDepth2RangeArray
A type for an array that converts raw depth to ranges in millimeters.
Definition: CKinect.h:271
x-axis acceleration (local/vehicle frame) (m/sec2)
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
mrpt::gui::CDisplayWindow::Ptr m_win_range
Definition: CKinect.h:496
bool m_grab_image
Default: all true.
Definition: CKinect.h:529
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
size_t m_preview_decim_counter_rgb
Definition: CKinect.h:495
mrpt::poses::CPose3D m_sensorPoseOnRobot
Definition: CKinect.h:489
Grayscale or RGB visible channel of the camera sensor.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
TVideoChannel
RGB or IR video channel identifiers.
Definition: CKinect.h:274
void open()
Try to open the camera (set all the parameters before calling this) - users may also call initialize(...
Definition: CKinect.cpp:362



Page generated by Doxygen 1.8.14 for MRPT 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020