Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
13 #include <mrpt/utils/CTimeLogger.h>
15 
16 // Universal include for all versions of OpenCV
17 #include <mrpt/otherlibs/do_opencv_includes.h>
18 
19 using namespace mrpt::hwdrivers;
20 using namespace mrpt::system;
21 using namespace mrpt::synch;
22 using namespace mrpt::utils;
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 
53 #ifdef KINECT_PROFILE_MEM_ALLOC
54 mrpt::utils::CTimeLogger alloc_tim;
55 #endif
56 //int int a;
57 
59 {
60 #ifdef MRPT_KINECT_DEPTH_10BIT
61  const float k1 = 1.1863f;
62  const float k2 = 2842.5f;
63  const float k3 = 0.1236f;
64 
65  for (size_t i=0; i<KINECT_RANGES_TABLE_LEN; i++)
66  m_range2meters[i] = k3 * tanf(i/k2 + k1);
67 
68 #else
69  for (size_t i=0; i<KINECT_RANGES_TABLE_LEN; i++)
70  m_range2meters[i] = 1.0f / (i * (-0.0030711016) + 3.3309495161);
71 #endif
72 
73  // Minimum/Maximum range means error:
74  m_range2meters[0] = 0;
75  m_range2meters[KINECT_RANGES_TABLE_LEN-1] = 0;
76 }
77 
78 /*-------------------------------------------------------------
79  ctor
80  -------------------------------------------------------------*/
82  m_sensorPoseOnRobot(),
83  m_preview_window(false),
84  m_preview_window_decimation(1),
85  m_preview_decim_counter_range(0),
86  m_preview_decim_counter_rgb(0),
87 
88 #if MRPT_HAS_KINECT_FREENECT
89  m_f_ctx(NULL), // The "freenect_context", or NULL if closed
90  m_f_dev(NULL), // The "freenect_device", or NULL if closed
91  m_tim_latest_depth(0),
92  m_tim_latest_rgb(0),
93  m_latest_obs_cs("m_latest_obs_cs"),
94 #endif
95  m_relativePoseIntensityWRTDepth(0,-0.02,0, DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90)),
96  m_initial_tilt_angle(360),
97  m_user_device_number(0),
98  m_grab_image(true),
99  m_grab_depth(true),
100  m_grab_3D_points(true),
101  m_grab_IMU(true),
102  m_video_channel(VIDEO_CHANNEL_RGB)
103 {
105 
106  // Get maximum range:
107  m_maxRange=m_range2meters[KINECT_RANGES_TABLE_LEN-2]; // Recall: r[Max-1] means error.
108 
109  // Default label:
110  m_sensorLabel = "KINECT";
111 
112  // =========== Default params ===========
113  // ----- RGB -----
114  m_cameraParamsRGB.ncols = 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.zeros();
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.zeros();
134 
135 #if !MRPT_HAS_KINECT
136  THROW_EXCEPTION("MRPT was compiled without support for Kinect. Please, rebuild it.")
137 #endif
138 }
139 
140 /*-------------------------------------------------------------
141  dtor
142  -------------------------------------------------------------*/
144 {
145  this->close();
146 }
147 
148 /** This method can or cannot be implemented in the derived class, depending on the need for it.
149 * \exception This method must throw an exception with a descriptive message if some critical error is found.
150 */
152 {
153  open();
154 }
155 
156 /** This method will be invoked at a minimum rate of "process_rate" (Hz)
157 * \exception This method must throw an exception with a descriptive message if some critical error is found.
158 */
160 {
161  bool thereIs, hwError;
162 
163  CObservation3DRangeScanPtr newObs = CObservation3DRangeScan::Create();
164  CObservationIMUPtr newObs_imu = CObservationIMU::Create();
165 
166  getNextObservation( *newObs, *newObs_imu, thereIs, hwError );
167 
168  if (hwError)
169  {
170  m_state = ssError;
171  THROW_EXCEPTION("Couldn't communicate to the Kinect sensor!");
172  }
173 
174  if (thereIs)
175  {
176  m_state = ssWorking;
177 
178  vector<CSerializablePtr> objs;
179  if (m_grab_image || m_grab_depth || m_grab_3D_points) objs.push_back(newObs);
180  if (m_grab_IMU) objs.push_back(newObs_imu);
181 
182  appendObservations( objs );
183  }
184 }
185 
186 /** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes)
187 * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
188 */
190  const mrpt::utils::CConfigFileBase &configSource,
191  const std::string &iniSection )
192 {
194  configSource.read_float(iniSection,"pose_x",0),
195  configSource.read_float(iniSection,"pose_y",0),
196  configSource.read_float(iniSection,"pose_z",0),
197  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
198  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
199  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
200  );
201 
202  m_preview_window = configSource.read_bool(iniSection,"preview_window",m_preview_window);
203 
204  // "Stereo" calibration data:
205  // [<SECTION>_LEFT] // Depth
206  // ...
207  // [<SECTION>_RIGHT] // RGB
208  // ...
209  // [<SECTION>_LEFT2RIGHT_POSE]
210  // pose_quaternion = [x y z qr qx qy qz]
211 
212  const mrpt::poses::CPose3D twist(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90));
213 
215  sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we fail to load from cfg at least we have some reasonable numbers.
218 
219  try {
220  sc.loadFromConfigFile(iniSection,configSource);
221  } catch (std::exception &e) {
222  std::cout << "[CKinect::loadConfig_sensorSpecific] Warning: Ignoring error loading calibration parameters:\n" << e.what();
223  }
227 
228  // Id:
229  m_user_device_number = configSource.read_int(iniSection,"device_number",m_user_device_number );
230 
231  m_grab_image = configSource.read_bool(iniSection,"grab_image",m_grab_image);
232  m_grab_depth = configSource.read_bool(iniSection,"grab_depth",m_grab_depth);
233  m_grab_3D_points = 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>(iniSection,"video_channel",m_video_channel);
237 
238  {
239  std::string s = configSource.read_string(iniSection,"relativePoseIntensityWRTDepth","");
240  if (!s.empty())
242  }
243 
244  m_initial_tilt_angle = configSource.read_int(iniSection,"initial_tilt_angle",m_initial_tilt_angle);
245 }
246 
247 bool CKinect::isOpen() const
248 {
249 #if MRPT_HAS_KINECT_FREENECT
250  return f_dev != NULL;
251 #else
252  return false;
253 #endif
254 
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  uint16_t *depth = reinterpret_cast<uint16_t *>(v_depth);
266 
267  CKinect *obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
268 
269  // Update of the timestamps at the end:
270  mrpt::synch::CCriticalSectionLocker 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, it'll
292  // transformed later on in getNextObservation()
293  const uint16_t v = *depth++;
294  obs.rangeImage.coeffRef(r,c) = r2m[v & KINECT_RANGES_TABLE_MASK];
295  }
296  obj->internal_tim_latest_depth() = timestamp;
297 }
298 
299 void rgb_cb(freenect_device *dev, void *img_data, uint32_t timestamp)
300 {
301  CKinect *obj = reinterpret_cast<CKinect*>(freenect_get_user(dev));
302  const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
303 
304  // Update of the timestamps at the end:
305  mrpt::synch::CCriticalSectionLocker lock( &obj->internal_latest_obs_cs() );
306  CObservation3DRangeScan &obs = obj->internal_latest_obs();
307 
308 #ifdef KINECT_PROFILE_MEM_ALLOC
309  alloc_tim.enter("depth_rgb loadFromMemoryBuffer");
310 #endif
311 
312  obs.hasIntensityImage = true;
313  if (obj->getVideoChannel()==CKinect::VIDEO_CHANNEL_RGB)
314  {
315  // Color image: We asked for Bayer data, so we can decode it outselves here
316  // and avoid having to reorder Green<->Red channels, as would be needed with
317  // the RGB image from freenect.
318  obs.intensityImageChannel = mrpt::obs::CObservation3DRangeScan::CH_VISIBLE;
319  obs.intensityImage.resize(frMode.width, frMode.height, CH_RGB, true /* origin=top-left */ );
320 
321 #if MRPT_HAS_OPENCV
322 # if MRPT_OPENCV_VERSION_NUM<0x200
323  // Version for VERY OLD OpenCV versions:
324  IplImage *src_img_bayer = cvCreateImageHeader(cvSize(frMode.width,frMode.height),8,1);
325  src_img_bayer->imageDataOrigin = reinterpret_cast<char*>(img_data);
326  src_img_bayer->imageData = src_img_bayer->imageDataOrigin;
327  src_img_bayer->widthStep = frMode.width;
328 
329  IplImage *dst_img_RGB = obs.intensityImage.getAs<IplImage>();
330 
331  // Decode Bayer image:
332  cvCvtColor(src_img_bayer, dst_img_RGB, CV_BayerGB2BGR);
333 
334 # else
335  // Version for modern OpenCV:
336  const cv::Mat src_img_bayer( frMode.height, frMode.width, CV_8UC1, img_data, frMode.width );
337 
338  cv::Mat dst_img_RGB= cv::cvarrToMat( obs.intensityImage.getAs<IplImage>(), false /* dont copy buffers */ );
339 
340  // Decode Bayer image:
341  cv::cvtColor(src_img_bayer, dst_img_RGB, CV_BayerGB2BGR);
342 # endif
343 #else
344  THROW_EXCEPTION("Need building with OpenCV!")
345 #endif
346 
347  }
348  else
349  {
350  // IR data: grayscale 8bit
351  obs.intensityImageChannel = mrpt::obs::CObservation3DRangeScan::CH_IR;
352  obs.intensityImage.loadFromMemoryBuffer(
353  frMode.width,
354  frMode.height,
355  false, // Color image?
356  reinterpret_cast<unsigned char*>(img_data)
357  );
358 
359  }
360 
361  //obs.intensityImage.setChannelsOrder_RGB();
362 
363 #ifdef KINECT_PROFILE_MEM_ALLOC
364  alloc_tim.leave("depth_rgb loadFromMemoryBuffer");
365 #endif
366 
367  obj->internal_tim_latest_rgb() = timestamp;
368 }
369 // ======== END OF GLOBAL CALLBACK FUNCTIONS ========
370 #endif // MRPT_HAS_KINECT_FREENECT
371 
372 
374 {
375  if (isOpen())
376  close();
377 
378  // Alloc memory, if this is the first time:
379  m_buf_depth.resize(640*480*3); // We'll resize this below if needed
380  m_buf_rgb.resize(640*480*3);
381 
382 #if MRPT_HAS_KINECT_FREENECT // ----> libfreenect
383  // Try to open the device:
384  if (freenect_init(f_ctx_ptr, NULL) < 0)
385  THROW_EXCEPTION("freenect_init() failed")
386 
387  freenect_set_log_level(f_ctx,
388 #ifdef _DEBUG
389  FREENECT_LOG_DEBUG
390 #else
391  FREENECT_LOG_WARNING
392 #endif
393  );
394 
395  int nr_devices = freenect_num_devices(f_ctx);
396  //printf("[CKinect] Number of devices found: %d\n", nr_devices);
397 
398  if (!nr_devices)
399  THROW_EXCEPTION("No Kinect devices found.")
400 
401  // Open the given device number:
402  if (freenect_open_device(f_ctx, f_dev_ptr, m_user_device_number) < 0)
403  THROW_EXCEPTION_FMT("Error opening Kinect sensor with index: %d",m_user_device_number)
404 
405  // Setup:
406  if (m_initial_tilt_angle!=360) // 360 means no motor command.
408  freenect_set_led(f_dev,LED_RED);
409  freenect_set_depth_callback(f_dev, depth_cb);
410  freenect_set_video_callback(f_dev, rgb_cb);
411 
412  // rgb or IR channel:
413  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
414  FREENECT_RESOLUTION_MEDIUM,
416  FREENECT_VIDEO_IR_8BIT
417  :
418  FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so we can directly decode it here
419  );
420 
421  // Switch to that video mode:
422  if (freenect_set_video_mode(f_dev, desiredFrMode)<0)
423  THROW_EXCEPTION("Error setting Kinect video mode.")
424 
425 
426  // Get video mode:
427  const freenect_frame_mode frMode = freenect_get_current_video_mode(f_dev);
428 
429  // Realloc mem:
430  m_buf_depth.resize(frMode.width*frMode.height*3);
431  m_buf_rgb.resize(frMode.width*frMode.height*3);
432 
433  // Save resolution:
434  m_cameraParamsRGB.ncols = frMode.width;
435  m_cameraParamsRGB.nrows = frMode.height;
436 
437  m_cameraParamsDepth.ncols = frMode.width;
438  m_cameraParamsDepth.nrows = frMode.height;
439 
440  freenect_set_video_buffer(f_dev, &m_buf_rgb[0]);
441  freenect_set_depth_buffer(f_dev, &m_buf_depth[0]);
442 
443  freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_10BIT));
444 
445  // Set user data = pointer to "this":
446  freenect_set_user(f_dev, this);
447 
448  if (freenect_start_depth(f_dev)<0)
449  THROW_EXCEPTION("Error starting depth streaming.")
450 
451  if (freenect_start_video(f_dev)<0)
452  THROW_EXCEPTION("Error starting video streaming.")
453 
454 #endif // MRPT_HAS_KINECT_FREENECT
455 }
456 
458 {
459 #if MRPT_HAS_KINECT_FREENECT
460  if (f_dev)
461  {
462  freenect_stop_depth(f_dev);
463  freenect_stop_video(f_dev);
464  freenect_close_device(f_dev);
465  }
466  m_f_dev = NULL;
467 
468  if (f_ctx)
469  freenect_shutdown(f_ctx);
470  m_f_ctx = NULL;
471 #endif // MRPT_HAS_KINECT_FREENECT
472 }
473 
474 /** Changes the video channel to open (RGB or IR) - you can call this method before start grabbing or in the middle of streaming and the video source will change on the fly.
475  Default is RGB channel.
476 */
478 {
479 #if MRPT_HAS_KINECT_FREENECT
480  m_video_channel = vch;
481  if (!isOpen()) return; // Nothing else to do here.
482 
483  // rgb or IR channel:
484  freenect_stop_video(f_dev);
485 
486  // rgb or IR channel:
487  const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
488  FREENECT_RESOLUTION_MEDIUM,
490  FREENECT_VIDEO_IR_8BIT
491  :
492  FREENECT_VIDEO_BAYER // FREENECT_VIDEO_RGB: Use Bayer instead so we can directly decode it here
493  );
494 
495  // Switch to that video mode:
496  if (freenect_set_video_mode(f_dev, desiredFrMode)<0)
497  THROW_EXCEPTION("Error setting Kinect video mode.")
498 
499  freenect_start_video(f_dev);
500 
501 #else
502  MRPT_UNUSED_PARAM(vch);
503 #endif // MRPT_HAS_KINECT_FREENECT
504 }
505 
506 
507 /** The main data retrieving function, to be called after calling loadConfig() and initialize().
508  * \param out_obs The output retrieved observation (only if there_is_obs=true).
509  * \param there_is_obs If set to false, there was no new observation.
510  * \param hardware_error True on hardware/comms error.
511  *
512  * \sa doProcess
513  */
516  bool &there_is_obs,
517  bool &hardware_error )
518 {
519  there_is_obs=false;
520  hardware_error = false;
521 
522 #if MRPT_HAS_KINECT_FREENECT
523 
524  static const double max_wait_seconds = 1./25.;
525  static const TTimeStamp max_wait = mrpt::system::secondsToTimestamp(max_wait_seconds);
526 
527  // Mark previous observation's timestamp as out-dated:
528  m_latest_obs.hasPoints3D = false;
529  m_latest_obs.hasRangeImage = false;
530  m_latest_obs.hasIntensityImage = false;
531  m_latest_obs.hasConfidenceImage = false;
532 
533  const TTimeStamp tim0 = mrpt::system::now();
534 
535  // Reset these timestamp flags so if they are !=0 in the next call we're sure they're new frames.
536  m_latest_obs_cs.enter();
537  m_tim_latest_rgb = 0;
538  m_tim_latest_depth = 0;
539  m_latest_obs_cs.leave();
540 
541  while (freenect_process_events(f_ctx)>=0 && mrpt::system::now()<(tim0+max_wait) )
542  {
543  // Got a new frame?
544  if ( (!m_grab_image || m_tim_latest_rgb!=0) && // If we are NOT grabbing RGB or we are and there's a new frame...
545  (!m_grab_depth || m_tim_latest_depth!=0) // If we are NOT grabbing Depth or we are and there's a new frame...
546  )
547  {
548  // Approx: 0.5ms delay between depth frame (first) and RGB frame (second).
549  //cout << "m_tim_latest_rgb: " << m_tim_latest_rgb << " m_tim_latest_depth: "<< m_tim_latest_depth <<endl;
550  there_is_obs=true;
551  break;
552  }
553  }
554 
555  // Handle the case when there is NOT depth frames (if there's something very close blocking the IR sensor) but we have RGB:
556  if ( (m_grab_image && m_tim_latest_rgb!=0) &&
557  (m_grab_depth && m_tim_latest_depth==0) )
558  {
559  // Mark the entire range data as invalid:
560  m_latest_obs.hasRangeImage = true;
561  m_latest_obs.range_is_depth = true;
562 
563  m_latest_obs_cs.enter(); // Important: if system is running slow, etc. we cannot tell for sure that the depth buffer is not beeing filled right now:
564  m_latest_obs.rangeImage.setSize(m_cameraParamsDepth.nrows,m_cameraParamsDepth.ncols);
565  m_latest_obs.rangeImage.setConstant(0); // "0" means: error in range
566  m_latest_obs_cs.leave();
567  there_is_obs=true;
568  }
569 
570 
571  if (!there_is_obs)
572  return;
573 
574 
575  // We DO have a fresh new observation:
576 
577  // Quick save the observation to the user's object:
578  m_latest_obs_cs.enter();
579  _out_obs.swap(m_latest_obs);
580  m_latest_obs_cs.leave();
581 #endif
582 
583  // Set common data into observation:
584  // --------------------------------------
585  _out_obs.sensorLabel = m_sensorLabel;
586  _out_obs.timestamp = mrpt::system::now();
587  _out_obs.sensorPose = m_sensorPoseOnRobot;
589 
592 
593  // 3D point cloud:
594  if ( _out_obs.hasRangeImage && m_grab_3D_points )
595  {
597 
598  if ( !m_grab_depth )
599  {
600  _out_obs.hasRangeImage = false;
601  _out_obs.rangeImage.resize(0,0);
602  }
603 
604  }
605 
606  // preview in real-time?
607  if (m_preview_window)
608  {
609  if ( _out_obs.hasRangeImage )
610  {
612  {
614  if (!m_win_range) { m_win_range = mrpt::gui::CDisplayWindow::Create("Preview RANGE"); m_win_range->setPos(5,5); }
615 
616  // Normalize the image
618  img.setFromMatrix(_out_obs.rangeImage);
619  CMatrixFloat r = _out_obs.rangeImage * float(1.0/this->m_maxRange);
620  m_win_range->showImage(img);
621  }
622  }
623  if ( _out_obs.hasIntensityImage )
624  {
626  {
628  if (!m_win_int) { m_win_int = mrpt::gui::CDisplayWindow::Create("Preview INTENSITY"); m_win_int->setPos(300,5); }
629  m_win_int->showImage(_out_obs.intensityImage );
630  }
631  }
632  }
633  else
634  {
635  if (m_win_range) m_win_range.clear();
636  if (m_win_int) m_win_int.clear();
637  }
638 }
639 
640 /* -----------------------------------------------------
641  getNextObservation (with IMU)
642 ----------------------------------------------------- */
645  mrpt::obs::CObservationIMU &out_obs_imu,
646  bool &there_is_obs,
647  bool &hardware_error )
648 {
649  // First, try getting the RGB+Depth data:
650  getNextObservation(out_obs,there_is_obs, hardware_error);
651 
652  // If successful, fill out the accelerometer data:
653  if (there_is_obs && this->m_grab_IMU)
654  {
655  double acc_x=0,acc_y=0,acc_z=0; // In m/s^2
656  bool has_good_acc=false;
657 
658 #if MRPT_HAS_KINECT_FREENECT
659  {
660  freenect_update_tilt_state(f_dev);
661  freenect_raw_tilt_state* state = freenect_get_tilt_state(f_dev);
662  if (state)
663  {
664  has_good_acc = true;
665  double lx,ly,lz;
666  freenect_get_mks_accel(state, &lx, &ly, &lz);
667 
668  // Convert to a unified coordinate system:
669  // +x: forward
670  // +y: left
671  // +z: upward
672  acc_x = -lz;
673  acc_y = -lx;
674  acc_z = -ly;
675  }
676  }
677 #endif
678 
679  // Common part for any implementation:
680  if (has_good_acc)
681  {
682  out_obs_imu.sensorLabel = out_obs.sensorLabel + "_IMU";
683  out_obs_imu.timestamp = out_obs.timestamp;
684  out_obs_imu.sensorPose = out_obs.sensorPose;
685 
686  for (size_t i=0;i<out_obs_imu.dataIsPresent.size();i++)
687  out_obs_imu.dataIsPresent[i] = false;
688 
689  out_obs_imu.dataIsPresent[IMU_X_ACC] = true;
690  out_obs_imu.dataIsPresent[IMU_Y_ACC] = true;
691  out_obs_imu.dataIsPresent[IMU_Z_ACC] = true;
692 
693  out_obs_imu.rawMeasurements[IMU_X_ACC] = acc_x;
694  out_obs_imu.rawMeasurements[IMU_Y_ACC] = acc_y;
695  out_obs_imu.rawMeasurements[IMU_Z_ACC] = acc_z;
696  }
697  }
698 }
699 
700 
701 /* -----------------------------------------------------
702  setPathForExternalImages
703 ----------------------------------------------------- */
705 {
706  MRPT_UNUSED_PARAM(directory);
707  // Ignore for now. It seems performance is better grabbing everything
708  // to a single big file than creating hundreds of smaller files per second...
709  return;
710 
711 // if (!mrpt::system::createDirectory( directory ))
712 // {
713 // THROW_EXCEPTION_FMT("Error: Cannot create the directory for externally saved images: %s",directory.c_str() )
714 // }
715 // m_path_for_external_images = directory;
716 }
717 
718 
719 /** Change tilt angle \note Sensor must be open first. */
721 {
722  ASSERTMSG_(isOpen(),"Sensor must be open first")
723 
724 #if MRPT_HAS_KINECT_FREENECT
725  freenect_set_tilt_degs(f_dev,angle);
726 #else
727  MRPT_UNUSED_PARAM(angle);
728 #endif
729 
730 }
731 
733 {
734  ASSERTMSG_(isOpen(),"Sensor must be open first")
735 
736 #if MRPT_KINECT_WITH_FREENECT
737  freenect_update_tilt_state(f_dev);
738  freenect_raw_tilt_state *ts=freenect_get_tilt_state(f_dev);
739  return freenect_get_tilt_degs(ts);
740 #else
741  return 0;
742 #endif
743 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
void appendObservations(const std::vector< mrpt::utils::CSerializablePtr > &obj)
This method must be called by derived classes to enqueue a new observation in the list to be returned...
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
unsigned __int16 uint16_t
Definition: rptypes.h:46
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:29
TVideoChannel m_video_channel
The video channel to open: RGB or IR.
Definition: CKinect.h:391
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
virtual void doProcess()
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
Definition: CKinect.cpp:159
double getTiltAngleDegrees()
Definition: CKinect.cpp:732
std::string m_sensorLabel
See CGenericSensor.
size_t m_preview_window_decimation
If preview is enabled, only show 1 out of N images.
Definition: CKinect.h:365
#define THROW_EXCEPTION(msg)
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:156
void setTiltAngleDegrees(double angle)
Change tilt angle.
Definition: CKinect.cpp:720
double m_maxRange
Sensor max range (meters)
Definition: CKinect.h:385
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:143
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
Contains classes for various device interfaces.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3545
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:3602
virtual void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &section)
See the class documentation at the top for expected parameters.
Definition: CKinect.cpp:189
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
#define KINECT_RANGES_TABLE_LEN
Definition: CKinect.h:33
void close()
Close the Connection to the sensor (not need to call it manually unless desired for some reason...
Definition: CKinect.cpp:457
std::vector< uint8_t > m_buf_rgb
Temporary buffers for image grabbing.
Definition: CKinect.h:394
#define KINECT_RANGES_TABLE_MASK
Definition: CKinect.h:34
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
void calculate_range2meters()
Compute m_range2meters at construction.
Definition: CKinect.cpp:58
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:158
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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:439
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:160
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
Definition: CKinect.h:207
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
bool m_grab_IMU
Default: all true.
Definition: CKinect.h:389
const GLubyte * c
Definition: glext.h:5590
GLint GLvoid * img
Definition: glext.h:3645
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:383
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
#define CH_RGB
Definition: CImage.h:42
TCamera rightCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:29
TDepth2RangeArray m_range2meters
The table raw depth -> range in meters.
Definition: CKinect.h:395
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:704
This namespace contains representation of robot actions and observations.
uint32_t ncols
Definition: TCamera.h:53
bool hasRangeImage
true means the field rangeImage contains valid data
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
This namespace provides multitask, synchronization utilities.
#define DEG2RAD
float TDepth2RangeArray[KINECT_RANGES_TABLE_LEN]
A typedef for an array that converts raw depth to ranges in meters.
Definition: CKinect.h:212
GLsizei const GLchar ** string
Definition: glext.h:3919
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:154
#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...
Definition: CPoint.h:17
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...
int m_user_device_number
Number of device to open (0:first,...)
Definition: CKinect.h:387
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:477
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:514
mrpt::utils::TCamera m_cameraParamsRGB
Params for the RGB camera.
Definition: CKinect.h:379
mrpt::gui::CDisplayWindowPtr m_win_int
Definition: CKinect.h:367
CKinect()
Default ctor.
Definition: CKinect.cpp:81
virtual void initialize()
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
Definition: CKinect.cpp:151
mrpt::utils::TCamera m_cameraParamsDepth
Params for the Depth camera.
Definition: CKinect.h:380
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
const GLdouble * v
Definition: glext.h:3603
static CDisplayWindowPtr Create()
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
void loadFromConfigFile(const std::string &section, const mrpt::utils::CConfigFileBase &cfg)
Load all the params from a config source, in the same format that used in saveToConfigFile().
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
size_t m_preview_decim_counter_range
Definition: CKinect.h:366
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:55
mrpt::gui::CDisplayWindowPtr m_win_range
Definition: CKinect.h:367
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:72
mrpt::poses::CPose3D m_relativePoseIntensityWRTDepth
See mrpt::obs::CObservation3DRangeScan for a diagram of this pose.
Definition: CKinect.h:381
bool hasIntensityImage
true means the field intensityImage contains valid data
uint32_t nrows
Camera resolution.
Definition: TCamera.h:53
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:254
bool isOpen() const
Whether there is a working connection to the sensor.
Definition: CKinect.cpp:247
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
bool m_preview_window
Show preview window while grabbing.
Definition: CKinect.h:364
std::vector< uint8_t > m_buf_depth
Definition: CKinect.h:394
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
unsigned __int32 uint32_t
Definition: rptypes.h:49
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
#define ASSERTMSG_(f, __ERROR_MSG)
x-axis acceleration (local/vehicle frame) (m/sec2)
size_t m_preview_decim_counter_rgb
Definition: CKinect.h:366
mrpt::poses::CPose3D m_sensorPoseOnRobot
Definition: CKinect.h:362
Grayscale or RGB visible channel of the camera sensor.
vector_bool dataIsPresent
Each entry in this vector is true if the corresponding data index contains valid data (the IMU unit s...
mrpt::poses::CPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:30
TVideoChannel
RGB or IR video channel identifiers.
Definition: CKinect.h:215
void open()
Try to open the camera (set all the parameters before calling this) - users may also call initialize(...
Definition: CKinect.cpp:373



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019