Main MRPT website > C++ reference for MRPT 1.9.9
CKinect.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/hwdrivers/CKinect.h>
14 #include <mrpt/img/TStereoCamera.h>
15 #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_preview_window(false),
83  m_preview_window_decimation(1),
84  m_preview_decim_counter_range(0),
85  m_preview_decim_counter_rgb(0),
86 
87 #if MRPT_HAS_KINECT_FREENECT
88  m_f_ctx(nullptr), // The "freenect_context", or nullptr if closed
89  m_f_dev(nullptr), // The "freenect_device", or nullptr if closed
90  m_tim_latest_depth(0),
91  m_tim_latest_rgb(0),
92 #endif
93  m_relativePoseIntensityWRTDepth(
94  0, -0.02, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90)),
95  m_initial_tilt_angle(360),
96  m_user_device_number(0),
97  m_grab_image(true),
98  m_grab_depth(true),
99  m_grab_3D_points(true),
100  m_grab_IMU(true)
101 {
103 
104  // Get maximum range:
106  2]; // Recall: r[Max-1] means error.
107 
108  // Default label:
109  m_sensorLabel = "KINECT";
110 
111  // =========== Default params ===========
112  // ----- RGB -----
114  640; // By default set 640x480, on connect we'll update this.
115  m_cameraParamsRGB.nrows = 480;
116 
117  m_cameraParamsRGB.cx(328.94272028759258);
118  m_cameraParamsRGB.cy(267.48068171871557);
119  m_cameraParamsRGB.fx(529.2151);
120  m_cameraParamsRGB.fy(525.5639);
121 
122  m_cameraParamsRGB.dist.fill(0);
123 
124  // ----- Depth -----
127 
128  m_cameraParamsDepth.cx(339.30781);
129  m_cameraParamsDepth.cy(242.7391);
130  m_cameraParamsDepth.fx(594.21434);
131  m_cameraParamsDepth.fy(591.04054);
132 
133  m_cameraParamsDepth.dist.fill(0);
134 
135 #if !MRPT_HAS_KINECT
137  "MRPT was compiled without support for Kinect. Please, rebuild it.")
138 #endif
139 }
140 
141 /*-------------------------------------------------------------
142  dtor
143  -------------------------------------------------------------*/
144 CKinect::~CKinect() { this->close(); }
145 /** This method can or cannot be implemented in the derived class, depending on
146 * the need for it.
147 * \exception This method must throw an exception with a descriptive message if
148 * some critical error is found.
149 */
151 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
152 * \exception This method must throw an exception with a descriptive message if
153 * some critical error is found.
154 */
156 {
157  bool thereIs, hwError;
158 
160  mrpt::make_aligned_shared<CObservation3DRangeScan>();
161  CObservationIMU::Ptr newObs_imu =
162  mrpt::make_aligned_shared<CObservationIMU>();
163 
164  getNextObservation(*newObs, *newObs_imu, thereIs, hwError);
165 
166  if (hwError)
167  {
168  m_state = ssError;
169  THROW_EXCEPTION("Couldn't communicate to the Kinect sensor!");
170  }
171 
172  if (thereIs)
173  {
174  m_state = ssWorking;
175 
176  vector<CSerializable::Ptr> objs;
178  objs.push_back(newObs);
179  if (m_grab_IMU) objs.push_back(newObs_imu);
180 
181  appendObservations(objs);
182  }
183 }
184 
185 /** Loads specific configuration for the device from a given source of
186 * configuration parameters, for example, an ".ini" file, loading from the
187 * section "[iniSection]" (see config::CConfigFileBase and derived classes)
188 * \exception This method must throw an exception with a descriptive message if
189 * some critical parameter is missing or has an invalid value.
190 */
192  const mrpt::config::CConfigFileBase& configSource,
193  const std::string& iniSection)
194 {
196  configSource.read_float(iniSection, "pose_x", 0),
197  configSource.read_float(iniSection, "pose_y", 0),
198  configSource.read_float(iniSection, "pose_z", 0),
199  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
200  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
201  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
202 
204  configSource.read_bool(iniSection, "preview_window", m_preview_window);
205 
206  // "Stereo" calibration data:
207  // [<SECTION>_LEFT] // Depth
208  // ...
209  // [<SECTION>_RIGHT] // RGB
210  // ...
211  // [<SECTION>_LEFT2RIGHT_POSE]
212  // pose_quaternion = [x y z qr qx qy qz]
213 
214  const mrpt::poses::CPose3D twist(
215  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90));
216 
218  sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we
219  // fail to load from cfg at least we
220  // have some reasonable numbers.
222  sc.rightCameraPose =
224  .asTPose();
225 
226  try
227  {
228  sc.loadFromConfigFile(iniSection, configSource);
229  }
230  catch (std::exception& e)
231  {
232  std::cout << "[CKinect::loadConfig_sensorSpecific] Warning: Ignoring "
233  "error loading calibration parameters:\n"
234  << e.what();
235  }
240 
241  // Id:
242  m_user_device_number = configSource.read_int(
243  iniSection, "device_number", m_user_device_number);
244 
245  m_grab_image =
246  configSource.read_bool(iniSection, "grab_image", m_grab_image);
247  m_grab_depth =
248  configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
250  configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
251  m_grab_IMU = configSource.read_bool(iniSection, "grab_IMU", m_grab_IMU);
252 
253  m_video_channel = configSource.read_enum<TVideoChannel>(
254  iniSection, "video_channel", m_video_channel);
255 
256  {
257  std::string s = configSource.read_string(
258  iniSection, "relativePoseIntensityWRTDepth", "");
260  }
261 
262  m_initial_tilt_angle = configSource.read_int(
263  iniSection, "initial_tilt_angle", m_initial_tilt_angle);
264 }
265 
266 bool CKinect::isOpen() const
267 {
268 #if MRPT_HAS_KINECT_FREENECT
269  return f_dev != nullptr;
270 #else
271  return false;
272 #endif
273 
274 #
275 }
276 
277 #if MRPT_HAS_KINECT_FREENECT
278 // ======== GLOBAL CALLBACK FUNCTIONS ========
279 void depth_cb(freenect_device* dev, void* v_depth, uint32_t timestamp)
280 {
281  const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
282 
283  uint16_t* depth = reinterpret_cast<uint16_t*>(v_depth);
284 
285  CKinect* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
286 
287  // Update of the timestamps at the end:
288  std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
289  CObservation3DRangeScan& obs = obj->internal_latest_obs();
290 
291  obs.hasRangeImage = true;
292  obs.range_is_depth = true;
293 
294 #ifdef KINECT_PROFILE_MEM_ALLOC
295  alloc_tim.enter("depth_cb alloc");
296 #endif
297 
298  // This method will try to exploit memory pooling if possible:
299  obs.rangeImage_setSize(frMode.height, frMode.width);
300 
301 #ifdef KINECT_PROFILE_MEM_ALLOC
302  alloc_tim.leave("depth_cb alloc");
303 #endif
304 
305  const CKinect::TDepth2RangeArray& r2m = obj->getRawDepth2RangeConversion();
306  for (int r = 0; r < frMode.height; r++)
307  for (int c = 0; c < frMode.width; c++)
308  {
309  // For now, quickly save the depth as it comes from the sensor,
310  // it'll
311  // transformed later on in getNextObservation()
312  const uint16_t v = *depth++;
313  obs.rangeImage.coeffRef(r, c) = r2m[v & KINECT_RANGES_TABLE_MASK];
314  }
315  obj->internal_tim_latest_depth() = timestamp;
316 }
317 
318 void rgb_cb(freenect_device* dev, void* img_data, uint32_t timestamp)
319 {
320  CKinect* obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
321  const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
322 
323  // Update of the timestamps at the end:
324  std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
325  CObservation3DRangeScan& obs = obj->internal_latest_obs();
326 
327 #ifdef KINECT_PROFILE_MEM_ALLOC
328  alloc_tim.enter("depth_rgb loadFromMemoryBuffer");
329 #endif
330 
331  obs.hasIntensityImage = true;
332  if (obj->getVideoChannel() == CKinect::VIDEO_CHANNEL_RGB)
333  {
334  // Color image: We asked for Bayer data, so we can decode it outselves
335  // here
336  // and avoid having to reorder Green<->Red channels, as would be needed
337  // with
338  // the RGB image from freenect.
339  obs.intensityImageChannel =
341  obs.intensityImage.resize(
342  frMode.width, frMode.height, CH_RGB, true /* origin=top-left */);
343 
344 #if MRPT_HAS_OPENCV
345 #if MRPT_OPENCV_VERSION_NUM < 0x200
346  // Version for VERY OLD OpenCV versions:
347  IplImage* src_img_bayer =
348  cvCreateImageHeader(cvSize(frMode.width, frMode.height), 8, 1);
349  src_img_bayer->imageDataOrigin = reinterpret_cast<char*>(img_data);
350  src_img_bayer->imageData = src_img_bayer->imageDataOrigin;
351  src_img_bayer->widthStep = frMode.width;
352 
353  IplImage* dst_img_RGB = obs.intensityImage.getAs<IplImage>();
354 
355  // Decode Bayer image:
356  cvCvtColor(src_img_bayer, dst_img_RGB, CV_BayerGB2BGR);
357 
358 #else
359  // Version for modern OpenCV:
360  const cv::Mat src_img_bayer(
361  frMode.height, frMode.width, CV_8UC1, img_data, frMode.width);
362 
363  cv::Mat dst_img_RGB = cv::cvarrToMat(
364  obs.intensityImage.getAs<IplImage>(),
365  false /* dont copy buffers */);
366 
367  // Decode Bayer image:
368  cv::cvtColor(src_img_bayer, dst_img_RGB, CV_BayerGB2BGR);
369 #endif
370 #else
371  THROW_EXCEPTION("Need building with OpenCV!");
372 #endif
373  }
374  else
375  {
376  // IR data: grayscale 8bit
377  obs.intensityImageChannel = mrpt::obs::CObservation3DRangeScan::CH_IR;
378  obs.intensityImage.loadFromMemoryBuffer(
379  frMode.width, frMode.height,
380  false, // Color image?
381  reinterpret_cast<unsigned char*>(img_data));
382  }
383 
384 // obs.intensityImage.setChannelsOrder_RGB();
385 
386 #ifdef KINECT_PROFILE_MEM_ALLOC
387  alloc_tim.leave("depth_rgb loadFromMemoryBuffer");
388 #endif
389 
390  obj->internal_tim_latest_rgb() = timestamp;
391 }
392 // ======== END OF GLOBAL CALLBACK FUNCTIONS ========
393 #endif // MRPT_HAS_KINECT_FREENECT
394 
396 {
397  if (isOpen()) close();
398 
399  // Alloc memory, if this is the first time:
400  m_buf_depth.resize(640 * 480 * 3); // We'll resize this below if needed
401  m_buf_rgb.resize(640 * 480 * 3);
402 
403 #if MRPT_HAS_KINECT_FREENECT // ----> libfreenect
404  // Try to open the device:
405  if (freenect_init(f_ctx_ptr, nullptr) < 0)
406  THROW_EXCEPTION("freenect_init() failed");
407 
408  freenect_set_log_level(
409  f_ctx,
410 #ifdef _DEBUG
411  FREENECT_LOG_DEBUG
412 #else
413  FREENECT_LOG_WARNING
414 #endif
415  );
416 
417  int nr_devices = freenect_num_devices(f_ctx);
418  // printf("[CKinect] Number of devices found: %d\n", nr_devices);
419 
420  if (!nr_devices) THROW_EXCEPTION("No Kinect devices found.");
421 
422  // Open the given device number:
423  if (freenect_open_device(f_ctx, f_dev_ptr, m_user_device_number) < 0)
425  "Error opening Kinect sensor with index: %d", m_user_device_number)
426 
427  // Setup:
428  if (m_initial_tilt_angle != 360) // 360 means no motor command.
430  freenect_set_led(f_dev, LED_RED);
431  freenect_set_depth_callback(f_dev, depth_cb);
432  freenect_set_video_callback(f_dev, rgb_cb);
433 
434  // rgb or IR channel:
435  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
436  FREENECT_RESOLUTION_MEDIUM,
438  ? FREENECT_VIDEO_IR_8BIT
439  : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so
440  // we can directly decode it here
441  );
442 
443  // Switch to that video mode:
444  if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
445  THROW_EXCEPTION("Error setting Kinect video mode.");
446 
447  // Get video mode:
448  const freenect_frame_mode frMode = freenect_get_current_video_mode(f_dev);
449 
450  // Realloc mem:
451  m_buf_depth.resize(frMode.width * frMode.height * 3);
452  m_buf_rgb.resize(frMode.width * frMode.height * 3);
453 
454  // Save resolution:
455  m_cameraParamsRGB.ncols = frMode.width;
456  m_cameraParamsRGB.nrows = frMode.height;
457 
458  m_cameraParamsDepth.ncols = frMode.width;
459  m_cameraParamsDepth.nrows = frMode.height;
460 
461  freenect_set_video_buffer(f_dev, &m_buf_rgb[0]);
462  freenect_set_depth_buffer(f_dev, &m_buf_depth[0]);
463 
464  freenect_set_depth_mode(
465  f_dev, freenect_find_depth_mode(
466  FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_10BIT));
467 
468  // Set user data = pointer to "this":
469  freenect_set_user(f_dev, this);
470 
471  if (freenect_start_depth(f_dev) < 0)
472  THROW_EXCEPTION("Error starting depth streaming.");
473 
474  if (freenect_start_video(f_dev) < 0)
475  THROW_EXCEPTION("Error starting video streaming.");
476 
477 #endif // MRPT_HAS_KINECT_FREENECT
478 }
479 
481 {
482 #if MRPT_HAS_KINECT_FREENECT
483  if (f_dev)
484  {
485  freenect_stop_depth(f_dev);
486  freenect_stop_video(f_dev);
487  freenect_close_device(f_dev);
488  }
489  m_f_dev = nullptr;
490 
491  if (f_ctx) freenect_shutdown(f_ctx);
492  m_f_ctx = nullptr;
493 #endif // MRPT_HAS_KINECT_FREENECT
494 }
495 
496 /** Changes the video channel to open (RGB or IR) - you can call this method
497  before start grabbing or in the middle of streaming and the video source will
498  change on the fly.
499  Default is RGB channel.
500 */
502 {
503 #if MRPT_HAS_KINECT_FREENECT
504  m_video_channel = vch;
505  if (!isOpen()) return; // Nothing else to do here.
506 
507  // rgb or IR channel:
508  freenect_stop_video(f_dev);
509 
510  // rgb or IR channel:
511  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
512  FREENECT_RESOLUTION_MEDIUM,
514  ? FREENECT_VIDEO_IR_8BIT
515  : FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so
516  // we can directly decode it here
517  );
518 
519  // Switch to that video mode:
520  if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
521  THROW_EXCEPTION("Error setting Kinect video mode.");
522 
523  freenect_start_video(f_dev);
524 
525 #else
526  MRPT_UNUSED_PARAM(vch);
527 #endif // MRPT_HAS_KINECT_FREENECT
528 }
529 
530 /** The main data retrieving function, to be called after calling loadConfig()
531  * and initialize().
532  * \param out_obs The output retrieved observation (only if
533  * there_is_obs=true).
534  * \param there_is_obs If set to false, there was no new observation.
535  * \param hardware_error True on hardware/comms error.
536  *
537  * \sa doProcess
538  */
540  mrpt::obs::CObservation3DRangeScan& _out_obs, bool& there_is_obs,
541  bool& hardware_error)
542 {
543  there_is_obs = false;
544  hardware_error = false;
545 
546 #if MRPT_HAS_KINECT_FREENECT
547 
548  static const double max_wait_seconds = 1. / 25.;
549  static const TTimeStamp max_wait =
550  mrpt::system::secondsToTimestamp(max_wait_seconds);
551 
552  // Mark previous observation's timestamp as out-dated:
553  m_latest_obs.hasPoints3D = false;
554  m_latest_obs.hasRangeImage = false;
555  m_latest_obs.hasIntensityImage = false;
556  m_latest_obs.hasConfidenceImage = false;
557 
558  const TTimeStamp tim0 = mrpt::system::now();
559 
560  // Reset these timestamp flags so if they are !=0 in the next call we're
561  // sure they're new frames.
562  m_latest_obs_cs.lock();
563  m_tim_latest_rgb = 0;
564  m_tim_latest_depth = 0;
565  m_latest_obs_cs.unlock();
566 
567  while (freenect_process_events(f_ctx) >= 0 &&
568  mrpt::system::now() < (tim0 + max_wait))
569  {
570  // Got a new frame?
571  if ((!m_grab_image ||
572  m_tim_latest_rgb != 0) && // If we are NOT grabbing RGB or we are
573  // and there's a new frame...
574  (!m_grab_depth ||
575  m_tim_latest_depth != 0) // If we are NOT grabbing Depth or we are
576  // and there's a new frame...
577  )
578  {
579  // Approx: 0.5ms delay between depth frame (first) and RGB frame
580  // (second).
581  // cout << "m_tim_latest_rgb: " << m_tim_latest_rgb << "
582  // m_tim_latest_depth: "<< m_tim_latest_depth <<endl;
583  there_is_obs = true;
584  break;
585  }
586  }
587 
588  // Handle the case when there is NOT depth frames (if there's something very
589  // close blocking the IR sensor) but we have RGB:
590  if ((m_grab_image && m_tim_latest_rgb != 0) &&
591  (m_grab_depth && m_tim_latest_depth == 0))
592  {
593  // Mark the entire range data as invalid:
594  m_latest_obs.hasRangeImage = true;
595  m_latest_obs.range_is_depth = true;
596 
597  m_latest_obs_cs.lock(); // Important: if system is running slow, etc.
598  // we cannot tell for sure that the depth
599  // buffer is not beeing filled right now:
600  m_latest_obs.rangeImage.setSize(
602  m_latest_obs.rangeImage.setConstant(0); // "0" means: error in range
603  m_latest_obs_cs.unlock();
604  there_is_obs = true;
605  }
606 
607  if (!there_is_obs) return;
608 
609  // We DO have a fresh new observation:
610 
611  // Quick save the observation to the user's object:
612  m_latest_obs_cs.lock();
613  _out_obs.swap(m_latest_obs);
614  m_latest_obs_cs.unlock();
615 #endif
616 
617  // Set common data into observation:
618  // --------------------------------------
619  _out_obs.sensorLabel = m_sensorLabel;
620  _out_obs.timestamp = mrpt::system::now();
621  _out_obs.sensorPose = m_sensorPoseOnRobot;
623 
626 
627  // 3D point cloud:
628  if (_out_obs.hasRangeImage && m_grab_3D_points)
629  {
631 
632  if (!m_grab_depth)
633  {
634  _out_obs.hasRangeImage = false;
635  _out_obs.rangeImage.resize(0, 0);
636  }
637  }
638 
639  // preview in real-time?
640  if (m_preview_window)
641  {
642  if (_out_obs.hasRangeImage)
643  {
645  {
647  if (!m_win_range)
648  {
649  m_win_range =
650  mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
651  "Preview RANGE");
652  m_win_range->setPos(5, 5);
653  }
654 
655  // Normalize the image
657  img.setFromMatrix(_out_obs.rangeImage);
658  CMatrixFloat r =
659  _out_obs.rangeImage * float(1.0 / this->m_maxRange);
660  m_win_range->showImage(img);
661  }
662  }
663  if (_out_obs.hasIntensityImage)
664  {
666  {
668  if (!m_win_int)
669  {
670  m_win_int =
671  mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
672  "Preview INTENSITY");
673  m_win_int->setPos(300, 5);
674  }
675  m_win_int->showImage(_out_obs.intensityImage);
676  }
677  }
678  }
679  else
680  {
681  if (m_win_range) m_win_range.reset();
682  if (m_win_int) m_win_int.reset();
683  }
684 }
685 
686 /* -----------------------------------------------------
687  getNextObservation (with IMU)
688 ----------------------------------------------------- */
691  mrpt::obs::CObservationIMU& out_obs_imu, bool& there_is_obs,
692  bool& hardware_error)
693 {
694  // First, try getting the RGB+Depth data:
695  getNextObservation(out_obs, there_is_obs, hardware_error);
696 
697  // If successful, fill out the accelerometer data:
698  if (there_is_obs && this->m_grab_IMU)
699  {
700  double acc_x = 0, acc_y = 0, acc_z = 0; // In m/s^2
701  bool has_good_acc = false;
702 
703 #if MRPT_HAS_KINECT_FREENECT
704  {
705  freenect_update_tilt_state(f_dev);
706  freenect_raw_tilt_state* state = freenect_get_tilt_state(f_dev);
707  if (state)
708  {
709  has_good_acc = true;
710  double lx, ly, lz;
711  freenect_get_mks_accel(state, &lx, &ly, &lz);
712 
713  // Convert to a unified coordinate system:
714  // +x: forward
715  // +y: left
716  // +z: upward
717  acc_x = -lz;
718  acc_y = -lx;
719  acc_z = -ly;
720  }
721  }
722 #endif
723 
724  // Common part for any implementation:
725  if (has_good_acc)
726  {
727  out_obs_imu.sensorLabel = out_obs.sensorLabel + "_IMU";
728  out_obs_imu.timestamp = out_obs.timestamp;
729  out_obs_imu.sensorPose = out_obs.sensorPose;
730 
731  for (size_t i = 0; i < out_obs_imu.dataIsPresent.size(); i++)
732  out_obs_imu.dataIsPresent[i] = false;
733 
734  out_obs_imu.dataIsPresent[IMU_X_ACC] = true;
735  out_obs_imu.dataIsPresent[IMU_Y_ACC] = true;
736  out_obs_imu.dataIsPresent[IMU_Z_ACC] = true;
737 
738  out_obs_imu.rawMeasurements[IMU_X_ACC] = acc_x;
739  out_obs_imu.rawMeasurements[IMU_Y_ACC] = acc_y;
740  out_obs_imu.rawMeasurements[IMU_Z_ACC] = acc_z;
741  }
742  }
743 }
744 
745 /* -----------------------------------------------------
746  setPathForExternalImages
747 ----------------------------------------------------- */
749 {
750  MRPT_UNUSED_PARAM(directory);
751  // Ignore for now. It seems performance is better grabbing everything
752  // to a single big file than creating hundreds of smaller files per
753  // second...
754  return;
755 
756  // if (!mrpt::system::createDirectory( directory ))
757  // {
758  // THROW_EXCEPTION_FMT("Error: Cannot create the directory for
759  // externally
760  // saved images: %s",directory.c_str() )
761  // }
762  // m_path_for_external_images = directory;
763 }
764 
765 /** Change tilt angle \note Sensor must be open first. */
767 {
768  ASSERTMSG_(isOpen(), "Sensor must be open first");
769 
770 #if MRPT_HAS_KINECT_FREENECT
771  freenect_set_tilt_degs(f_dev, angle);
772 #else
773  MRPT_UNUSED_PARAM(angle);
774 #endif
775 }
776 
778 {
779  ASSERTMSG_(isOpen(), "Sensor must be open first");
780 
781 #if MRPT_KINECT_WITH_FREENECT
782  freenect_update_tilt_state(f_dev);
783  freenect_raw_tilt_state* ts = freenect_get_tilt_state(f_dev);
784  return freenect_get_tilt_degs(ts);
785 #else
786  return 0;
787 #endif
788 }
mrpt::img::TCamera m_cameraParamsRGB
Params for the RGB camera.
Definition: CKinect.h:514
mrpt::img::TCamera m_cameraParamsDepth
Params for the Depth camera.
Definition: CKinect.h:516
uint32_t nrows
Definition: TCamera.h:41
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...
unsigned __int16 uint16_t
Definition: rptypes.h:44
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.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
TVideoChannel m_video_channel
The video channel to open: RGB or IR.
Definition: CKinect.h:534
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:165
virtual void doProcess()
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
Definition: CKinect.cpp:155
double getTiltAngleDegrees()
Definition: CKinect.cpp:777
std::string m_sensorLabel
See CGenericSensor.
mrpt::gui::CDisplayWindow::Ptr m_win_int
Definition: CKinect.h:499
size_t m_preview_window_decimation
If preview is enabled, only show 1 out of N images.
Definition: CKinect.h:497
double DEG2RAD(const double x)
Degrees to radians.
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:766
double m_maxRange
Sensor max range (meters)
Definition: CKinect.h:525
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:167
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.
~CKinect()
Default ctor.
Definition: CKinect.cpp:144
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
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:3600
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:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
#define KINECT_RANGES_TABLE_LEN
Definition: CKinect.h:31
void close()
Close the Connection to the sensor (not need to call it manually unless desired for some reason...
Definition: CKinect.cpp:480
std::vector< uint8_t > m_buf_rgb
Definition: CKinect.h:538
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:32
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
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
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.h:622
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
Definition: CKinect.h:268
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:163
const GLubyte * c
Definition: glext.h:6313
GLint GLvoid * img
Definition: glext.h:3763
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:522
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:540
virtual void setPathForExternalImages(const std::string &directory)
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
Definition: CKinect.cpp:748
This namespace contains representation of robot actions and observations.
bool hasRangeImage
true means the field rangeImage contains valid data
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
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:48
double leave(const char *func_name)
End of a named section.
GLsizei const GLchar ** string
Definition: glext.h:4101
virtual void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section)
See the class documentation at the top for expected parameters.
Definition: CKinect.cpp:191
#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...
int m_user_device_number
Number of device to open (0:first,...)
Definition: CKinect.h:528
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:501
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:539
std::array< double, 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:47
CKinect()
Default ctor.
Definition: CKinect.cpp:80
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:161
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
Definition: CKinect.cpp:150
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
const GLdouble * v
Definition: glext.h:3678
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
size_t m_preview_decim_counter_range
Definition: CKinect.h:498
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:88
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:274
mrpt::poses::CPose3D m_relativePoseIntensityWRTDepth
See mrpt::obs::CObservation3DRangeScan for a diagram of this pose.
Definition: CKinect.h:518
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:239
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:30
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
bool isOpen() const
Whether there is a working connection to the sensor.
Definition: CKinect.cpp:266
bool m_preview_window
Show preview window while grabbing.
Definition: CKinect.h:495
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:33
std::vector< uint8_t > m_buf_depth
Temporary buffers for image grabbing.
Definition: CKinect.h:538
mrpt::system::TTimeStamp secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:224
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
unsigned __int32 uint32_t
Definition: rptypes.h:47
x-axis acceleration (local/vehicle frame) (m/sec2)
uint32_t ncols
Camera resolution.
Definition: TCamera.h:41
mrpt::gui::CDisplayWindow::Ptr m_win_range
Definition: CKinect.h:499
bool m_grab_image
Default: all true.
Definition: CKinect.h:531
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
size_t m_preview_decim_counter_rgb
Definition: CKinect.h:498
mrpt::poses::CPose3D m_sensorPoseOnRobot
Definition: CKinect.h:492
Grayscale or RGB visible channel of the camera sensor.
void enter(const char *func_name)
Start of a named section.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
TVideoChannel
RGB or IR video channel identifiers.
Definition: CKinect.h:277
#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:395
#define CH_RGB
Definition: img/CImage.h:45



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019