MRPT  1.9.9
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-2019, 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/otherlibs/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] = k3 * tanf(i / k2 + k1);
66 
67 #else
68  for (size_t i = 0; i < KINECT_RANGES_TABLE_LEN; i++)
69  m_range2meters[i] = 1.0f / (i * (-0.0030711016) + 3.3309495161);
70 #endif
71 
72  // Minimum/Maximum range means error:
73  m_range2meters[0] = 0;
74  m_range2meters[KINECT_RANGES_TABLE_LEN - 1] = 0;
75 }
76 
77 /*-------------------------------------------------------------
78  ctor
79  -------------------------------------------------------------*/
81  : m_sensorPoseOnRobot(),
82  m_relativePoseIntensityWRTDepth(
83  0, -0.02, 0, -90.0_deg, 0.0_deg, -90.0_deg)
84 {
86 
87  // Get maximum range:
88  m_maxRange =
89  m_range2meters[KINECT_RANGES_TABLE_LEN - 2]; // Recall: r[Max-1] means
90  // error.
91 
92  // Default label:
93  m_sensorLabel = "KINECT";
94 
95  // =========== Default params ===========
96  // ----- RGB -----
98  640; // By default set 640x480, on connect we'll update this.
100 
101  m_cameraParamsRGB.cx(328.94272028759258);
102  m_cameraParamsRGB.cy(267.48068171871557);
103  m_cameraParamsRGB.fx(529.2151);
104  m_cameraParamsRGB.fy(525.5639);
105 
106  m_cameraParamsRGB.dist.fill(0);
107 
108  // ----- Depth -----
111 
112  m_cameraParamsDepth.cx(339.30781);
113  m_cameraParamsDepth.cy(242.7391);
114  m_cameraParamsDepth.fx(594.21434);
115  m_cameraParamsDepth.fy(591.04054);
116 
117  m_cameraParamsDepth.dist.fill(0);
118 
119 #if !MRPT_HAS_KINECT
121  "MRPT was compiled without support for Kinect. Please, rebuild it.");
122 #endif
123 }
124 
125 /*-------------------------------------------------------------
126  dtor
127  -------------------------------------------------------------*/
128 CKinect::~CKinect() { this->close(); }
129 /** This method can or cannot be implemented in the derived class, depending on
130  * the need for it.
131  * \exception This method must throw an exception with a descriptive message if
132  * some critical error is found.
133  */
135 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
136  * \exception This method must throw an exception with a descriptive message if
137  * some critical error is found.
138  */
140 {
141  bool thereIs, hwError;
142 
144  std::make_shared<CObservation3DRangeScan>();
145  CObservationIMU::Ptr newObs_imu = std::make_shared<CObservationIMU>();
146 
147  getNextObservation(*newObs, *newObs_imu, thereIs, hwError);
148 
149  if (hwError)
150  {
151  m_state = ssError;
152  THROW_EXCEPTION("Couldn't communicate to the Kinect sensor!");
153  }
154 
155  if (thereIs)
156  {
157  m_state = ssWorking;
158 
159  vector<CSerializable::Ptr> objs;
161  objs.push_back(newObs);
162  if (m_grab_IMU) objs.push_back(newObs_imu);
163 
164  appendObservations(objs);
165  }
166 }
167 
168 /** Loads specific configuration for the device from a given source of
169  * configuration parameters, for example, an ".ini" file, loading from the
170  * section "[iniSection]" (see config::CConfigFileBase and derived classes)
171  * \exception This method must throw an exception with a descriptive message if
172  * some critical parameter is missing or has an invalid value.
173  */
175  const mrpt::config::CConfigFileBase& configSource,
176  const std::string& iniSection)
177 {
179  configSource.read_float(iniSection, "pose_x", 0),
180  configSource.read_float(iniSection, "pose_y", 0),
181  configSource.read_float(iniSection, "pose_z", 0),
182  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
183  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
184  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
185 
187  configSource.read_bool(iniSection, "preview_window", m_preview_window);
188 
189  // "Stereo" calibration data:
190  // [<SECTION>_LEFT] // Depth
191  // ...
192  // [<SECTION>_RIGHT] // RGB
193  // ...
194  // [<SECTION>_LEFT2RIGHT_POSE]
195  // pose_quaternion = [x y z qr qx qy qz]
196 
197  const mrpt::poses::CPose3D twist(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg);
198 
200  sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we
201  // fail to load from cfg at least we
202  // have some reasonable numbers.
204  sc.rightCameraPose =
206  .asTPose();
207 
208  try
209  {
210  sc.loadFromConfigFile(iniSection, configSource);
211  }
212  catch (const std::exception& e)
213  {
214  std::cout << "[CKinect::loadConfig_sensorSpecific] Warning: Ignoring "
215  "error loading calibration parameters:\n"
216  << e.what();
217  }
222 
223  // Id:
224  m_user_device_number = configSource.read_int(
225  iniSection, "device_number", m_user_device_number);
226 
227  m_grab_image =
228  configSource.read_bool(iniSection, "grab_image", m_grab_image);
229  m_grab_depth =
230  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
232  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
233  m_grab_IMU = configSource.read_bool(iniSection, "grab_IMU", m_grab_IMU);
234 
235  m_video_channel = configSource.read_enum<TVideoChannel>(
236  iniSection, "video_channel", m_video_channel);
237 
238  {
239  std::string s = configSource.read_string(
240  iniSection, "relativePoseIntensityWRTDepth", "");
242  }
243 
244  m_initial_tilt_angle = configSource.read_int(
245  iniSection, "initial_tilt_angle", m_initial_tilt_angle);
246 }
247 
248 bool CKinect::isOpen() const
249 {
250 #if MRPT_HAS_KINECT_FREENECT
251  return f_dev != nullptr;
252 #else
253  return false;
254 #endif
255 
256 #
257 }
258 
259 #if MRPT_HAS_KINECT_FREENECT
260 // ======== GLOBAL CALLBACK FUNCTIONS ========
261 void depth_cb(freenect_device* dev, void* v_depth, uint32_t timestamp)
262 {
263  const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
264 
265  auto* depth = reinterpret_cast<uint16_t*>(v_depth);
266 
267  auto* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
268 
269  // Update of the timestamps at the end:
270  std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
271  CObservation3DRangeScan& obs = obj->internal_latest_obs();
272 
273  obs.hasRangeImage = true;
274  obs.range_is_depth = true;
275 
276 #ifdef KINECT_PROFILE_MEM_ALLOC
277  alloc_tim.enter("depth_cb alloc");
278 #endif
279 
280  // This method will try to exploit memory pooling if possible:
281  obs.rangeImage_setSize(frMode.height, frMode.width);
282 
283 #ifdef KINECT_PROFILE_MEM_ALLOC
284  alloc_tim.leave("depth_cb alloc");
285 #endif
286 
287  const CKinect::TDepth2RangeArray& r2m = obj->getRawDepth2RangeConversion();
288  for (int r = 0; r < frMode.height; r++)
289  for (int c = 0; c < frMode.width; c++)
290  {
291  // For now, quickly save the depth as it comes from the sensor,
292  // it'll
293  // transformed later on in getNextObservation()
294  const uint16_t v = *depth++;
295  obs.rangeImage.coeffRef(r, c) = r2m[v & KINECT_RANGES_TABLE_MASK];
296  }
297  obj->internal_tim_latest_depth() = timestamp;
298 }
299 
300 void rgb_cb(freenect_device* dev, void* img_data, uint32_t timestamp)
301 {
302  auto* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
303  const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
304 
305  // Update of the timestamps at the end:
306  std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
307  CObservation3DRangeScan& obs = obj->internal_latest_obs();
308 
309 #ifdef KINECT_PROFILE_MEM_ALLOC
310  alloc_tim.enter("depth_rgb loadFromMemoryBuffer");
311 #endif
312 
313  obs.hasIntensityImage = true;
314  if (obj->getVideoChannel() == CKinect::VIDEO_CHANNEL_RGB)
315  {
316  // Color image: We asked for Bayer data, so we can decode it outselves
317  // here
318  // and avoid having to reorder Green<->Red channels, as would be needed
319  // with
320  // the RGB image from freenect.
321  obs.intensityImageChannel =
323  obs.intensityImage.resize(
324  frMode.width, frMode.height, mrpt::img::CH_RGB);
325 
326 #if MRPT_HAS_OPENCV
327  const cv::Mat src_img_bayer(
328  frMode.height, frMode.width, CV_8UC1, img_data, frMode.width);
329 
330  cv::Mat& dst_img_RGB = obs.intensityImage.asCvMatRef();
331 
332  // Decode Bayer image:
333  cv::cvtColor(src_img_bayer, dst_img_RGB, cv::COLOR_BayerGB2BGR);
334 #else
335  THROW_EXCEPTION("Need building with OpenCV!");
336 #endif
337  }
338  else
339  {
340  // IR data: grayscale 8bit
341  obs.intensityImageChannel = mrpt::obs::CObservation3DRangeScan::CH_IR;
342  obs.intensityImage.loadFromMemoryBuffer(
343  frMode.width, frMode.height,
344  false, // Color image?
345  reinterpret_cast<unsigned char*>(img_data));
346  }
347 
348  // obs.intensityImage.setChannelsOrder_RGB();
349 
350 #ifdef KINECT_PROFILE_MEM_ALLOC
351  alloc_tim.leave("depth_rgb loadFromMemoryBuffer");
352 #endif
353 
354  obj->internal_tim_latest_rgb() = timestamp;
355 }
356 // ======== END OF GLOBAL CALLBACK FUNCTIONS ========
357 #endif // MRPT_HAS_KINECT_FREENECT
358 
360 {
361  if (isOpen()) close();
362 
363  // Alloc memory, if this is the first time:
364  m_buf_depth.resize(640 * 480 * 3); // We'll resize this below if needed
365  m_buf_rgb.resize(640 * 480 * 3);
366 
367 #if MRPT_HAS_KINECT_FREENECT // ----> libfreenect
368  // Try to open the device:
369  if (freenect_init(f_ctx_ptr, nullptr) < 0)
370  THROW_EXCEPTION("freenect_init() failed");
371 
372  freenect_set_log_level(
373  f_ctx,
374 #ifdef _DEBUG
375  FREENECT_LOG_DEBUG
376 #else
377  FREENECT_LOG_WARNING
378 #endif
379  );
380 
381  int nr_devices = freenect_num_devices(f_ctx);
382  // printf("[CKinect] Number of devices found: %d\n", nr_devices);
383 
384  if (!nr_devices) THROW_EXCEPTION("No Kinect devices found.");
385 
386  // Open the given device number:
387  if (freenect_open_device(f_ctx, f_dev_ptr, m_user_device_number) < 0)
389  "Error opening Kinect sensor with index: %d", m_user_device_number);
390 
391  // Setup:
392  if (m_initial_tilt_angle != 360) // 360 means no motor command.
394  freenect_set_led(f_dev, LED_RED);
395  freenect_set_depth_callback(f_dev, depth_cb);
396  freenect_set_video_callback(f_dev, rgb_cb);
397 
398  // rgb or IR channel:
399  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
400  FREENECT_RESOLUTION_MEDIUM,
402  ? FREENECT_VIDEO_IR_8BIT
403  : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so
404  // we can directly decode it here
405  );
406 
407  // Switch to that video mode:
408  if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
409  THROW_EXCEPTION("Error setting Kinect video mode.");
410 
411  // Get video mode:
412  const freenect_frame_mode frMode = freenect_get_current_video_mode(f_dev);
413 
414  // Realloc mem:
415  m_buf_depth.resize(frMode.width * frMode.height * 3);
416  m_buf_rgb.resize(frMode.width * frMode.height * 3);
417 
418  // Save resolution:
419  m_cameraParamsRGB.ncols = frMode.width;
420  m_cameraParamsRGB.nrows = frMode.height;
421 
422  m_cameraParamsDepth.ncols = frMode.width;
423  m_cameraParamsDepth.nrows = frMode.height;
424 
425  freenect_set_video_buffer(f_dev, &m_buf_rgb[0]);
426  freenect_set_depth_buffer(f_dev, &m_buf_depth[0]);
427 
428  freenect_set_depth_mode(
429  f_dev, freenect_find_depth_mode(
430  FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_10BIT));
431 
432  // Set user data = pointer to "this":
433  freenect_set_user(f_dev, this);
434 
435  if (freenect_start_depth(f_dev) < 0)
436  THROW_EXCEPTION("Error starting depth streaming.");
437 
438  if (freenect_start_video(f_dev) < 0)
439  THROW_EXCEPTION("Error starting video streaming.");
440 
441 #endif // MRPT_HAS_KINECT_FREENECT
442 }
443 
445 {
446 #if MRPT_HAS_KINECT_FREENECT
447  if (f_dev)
448  {
449  freenect_stop_depth(f_dev);
450  freenect_stop_video(f_dev);
451  freenect_close_device(f_dev);
452  }
453  m_f_dev = nullptr;
454 
455  if (f_ctx) freenect_shutdown(f_ctx);
456  m_f_ctx = nullptr;
457 #endif // MRPT_HAS_KINECT_FREENECT
458 }
459 
460 /** Changes the video channel to open (RGB or IR) - you can call this method
461  before start grabbing or in the middle of streaming and the video source will
462  change on the fly.
463  Default is RGB channel.
464 */
466 {
467 #if MRPT_HAS_KINECT_FREENECT
468  m_video_channel = vch;
469  if (!isOpen()) return; // Nothing else to do here.
470 
471  // rgb or IR channel:
472  freenect_stop_video(f_dev);
473 
474  // rgb or IR channel:
475  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
476  FREENECT_RESOLUTION_MEDIUM,
478  ? FREENECT_VIDEO_IR_8BIT
479  : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so
480  // we can directly decode it here
481  );
482 
483  // Switch to that video mode:
484  if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
485  THROW_EXCEPTION("Error setting Kinect video mode.");
486 
487  freenect_start_video(f_dev);
488 
489 #else
490  MRPT_UNUSED_PARAM(vch);
491 #endif // MRPT_HAS_KINECT_FREENECT
492 }
493 
494 /** The main data retrieving function, to be called after calling loadConfig()
495  * and initialize().
496  * \param out_obs The output retrieved observation (only if
497  * there_is_obs=true).
498  * \param there_is_obs If set to false, there was no new observation.
499  * \param hardware_error True on hardware/comms error.
500  *
501  * \sa doProcess
502  */
504  mrpt::obs::CObservation3DRangeScan& _out_obs, bool& there_is_obs,
505  bool& hardware_error)
506 {
507  there_is_obs = false;
508  hardware_error = false;
509 
510 #if MRPT_HAS_KINECT_FREENECT
511 
512  using namespace std::chrono_literals;
513  const auto max_wait = 40ms; // 1/25 FPS
514 
515  // Mark previous observation's timestamp as out-dated:
516  m_latest_obs.hasPoints3D = false;
517  m_latest_obs.hasRangeImage = false;
518  m_latest_obs.hasIntensityImage = false;
519  m_latest_obs.hasConfidenceImage = false;
520 
521  const TTimeStamp tim0 = mrpt::system::now();
522 
523  // Reset these timestamp flags so if they are !=0 in the next call we're
524  // sure they're new frames.
525  m_latest_obs_cs.lock();
526  m_tim_latest_rgb = 0;
527  m_tim_latest_depth = 0;
528  m_latest_obs_cs.unlock();
529 
530  while (freenect_process_events(f_ctx) >= 0 &&
531  mrpt::system::now() < (tim0 + max_wait))
532  {
533  // Got a new frame?
534  if ((!m_grab_image ||
535  m_tim_latest_rgb != 0) && // If we are NOT grabbing RGB or we are
536  // and there's a new frame...
537  (!m_grab_depth ||
538  m_tim_latest_depth != 0) // If we are NOT grabbing Depth or we are
539  // and there's a new frame...
540  )
541  {
542  // Approx: 0.5ms delay between depth frame (first) and RGB frame
543  // (second).
544  // cout << "m_tim_latest_rgb: " << m_tim_latest_rgb << "
545  // m_tim_latest_depth: "<< m_tim_latest_depth <<endl;
546  there_is_obs = true;
547  break;
548  }
549  }
550 
551  // Handle the case when there is NOT depth frames (if there's something very
552  // close blocking the IR sensor) but we have RGB:
553  if ((m_grab_image && m_tim_latest_rgb != 0) &&
554  (m_grab_depth && m_tim_latest_depth == 0))
555  {
556  // Mark the entire range data as invalid:
557  m_latest_obs.hasRangeImage = true;
558  m_latest_obs.range_is_depth = true;
559 
560  m_latest_obs_cs.lock(); // Important: if system is running slow, etc.
561  // we cannot tell for sure that the depth
562  // buffer is not beeing filled right now:
563  m_latest_obs.rangeImage.setSize(
565  m_latest_obs.rangeImage.fill(0); // "0" means: error in range
566  m_latest_obs_cs.unlock();
567  there_is_obs = true;
568  }
569 
570  if (!there_is_obs) return;
571 
572  // We DO have a fresh new observation:
573 
574  // Quick save the observation to the user's object:
575  m_latest_obs_cs.lock();
576  _out_obs.swap(m_latest_obs);
577  m_latest_obs_cs.unlock();
578 #endif
579 
580  // Set common data into observation:
581  // --------------------------------------
582  _out_obs.sensorLabel = m_sensorLabel;
583  _out_obs.timestamp = mrpt::system::now();
584  _out_obs.sensorPose = m_sensorPoseOnRobot;
586 
589 
590  // 3D point cloud:
591  if (_out_obs.hasRangeImage && m_grab_3D_points)
592  {
594 
595  if (!m_grab_depth)
596  {
597  _out_obs.hasRangeImage = false;
598  _out_obs.rangeImage.resize(0, 0);
599  }
600  }
601 
602  // preview in real-time?
603  if (m_preview_window)
604  {
605  if (_out_obs.hasRangeImage)
606  {
608  {
610  if (!m_win_range)
611  {
612  m_win_range =
613  mrpt::gui::CDisplayWindow::Create("Preview RANGE");
614  m_win_range->setPos(5, 5);
615  }
616 
617  // Normalize the image
619  img.setFromMatrix(_out_obs.rangeImage);
620  CMatrixFloat r = _out_obs.rangeImage;
621  r *= float(1.0 / this->m_maxRange);
622  m_win_range->showImage(img);
623  }
624  }
625  if (_out_obs.hasIntensityImage)
626  {
628  {
630  if (!m_win_int)
631  {
632  m_win_int =
633  mrpt::gui::CDisplayWindow::Create("Preview INTENSITY");
634  m_win_int->setPos(300, 5);
635  }
636  m_win_int->showImage(_out_obs.intensityImage);
637  }
638  }
639  }
640  else
641  {
642  if (m_win_range) m_win_range.reset();
643  if (m_win_int) m_win_int.reset();
644  }
645 }
646 
647 /* -----------------------------------------------------
648  getNextObservation (with IMU)
649 ----------------------------------------------------- */
652  mrpt::obs::CObservationIMU& out_obs_imu, bool& there_is_obs,
653  bool& hardware_error)
654 {
655  // First, try getting the RGB+Depth data:
656  getNextObservation(out_obs, there_is_obs, hardware_error);
657 
658  // If successful, fill out the accelerometer data:
659  if (there_is_obs && this->m_grab_IMU)
660  {
661  double acc_x = 0, acc_y = 0, acc_z = 0; // In m/s^2
662  bool has_good_acc = false;
663 
664 #if MRPT_HAS_KINECT_FREENECT
665  {
666  freenect_update_tilt_state(f_dev);
667  freenect_raw_tilt_state* state = freenect_get_tilt_state(f_dev);
668  if (state)
669  {
670  has_good_acc = true;
671  double lx, ly, lz;
672  freenect_get_mks_accel(state, &lx, &ly, &lz);
673 
674  // Convert to a unified coordinate system:
675  // +x: forward
676  // +y: left
677  // +z: upward
678  acc_x = -lz;
679  acc_y = -lx;
680  acc_z = -ly;
681  }
682  }
683 #endif
684 
685  // Common part for any implementation:
686  if (has_good_acc)
687  {
688  out_obs_imu.sensorLabel = out_obs.sensorLabel + "_IMU";
689  out_obs_imu.timestamp = out_obs.timestamp;
690  out_obs_imu.sensorPose = out_obs.sensorPose;
691 
692  for (auto&& i : out_obs_imu.dataIsPresent) i = false;
693 
694  out_obs_imu.dataIsPresent[IMU_X_ACC] = true;
695  out_obs_imu.dataIsPresent[IMU_Y_ACC] = true;
696  out_obs_imu.dataIsPresent[IMU_Z_ACC] = true;
697 
698  out_obs_imu.rawMeasurements[IMU_X_ACC] = acc_x;
699  out_obs_imu.rawMeasurements[IMU_Y_ACC] = acc_y;
700  out_obs_imu.rawMeasurements[IMU_Z_ACC] = acc_z;
701  }
702  }
703 }
704 
705 /* -----------------------------------------------------
706  setPathForExternalImages
707 ----------------------------------------------------- */
709 {
710  MRPT_UNUSED_PARAM(directory);
711  // Ignore for now. It seems performance is better grabbing everything
712  // to a single big file than creating hundreds of smaller files per
713  // second...
714  return;
715 
716  // if (!mrpt::system::createDirectory( directory ))
717  // {
718  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
719  // externally
720  // saved images: %s",directory.c_str() );
721  // }
722  // m_path_for_external_images = directory;
723 }
724 
725 /** Change tilt angle \note Sensor must be open first. */
727 {
728  ASSERTMSG_(isOpen(), "Sensor must be open first");
729 
730 #if MRPT_HAS_KINECT_FREENECT
731  freenect_set_tilt_degs(f_dev, angle);
732 #else
733  MRPT_UNUSED_PARAM(angle);
734 #endif
735 }
736 
738 {
739  ASSERTMSG_(isOpen(), "Sensor must be open first");
740 
741 #if MRPT_KINECT_WITH_FREENECT
742  freenect_update_tilt_state(f_dev);
743  freenect_raw_tilt_state* ts = freenect_get_tilt_state(f_dev);
744  return freenect_get_tilt_degs(ts);
745 #else
746  return 0;
747 #endif
748 }
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:39
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
Definition: CKinect.cpp:139
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
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:174
double getTiltAngleDegrees()
Definition: CKinect.cpp:737
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 project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
void setTiltAngleDegrees(double angle)
Change tilt angle.
Definition: CKinect.cpp:726
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
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:176
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
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
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3606
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)
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
#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:174
void close()
Close the Connection to the sensor (not need to call it manually unless desired for some reason...
Definition: CKinect.cpp:444
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:172
constexpr double DEG2RAD(const double x)
Degrees to radians.
const GLubyte * c
Definition: glext.h:6406
GLint GLvoid * img
Definition: glext.h:3769
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:134
bool hasRangeImage
true means the field rangeImage contains valid data
#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
GLsizei const GLchar ** string
Definition: glext.h:4116
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:708
#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:52
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:465
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:503
CKinect()
Default ctor.
Definition: CKinect.cpp:80
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:170
void enter(const std::string_view &func_name) noexcept
Start of a named section.
~CKinect() override
Default ctor.
Definition: CKinect.cpp:128
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
const GLdouble * v
Definition: glext.h:3684
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:780
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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
float[KINECT_RANGES_TABLE_LEN] TDepth2RangeArray
A type for an array that converts raw depth to ranges in meters.
Definition: CKinect.h:271
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:248
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
This template class provides the basic functionality for a general 2D any-size, resizable container o...
x-axis acceleration (local/vehicle frame) (m/sec2)
uint32_t ncols
Camera resolution.
Definition: TCamera.h:39
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:147
TVideoChannel
RGB or IR video channel identifiers.
Definition: CKinect.h:274
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void open()
Try to open the camera (set all the parameters before calling this) - users may also call initialize(...
Definition: CKinect.cpp:359



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb560b230 Wed Nov 13 08:06:48 2019 +0100 at miƩ nov 13 08:15:10 CET 2019