CameraRGBD.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
27 #include "rtabmap/core/CameraRGBD.h"
28 #include "rtabmap/core/util2d.h"
29 #include "rtabmap/core/CameraRGB.h"
30 #include "rtabmap/core/Graph.h"
31 #include "rtabmap/core/Version.h"
32 
33 #include <rtabmap/utilite/UEventsManager.h>
34 #include <rtabmap/utilite/UConversion.h>
35 #include <rtabmap/utilite/UStl.h>
36 #include <rtabmap/utilite/UConversion.h>
37 #include <rtabmap/utilite/UFile.h>
38 #include <rtabmap/utilite/UDirectory.h>
39 #include <rtabmap/utilite/UTimer.h>
40 #include <rtabmap/utilite/UMath.h>
41 
42 #include <opencv2/imgproc/types_c.h>
43 #if CV_MAJOR_VERSION >= 3
44 #include <opencv2/videoio/videoio_c.h>
45 #endif
46 
47 #ifdef RTABMAP_OPENNI
48 #include <pcl/io/openni_grabber.h>
49 #include <pcl/io/oni_grabber.h>
50 #include <pcl/io/openni_camera/openni_depth_image.h>
51 #include <pcl/io/openni_camera/openni_image.h>
52 #endif
53 
54 #ifdef RTABMAP_FREENECT
55 #include <libfreenect.h>
56 #ifdef FREENECT_DASH_INCLUDES
57 #include <libfreenect-registration.h>
58 #else
59 #include <libfreenect_registration.h>
60 #endif
61 #endif
62 
63 #ifdef RTABMAP_FREENECT2
64 #include <libfreenect2/libfreenect2.hpp>
65 #include <libfreenect2/frame_listener_impl.h>
66 #include <libfreenect2/registration.h>
67 #include <libfreenect2/packet_pipeline.h>
68 #include <libfreenect2/config.h>
69 #endif
70 
71 #ifdef RTABMAP_K4W2
72 #include <Kinect.h>
73 #endif
74 
75 #ifdef RTABMAP_REALSENSE
76 #include <librealsense/rs.hpp>
77 #ifdef RTABMAP_REALSENSE_SLAM
78 #include <rs_core.h>
79 #include <rs_utils.h>
80 #include <librealsense/slam/slam.h>
81 #endif
82 #endif
83 
84 #ifdef RTABMAP_REALSENSE2
85 #include <librealsense2/rs.hpp>
86 #include <librealsense2/rsutil.h>
87 #include <librealsense2/hpp/rs_processing.hpp>
88 #include <librealsense2/rs_advanced_mode.hpp>
89 #endif
90 
91 #ifdef RTABMAP_OPENNI2
92 #include <OniVersion.h>
93 #include <OpenNI.h>
94 #endif
95 
96 namespace rtabmap
97 {
98 
100 // CameraOpenNIPCL
102 CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
103  Camera(imageRate, localTransform),
104  interface_(0),
105  deviceId_(deviceId),
106  depthConstant_(0.0f)
107 {
108 }
109 
110 bool CameraOpenni::available()
111 {
112 #ifdef RTABMAP_OPENNI
113  return true;
114 #else
115  return false;
116 #endif
117 }
118 
119 CameraOpenni::~CameraOpenni()
120 {
121 #ifdef RTABMAP_OPENNI
122  UDEBUG("");
123  if(connection_.connected())
124  {
125  connection_.disconnect();
126  }
127 
128  if(interface_)
129  {
130  interface_->stop();
131  uSleep(1000); // make sure it is stopped
132  delete interface_;
133  interface_ = 0;
134  }
135 #endif
136 }
137 #ifdef RTABMAP_OPENNI
138 void CameraOpenni::image_cb (
139  const boost::shared_ptr<openni_wrapper::Image>& rgb,
140  const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
141  float constant)
142 {
143  UScopeMutex s(dataMutex_);
144 
145  bool notify = rgb_.empty();
146 
147  cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
148  rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
149  cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
150 
151  depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
152  depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
153 
154  depthConstant_ = constant;
155 
156  if(notify)
157  {
158  dataReady_.release();
159  }
160 }
161 #endif
162 
163 bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName)
164 {
165 #ifdef RTABMAP_OPENNI
166  if(interface_)
167  {
168  interface_->stop();
169  uSleep(100); // make sure it is stopped
170  delete interface_;
171  interface_ = 0;
172  }
173 
174  try
175  {
176  if(UFile::getExtension(deviceId_).compare("oni") == 0)
177  {
178  interface_ = new pcl::ONIGrabber(deviceId_, false, true);
179  }
180  else
181  {
182  interface_ = new pcl::OpenNIGrabber(deviceId_);
183  }
184 
185  boost::function<void (
186  const boost::shared_ptr<openni_wrapper::Image>&,
187  const boost::shared_ptr<openni_wrapper::DepthImage>&,
188  float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
189  connection_ = interface_->registerCallback (f);
190 
191  interface_->start ();
192  }
193  catch(const pcl::IOException& ex)
194  {
195  UERROR("OpenNI exception: %s", ex.what());
196  if(interface_)
197  {
198  delete interface_;
199  interface_ = 0;
200  }
201  return false;
202  }
203  return true;
204 #else
205  UERROR("PCL not built with OpenNI! Cannot initialize CameraOpenNI");
206  return false;
207 #endif
208 }
209 
210 bool CameraOpenni::isCalibrated() const
211 {
212 #ifdef RTABMAP_OPENNI
213  return true;
214 #else
215  return false;
216 #endif
217 }
218 
219 std::string CameraOpenni::getSerial() const
220 {
221 #ifdef RTABMAP_OPENNI
222  if(interface_)
223  {
224  return interface_->getName();
225  }
226 #endif
227  return "";
228 }
229 
230 SensorData CameraOpenni::captureImage(CameraInfo * info)
231 {
232  SensorData data;
233 #ifdef RTABMAP_OPENNI
234  if(interface_ && interface_->isRunning())
235  {
236  if(!dataReady_.acquire(1, 5000))
237  {
238  UWARN("Not received new frames since 5 seconds, end of stream reached!");
239  }
240  else
241  {
242  UScopeMutex s(dataMutex_);
243  if(depthConstant_ && !rgb_.empty() && !depth_.empty())
244  {
245  CameraModel model(
246  1.0f/depthConstant_, //fx
247  1.0f/depthConstant_, //fy
248  float(rgb_.cols/2) - 0.5f, //cx
249  float(rgb_.rows/2) - 0.5f, //cy
250  this->getLocalTransform(),
251  0,
252  rgb_.size());
253  data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now());
254  }
255 
256  depth_ = cv::Mat();
257  rgb_ = cv::Mat();
258  depthConstant_ = 0.0f;
259  }
260  }
261 #else
262  UERROR("CameraOpenNI: RTAB-Map is not built with PCL having OpenNI support!");
263 #endif
264  return data;
265 }
266 
267 
268 
270 // CameraOpenNICV
272 bool CameraOpenNICV::available()
273 {
274  return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos;
275 }
276 
277 CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
278  Camera(imageRate, localTransform),
279  _asus(asus),
280  _depthFocal(0.0f)
281 {
282 
283 }
284 
285 CameraOpenNICV::~CameraOpenNICV()
286 {
287  _capture.release();
288 }
289 
290 bool CameraOpenNICV::init(const std::string & calibrationFolder, const std::string & cameraName)
291 {
292  if(_capture.isOpened())
293  {
294  _capture.release();
295  }
296 
297  ULOGGER_DEBUG("Camera::init()");
298  _capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
299  if(_capture.isOpened())
300  {
301  _capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
302  _depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
303  // Print some avalible device settings.
304  UINFO("Depth generator output mode:");
305  UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
306  UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
307  UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
308  UINFO("BASELINE %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
309  UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
310  UINFO("Focal %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
311  UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
312  if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
313  {
314  UERROR("Depth registration is not activated on this device!");
315  }
316  if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
317  {
318  UINFO("Image generator output mode:");
319  UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
320  UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
321  UINFO("FPS %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
322  }
323  else
324  {
325  UERROR("Camera: Device doesn't contain image generator.");
326  _capture.release();
327  return false;
328  }
329  }
330  else
331  {
332  ULOGGER_ERROR("Camera: Failed to create a capture object!");
333  _capture.release();
334  return false;
335  }
336  return true;
337 }
338 
339 bool CameraOpenNICV::isCalibrated() const
340 {
341  return true;
342 }
343 
344 SensorData CameraOpenNICV::captureImage(CameraInfo * info)
345 {
346  SensorData data;
347  if(_capture.isOpened())
348  {
349  _capture.grab();
350  cv::Mat depth, rgb;
351  _capture.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP );
352  _capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE );
353 
354  depth = depth.clone();
355  rgb = rgb.clone();
356 
357  UASSERT(_depthFocal>0.0f);
358  if(!rgb.empty() && !depth.empty())
359  {
360  CameraModel model(
361  _depthFocal, //fx
362  _depthFocal, //fy
363  float(rgb.cols/2) - 0.5f, //cx
364  float(rgb.rows/2) - 0.5f, //cy
365  this->getLocalTransform(),
366  0,
367  rgb.size());
368  data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
369  }
370  }
371  else
372  {
373  ULOGGER_WARN("The camera must be initialized before requesting an image.");
374  }
375  return data;
376 }
377 
378 
380 // CameraOpenNI2
382 bool CameraOpenNI2::available()
383 {
384 #ifdef RTABMAP_OPENNI2
385  return true;
386 #else
387  return false;
388 #endif
389 }
390 
391 bool CameraOpenNI2::exposureGainAvailable()
392 {
393 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
394  return true;
395 #else
396  return false;
397 #endif
398 }
399 
400 CameraOpenNI2::CameraOpenNI2(
401  const std::string & deviceId,
402  Type type,
403  float imageRate,
404  const rtabmap::Transform & localTransform) :
405  Camera(imageRate, localTransform)
406 #ifdef RTABMAP_OPENNI2
407  ,
408  _type(type),
409  _device(new openni::Device()),
410  _color(new openni::VideoStream()),
411  _depth(new openni::VideoStream()),
412  _depthFx(0.0f),
413  _depthFy(0.0f),
414  _deviceId(deviceId),
415  _openNI2StampsAndIDsUsed(false),
416  _depthHShift(0),
417  _depthVShift(0)
418 #endif
419 {
420 }
421 
422 CameraOpenNI2::~CameraOpenNI2()
423 {
424 #ifdef RTABMAP_OPENNI2
425  _color->stop();
426  _color->destroy();
427  _depth->stop();
428  _depth->destroy();
429  _device->close();
430  openni::OpenNI::shutdown();
431 
432  delete _device;
433  delete _color;
434  delete _depth;
435 #endif
436 }
437 
438 bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
439 {
440 #ifdef RTABMAP_OPENNI2
441  if(_color && _color->getCameraSettings())
442  {
443  return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
444  }
445 #else
446  UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
447 #endif
448  return false;
449 }
450 
451 bool CameraOpenNI2::setAutoExposure(bool enabled)
452 {
453 #ifdef RTABMAP_OPENNI2
454  if(_color && _color->getCameraSettings())
455  {
456  return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
457  }
458 #else
459  UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
460 #endif
461  return false;
462 }
463 
464 bool CameraOpenNI2::setExposure(int value)
465 {
466 #ifdef RTABMAP_OPENNI2
467 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
468  if(_color && _color->getCameraSettings())
469  {
470  return _color->getCameraSettings()->setExposure(value) == openni::STATUS_OK;
471  }
472 #else
473  UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
474 #endif
475 #else
476  UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
477 #endif
478  return false;
479 }
480 
481 bool CameraOpenNI2::setGain(int value)
482 {
483 #ifdef RTABMAP_OPENNI2
484 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
485  if(_color && _color->getCameraSettings())
486  {
487  return _color->getCameraSettings()->setGain(value) == openni::STATUS_OK;
488  }
489 #else
490  UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
491 #endif
492 #else
493  UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
494 #endif
495  return false;
496 }
497 
498 bool CameraOpenNI2::setMirroring(bool enabled)
499 {
500 #ifdef RTABMAP_OPENNI2
501  if(_color->isValid() && _depth->isValid())
502  {
503  return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
504  _color->setMirroringEnabled(enabled) == openni::STATUS_OK;
505  }
506 #endif
507  return false;
508 }
509 
510 void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
511 {
512 #ifdef RTABMAP_OPENNI2
513  _openNI2StampsAndIDsUsed = used;
514 #endif
515 }
516 
517 void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
518 {
519 #ifdef RTABMAP_OPENNI2
520  UASSERT(horizontal >= 0);
521  UASSERT(vertical >= 0);
522  _depthHShift = horizontal;
523  _depthVShift = vertical;
524 #endif
525 }
526 
527 bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
528 {
529 #ifdef RTABMAP_OPENNI2
530  openni::OpenNI::initialize();
531 
532  openni::Array<openni::DeviceInfo> devices;
533  openni::OpenNI::enumerateDevices(&devices);
534  for(int i=0; i<devices.getSize(); ++i)
535  {
536  UINFO("Device %d: Name=%s URI=%s Vendor=%s",
537  i,
538  devices[i].getName(),
539  devices[i].getUri(),
540  devices[i].getVendor());
541  }
542  if(_deviceId.empty() && devices.getSize() == 0)
543  {
544  UERROR("CameraOpenNI2: No device detected!");
545  return false;
546  }
547 
548  openni::Status error = _device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str());
549  if(error != openni::STATUS_OK)
550  {
551  if(!_deviceId.empty())
552  {
553  UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", _deviceId.c_str(), error);
554  }
555  else
556  {
557 #ifdef _WIN32
558  UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", devices[0].getName(), error);
559 #else
560  UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d). Verify if \"%s\" is in udev rules: \"/lib/udev/rules.d/40-libopenni2-0.rules\". If not, add it and reboot.", devices[0].getName(), error, devices[0].getUri());
561 #endif
562  }
563 
564  _device->close();
565  openni::OpenNI::shutdown();
566  return false;
567  }
568 
569  // look for calibration files
570  _stereoModel = StereoCameraModel();
571  bool hardwareRegistration = true;
572  if(!calibrationFolder.empty())
573  {
574  // we need the serial
575  std::string calibrationName = _device->getDeviceInfo().getName();
576  if(!cameraName.empty())
577  {
578  calibrationName = cameraName;
579  }
580  _stereoModel.setName(calibrationName, "depth", "rgb");
581  hardwareRegistration = !_stereoModel.load(calibrationFolder, calibrationName, false);
582 
583  if(_type != kTypeColorDepth)
584  {
585  hardwareRegistration = false;
586  }
587 
588 
589  if((_type != kTypeColorDepth && !_stereoModel.left().isValidForRectification()) ||
590  (_type == kTypeColorDepth && !_stereoModel.right().isValidForRectification()))
591  {
592  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
593  calibrationName.c_str(), calibrationFolder.c_str());
594  }
595  else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && hardwareRegistration)
596  {
597  UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
598  calibrationName.c_str(), calibrationFolder.c_str());
599  }
600  else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && !hardwareRegistration)
601  {
602  UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
603  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
604  }
605  }
606 
607  if(UFile::getExtension(_deviceId).compare("oni")==0)
608  {
609  if(_device->getPlaybackControl() &&
610  _device->getPlaybackControl()->setRepeatEnabled(false) != openni::STATUS_OK)
611  {
612  UERROR("CameraOpenNI2: Cannot set repeat mode to false.");
613  _device->close();
614  openni::OpenNI::shutdown();
615  return false;
616  }
617  }
618  else if(_type==kTypeColorDepth && hardwareRegistration &&
619  !_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
620  {
621  UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
622  _device->close();
623  openni::OpenNI::shutdown();
624  return false;
625  }
626 
627  if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
628  _device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) == NULL)
629  {
630  UERROR("CameraOpenNI2: Cannot get sensor info for depth and %s.", _type==kTypeColorDepth?"color":"ir");
631  _device->close();
632  openni::OpenNI::shutdown();
633  return false;
634  }
635 
636  if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
637  {
638  UERROR("CameraOpenNI2: Cannot create depth stream.");
639  _device->close();
640  openni::OpenNI::shutdown();
641  return false;
642  }
643 
644  if(_color->create(*_device, _type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != openni::STATUS_OK)
645  {
646  UERROR("CameraOpenNI2: Cannot create %s stream.", _type==kTypeColorDepth?"color":"ir");
647  _depth->destroy();
648  _device->close();
649  openni::OpenNI::shutdown();
650  return false;
651  }
652 
653  if(_type==kTypeColorDepth && hardwareRegistration &&
654  _device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
655  {
656  UERROR("CameraOpenNI2: Failed to set depth/color registration.");
657  }
658 
659  if (_device->setDepthColorSyncEnabled(true) != openni::STATUS_OK)
660  {
661  UERROR("CameraOpenNI2: Failed to set depth color sync");
662  }
663 
664  _depth->setMirroringEnabled(false);
665  _color->setMirroringEnabled(false);
666 
667  const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
668  for(int i=0; i<depthVideoModes.getSize(); ++i)
669  {
670  UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
671  i,
672  depthVideoModes[i].getFps(),
673  depthVideoModes[i].getPixelFormat(),
674  depthVideoModes[i].getResolutionX(),
675  depthVideoModes[i].getResolutionY());
676  }
677 
678  const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
679  for(int i=0; i<colorVideoModes.getSize(); ++i)
680  {
681  UINFO("CameraOpenNI2: %s video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
682  _type==kTypeColorDepth?"color":"ir",
683  i,
684  colorVideoModes[i].getFps(),
685  colorVideoModes[i].getPixelFormat(),
686  colorVideoModes[i].getResolutionX(),
687  colorVideoModes[i].getResolutionY());
688  }
689 
690  openni::VideoMode mMode;
691  mMode.setFps(30);
692  mMode.setResolution(640,480);
693  mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
694  _depth->setVideoMode(mMode);
695 
696  openni::VideoMode mModeColor;
697  mModeColor.setFps(30);
698  mModeColor.setResolution(640,480);
699  mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
700  _color->setVideoMode(mModeColor);
701 
702  UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
703  _depth->getVideoMode().getFps(),
704  _depth->getVideoMode().getPixelFormat(),
705  _depth->getVideoMode().getResolutionX(),
706  _depth->getVideoMode().getResolutionY(),
707  _depth->getHorizontalFieldOfView(),
708  _depth->getVerticalFieldOfView());
709  UINFO("CameraOpenNI2: Using %s video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
710  _type==kTypeColorDepth?"color":"ir",
711  _color->getVideoMode().getFps(),
712  _color->getVideoMode().getPixelFormat(),
713  _color->getVideoMode().getResolutionX(),
714  _color->getVideoMode().getResolutionY(),
715  _color->getHorizontalFieldOfView(),
716  _color->getVerticalFieldOfView());
717 
718  if(_depth->getVideoMode().getResolutionX() != 640 ||
719  _depth->getVideoMode().getResolutionY() != 480 ||
720  _depth->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_1_MM)
721  {
722  UERROR("Could not set depth format to 640x480 pixel=%d(mm)!",
723  openni::PIXEL_FORMAT_DEPTH_1_MM);
724  _depth->destroy();
725  _color->destroy();
726  _device->close();
727  openni::OpenNI::shutdown();
728  return false;
729  }
730  if(_color->getVideoMode().getResolutionX() != 640 ||
731  _color->getVideoMode().getResolutionY() != 480 ||
732  _color->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888)
733  {
734  UERROR("Could not set %s format to 640x480 pixel=%d!",
735  _type==kTypeColorDepth?"color":"ir",
736  openni::PIXEL_FORMAT_RGB888);
737  _depth->destroy();
738  _color->destroy();
739  _device->close();
740  openni::OpenNI::shutdown();
741  return false;
742  }
743 
744  if(_color->getCameraSettings())
745  {
746  UINFO("CameraOpenNI2: AutoWhiteBalanceEnabled = %d", _color->getCameraSettings()->getAutoWhiteBalanceEnabled()?1:0);
747  UINFO("CameraOpenNI2: AutoExposureEnabled = %d", _color->getCameraSettings()->getAutoExposureEnabled()?1:0);
748 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
749  UINFO("CameraOpenNI2: Exposure = %d", _color->getCameraSettings()->getExposure());
750  UINFO("CameraOpenNI2: GAIN = %d", _color->getCameraSettings()->getGain());
751 #endif
752  }
753 
754  if(_type==kTypeColorDepth && hardwareRegistration)
755  {
756  _depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
757  _depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
758  }
759  else
760  {
761  _depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
762  _depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
763  }
764  UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
765 
766  if(_type == kTypeIR)
767  {
768  UWARN("With type IR-only, depth stream will not be started");
769  }
770 
771  if((_type != kTypeIR && _depth->start() != openni::STATUS_OK) ||
772  _color->start() != openni::STATUS_OK)
773  {
774  UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
775  _depth->stop();
776  _color->stop();
777  _depth->destroy();
778  _color->destroy();
779  _device->close();
780  openni::OpenNI::shutdown();
781  return false;
782  }
783 
784  uSleep(3000); // just to make sure the sensor is correctly initialized and exposure is set
785 
786  return true;
787 #else
788  UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
789  return false;
790 #endif
791 }
792 
793 bool CameraOpenNI2::isCalibrated() const
794 {
795  return true;
796 }
797 
798 std::string CameraOpenNI2::getSerial() const
799 {
800 #ifdef RTABMAP_OPENNI2
801  if(_device)
802  {
803  return _device->getDeviceInfo().getName();
804  }
805 #endif
806  return "";
807 }
808 
809 SensorData CameraOpenNI2::captureImage(CameraInfo * info)
810 {
811  SensorData data;
812 #ifdef RTABMAP_OPENNI2
813  int readyStream = -1;
814  if(_device->isValid() &&
815  _depth->isValid() &&
816  _color->isValid() &&
817  _device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
818  _device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != NULL)
819  {
820  openni::VideoStream* depthStream[] = {_depth};
821  openni::VideoStream* colorStream[] = {_color};
822  if((_type != kTypeIR && openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 5000) != openni::STATUS_OK) ||
823  openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 5000) != openni::STATUS_OK)
824  {
825  UWARN("No frames received since the last 5 seconds, end of stream is reached!");
826  }
827  else
828  {
829  openni::VideoFrameRef depthFrame, colorFrame;
830  if(_type != kTypeIR)
831  {
832  _depth->readFrame(&depthFrame);
833  }
834  _color->readFrame(&colorFrame);
835  cv::Mat depth, rgb;
836  if((_type == kTypeIR || depthFrame.isValid()) && colorFrame.isValid())
837  {
838  int h,w;
839  if(_type != kTypeIR)
840  {
841  h=depthFrame.getHeight();
842  w=depthFrame.getWidth();
843  depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
844  }
845  h=colorFrame.getHeight();
846  w=colorFrame.getWidth();
847  cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
848  if(_type==kTypeColorDepth)
849  {
850  cv::cvtColor(tmp, rgb, CV_RGB2BGR);
851  }
852  else // IR
853  {
854  rgb = tmp.clone();
855  }
856  }
857  UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
858  if(!rgb.empty() && (_type == kTypeIR || !depth.empty()))
859  {
860  // default calibration
861  CameraModel model(
862  _depthFx, //fx
863  _depthFy, //fy
864  float(rgb.cols/2) - 0.5f, //cx
865  float(rgb.rows/2) - 0.5f, //cy
866  this->getLocalTransform(),
867  0,
868  rgb.size());
869 
870  if(_type==kTypeColorDepth)
871  {
872  if(_stereoModel.right().isValidForRectification())
873  {
874  rgb = _stereoModel.right().rectifyImage(rgb);
875  model = _stereoModel.right();
876 
877  if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
878  {
879  if (_depthHShift > 0 || _depthVShift > 0)
880  {
881  cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
882  depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
883  depth = out;
884  }
885  depth = _stereoModel.left().rectifyImage(depth, 0);
886  depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
887  }
888  }
889  }
890  else // IR
891  {
892  if(_stereoModel.left().isValidForRectification())
893  {
894  rgb = _stereoModel.left().rectifyImage(rgb);
895  if(_type!=kTypeIR)
896  {
897  depth = _stereoModel.left().rectifyImage(depth, 0);
898  }
899  model = _stereoModel.left();
900  }
901  }
902  model.setLocalTransform(this->getLocalTransform());
903 
904  if(_openNI2StampsAndIDsUsed)
905  {
906  data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
907  }
908  else
909  {
910  data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
911  }
912  }
913  }
914  }
915  else
916  {
917  ULOGGER_WARN("The camera must be initialized before requesting an image.");
918  }
919 #else
920  UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
921 #endif
922  return data;
923 }
924 
925 #ifdef RTABMAP_FREENECT
926 //
927 // FreenectDevice
928 //
929 class FreenectDevice : public UThread {
930  public:
931  FreenectDevice(freenect_context * ctx, int index, bool color = true, bool registered = true) :
932  index_(index),
933  color_(color),
934  registered_(registered),
935  ctx_(ctx),
936  device_(0),
937  depthFocal_(0.0f)
938  {
939  UASSERT(ctx_ != 0);
940  }
941 
942  ~FreenectDevice()
943  {
944  this->join(true);
945  if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
946  }
947 
948  const std::string & getSerial() const {return serial_;}
949 
950  bool init()
951  {
952  if(device_)
953  {
954  this->join(true);
955  freenect_close_device(device_);
956  device_ = 0;
957  }
958  serial_.clear();
959  std::vector<std::string> deviceSerials;
960  freenect_device_attributes* attr_list;
961  freenect_device_attributes* item;
962  freenect_list_device_attributes(ctx_, &attr_list);
963  for (item = attr_list; item != NULL; item = item->next) {
964  deviceSerials.push_back(std::string(item->camera_serial));
965  }
966  freenect_free_device_attributes(attr_list);
967 
968  if(freenect_open_device(ctx_, &device_, index_) < 0)
969  {
970  UERROR("FreenectDevice: Cannot open Kinect");
971  return false;
972  }
973 
974  if(index_ >= 0 && index_ < (int)deviceSerials.size())
975  {
976  serial_ = deviceSerials[index_];
977  }
978  else
979  {
980  UERROR("Could not get serial for index %d", index_);
981  }
982 
983  UINFO("color=%d registered=%d", color_?1:0, registered_?1:0);
984 
985  freenect_set_user(device_, this);
986  freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, color_?FREENECT_VIDEO_RGB:FREENECT_VIDEO_IR_8BIT);
987  freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, color_ && registered_?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
988  if(!videoMode.is_valid)
989  {
990  UERROR("Freenect: video mode selected not valid!");
991  return false;
992  }
993  if(!depthMode.is_valid)
994  {
995  UERROR("Freenect: depth mode selected not valid!");
996  return false;
997  }
998  UASSERT(videoMode.data_bits_per_pixel == 8 || videoMode.data_bits_per_pixel == 24);
999  UASSERT(depthMode.data_bits_per_pixel == 16);
1000  freenect_set_video_mode(device_, videoMode);
1001  freenect_set_depth_mode(device_, depthMode);
1002  rgbIrBuffer_ = cv::Mat(cv::Size(videoMode.width,videoMode.height), color_?CV_8UC3:CV_8UC1);
1003  depthBuffer_ = cv::Mat(cv::Size(depthMode.width,depthMode.height), CV_16UC1);
1004  freenect_set_depth_buffer(device_, depthBuffer_.data);
1005  freenect_set_video_buffer(device_, rgbIrBuffer_.data);
1006  freenect_set_depth_callback(device_, freenect_depth_callback);
1007  freenect_set_video_callback(device_, freenect_video_callback);
1008 
1009  float rgb_focal_length_sxga = 1050.0f;
1010  float width_sxga = 1280.0f;
1011  float width = freenect_get_current_depth_mode(device_).width;
1012  float scale = width / width_sxga;
1013  if(color_ && registered_)
1014  {
1015  depthFocal_ = rgb_focal_length_sxga * scale;
1016  }
1017  else
1018  {
1019  freenect_registration reg = freenect_copy_registration(device_);
1020  float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
1021  freenect_destroy_registration(&reg);
1022 
1023  depthFocal_ = depth_focal_length_sxga * scale;
1024  }
1025 
1026  UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
1027  return true;
1028  }
1029 
1030  float getDepthFocal() const {return depthFocal_;}
1031 
1032  void getData(cv::Mat & rgb, cv::Mat & depth)
1033  {
1034  if(this->isRunning())
1035  {
1036  if(!dataReady_.acquire(1, 5000))
1037  {
1038  UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
1039  }
1040  else
1041  {
1042  UScopeMutex s(dataMutex_);
1043  rgb = rgbIrLastFrame_;
1044  depth = depthLastFrame_;
1045  rgbIrLastFrame_ = cv::Mat();
1046  depthLastFrame_= cv::Mat();
1047  }
1048  }
1049  }
1050 
1051 private:
1052  // Do not call directly even in child
1053  void VideoCallback(void* rgb)
1054  {
1055  UASSERT(rgbIrBuffer_.data == rgb);
1056  UScopeMutex s(dataMutex_);
1057  bool notify = rgbIrLastFrame_.empty();
1058 
1059  if(color_)
1060  {
1061  cv::cvtColor(rgbIrBuffer_, rgbIrLastFrame_, CV_RGB2BGR);
1062  }
1063  else // IrDepth
1064  {
1065  rgbIrLastFrame_ = rgbIrBuffer_.clone();
1066  }
1067  if(!depthLastFrame_.empty() && notify)
1068  {
1069  dataReady_.release();
1070  }
1071  }
1072 
1073  // Do not call directly even in child
1074  void DepthCallback(void* depth)
1075  {
1076  UASSERT(depthBuffer_.data == depth);
1077  UScopeMutex s(dataMutex_);
1078  bool notify = depthLastFrame_.empty();
1079  depthLastFrame_ = depthBuffer_.clone();
1080  if(!rgbIrLastFrame_.empty() && notify)
1081  {
1082  dataReady_.release();
1083  }
1084  }
1085 
1086  void startVideo() {
1087  if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
1088  }
1089  void stopVideo() {
1090  if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
1091  }
1092  void startDepth() {
1093  if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
1094  }
1095  void stopDepth() {
1096  if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
1097  }
1098 
1099  virtual void mainLoopBegin()
1100  {
1101  this->startDepth();
1102  this->startVideo();
1103  }
1104 
1105  virtual void mainLoop()
1106  {
1107  timeval t;
1108  t.tv_sec = 0;
1109  t.tv_usec = 10000;
1110  if(freenect_process_events_timeout(ctx_, &t) < 0)
1111  {
1112  UERROR("FreenectDevice: Cannot process freenect events");
1113  this->kill();
1114  }
1115  }
1116 
1117  virtual void mainLoopEnd()
1118  {
1119  this->stopDepth();
1120  this->stopVideo();
1121  dataReady_.release();
1122  }
1123 
1124  static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
1125  FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
1126  device->DepthCallback(depth);
1127  }
1128  static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
1129  FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
1130  device->VideoCallback(video);
1131  }
1132 
1133  //noncopyable
1134  FreenectDevice( const FreenectDevice& );
1135  const FreenectDevice& operator=( const FreenectDevice& );
1136 
1137  private:
1138  int index_;
1139  bool color_;
1140  bool registered_;
1141  std::string serial_;
1142  freenect_context * ctx_;
1143  freenect_device * device_;
1144  cv::Mat depthBuffer_;
1145  cv::Mat rgbIrBuffer_;
1146  UMutex dataMutex_;
1147  cv::Mat depthLastFrame_;
1148  cv::Mat rgbIrLastFrame_;
1149  float depthFocal_;
1150  USemaphore dataReady_;
1151 };
1152 #endif
1153 
1154 //
1155 // CameraFreenect
1156 //
1157 bool CameraFreenect::available()
1158 {
1159 #ifdef RTABMAP_FREENECT
1160  return true;
1161 #else
1162  return false;
1163 #endif
1164 }
1165 
1166 CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const Transform & localTransform) :
1167  Camera(imageRate, localTransform)
1168 #ifdef RTABMAP_FREENECT
1169  ,
1170  deviceId_(deviceId),
1171  type_(type),
1172  ctx_(0),
1173  freenectDevice_(0)
1174 #endif
1175 {
1176 #ifdef RTABMAP_FREENECT
1177  if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
1178  // claim camera
1179  freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
1180 #endif
1181 }
1182 
1183 CameraFreenect::~CameraFreenect()
1184 {
1185 #ifdef RTABMAP_FREENECT
1186  if(freenectDevice_)
1187  {
1188  freenectDevice_->join(true);
1189  delete freenectDevice_;
1190  freenectDevice_ = 0;
1191  }
1192  if(ctx_)
1193  {
1194  if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
1195  }
1196 #endif
1197 }
1198 
1199 bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
1200 {
1201 #ifdef RTABMAP_FREENECT
1202  if(freenectDevice_)
1203  {
1204  freenectDevice_->join(true);
1205  delete freenectDevice_;
1206  freenectDevice_ = 0;
1207  }
1208 
1209  if(ctx_ && freenect_num_devices(ctx_) > 0)
1210  {
1211  // look for calibration files
1212  bool hardwareRegistration = true;
1213  stereoModel_ = StereoCameraModel();
1214  if(!calibrationFolder.empty())
1215  {
1216  // we need the serial, HACK: init a temp device to get it
1217  FreenectDevice dev(ctx_, deviceId_);
1218  if(!dev.init())
1219  {
1220  UERROR("CameraFreenect: Init failed!");
1221  }
1222  std::string calibrationName = dev.getSerial();
1223  if(!cameraName.empty())
1224  {
1225  calibrationName = cameraName;
1226  }
1227  stereoModel_.setName(calibrationName, "depth", "rgb");
1228  hardwareRegistration = !stereoModel_.load(calibrationFolder, calibrationName, false);
1229 
1230  if(type_ == kTypeIRDepth)
1231  {
1232  hardwareRegistration = false;
1233  }
1234 
1235 
1236  if((type_ == kTypeIRDepth && !stereoModel_.left().isValidForRectification()) ||
1237  (type_ == kTypeColorDepth && !stereoModel_.right().isValidForRectification()))
1238  {
1239  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
1240  calibrationName.c_str(), calibrationFolder.c_str());
1241  }
1242  else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && hardwareRegistration)
1243  {
1244  UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
1245  calibrationName.c_str(), calibrationFolder.c_str());
1246  }
1247  else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && !hardwareRegistration)
1248  {
1249  UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
1250  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
1251  }
1252  }
1253 
1254  freenectDevice_ = new FreenectDevice(ctx_, deviceId_, type_==kTypeColorDepth, hardwareRegistration);
1255  if(freenectDevice_->init())
1256  {
1257  freenectDevice_->start();
1258  uSleep(3000);
1259  return true;
1260  }
1261  else
1262  {
1263  UERROR("CameraFreenect: Init failed!");
1264  }
1265  delete freenectDevice_;
1266  freenectDevice_ = 0;
1267  }
1268  else
1269  {
1270  UERROR("CameraFreenect: No devices connected!");
1271  }
1272 #else
1273  UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
1274 #endif
1275  return false;
1276 }
1277 
1278 bool CameraFreenect::isCalibrated() const
1279 {
1280  return true;
1281 }
1282 
1283 std::string CameraFreenect::getSerial() const
1284 {
1285 #ifdef RTABMAP_FREENECT
1286  if(freenectDevice_)
1287  {
1288  return freenectDevice_->getSerial();
1289  }
1290 #endif
1291  return "";
1292 }
1293 
1294 SensorData CameraFreenect::captureImage(CameraInfo * info)
1295 {
1296  SensorData data;
1297 #ifdef RTABMAP_FREENECT
1298  if(ctx_ && freenectDevice_)
1299  {
1300  if(freenectDevice_->isRunning())
1301  {
1302  cv::Mat depth,rgb;
1303  freenectDevice_->getData(rgb, depth);
1304  if(!rgb.empty() && !depth.empty())
1305  {
1306  UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
1307 
1308  // default calibration
1309  CameraModel model(
1310  freenectDevice_->getDepthFocal(), //fx
1311  freenectDevice_->getDepthFocal(), //fy
1312  float(rgb.cols/2) - 0.5f, //cx
1313  float(rgb.rows/2) - 0.5f, //cy
1314  this->getLocalTransform(),
1315  0,
1316  rgb.size());
1317 
1318  if(type_==kTypeIRDepth)
1319  {
1320  if(stereoModel_.left().isValidForRectification())
1321  {
1322  rgb = stereoModel_.left().rectifyImage(rgb);
1323  depth = stereoModel_.left().rectifyImage(depth, 0);
1324  model = stereoModel_.left();
1325  }
1326  }
1327  else
1328  {
1329  if(stereoModel_.right().isValidForRectification())
1330  {
1331  rgb = stereoModel_.right().rectifyImage(rgb);
1332  model = stereoModel_.right();
1333 
1334  if(stereoModel_.left().isValidForRectification() && !stereoModel_.stereoTransform().isNull())
1335  {
1336  depth = stereoModel_.left().rectifyImage(depth, 0);
1337  depth = util2d::registerDepth(depth, stereoModel_.left().K(), rgb.size(), stereoModel_.right().K(), stereoModel_.stereoTransform());
1338  }
1339  }
1340  }
1341  model.setLocalTransform(this->getLocalTransform());
1342 
1343  data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
1344  }
1345  }
1346  else
1347  {
1348  UERROR("CameraFreenect: Re-initialization needed!");
1349  delete freenectDevice_;
1350  freenectDevice_ = 0;
1351  }
1352  }
1353 #else
1354  UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
1355 #endif
1356  return data;
1357 }
1358 
1359 //
1360 // CameraFreenect2
1361 //
1362 bool CameraFreenect2::available()
1363 {
1364 #ifdef RTABMAP_FREENECT2
1365  return true;
1366 #else
1367  return false;
1368 #endif
1369 }
1370 
1371 CameraFreenect2::CameraFreenect2(
1372  int deviceId,
1373  Type type,
1374  float imageRate,
1375  const Transform & localTransform,
1376  float minDepth,
1377  float maxDepth,
1378  bool bilateralFiltering,
1379  bool edgeAwareFiltering,
1380  bool noiseFiltering,
1381  const std::string & pipelineName) :
1382  Camera(imageRate, localTransform)
1383 #ifdef RTABMAP_FREENECT2
1384  ,
1385  deviceId_(deviceId),
1386  type_(type),
1387  freenect2_(0),
1388  dev_(0),
1389  listener_(0),
1390  reg_(0),
1391  minKinect2Depth_(minDepth),
1392  maxKinect2Depth_(maxDepth),
1393  bilateralFiltering_(bilateralFiltering),
1394  edgeAwareFiltering_(edgeAwareFiltering),
1395  noiseFiltering_(noiseFiltering),
1396  pipelineName_(pipelineName)
1397 #endif
1398 {
1399 #ifdef RTABMAP_FREENECT2
1400  UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
1401  freenect2_ = new libfreenect2::Freenect2();
1402  switch(type_)
1403  {
1404  case kTypeColorIR:
1405  listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
1406  break;
1407  case kTypeIRDepth:
1408  listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
1409  break;
1410  case kTypeColor2DepthSD:
1411  case kTypeDepth2ColorHD:
1412  case kTypeDepth2ColorSD:
1413  default:
1414  listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
1415  break;
1416  }
1417 #endif
1418 }
1419 
1420 CameraFreenect2::~CameraFreenect2()
1421 {
1422 #ifdef RTABMAP_FREENECT2
1423  UDEBUG("");
1424  if(dev_)
1425  {
1426  dev_->stop();
1427  dev_->close();
1428  //deleted in freenect2_ destructor (Freeenect2Impl::clearDevices())
1429  }
1430  delete listener_;
1431  delete reg_;
1432  delete freenect2_;
1433  UDEBUG("");
1434 #endif
1435 }
1436 
1437 #ifdef RTABMAP_FREENECT2
1438 libfreenect2::PacketPipeline *createPacketPipelineByName(const std::string & name)
1439 {
1440  std::string availablePipelines;
1441 #if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
1442  availablePipelines += "gl ";
1443  if (name == "gl")
1444  {
1445  UINFO("Using 'gl' pipeline.");
1446  return new libfreenect2::OpenGLPacketPipeline();
1447  }
1448 #endif
1449 #if defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
1450  availablePipelines += "cuda cudakde ";
1451  if (name == "cuda")
1452  {
1453  UINFO("Using 'cuda' pipeline.");
1454  return new libfreenect2::CudaPacketPipeline();
1455  }
1456  if (name == "cudakde")
1457  {
1458  UINFO("Using 'cudakde' pipeline.");
1459  return new libfreenect2::CudaKdePacketPipeline();
1460  }
1461 #endif
1462 #if defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
1463  availablePipelines += "cl clkde ";
1464  if (name == "cl")
1465  {
1466  UINFO("Using 'cl' pipeline.");
1467  return new libfreenect2::OpenCLPacketPipeline();
1468  }
1469  if (name == "clkde")
1470  {
1471  UINFO("Using 'clkde' pipeline.");
1472  return new libfreenect2::OpenCLKdePacketPipeline();
1473  }
1474 #endif
1475  availablePipelines += "cpu";
1476  if (name == "cpu")
1477  {
1478  UINFO("Using 'cpu' pipeline.");
1479  return new libfreenect2::CpuPacketPipeline();
1480  }
1481 
1482  if (!name.empty())
1483  {
1484  UERROR("'%s' pipeline is not available. Available pipelines are: \"%s\". Default one is used instead (first one in the list).",
1485  name.c_str(), availablePipelines.c_str());
1486  }
1487 
1488  // create default pipeline
1489 #if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
1490  UINFO("Using 'gl' pipeline.");
1491  return new libfreenect2::OpenGLPacketPipeline();
1492 #elif defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
1493  UINFO("Using 'cuda' pipeline.");
1494  return new libfreenect2::CudaPacketPipeline();
1495 #elif defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
1496  UINFO("Using 'cl' pipeline.");
1497  return new libfreenect2::OpenCLPacketPipeline();
1498 #else
1499  UINFO("Using 'cpu' pipeline.");
1500  return new libfreenect2::CpuPacketPipeline();
1501 #endif
1502 }
1503 #endif
1504 
1505 bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
1506 {
1507 #ifdef RTABMAP_FREENECT2
1508  if(dev_)
1509  {
1510  dev_->stop();
1511  dev_->close();
1512  dev_ = 0; //deleted in freenect2_ destructor (Freeenect2Impl::clearDevices())
1513  }
1514 
1515  if(reg_)
1516  {
1517  delete reg_;
1518  reg_ = 0;
1519  }
1520 
1521  libfreenect2::PacketPipeline * pipeline = createPacketPipelineByName(pipelineName_);
1522 
1523  if(deviceId_ <= 0)
1524  {
1525  UDEBUG("Opening default device...");
1526  dev_ = freenect2_->openDefaultDevice(pipeline);
1527  pipeline = 0;// pipeline deleted in dev_ (Freenect2DeviceImpl::~Freenect2DeviceImpl())
1528  }
1529  else
1530  {
1531  UDEBUG("Opening device ID=%d...", deviceId_);
1532  dev_ = freenect2_->openDevice(deviceId_, pipeline);
1533  pipeline = 0;// pipeline deleted in dev_ (Freenect2DeviceImpl::~Freenect2DeviceImpl())
1534  }
1535 
1536  if(dev_)
1537  {
1538  //default
1539  //MinDepth(0.5f),
1540  //MaxDepth(4.5f),
1541  //EnableBilateralFilter(true),
1542  //EnableEdgeAwareFilter(true)
1543  libfreenect2::Freenect2Device::Config config;
1544  config.EnableBilateralFilter = bilateralFiltering_;
1545  config.EnableEdgeAwareFilter = edgeAwareFiltering_;
1546  config.MinDepth = minKinect2Depth_;
1547  config.MaxDepth = maxKinect2Depth_;
1548  dev_->setConfiguration(config);
1549 
1550  dev_->setColorFrameListener(listener_);
1551  dev_->setIrAndDepthFrameListener(listener_);
1552 
1553  dev_->start();
1554 
1555  UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
1556  UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
1557 
1558  //default registration params
1559  libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
1560  libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
1561  reg_ = new libfreenect2::Registration(depthParams, colorParams);
1562 
1563  // look for calibration files
1564  stereoModel_ = StereoCameraModel();
1565  if(!calibrationFolder.empty())
1566  {
1567  std::string calibrationName = dev_->getSerialNumber();
1568  if(!cameraName.empty())
1569  {
1570  calibrationName = cameraName;
1571  }
1572  stereoModel_.setName(calibrationName, "depth", "rgb");
1573  if(!stereoModel_.load(calibrationFolder, calibrationName, false))
1574  {
1575  UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration "
1576  "is used. Note that from version 0.11.10, calibration suffixes for Freenect2 driver have "
1577  "changed from \"_left\"->\"_depth\" and \"_right\"->\"_rgb\". You can safely rename "
1578  "the calibration files to avoid recalibrating.",
1579  calibrationName.c_str(), calibrationFolder.c_str());
1580  }
1581  else
1582  {
1583  UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
1584  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
1585 
1586  if(type_==kTypeColor2DepthSD)
1587  {
1588  UWARN("Freenect2: When using custom calibration file, type "
1589  "kTypeColor2DepthSD is not supported. kTypeDepth2ColorSD is used instead...");
1590  type_ = kTypeDepth2ColorSD;
1591  }
1592 
1593  // downscale color image by 2
1594  cv::Mat colorP = stereoModel_.right().P();
1595  cv::Size colorSize = stereoModel_.right().imageSize();
1596  if(type_ == kTypeDepth2ColorSD)
1597  {
1598  colorP.at<double>(0,0)/=2.0f; //fx
1599  colorP.at<double>(1,1)/=2.0f; //fy
1600  colorP.at<double>(0,2)/=2.0f; //cx
1601  colorP.at<double>(1,2)/=2.0f; //cy
1602  colorSize.width/=2;
1603  colorSize.height/=2;
1604  }
1605  cv::Mat depthP = stereoModel_.left().P();
1606  cv::Size depthSize = stereoModel_.left().imageSize();
1607  float ratioY = float(colorSize.height)/float(depthSize.height);
1608  float ratioX = float(colorSize.width)/float(depthSize.width);
1609  depthP.at<double>(0,0)*=ratioX; //fx
1610  depthP.at<double>(1,1)*=ratioY; //fy
1611  depthP.at<double>(0,2)*=ratioX; //cx
1612  depthP.at<double>(1,2)*=ratioY; //cy
1613  depthSize.width*=ratioX;
1614  depthSize.height*=ratioY;
1615  const CameraModel & l = stereoModel_.left();
1616  const CameraModel & r = stereoModel_.right();
1617  stereoModel_ = StereoCameraModel(stereoModel_.name(),
1618  depthSize, l.K_raw(), l.D_raw(), l.R(), depthP,
1619  colorSize, r.K_raw(), r.D_raw(), r.R(), colorP,
1620  stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
1621  stereoModel_.initRectificationMap();
1622  }
1623  }
1624 
1625  return true;
1626  }
1627  else
1628  {
1629  UERROR("CameraFreenect2: no device connected or failure opening the default one! Note that rtabmap should link on libusb of libfreenect2. "
1630  "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
1631  }
1632 #else
1633  UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
1634 #endif
1635  return false;
1636 }
1637 
1638 bool CameraFreenect2::isCalibrated() const
1639 {
1640  return true;
1641 }
1642 
1643 std::string CameraFreenect2::getSerial() const
1644 {
1645 #ifdef RTABMAP_FREENECT2
1646  if(dev_)
1647  {
1648  return dev_->getSerialNumber();
1649  }
1650 #endif
1651  return "";
1652 }
1653 
1654 SensorData CameraFreenect2::captureImage(CameraInfo * info)
1655 {
1656  SensorData data;
1657 #ifdef RTABMAP_FREENECT2
1658  if(dev_ && listener_)
1659  {
1660  libfreenect2::FrameMap frames;
1661 #ifndef LIBFREENECT2_THREADING_STDLIB
1662  UDEBUG("Waiting for new frames... If it is stalled here, rtabmap should link on libusb of libfreenect2. "
1663  "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
1664  listener_->waitForNewFrame(frames);
1665 #else
1666  if(!listener_->waitForNewFrame(frames, 1000))
1667  {
1668  UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of libfreenect2. "
1669  "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
1670  }
1671  else
1672 #endif
1673  {
1674  double stamp = UTimer::now();
1675  libfreenect2::Frame *rgbFrame = 0;
1676  libfreenect2::Frame *irFrame = 0;
1677  libfreenect2::Frame *depthFrame = 0;
1678 
1679  switch(type_)
1680  {
1681  case kTypeColorIR: //used for calibration
1682  rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
1683  irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
1684  break;
1685  case kTypeIRDepth:
1686  irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
1687  depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
1688  break;
1689  case kTypeColor2DepthSD:
1690  case kTypeDepth2ColorSD:
1691  case kTypeDepth2ColorHD:
1692  case kTypeDepth2ColorHD2:
1693  default:
1694  rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
1695  depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
1696  break;
1697  }
1698 
1699  cv::Mat rgb, depth;
1700  float fx=0,fy=0,cx=0,cy=0;
1701  if(irFrame && depthFrame)
1702  {
1703  cv::Mat irMat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data);
1704  //convert to gray scaled
1705  float maxIr_ = 0x7FFF;
1706  float minIr_ = 0x0;
1707  const float factor = 255.0f / float((maxIr_ - minIr_));
1708  rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
1709  for(int i=0; i<irMat.rows; ++i)
1710  {
1711  for(int j=0; j<irMat.cols; ++j)
1712  {
1713  rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
1714  }
1715  }
1716 
1717  cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
1718 
1719  cv::flip(rgb, rgb, 1);
1720  cv::flip(depth, depth, 1);
1721 
1722  if(stereoModel_.isValidForRectification())
1723  {
1724  //rectify
1725  rgb = stereoModel_.left().rectifyImage(rgb);
1726  depth = stereoModel_.left().rectifyDepth(depth);
1727  fx = stereoModel_.left().fx();
1728  fy = stereoModel_.left().fy();
1729  cx = stereoModel_.left().cx();
1730  cy = stereoModel_.left().cy();
1731  }
1732  else
1733  {
1734  libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
1735  fx = params.fx;
1736  fy = params.fy;
1737  cx = params.cx;
1738  cy = params.cy;
1739  }
1740  }
1741  else
1742  {
1743  //rgb + ir or rgb + depth
1744  if(stereoModel_.isValidForRectification())
1745  {
1746  cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
1747  cv::Mat rgbMat; // rtabmap uses 3 channels RGB
1748  #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
1749 
1750  cv::cvtColor(rgbMatC4, rgbMat, CV_RGBA2BGR);
1751 
1752  #else
1753 
1754  cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
1755 
1756  #endif
1757  cv::flip(rgbMat, rgb, 1);
1758 
1759  //rectify color
1760  rgb = stereoModel_.right().rectifyImage(rgb);
1761  if(irFrame)
1762  {
1763  //rectify IR
1764  cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
1765  cv::flip(depth, depth, 1);
1766  depth = stereoModel_.left().rectifyImage(depth);
1767  }
1768  else
1769  {
1770  //rectify depth
1771  cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
1772  cv::flip(depth, depth, 1);
1773 
1774  //depth = stereoModel_.left().rectifyImage(depth, 0); // ~0.5/4 ms but is more noisy
1775  depth = stereoModel_.left().rectifyDepth(depth); // ~16/25 ms
1776 
1777  bool registered = true;
1778  if(registered)
1779  {
1780  depth = util2d::registerDepth(
1781  depth,
1782  stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
1783  depth.size(),
1784  stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
1785  stereoModel_.stereoTransform());
1786  util2d::fillRegisteredDepthHoles(depth, true, false);
1787  fx = stereoModel_.right().fx();
1788  fy = stereoModel_.right().fy();
1789  cx = stereoModel_.right().cx();
1790  cy = stereoModel_.right().cy();
1791  }
1792  else
1793  {
1794  fx = stereoModel_.left().fx();
1795  fy = stereoModel_.left().fy();
1796  cx = stereoModel_.left().cx();
1797  cy = stereoModel_.left().cy();
1798  }
1799  }
1800  }
1801  else
1802  {
1803  //use data from libfreenect2
1804  if(irFrame)
1805  {
1806  cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
1807  cv::Mat rgbMat; // rtabmap uses 3 channels RGB
1808  #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
1809 
1810  cv::cvtColor(rgbMatC4, rgbMat, CV_RGB2BGR);
1811 
1812  #else
1813 
1814  cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
1815 
1816  #endif
1817  cv::flip(rgbMat, rgb, 1);
1818 
1819  cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
1820  cv::flip(depth, depth, 1);
1821  }
1822  else
1823  {
1824  //registration of the depth
1825  UASSERT(reg_!=0);
1826 
1827  float maxDepth = maxKinect2Depth_*1000.0f;
1828  float minDepth = minKinect2Depth_*1000.0f;
1829  if(type_ == kTypeColor2DepthSD || type_ == kTypeDepth2ColorHD)
1830  {
1831  cv::Mat rgbMatBGRA;
1832  libfreenect2::Frame depthUndistorted(512, 424, 4);
1833  libfreenect2::Frame rgbRegistered(512, 424, 4);
1834 
1835  // do it before registration
1836  if(noiseFiltering_)
1837  {
1838  cv::Mat depthMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
1839  for(int dx=0; dx<depthMat.cols; ++dx)
1840  {
1841  bool onEdgeX = dx==depthMat.cols-1;
1842  for(int dy=0; dy<depthMat.rows; ++dy)
1843  {
1844  bool onEdge = onEdgeX || dy==depthMat.rows-1;
1845  float z = 0.0f;
1846  float & dz = depthMat.at<float>(dy,dx);
1847  if(dz>=minDepth && dz <= maxDepth)
1848  {
1849  z = dz;
1850  if(noiseFiltering_ && !onEdge)
1851  {
1852  z=0;
1853  const float & dz1 = depthMat.at<float>(dy,dx+1);
1854  const float & dz2 = depthMat.at<float>(dy+1,dx);
1855  const float & dz3 = depthMat.at<float>(dy+1,dx+1);
1856  if( dz1>=minDepth && dz1 <= maxDepth &&
1857  dz2>=minDepth && dz2 <= maxDepth &&
1858  dz3>=minDepth && dz3 <= maxDepth)
1859  {
1860  float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
1861  float thres = 0.01f*avg;
1862 
1863  if( fabs(dz-avg) < thres &&
1864  fabs(dz1-avg) < thres &&
1865  fabs(dz2-avg) < thres &&
1866  fabs(dz3-avg) < thres)
1867  {
1868  z = dz;
1869  }
1870  }
1871  }
1872  }
1873  dz = z;
1874  }
1875  }
1876  }
1877 
1878  libfreenect2::Frame bidDepth(1920, 1082, 4); // HD
1879  reg_->apply(rgbFrame, depthFrame, &depthUndistorted, &rgbRegistered, true, &bidDepth);
1880 
1881  cv::Mat depthMat;
1882  if(type_ == kTypeColor2DepthSD)
1883  {
1884  rgbMatBGRA = cv::Mat((int)rgbRegistered.height, (int)rgbRegistered.width, CV_8UC4, rgbRegistered.data);
1885  depthMat = cv::Mat((int)depthUndistorted.height, (int)depthUndistorted.width, CV_32FC1, depthUndistorted.data);
1886 
1887  //use IR params
1888  libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
1889  fx = params.fx;
1890  fy = params.fy;
1891  cx = params.cx;
1892  cy = params.cy;
1893  }
1894  else
1895  {
1896  rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
1897  depthMat = cv::Mat((int)bidDepth.height, (int)bidDepth.width, CV_32FC1, bidDepth.data);
1898  depthMat = depthMat(cv::Range(1, 1081), cv::Range::all());
1899 
1900  //use color params
1901  libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
1902  fx = params.fx;
1903  fy = params.fy;
1904  cx = params.cx;
1905  cy = params.cy;
1906  }
1907 
1908  //filter max depth and flip
1909  depth = cv::Mat(depthMat.size(), CV_16UC1);
1910  for(int dx=0; dx<depthMat.cols; ++dx)
1911  {
1912  for(int dy=0; dy<depthMat.rows; ++dy)
1913  {
1914  unsigned short z = 0;
1915  const float & dz = depthMat.at<float>(dy,dx);
1916  if(dz>=minDepth && dz <= maxDepth)
1917  {
1918  z = (unsigned short)dz;
1919  }
1920  depth.at<unsigned short>(dy,(depthMat.cols-1)-dx) = z; //flip
1921  }
1922  }
1923 
1924  // rtabmap uses 3 channels RGB
1925  #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
1926 
1927  cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
1928 
1929  #else
1930 
1931  cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
1932 
1933  #endif
1934  cv::flip(rgb, rgb, 1);
1935  }
1936  else //register depth to color (OLD WAY)
1937  {
1938  UASSERT(type_ == kTypeDepth2ColorSD || type_ == kTypeDepth2ColorHD2);
1939  cv::Mat rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
1940  if(type_ == kTypeDepth2ColorSD)
1941  {
1942  cv::Mat tmp;
1943  cv::resize(rgbMatBGRA, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
1944  rgbMatBGRA = tmp;
1945  }
1946  // rtabmap uses 3 channels RGB
1947  #ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT
1948 
1949  cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR);
1950 
1951  #else
1952 
1953  cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
1954 
1955  #endif
1956  cv::flip(rgb, rgb, 1);
1957 
1958  cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
1959  depth = cv::Mat::zeros(rgbMatBGRA.rows, rgbMatBGRA.cols, CV_16U);
1960  for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
1961  {
1962  for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
1963  {
1964  float dz = depthFrameMat.at<float>(dy,dx);
1965  if(dz>=minDepth && dz<=maxDepth)
1966  {
1967  bool goodDepth = true;
1968  if(noiseFiltering_)
1969  {
1970  goodDepth = false;
1971  float dz1 = depthFrameMat.at<float>(dy,dx+1);
1972  float dz2 = depthFrameMat.at<float>(dy+1,dx);
1973  float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
1974  if(dz1>=minDepth && dz1 <= maxDepth &&
1975  dz2>=minDepth && dz2 <= maxDepth &&
1976  dz3>=minDepth && dz3 <= maxDepth)
1977  {
1978  float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
1979  float thres = 0.01 * avg;
1980  if( fabs(dz-avg) < thres &&
1981  fabs(dz1-avg) < thres &&
1982  fabs(dz2-avg) < thres &&
1983  fabs(dz3-avg) < thres)
1984  {
1985  goodDepth = true;
1986  }
1987  }
1988  }
1989  if(goodDepth)
1990  {
1991  float cx=-1,cy=-1;
1992  reg_->apply(dx, dy, dz, cx, cy);
1993  if(type_ == kTypeDepth2ColorSD)
1994  {
1995  cx/=2.0f;
1996  cy/=2.0f;
1997  }
1998  int rcx = cvRound(cx);
1999  int rcy = cvRound(cy);
2000  if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
2001  {
2002  unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
2003  if(zReg == 0 || zReg > (unsigned short)dz)
2004  {
2005  zReg = (unsigned short)dz;
2006  }
2007  }
2008  }
2009  }
2010  }
2011  }
2012  util2d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeDepth2ColorHD2);
2013  util2d::fillRegisteredDepthHoles(depth, type_==kTypeDepth2ColorSD, type_==kTypeDepth2ColorHD2);//second pass
2014  cv::flip(depth, depth, 1);
2015  libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
2016  fx = params.fx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
2017  fy = params.fy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
2018  cx = params.cx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
2019  cy = params.cy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
2020  }
2021  }
2022  }
2023  }
2024 
2025  CameraModel model;
2026  if(fx && fy)
2027  {
2028  model=CameraModel(
2029  fx, //fx
2030  fy, //fy
2031  cx, //cx
2032  cy, // cy
2033  this->getLocalTransform(),
2034  0,
2035  rgb.size());
2036  }
2037  data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp);
2038 
2039  listener_->release(frames);
2040  }
2041  }
2042 #else
2043  UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
2044 #endif
2045  return data;
2046 }
2047 
2048 //
2049 // CameraK4W2
2050 //
2051 
2052 #ifdef RTABMAP_K4W2
2053 // Safe release for interfaces
2054 template<class Interface>
2055 inline void SafeRelease(Interface *& pInterfaceToRelease)
2056 {
2057  if (pInterfaceToRelease != NULL)
2058  {
2059  pInterfaceToRelease->Release();
2060  pInterfaceToRelease = NULL;
2061  }
2062 }
2063 #endif
2064 
2065 bool CameraK4W2::available()
2066 {
2067 #ifdef RTABMAP_K4W2
2068  return true;
2069 #else
2070  return false;
2071 #endif
2072 }
2073 
2074 CameraK4W2::CameraK4W2(
2075  int deviceId,
2076  Type type,
2077  float imageRate,
2078  const Transform & localTransform) :
2079  Camera(imageRate, localTransform)
2080 #ifdef RTABMAP_K4W2
2081  ,
2082  type_(type),
2083  pKinectSensor_(NULL),
2084  pCoordinateMapper_(NULL),
2085  pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
2086  pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
2087  pMultiSourceFrameReader_(NULL),
2088  pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
2089  hMSEvent(NULL)
2090 #endif
2091 {
2092 }
2093 
2094 CameraK4W2::~CameraK4W2()
2095 {
2096 #ifdef RTABMAP_K4W2
2097  if (pDepthCoordinates_)
2098  {
2099  delete[] pDepthCoordinates_;
2100  pDepthCoordinates_ = NULL;
2101  }
2102 
2103  if (pColorCoordinates_)
2104  {
2105  delete[] pColorCoordinates_;
2106  pColorCoordinates_ = NULL;
2107  }
2108 
2109  if (pColorRGBX_)
2110  {
2111  delete[] pColorRGBX_;
2112  pColorRGBX_ = NULL;
2113  }
2114 
2115  close();
2116 #endif
2117 }
2118 
2119 void CameraK4W2::close()
2120 {
2121 #ifdef RTABMAP_K4W2
2122  if (pMultiSourceFrameReader_)
2123  {
2124  pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
2125  CloseHandle((HANDLE)hMSEvent);
2126  hMSEvent = NULL;
2127  }
2128 
2129  // done with frame reader
2130  SafeRelease(pMultiSourceFrameReader_);
2131 
2132  // done with coordinate mapper
2133  SafeRelease(pCoordinateMapper_);
2134 
2135  // close the Kinect Sensor
2136  if (pKinectSensor_)
2137  {
2138  pKinectSensor_->Close();
2139  }
2140 
2141  SafeRelease(pKinectSensor_);
2142 
2143  colorCameraModel_ = CameraModel();
2144 #endif
2145 }
2146 
2147 bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & cameraName)
2148 {
2149 #ifdef RTABMAP_K4W2
2150  HRESULT hr;
2151 
2152  close();
2153 
2154  hr = GetDefaultKinectSensor(&pKinectSensor_);
2155  if (FAILED(hr))
2156  {
2157  return false;
2158  }
2159 
2160  if (pKinectSensor_)
2161  {
2162  // Initialize the Kinect and get coordinate mapper and the frame reader
2163 
2164  hr = pKinectSensor_->Open();
2165 
2166  if (SUCCEEDED(hr))
2167  {
2168  hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
2169 
2170  if (SUCCEEDED(hr))
2171  {
2172  hr = pKinectSensor_->OpenMultiSourceFrameReader(
2173  FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
2174  &pMultiSourceFrameReader_);
2175 
2176  if (SUCCEEDED(hr))
2177  {
2178  hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
2179  }
2180  }
2181  }
2182  }
2183 
2184  if (!pKinectSensor_ || FAILED(hr))
2185  {
2186  UERROR("No ready Kinect found!");
2187  close();
2188  return false;
2189  }
2190 
2191  // to query camera parameters, we should wait a little
2192  uSleep(3000);
2193 
2194  // initialize color calibration if not set yet
2195  CameraIntrinsics intrinsics;
2196  hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
2197  if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
2198  {
2199  // guess color intrinsics by comparing two reprojections
2200  CameraModel depthModel(
2201  intrinsics.FocalLengthX,
2202  intrinsics.FocalLengthY,
2203  intrinsics.PrincipalPointX,
2204  intrinsics.PrincipalPointY);
2205 
2206  cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
2207  hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
2208  if (SUCCEEDED(hr))
2209  {
2210  int firstIndex = -1;
2211  int lastIndex = -1;
2212  for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
2213  {
2214  ColorSpacePoint p = pColorCoordinates_[depthIndex];
2215  // Values that are negative infinity means it is an invalid color to depth mapping so we
2216  // skip processing for this pixel
2217  if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
2218  {
2219  if (firstIndex == -1)
2220  {
2221  firstIndex = depthIndex;
2222  }
2223  lastIndex = depthIndex;
2224  }
2225  }
2226 
2227  UASSERT(firstIndex >= 0 && lastIndex >= 0);
2228  float fx, fy, cx, cy;
2229  float x1, y1, z1, x2, y2, z2;
2230  depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
2231  depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
2232  ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
2233  ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
2234  fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
2235  cx = uv1.X - (x1 / z1) * fx;
2236  fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
2237  cy = uv1.Y - (y1 / z1) * fy;
2238 
2239  colorCameraModel_ = CameraModel(
2240  fx,
2241  fy,
2242  float(cColorWidth) - cx,
2243  cy,
2244  this->getLocalTransform(),
2245  0,
2246  cv::Size(cColorWidth, cColorHeight));
2247  }
2248  }
2249 
2250  if (!colorCameraModel_.isValidForProjection())
2251  {
2252  UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
2253  close();
2254  return false;
2255  }
2256 
2257  std::string serial = getSerial();
2258  if (!serial.empty())
2259  {
2260  UINFO("Running kinect device \"%s\"", serial.c_str());
2261  }
2262 
2263  return true;
2264 #else
2265  UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
2266  return false;
2267 #endif
2268 }
2269 
2270 bool CameraK4W2::isCalibrated() const
2271 {
2272  return true;
2273 }
2274 
2275 std::string CameraK4W2::getSerial() const
2276 {
2277 #ifdef RTABMAP_K4W2
2278  if (pKinectSensor_)
2279  {
2280  wchar_t uid[255] = { 0 };
2281  // It seems to fail every time!?
2282  HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
2283  if (SUCCEEDED(hr))
2284  {
2285  std::wstring ws(uid);
2286  return std::string(ws.begin(), ws.end());
2287  }
2288  }
2289 #endif
2290  return "";
2291 }
2292 
2293 SensorData CameraK4W2::captureImage(CameraInfo * info)
2294 {
2295  SensorData data;
2296 
2297 #ifdef RTABMAP_K4W2
2298 
2299  if (!pMultiSourceFrameReader_)
2300  {
2301  return data;
2302  }
2303 
2304  HRESULT hr;
2305 
2306  //now check for frame events
2307  HANDLE handles[] = { reinterpret_cast<HANDLE>(hMSEvent) };
2308 
2309  double t = UTimer::now();
2310  while((UTimer::now()-t < 5.0) && WaitForMultipleObjects(_countof(handles), handles, false, 5000) == WAIT_OBJECT_0)
2311  {
2312  IMultiSourceFrameArrivedEventArgs* pArgs = NULL;
2313 
2314  hr = pMultiSourceFrameReader_->GetMultiSourceFrameArrivedEventData(hMSEvent, &pArgs);
2315  if (SUCCEEDED(hr))
2316  {
2317  IMultiSourceFrameReference * pFrameRef = NULL;
2318  hr = pArgs->get_FrameReference(&pFrameRef);
2319  if (SUCCEEDED(hr))
2320  {
2321  IMultiSourceFrame* pMultiSourceFrame = NULL;
2322  IDepthFrame* pDepthFrame = NULL;
2323  IColorFrame* pColorFrame = NULL;
2324 
2325  hr = pFrameRef->AcquireFrame(&pMultiSourceFrame);
2326  if (FAILED(hr))
2327  {
2328  UERROR("Failed getting latest frame.");
2329  }
2330 
2331  IDepthFrameReference* pDepthFrameReference = NULL;
2332  hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
2333  if (SUCCEEDED(hr))
2334  {
2335  hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
2336  }
2337  SafeRelease(pDepthFrameReference);
2338 
2339  IColorFrameReference* pColorFrameReference = NULL;
2340  hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
2341  if (SUCCEEDED(hr))
2342  {
2343  hr = pColorFrameReference->AcquireFrame(&pColorFrame);
2344  }
2345  SafeRelease(pColorFrameReference);
2346 
2347  if (pDepthFrame && pColorFrame)
2348  {
2349  IFrameDescription* pDepthFrameDescription = NULL;
2350  int nDepthWidth = 0;
2351  int nDepthHeight = 0;
2352  UINT nDepthBufferSize = 0;
2353  UINT16 *pDepthBuffer = NULL;
2354 
2355  IFrameDescription* pColorFrameDescription = NULL;
2356  int nColorWidth = 0;
2357  int nColorHeight = 0;
2358  ColorImageFormat imageFormat = ColorImageFormat_None;
2359  UINT nColorBufferSize = 0;
2360  RGBQUAD *pColorBuffer = NULL;
2361 
2362  // get depth frame data
2363  if (SUCCEEDED(hr))
2364  hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
2365  if (SUCCEEDED(hr))
2366  hr = pDepthFrameDescription->get_Width(&nDepthWidth);
2367  if (SUCCEEDED(hr))
2368  hr = pDepthFrameDescription->get_Height(&nDepthHeight);
2369  if (SUCCEEDED(hr))
2370  hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
2371 
2372  // get color frame data
2373  if (SUCCEEDED(hr))
2374  hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
2375  if (SUCCEEDED(hr))
2376  hr = pColorFrameDescription->get_Width(&nColorWidth);
2377  if (SUCCEEDED(hr))
2378  hr = pColorFrameDescription->get_Height(&nColorHeight);
2379  if (SUCCEEDED(hr))
2380  hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
2381  if (SUCCEEDED(hr))
2382  {
2383  if (imageFormat == ColorImageFormat_Bgra)
2384  {
2385  hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
2386  }
2387  else if (pColorRGBX_)
2388  {
2389  pColorBuffer = pColorRGBX_;
2390  nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
2391  hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
2392  }
2393  else
2394  {
2395  hr = E_FAIL;
2396  }
2397  }
2398 
2399  if(SUCCEEDED(hr))
2400  {
2401  //ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
2402  // pColorBuffer, nColorWidth, nColorHeight,
2403  // pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight);
2404 
2405  // Make sure we've received valid data
2406  if (pCoordinateMapper_ &&
2407  pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
2408  pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
2409  {
2410  if (type_ == kTypeColor2DepthSD)
2411  {
2412  HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
2413  if (SUCCEEDED(hr))
2414  {
2415  cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
2416  cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
2417  // loop over output pixels
2418  for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
2419  {
2420  DepthSpacePoint p = pDepthCoordinates_[colorIndex];
2421  // Values that are negative infinity means it is an invalid color to depth mapping so we
2422  // skip processing for this pixel
2423  if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
2424  {
2425  // To avoid black lines caused by rounding pixel values, we should set 4 pixels
2426  // At the same do mirror
2427  int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
2428  pixel_x_l = nDepthWidth - static_cast<int>(p.X);
2429  pixel_y_l = static_cast<int>(p.Y);
2430  pixel_x_h = pixel_x_l - 1;
2431  pixel_y_h = pixel_y_l + 1;
2432 
2433  const RGBQUAD* pSrc = pColorBuffer + colorIndex;
2434  if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
2435  {
2436  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
2437  ptr[0] = pSrc->rgbBlue;
2438  ptr[1] = pSrc->rgbGreen;
2439  ptr[2] = pSrc->rgbRed;
2440  depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
2441  }
2442  if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
2443  {
2444  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
2445  ptr[0] = pSrc->rgbBlue;
2446  ptr[1] = pSrc->rgbGreen;
2447  ptr[2] = pSrc->rgbRed;
2448  depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
2449  }
2450  if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
2451  {
2452  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
2453  ptr[0] = pSrc->rgbBlue;
2454  ptr[1] = pSrc->rgbGreen;
2455  ptr[2] = pSrc->rgbRed;
2456  depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
2457  }
2458  if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
2459  {
2460  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
2461  ptr[0] = pSrc->rgbBlue;
2462  ptr[1] = pSrc->rgbGreen;
2463  ptr[2] = pSrc->rgbRed;
2464  depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
2465  }
2466  }
2467  }
2468 
2469  CameraIntrinsics intrinsics;
2470  pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
2471  CameraModel model(
2472  intrinsics.FocalLengthX,
2473  intrinsics.FocalLengthY,
2474  intrinsics.PrincipalPointX,
2475  intrinsics.PrincipalPointY,
2476  this->getLocalTransform(),
2477  0,
2478  depth.size());
2479  data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
2480  }
2481  else
2482  {
2483  UERROR("Failed color to depth registration!");
2484  }
2485  }
2486  else //depthToColor
2487  {
2488  HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
2489  if (SUCCEEDED(hr))
2490  {
2491  cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
2492  cv::Mat depthRegistered = cv::Mat::zeros(
2493  type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight,
2494  type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
2495  CV_16UC1);
2496  cv::Mat imageColor;
2497  if(type_ == kTypeDepth2ColorSD)
2498  {
2499  cv::Mat tmp;
2500  cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
2501  cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
2502  }
2503  else
2504  {
2505  cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
2506  }
2507  // loop over output pixels
2508  for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
2509  {
2510  ColorSpacePoint p = pColorCoordinates_[depthIndex];
2511  // Values that are negative infinity means it is an invalid color to depth mapping so we
2512  // skip processing for this pixel
2513  if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
2514  {
2515  if (type_ == kTypeDepth2ColorSD)
2516  {
2517  p.X /= 2.0f;
2518  p.Y /= 2.0f;
2519  }
2520  const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
2521  int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
2522  // get the coordinate on image plane.
2523  pixel_x_l = depthRegistered.cols - p.X; // flip depth
2524  pixel_y_l = p.Y;
2525  pixel_x_h = pixel_x_l - 1;
2526  pixel_y_h = pixel_y_l + 1;
2527 
2528  if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
2529  pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line
2530  depth_value)
2531  {
2532  unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
2533  if (depthPixel == 0 || depthPixel > depth_value)
2534  {
2535  depthPixel = depth_value;
2536  }
2537  }
2538  if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
2539  pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line
2540  depth_value)
2541  {
2542  unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
2543  if (depthPixel == 0 || depthPixel > depth_value)
2544  {
2545  depthPixel = depth_value;
2546  }
2547  }
2548  }
2549  }
2550 
2551  CameraModel model = colorCameraModel_;
2552  if (type_ == kTypeDepth2ColorSD)
2553  {
2554  model = model.scaled(0.5);
2555  }
2556  util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
2557  depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
2558  cv::flip(imageColor, imageColor, 1);
2559  data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
2560  }
2561  else
2562  {
2563  UERROR("Failed depth to color registration!");
2564  }
2565  }
2566  }
2567  }
2568 
2569  SafeRelease(pDepthFrameDescription);
2570  SafeRelease(pColorFrameDescription);
2571  }
2572 
2573  pFrameRef->Release();
2574 
2575  SafeRelease(pDepthFrame);
2576  SafeRelease(pColorFrame);
2577  SafeRelease(pMultiSourceFrame);
2578  }
2579  pArgs->Release();
2580  }
2581  if (!data.imageRaw().empty())
2582  {
2583  break;
2584  }
2585  }
2586 #else
2587  UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
2588 #endif
2589  return data;
2590 }
2591 
2593 // CameraRealSense
2595 bool CameraRealSense::available()
2596 {
2597 #ifdef RTABMAP_REALSENSE
2598  return true;
2599 #else
2600  return false;
2601 #endif
2602 }
2603 
2604 CameraRealSense::CameraRealSense(
2605  int device,
2606  int presetRGB,
2607  int presetDepth,
2608  bool computeOdometry,
2609  float imageRate,
2610  const rtabmap::Transform & localTransform) :
2611  Camera(imageRate, localTransform)
2612 #ifdef RTABMAP_REALSENSE
2613  ,
2614  ctx_(0),
2615  dev_(0),
2616  deviceId_(device),
2617  presetRGB_(presetRGB),
2618  presetDepth_(presetDepth),
2619  computeOdometry_(computeOdometry),
2620  depthScaledToRGBSize_(false),
2621  rgbSource_(kColor),
2622  slam_(0)
2623 #endif
2624 {
2625  UDEBUG("");
2626 }
2627 
2628 CameraRealSense::~CameraRealSense()
2629 {
2630  UDEBUG("");
2631 #ifdef RTABMAP_REALSENSE
2632  UDEBUG("");
2633  if(dev_)
2634  {
2635  try
2636  {
2637  if(slam_!=0)
2638  {
2639  dev_->stop(rs::source::all_sources);
2640  }
2641  else
2642  {
2643  dev_->stop();
2644  }
2645  }
2646  catch(const rs::error & error){UWARN("%s", error.what());}
2647  dev_ = 0;
2648  }
2649  UDEBUG("");
2650  if (ctx_)
2651  {
2652  delete ctx_;
2653  }
2654 #ifdef RTABMAP_REALSENSE_SLAM
2655  UDEBUG("");
2656  if(slam_)
2657  {
2658  UScopeMutex lock(slamLock_);
2659  slam_->flush_resources();
2660  delete slam_;
2661  slam_ = 0;
2662  }
2663 #endif
2664 #endif
2665 }
2666 
2667 #ifdef RTABMAP_REALSENSE_SLAM
2668 bool setStreamConfigIntrin(
2669  rs::core::stream_type stream,
2670  std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics,
2671  rs::core::video_module_interface::supported_module_config & supported_config,
2672  rs::core::video_module_interface::actual_module_config & actual_config)
2673 {
2674  auto & supported_stream_config = supported_config[stream];
2675  if (!supported_stream_config.is_enabled || supported_stream_config.size.width != intrinsics[stream].width || supported_stream_config.size.height != intrinsics[stream].height)
2676  {
2677  UERROR("size of stream is not supported by slam");
2678  UERROR(" supported: stream %d, width: %d height: %d", (uint32_t) stream, supported_stream_config.size.width, supported_stream_config.size.height);
2679  UERROR(" received: stream %d, width: %d height: %d", (uint32_t) stream, intrinsics[stream].width, intrinsics[stream].height);
2680 
2681  return false;
2682  }
2683  rs::core::video_module_interface::actual_image_stream_config &actual_stream_config = actual_config[stream];
2684  actual_config[stream].size.width = intrinsics[stream].width;
2685  actual_config[stream].size.height = intrinsics[stream].height;
2686  actual_stream_config.frame_rate = supported_stream_config.frame_rate;
2687  actual_stream_config.intrinsics = intrinsics[stream];
2688  actual_stream_config.is_enabled = true;
2689  return true;
2690 }
2691 #endif
2692 
2693 void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
2694 #ifdef RTABMAP_REALSENSE
2695  depthScaledToRGBSize_ = enabled;
2696 #endif
2697 }
2698 void CameraRealSense::setRGBSource(RGBSource source)
2699 {
2700 #ifdef RTABMAP_REALSENSE
2701  rgbSource_ = source;
2702 #endif
2703 }
2704 
2705 #ifdef RTABMAP_REALSENSE
2706 template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
2707 {
2708  // Iterate over the pixels of the depth image
2709 #pragma omp parallel for schedule(dynamic)
2710  for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
2711  {
2712  int depth_pixel_index = depth_y * depth_intrin.width;
2713  for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
2714  {
2715  // Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
2716  if(float depth = get_depth(depth_pixel_index))
2717  {
2718  // Map the top-left corner of the depth pixel onto the other image
2719  float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
2720  rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
2721  rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
2722  rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
2723  const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
2724  const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
2725 
2726  // Map the bottom-right corner of the depth pixel onto the other image
2727  depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
2728  rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
2729  rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
2730  rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
2731  const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
2732  const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
2733 
2734  if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
2735 
2736  // Transfer between the depth pixels and the pixels inside the rectangle on the other image
2737  for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
2738  }
2739  }
2740  }
2741 }
2742 typedef uint8_t byte;
2743 
2744 void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
2745 {
2746  auto out_z = (uint16_t *)(z_aligned_to_other);
2747  align_images(z_intrin, z_to_other, other_intrin,
2748  [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
2749  [out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
2750 }
2751 
2752 void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
2753 {
2754  auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
2755  align_images(disparity_intrin, disparity_to_other, other_intrin,
2756  [disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
2757  [out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
2758 }
2759 
2760 template<int N> struct bytes { char b[N]; };
2761 template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
2762 {
2763  auto in_other = (const bytes<N> *)(other_pixels);
2764  auto out_other = (bytes<N> *)(other_aligned_to_depth);
2765  align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
2766  [out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
2767 }
2768 
2770 // Image rectification //
2772 
2773 std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
2774 {
2775  std::vector<int> rectification_table;
2776  rectification_table.resize(rect_intrin.width * rect_intrin.height);
2777  align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
2778  [&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
2779  return rectification_table;
2780 }
2781 
2782 template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
2783 {
2784  for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
2785 }
2786 
2787 void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
2788 {
2789  switch(format)
2790  {
2791  case RS_FORMAT_Y8:
2792  return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
2793  case RS_FORMAT_Y16: case RS_FORMAT_Z16:
2794  return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
2795  case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
2796  return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
2797  case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
2798  return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
2799  default:
2800  assert(false); // NOTE: rectify_image_pixels(...) is not appropriate for RS_FORMAT_YUYV images, no logic prevents U/V channels from being written to one another
2801  }
2802 }
2803 #endif
2804 
2805 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
2806 {
2807  UDEBUG("");
2808 #ifdef RTABMAP_REALSENSE
2809 
2810  if(dev_)
2811  {
2812  try
2813  {
2814  if(slam_!=0)
2815  {
2816  dev_->stop(rs::source::all_sources);
2817  }
2818  else
2819  {
2820  dev_->stop();
2821  }
2822  }
2823  catch(const rs::error & error){UWARN("%s", error.what());}
2824  dev_ = 0;
2825  }
2826  bufferedFrames_.clear();
2827  rsRectificationTable_.clear();
2828 
2829 #ifdef RTABMAP_REALSENSE_SLAM
2830  motionSeq_[0] = motionSeq_[1] = 0;
2831  if(slam_)
2832  {
2833  UScopeMutex lock(slamLock_);
2834  UDEBUG("Flush slam");
2835  slam_->flush_resources();
2836  delete slam_;
2837  slam_ = 0;
2838  }
2839 #endif
2840 
2841  if (ctx_ == 0)
2842  {
2843  ctx_ = new rs::context();
2844  }
2845 
2846  UDEBUG("");
2847  if (ctx_->get_device_count() == 0)
2848  {
2849  UERROR("No RealSense device detected!");
2850  return false;
2851  }
2852 
2853  UDEBUG("");
2854  dev_ = ctx_->get_device(deviceId_);
2855  if (dev_ == 0)
2856  {
2857  UERROR("Cannot connect to device %d", deviceId_);
2858  return false;
2859  }
2860  std::string name = dev_->get_name();
2861  UINFO("Using device %d, an %s", deviceId_, name.c_str());
2862  UINFO(" Serial number: %s", dev_->get_serial());
2863  UINFO(" Firmware version: %s", dev_->get_firmware_version());
2864  UINFO(" Preset RGB: %d", presetRGB_);
2865  UINFO(" Preset Depth: %d", presetDepth_);
2866 
2867 #ifndef RTABMAP_REALSENSE_SLAM
2868  computeOdometry_ = false;
2869 #endif
2870 
2871  if (name.find("ZR300") == std::string::npos)
2872  {
2873  // Only enable ZR300 functionality odometry if fisheye stream is enabled.
2874  // Accel/Gyro automatically enabled when fisheye requested
2875  computeOdometry_ = false;
2876  // Only ZR300 has fisheye
2877  if(rgbSource_ == kFishEye)
2878  {
2879  UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
2880  rgbSource_ = kColor;
2881  }
2882  }
2883 
2884  rs::intrinsics depth_intrin;
2885  rs::intrinsics fisheye_intrin;
2886  rs::intrinsics color_intrin;
2887  // Configure depth and color to run with the device's preferred settings
2888  UINFO("Enabling streams...");
2889  // R200:
2890  // 0=640x480 vs 480x360
2891  // 1=1920x1080 vs 640x480
2892  // 2=640x480 vs 320x240
2893  try {
2894 
2895  // left/rgb stream
2896  if(rgbSource_==kFishEye || computeOdometry_)
2897  {
2898  dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
2899  if(computeOdometry_)
2900  {
2901  // Needed to align image timestamps to common clock-domain with the motion events
2902  dev_->set_option(rs::option::fisheye_strobe, 1);
2903  }
2904  // This option causes the fisheye image to be acquired in-sync with the depth image.
2905  dev_->set_option(rs::option::fisheye_external_trigger, 1);
2906  dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
2907  fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
2908  UINFO(" Fisheye: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
2909  if(rgbSource_==kFishEye)
2910  {
2911  color_intrin = fisheye_intrin; // not rectified
2912  }
2913  }
2914  if(rgbSource_!=kFishEye)
2915  {
2916  dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
2917  color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color); // rectified
2918  UINFO(" RGB: %dx%d", color_intrin.width, color_intrin.height);
2919 
2920  if(rgbSource_==kInfrared)
2921  {
2922  dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
2923  color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared); // rectified
2924  UINFO(" IR left: %dx%d", color_intrin.width, color_intrin.height);
2925  }
2926  }
2927 
2928  dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
2929  depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); // rectified
2930  UINFO(" Depth: %dx%d", depth_intrin.width, depth_intrin.height);
2931  }
2932  catch(const rs::error & error)
2933  {
2934  UERROR("Failed starting the streams: %s", error.what());
2935  return false;
2936  }
2937 
2938  Transform imu2Camera = Transform::getIdentity();
2939 
2940 #ifdef RTABMAP_REALSENSE_SLAM
2941  UDEBUG("Setup frame callback");
2942  // Define lambda callback for receiving stream data
2943  std::function<void(rs::frame)> frameCallback = [this](rs::frame frame)
2944  {
2945  if(slam_ != 0)
2946  {
2947  const auto timestampDomain = frame.get_frame_timestamp_domain();
2948  if (rs::timestamp_domain::microcontroller != timestampDomain)
2949  {
2950  UERROR("error: Junk time stamp in stream: %d\twith frame counter: %d",
2951  (int)(frame.get_stream_type()), frame.get_frame_number());
2952  return ;
2953  }
2954  }
2955 
2956  int width = frame.get_width();
2957  int height = frame.get_height();
2958  rs::core::correlated_sample_set sample_set = {};
2959 
2960  rs::core::image_info info =
2961  {
2962  width,
2963  height,
2964  rs::utils::convert_pixel_format(frame.get_format()),
2965  frame.get_stride()
2966  };
2967  cv::Mat image;
2968  if(frame.get_format() == rs::format::raw8 || frame.get_format() == rs::format::y8)
2969  {
2970  image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
2971  if(frame.get_stream_type() == rs::stream::fisheye)
2972  {
2973  // fisheye always received just after the depth image (doesn't have exact timestamp with depth)
2974  if(bufferedFrames_.size())
2975  {
2976  bufferedFrames_.rbegin()->second.first = image.clone();
2977  UScopeMutex lock(dataMutex_);
2978  bool notify = lastSyncFrames_.first.empty();
2979  lastSyncFrames_ = bufferedFrames_.rbegin()->second;
2980  if(notify)
2981  {
2982  dataReady_.release();
2983  }
2984  bufferedFrames_.clear();
2985  }
2986  }
2987  else if(frame.get_stream_type() == rs::stream::infrared) // infrared (does have exact timestamp with depth)
2988  {
2989  if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
2990  {
2991  bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
2992  UScopeMutex lock(dataMutex_);
2993  bool notify = lastSyncFrames_.first.empty();
2994  lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
2995  if(notify)
2996  {
2997  dataReady_.release();
2998  }
2999  bufferedFrames_.erase(frame.get_timestamp());
3000  }
3001  else
3002  {
3003  bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
3004  }
3005  if(bufferedFrames_.size()>5)
3006  {
3007  UWARN("Frames cannot be synchronized!");
3008  bufferedFrames_.clear();
3009  }
3010  return;
3011  }
3012  }
3013  else if(frame.get_format() == rs::format::z16)
3014  {
3015  image = cv::Mat(height, width, CV_16UC1, (unsigned char*)frame.get_data());
3016  if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
3017  {
3018  bufferedFrames_.find(frame.get_timestamp())->second.second = image.clone();
3019  UScopeMutex lock(dataMutex_);
3020  bool notify = lastSyncFrames_.first.empty();
3021  lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
3022  if(notify)
3023  {
3024  dataReady_.release();
3025  }
3026  bufferedFrames_.erase(frame.get_timestamp());
3027  }
3028  else
3029  {
3030  bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(cv::Mat(), image.clone())));
3031  }
3032  if(bufferedFrames_.size()>5)
3033  {
3034  UWARN("Frames cannot be synchronized!");
3035  bufferedFrames_.clear();
3036  }
3037  }
3038  else if(frame.get_format() == rs::format::rgb8)
3039  {
3040  if(rsRectificationTable_.size())
3041  {
3042  image = cv::Mat(height, width, CV_8UC3);
3043  rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
3044  }
3045  else
3046  {
3047  image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
3048  }
3049  if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
3050  {
3051  bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
3052  UScopeMutex lock(dataMutex_);
3053  bool notify = lastSyncFrames_.first.empty();
3054  lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
3055  if(notify)
3056  {
3057  dataReady_.release();
3058  }
3059  bufferedFrames_.erase(frame.get_timestamp());
3060  }
3061  else
3062  {
3063  bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
3064  }
3065  if(bufferedFrames_.size()>5)
3066  {
3067  UWARN("Frames cannot be synchronized!");
3068  bufferedFrames_.clear();
3069  }
3070  return;
3071  }
3072  else
3073  {
3074  return;
3075  }
3076 
3077  if(slam_ != 0)
3078  {
3079  rs::core::stream_type stream = rs::utils::convert_stream_type(frame.get_stream_type());
3080  sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data(
3081  & info,
3082  image.data,
3083  stream,
3084  rs::core::image_interface::flag::any,
3085  frame.get_timestamp(),
3086  (uint64_t)frame.get_frame_number(),
3087  rs::core::timestamp_domain::microcontroller);
3088 
3089  UScopeMutex lock(slamLock_);
3090  if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
3091  {
3092  UERROR("error: failed to process sample");
3093  }
3094  sample_set[stream]->release();
3095  }
3096  };
3097 
3098  UDEBUG("");
3099  // Setup stream callback for stream
3100  if(computeOdometry_ || rgbSource_ == kFishEye)
3101  {
3102  dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
3103  }
3104  if(rgbSource_ == kInfrared)
3105  {
3106  dev_->set_frame_callback(rs::stream::infrared, frameCallback);
3107  }
3108  else if(rgbSource_ == kColor)
3109  {
3110  dev_->set_frame_callback(rs::stream::color, frameCallback);
3111  }
3112 
3113  dev_->set_frame_callback(rs::stream::depth, frameCallback);
3114 
3115  UDEBUG("");
3116 
3117  if (computeOdometry_)
3118  {
3119  UDEBUG("Setup motion callback");
3120  //define callback to the motion events and set it.
3121  std::function<void(rs::motion_data)> motion_callback;
3122  motion_callback = [this](rs::motion_data entry)
3123  {
3124  if ((entry.timestamp_data.source_id != RS_EVENT_IMU_GYRO) &&
3125  (entry.timestamp_data.source_id != RS_EVENT_IMU_ACCEL))
3126  return;
3127 
3128  rs_event_source motionType = entry.timestamp_data.source_id;
3129 
3130  rs::core::correlated_sample_set sample_set = {};
3131  if (motionType == RS_EVENT_IMU_ACCEL)
3132  {
3133  sample_set[rs::core::motion_type::accel].timestamp = entry.timestamp_data.timestamp;
3134  sample_set[rs::core::motion_type::accel].data[0] = (float)entry.axes[0];
3135  sample_set[rs::core::motion_type::accel].data[1] = (float)entry.axes[1];
3136  sample_set[rs::core::motion_type::accel].data[2] = (float)entry.axes[2];
3137  sample_set[rs::core::motion_type::accel].type = rs::core::motion_type::accel;
3138  ++motionSeq_[0];
3139  sample_set[rs::core::motion_type::accel].frame_number = motionSeq_[0];
3140  }
3141  else if (motionType == RS_EVENT_IMU_GYRO)
3142  {
3143  sample_set[rs::core::motion_type::gyro].timestamp = entry.timestamp_data.timestamp;
3144  sample_set[rs::core::motion_type::gyro].data[0] = (float)entry.axes[0];
3145  sample_set[rs::core::motion_type::gyro].data[1] = (float)entry.axes[1];
3146  sample_set[rs::core::motion_type::gyro].data[2] = (float)entry.axes[2];
3147  sample_set[rs::core::motion_type::gyro].type = rs::core::motion_type::gyro;
3148  ++motionSeq_[1];
3149  sample_set[rs::core::motion_type::gyro].frame_number = motionSeq_[1];
3150  }
3151 
3152  UScopeMutex lock(slamLock_);
3153  if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
3154  {
3155  UERROR("error: failed to process sample");
3156  }
3157  };
3158 
3159  std::function<void(rs::timestamp_data)> timestamp_callback;
3160  timestamp_callback = [](rs::timestamp_data entry) {};
3161 
3162  dev_->enable_motion_tracking(motion_callback, timestamp_callback);
3163  UINFO(" enabled accel and gyro stream");
3164 
3165  rs::motion_intrinsics imuIntrinsics;
3166  rs::extrinsics fisheye2ImuExtrinsics;
3167  rs::extrinsics fisheye2DepthExtrinsics;
3168  try
3169  {
3170  imuIntrinsics = dev_->get_motion_intrinsics();
3171  fisheye2ImuExtrinsics = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
3172  fisheye2DepthExtrinsics = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
3173  }
3174  catch (const rs::error & e) {
3175  UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
3176  return false;
3177  }
3178 
3179  UDEBUG("Setup SLAM");
3180  UScopeMutex lock(slamLock_);
3181  slam_ = new rs::slam::slam();
3182  slam_->set_auto_occupancy_map_building(false);
3183  slam_->force_relocalization_pose(false);
3184 
3185  rs::core::video_module_interface::supported_module_config supported_config = {};
3186  if (slam_->query_supported_module_config(0, supported_config) < rs::core::status_no_error)
3187  {
3188  UERROR("Failed to query the first supported module configuration");
3189  return false;
3190  }
3191 
3192  rs::core::video_module_interface::actual_module_config actual_config = {};
3193 
3194  // Set camera intrinsics
3195  std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics;
3196  intrinsics[rs::core::stream_type::fisheye] = rs::utils::convert_intrinsics(fisheye_intrin);
3197  intrinsics[rs::core::stream_type::depth] = rs::utils::convert_intrinsics(depth_intrin);
3198 
3199  if(!setStreamConfigIntrin(rs::core::stream_type::fisheye, intrinsics, supported_config, actual_config))
3200  {
3201  return false;
3202  }
3203  if(!setStreamConfigIntrin(rs::core::stream_type::depth, intrinsics, supported_config, actual_config))
3204  {
3205  return false;
3206  }
3207 
3208  // Set IMU intrinsics
3209  actual_config[rs::core::motion_type::accel].is_enabled = true;
3210  actual_config[rs::core::motion_type::gyro].is_enabled = true;
3211  actual_config[rs::core::motion_type::gyro].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.gyro);
3212  actual_config[rs::core::motion_type::accel].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.acc);
3213 
3214  // Set extrinsics
3215  actual_config[rs::core::stream_type::fisheye].extrinsics_motion = rs::utils::convert_extrinsics(fisheye2ImuExtrinsics);
3216  actual_config[rs::core::stream_type::fisheye].extrinsics = rs::utils::convert_extrinsics(fisheye2DepthExtrinsics);
3217 
3218  UDEBUG("Set SLAM config");
3219  // Set actual config
3220  if (slam_->set_module_config(actual_config) < rs::core::status_no_error)
3221  {
3222  UERROR("error : failed to set the enabled module configuration");
3223  return false;
3224  }
3225 
3226  rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
3227  imu2Camera = Transform(
3228  fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
3229  fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
3230  fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
3231 
3232  if(rgbSource_ == kInfrared)
3233  {
3234  rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
3235  Transform fisheye2Color = Transform(
3236  color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
3237  color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
3238  color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
3239  imu2Camera *= fisheye2Color;
3240  }
3241  else if(rgbSource_ == kColor)
3242  {
3243  rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
3244  Transform fisheye2Color = Transform(
3245  color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
3246  color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
3247  color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
3248  imu2Camera *= fisheye2Color;
3249  }
3250 
3251  UDEBUG("start device!");
3252  try
3253  {
3254  dev_->start(rs::source::all_sources);
3255  }
3256  catch(const rs::error & error)
3257  {
3258  UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
3259  return false;
3260  }
3261  }
3262  else
3263  {
3264  UDEBUG("start device!");
3265  try
3266  {
3267  dev_->start();
3268  }
3269  catch(const rs::error & error)
3270  {
3271  UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
3272  return false;
3273  }
3274  }
3275 #else
3276  try {
3277  dev_->start();
3278  dev_->wait_for_frames();
3279  }
3280  catch (const rs::error & e)
3281  {
3282  UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
3283  }
3284 #endif
3285 
3286  cv::Mat D;
3287  if(rgbSource_ == kFishEye)
3288  {
3289  // ftheta/equidistant model
3290  D = cv::Mat::zeros(1,6,CV_64FC1);
3291  D.at<double>(0,0) = color_intrin.coeffs[0];
3292  D.at<double>(0,1) = color_intrin.coeffs[1];
3293  D.at<double>(0,4) = color_intrin.coeffs[2];
3294  D.at<double>(0,5) = color_intrin.coeffs[3];
3295  }
3296  else
3297  {
3298  // Brown-Conrady / radtan
3299  D = cv::Mat::zeros(1,5,CV_64FC1);
3300  D.at<double>(0,0) = color_intrin.coeffs[0];
3301  D.at<double>(0,1) = color_intrin.coeffs[1];
3302  D.at<double>(0,2) = color_intrin.coeffs[2];
3303  D.at<double>(0,3) = color_intrin.coeffs[3];
3304  D.at<double>(0,4) = color_intrin.coeffs[4];
3305  }
3306  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
3307  K.at<double>(0,0) = color_intrin.fx;
3308  K.at<double>(1,1) = color_intrin.fy;
3309  K.at<double>(0,2) = color_intrin.ppx;
3310  K.at<double>(1,2) = color_intrin.ppy;
3311  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
3312  cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
3313  K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
3314  cameraModel_ = CameraModel(
3315  dev_->get_name(),
3316  cv::Size(color_intrin.width, color_intrin.height),
3317  K,
3318  D,
3319  R,
3320  P,
3321  this->getLocalTransform()*imu2Camera);
3322 
3323  UDEBUG("");
3324 
3325  if(rgbSource_ == kColor)
3326  {
3327  rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
3328  rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
3329  rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
3330  }
3331  else if(rgbSource_ == kFishEye)
3332  {
3333  UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
3334  if(!calibrationFolder.empty() && !cameraName.empty())
3335  {
3336  CameraModel model;
3337  if(!model.load(calibrationFolder, cameraName))
3338  {
3339  UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
3340  cameraName.c_str(), calibrationFolder.c_str());
3341  }
3342  else
3343  {
3344  UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
3345  model.fx(),
3346  model.fy(),
3347  model.cx(),
3348  model.cy());
3349  cameraModel_ = model;
3350  cameraModel_.setName(cameraName);
3351  cameraModel_.initRectificationMap();
3352  cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
3353  }
3354  }
3355  }
3356 
3357  uSleep(1000); // ignore the first frames
3358  UINFO("Enabling streams...done!");
3359 
3360  return true;
3361 
3362 #else
3363  UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
3364  return false;
3365 #endif
3366 }
3367 
3368 bool CameraRealSense::isCalibrated() const
3369 {
3370  return true;
3371 }
3372 
3373 std::string CameraRealSense::getSerial() const
3374 {
3375 #ifdef RTABMAP_REALSENSE
3376  if (dev_)
3377  {
3378  return dev_->get_serial();
3379  }
3380  else
3381  {
3382  UERROR("Cannot get a serial before initialization. Call init() before.");
3383  }
3384 #endif
3385  return "NA";
3386 }
3387 
3388 bool CameraRealSense::odomProvided() const
3389 {
3390 #ifdef RTABMAP_REALSENSE_SLAM
3391  return slam_!=0;
3392 #else
3393  return false;
3394 #endif
3395 }
3396 
3397 #ifdef RTABMAP_REALSENSE_SLAM
3398 Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose)
3399 {
3400  return Transform(
3401  pose.m_data[0], pose.m_data[1], pose.m_data[2], pose.m_data[3],
3402  pose.m_data[4], pose.m_data[5], pose.m_data[6], pose.m_data[7],
3403  pose.m_data[8], pose.m_data[9], pose.m_data[10], pose.m_data[11]);
3404 }
3405 #endif
3406 
3407 SensorData CameraRealSense::captureImage(CameraInfo * info)
3408 {
3409  SensorData data;
3410 #ifdef RTABMAP_REALSENSE
3411  if (dev_)
3412  {
3413  cv::Mat rgb;
3414  cv::Mat depthIn;
3415 
3416  // Retrieve camera parameters for mapping between depth and color
3417  rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
3418  rs::extrinsics depth_to_color;
3419  rs::intrinsics color_intrin;
3420  if(rgbSource_ == kFishEye)
3421  {
3422  depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
3423  color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
3424  }
3425  else if(rgbSource_ == kInfrared)
3426  {
3427  depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
3428  color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
3429  }
3430  else // color
3431  {
3432  depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
3433  color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
3434  }
3435 
3436 #ifdef RTABMAP_REALSENSE_SLAM
3437  if(!dataReady_.acquire(1, 5000))
3438  {
3439  UWARN("Not received new frames since 5 seconds, end of stream reached!");
3440  return data;
3441  }
3442 
3443  {
3444  UScopeMutex lock(dataMutex_);
3445  rgb = lastSyncFrames_.first;
3446  depthIn = lastSyncFrames_.second;
3447  lastSyncFrames_.first = cv::Mat();
3448  lastSyncFrames_.second = cv::Mat();
3449  }
3450 
3451  if(rgb.empty() || depthIn.empty())
3452  {
3453  return data;
3454  }
3455 #else
3456  try {
3457  dev_->wait_for_frames();
3458  }
3459  catch (const rs::error & e)
3460  {
3461  UERROR("Exception: %s", e.what());
3462  return data;
3463  }
3464 
3465  // Retrieve our images
3466  depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
3467  if(rgbSource_ == kFishEye)
3468  {
3469  rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
3470  }
3471  else if(rgbSource_ == kInfrared)
3472  {
3473  rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
3474  }
3475  else
3476  {
3477  rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
3478  }
3479 #endif
3480 
3481  // factory registration...
3482  cv::Mat bgr;
3483  if(rgbSource_ != kColor)
3484  {
3485  bgr = rgb;
3486  }
3487  else
3488  {
3489  cv::cvtColor(rgb, bgr, CV_RGB2BGR);
3490  }
3491 
3492  bool rectified = false;
3493  if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
3494  {
3495  bgr = cameraModel_.rectifyImage(bgr);
3496  rectified = true;
3497  color_intrin.fx = cameraModel_.fx();
3498  color_intrin.fy = cameraModel_.fy();
3499  color_intrin.ppx = cameraModel_.cx();
3500  color_intrin.ppy = cameraModel_.cy();
3501  UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
3502  uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
3503  color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
3504  ((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
3505  }
3506 #ifndef RTABMAP_REALSENSE_SLAM
3507  else if(rgbSource_ != kColor)
3508  {
3509  bgr = bgr.clone();
3510  }
3511 #endif
3512 
3513  cv::Mat depth;
3514  if(rgbSource_ != kFishEye || rectified)
3515  {
3516  if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
3517  depth_intrin.width < color_intrin.width &&
3518  depth_intrin.height < color_intrin.height &&
3519  !depthScaledToRGBSize_)
3520  {
3521  //we can keep the depth image size as is
3522  depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
3523  float scaleX = float(depth_intrin.width) / float(color_intrin.width);
3524  float scaleY = float(depth_intrin.height) / float(color_intrin.height);
3525  color_intrin.fx *= scaleX;
3526  color_intrin.fy *= scaleY;
3527  color_intrin.ppx *= scaleX;
3528  color_intrin.ppy *= scaleY;
3529  color_intrin.height = depth_intrin.height;
3530  color_intrin.width = depth_intrin.width;
3531  }
3532  else
3533  {
3534  //depth to color
3535  depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
3536  }
3537 
3538  float scale = dev_->get_depth_scale();
3539  for (int dy = 0; dy < depth_intrin.height; ++dy)
3540  {
3541  for (int dx = 0; dx < depth_intrin.width; ++dx)
3542  {
3543  // Retrieve the 16-bit depth value and map it into a depth in meters
3544  uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
3545  float depth_in_meters = depth_value * scale;
3546 
3547  // Skip over pixels with a depth value of zero, which is used to indicate no data
3548  if (depth_value == 0 || depth_in_meters>10.0f) continue;
3549 
3550  // Map from pixel coordinates in the depth image to pixel coordinates in the color image
3551  int pdx = dx;
3552  int pdy = dy;
3553  if(rgbSource_ == kColor || rgbSource_ == kFishEye)
3554  {
3555  rs::float2 depth_pixel = { (float)dx, (float)dy };
3556  rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
3557  rs::float3 color_point = depth_to_color.transform(depth_point);
3558  rs::float2 color_pixel = color_intrin.project(color_point);
3559 
3560  pdx = color_pixel.x;
3561  pdy = color_pixel.y;
3562  }
3563  //else infrared is already registered
3564 
3565  if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
3566  {
3567  depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
3568  }
3569  }
3570  }
3571 
3572  if (color_intrin.width > depth_intrin.width)
3573  {
3574  // Fill holes
3575  UTimer time;
3576  util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
3577  util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
3578  UDEBUG("Filling depth holes: %fs", time.ticks());
3579  }
3580  }
3581 
3582  if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
3583  {
3584  data = SensorData(bgr, depth, cameraModel_, this->getNextSeqID(), UTimer::now());
3585 #ifdef RTABMAP_REALSENSE_SLAM
3586  if(info && slam_)
3587  {
3588  UScopeMutex lock(slamLock_);
3589  rs::slam::PoseMatrix4f pose;
3590  if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
3591  {
3592  /*rs::slam::tracking_accuracy accuracy = slam_->get_tracking_accuracy();
3593  if( accuracy == rs::slam::tracking_accuracy::low ||
3594  accuracy == rs::slam::tracking_accuracy::medium ||
3595  accuracy == rs::slam::tracking_accuracy::high)*/
3596  {
3597  // the pose is in camera link or IMU frame, get pose of the color camera
3598  Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
3599  info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
3600  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
3601  }
3602  /*else
3603  {
3604  UERROR("Odometry failed: accuracy=%d", accuracy);
3605  }*/
3606  }
3607  else
3608  {
3609  UERROR("Failed getting odometry pose");
3610  }
3611  }
3612 #endif
3613  }
3614  }
3615  else
3616  {
3617  ULOGGER_WARN("The camera must be initialized before requesting an image.");
3618  }
3619 #else
3620  UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
3621 #endif
3622  return data;
3623 }
3624 
3626 // CameraRealSense2
3628 bool CameraRealSense2::available()
3629 {
3630 #ifdef RTABMAP_REALSENSE2
3631  return true;
3632 #else
3633  return false;
3634 #endif
3635 }
3636 
3637 CameraRealSense2::CameraRealSense2(
3638  const std::string & device,
3639  float imageRate,
3640  const rtabmap::Transform & localTransform) :
3641  Camera(imageRate, localTransform)
3642 #ifdef RTABMAP_REALSENSE2
3643  ,
3644  ctx_(new rs2::context),
3645  dev_(new rs2::device),
3646  deviceId_(device),
3647  syncer_(new rs2::syncer),
3648  depth_scale_meters_(1.0f),
3649  depthIntrinsics_(new rs2_intrinsics),
3650  rgbIntrinsics_(new rs2_intrinsics),
3651  depthToRGBExtrinsics_(new rs2_extrinsics),
3652  emitterEnabled_(true),
3653  irDepth_(false)
3654 #endif
3655 {
3656  UDEBUG("");
3657 }
3658 
3659 CameraRealSense2::~CameraRealSense2()
3660 {
3661 #ifdef RTABMAP_REALSENSE2
3662  delete ctx_;
3663  delete dev_;
3664  delete syncer_;
3665  delete depthIntrinsics_;
3666  delete rgbIntrinsics_;
3667  delete depthToRGBExtrinsics_;
3668 #endif
3669  UDEBUG("");
3670 }
3671 
3672 #ifdef RTABMAP_REALSENSE2
3673 void alignFrame(const rs2_intrinsics& from_intrin,
3674  const rs2_intrinsics& other_intrin,
3675  rs2::frame from_image,
3676  uint32_t output_image_bytes_per_pixel,
3677  const rs2_extrinsics& from_to_other,
3678  cv::Mat & registeredDepth,
3679  float depth_scale_meters)
3680 {
3681  static const auto meter_to_mm = 0.001f;
3682  uint8_t* p_out_frame = registeredDepth.data;
3683  auto from_vid_frame = from_image.as<rs2::video_frame>();
3684  auto from_bytes_per_pixel = from_vid_frame.get_bytes_per_pixel();
3685 
3686  static const auto blank_color = 0x00;
3687  UASSERT(registeredDepth.total()*registeredDepth.channels()*registeredDepth.depth() == other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
3688  memset(p_out_frame, blank_color, other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
3689 
3690  auto p_from_frame = reinterpret_cast<const uint8_t*>(from_image.get_data());
3691  auto from_stream_type = from_image.get_profile().stream_type();
3692  float depth_units = ((from_stream_type == RS2_STREAM_DEPTH)? depth_scale_meters:1.f);
3693  UASSERT(from_stream_type == RS2_STREAM_DEPTH);
3694  UASSERT_MSG(depth_units > 0.0f, uFormat("depth_scale_meters=%f", depth_scale_meters).c_str());
3695 #pragma omp parallel for schedule(dynamic)
3696  for (int from_y = 0; from_y < from_intrin.height; ++from_y)
3697  {
3698  int from_pixel_index = from_y * from_intrin.width;
3699  for (int from_x = 0; from_x < from_intrin.width; ++from_x, ++from_pixel_index)
3700  {
3701  // Skip over depth pixels with the value of zero
3702  float depth = (from_stream_type == RS2_STREAM_DEPTH)?(depth_units * ((const uint16_t*)p_from_frame)[from_pixel_index]): 1.f;
3703  if (depth)
3704  {
3705  // Map the top-left corner of the depth pixel onto the other image
3706  float from_pixel[2] = { from_x - 0.5f, from_y - 0.5f }, from_point[3], other_point[3], other_pixel[2];
3707  rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
3708  rs2_transform_point_to_point(other_point, &from_to_other, from_point);
3709  rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
3710  const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
3711  const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
3712 
3713  // Map the bottom-right corner of the depth pixel onto the other image
3714  from_pixel[0] = from_x + 0.5f; from_pixel[1] = from_y + 0.5f;
3715  rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
3716  rs2_transform_point_to_point(other_point, &from_to_other, from_point);
3717  rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
3718  const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
3719  const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
3720 
3721  if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height)
3722  continue;
3723 
3724  for (int y = other_y0; y <= other_y1; ++y)
3725  {
3726  for (int x = other_x0; x <= other_x1; ++x)
3727  {
3728  int out_pixel_index = y * other_intrin.width + x;
3729  //Tranfer n-bit pixel to n-bit pixel
3730  for (int i = 0; i < from_bytes_per_pixel; i++)
3731  {
3732  const auto out_offset = out_pixel_index * output_image_bytes_per_pixel + i;
3733  const auto from_offset = from_pixel_index * output_image_bytes_per_pixel + i;
3734  p_out_frame[out_offset] = p_from_frame[from_offset] * (depth_units / meter_to_mm);
3735  }
3736  }
3737  }
3738  }
3739  }
3740  }
3741 }
3742 #endif
3743 
3744 bool CameraRealSense2::init(const std::string & calibrationFolder, const std::string & cameraName)
3745 {
3746  UDEBUG("");
3747 #ifdef RTABMAP_REALSENSE2
3748 
3749  UINFO("setupDevice...");
3750 
3751  auto list = ctx_->query_devices();
3752  if (0 == list.size())
3753  {
3754  UERROR("No RealSense2 devices were found!");
3755  return false;
3756  }
3757 
3758  bool found=false;
3759  for (auto&& dev : list)
3760  {
3761  auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
3762  auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
3763  uint16_t pid;
3764  std::stringstream ss;
3765  ss << std::hex << pid_str;
3766  ss >> pid;
3767  UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
3768  if (deviceId_.empty() || deviceId_ == sn)
3769  {
3770  *dev_ = dev;
3771  found=true;
3772  break;
3773  }
3774  }
3775 
3776  if (!found)
3777  {
3778  UERROR("The requested device %s is NOT found!", deviceId_.c_str());
3779  return false;
3780  }
3781 
3782  ctx_->set_devices_changed_callback([this](rs2::event_information& info)
3783  {
3784  if (info.was_removed(*dev_))
3785  {
3786  UERROR("The device has been disconnected!");
3787  }
3788  });
3789 
3790 
3791  auto camera_name = dev_->get_info(RS2_CAMERA_INFO_NAME);
3792  UINFO("Device Name: %s", camera_name);
3793 
3794  auto sn = dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
3795  UINFO("Device Serial No: %s", sn);
3796 
3797  auto fw_ver = dev_->get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
3798  UINFO("Device FW version: %s", fw_ver);
3799 
3800  auto pid = dev_->get_info(RS2_CAMERA_INFO_PRODUCT_ID);
3801  UINFO("Device Product ID: 0x%s", pid);
3802 
3803  auto dev_sensors = dev_->query_sensors();
3804 
3805  UINFO("Device Sensors: ");
3806  std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth
3807  for(auto&& elem : dev_sensors)
3808  {
3809  std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
3810  if ("Stereo Module" == module_name)
3811  {
3812  sensors[1] = elem;
3813  sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
3814  if(irDepth_)
3815  {
3816  sensors[0] = elem;
3817  }
3818  }
3819  else if ("Coded-Light Depth Sensor" == module_name)
3820  {
3821  }
3822  else if ("RGB Camera" == module_name)
3823  {
3824  if(!irDepth_)
3825  {
3826  sensors[0] = elem;
3827  }
3828  }
3829  else if ("Wide FOV Camera" == module_name)
3830  {
3831  }
3832  else if ("Motion Module" == module_name)
3833  {
3834  }
3835  else
3836  {
3837  UERROR("Module Name \"%s\" isn't supported by LibRealSense!", module_name.c_str());
3838  return false;
3839  }
3840  UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
3841  }
3842 
3843  UDEBUG("");
3844 
3845  model_ = CameraModel();
3846  rs2::stream_profile depthStreamProfile;
3847  rs2::stream_profile rgbStreamProfile;
3848  std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(2);
3849  for (unsigned int i=0; i<sensors.size(); ++i)
3850  {
3851  UDEBUG("i=%d", (int)i);
3852  auto profiles = sensors[i].get_stream_profiles();
3853  bool added = false;
3854  UDEBUG("profiles=%d", (int)profiles.size());
3855  for (auto& profile : profiles)
3856  {
3857  auto video_profile = profile.as<rs2::video_stream_profile>();
3858  if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
3859  video_profile.width() == 640 &&
3860  video_profile.height() == 480 &&
3861  video_profile.fps() == 30)
3862  {
3863  profilesPerSensor[irDepth_?1:i].push_back(profile);
3864  auto intrinsic = video_profile.get_intrinsics();
3865  if(i==1)
3866  {
3867  depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0));
3868  depthStreamProfile = profile;
3869  *depthIntrinsics_ = intrinsic;
3870  }
3871  else
3872  {
3873  rgbBuffer_ = cv::Mat(cv::Size(640, 480), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
3874  model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
3875  rgbStreamProfile = profile;
3876  *rgbIntrinsics_ = intrinsic;
3877  }
3878  added = true;
3879  break;
3880  }
3881  }
3882  if (!added)
3883  {
3884  UERROR("Given stream configuration is not supported by the device! "
3885  "Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, 640, 480, 30);
3886  return false;
3887  }
3888  }
3889 
3890  if(!model_.isValidForProjection())
3891  {
3892  UERROR("Calibration info not valid!");
3893  return false;
3894  }
3895  *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
3896 
3897  for (unsigned int i=0; i<sensors.size(); ++i)
3898  {
3899  if(profilesPerSensor[i].size())
3900  {
3901  UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
3902  sensors[i].open(profilesPerSensor[i]);
3903  if(i ==1)
3904  {
3905  auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
3906  depth_scale_meters_ = depth_sensor.get_depth_scale();
3907  }
3908  sensors[i].start(*syncer_);
3909  }
3910  }
3911 
3912  uSleep(1000); // ignore the first frames
3913  UINFO("Enabling streams...done!");
3914 
3915  return true;
3916 
3917 #else
3918  UERROR("CameraRealSense: RTAB-Map is not built with RealSense2 support!");
3919  return false;
3920 #endif
3921 }
3922 
3923 bool CameraRealSense2::isCalibrated() const
3924 {
3925  return true;
3926 }
3927 
3928 std::string CameraRealSense2::getSerial() const
3929 {
3930 #ifdef RTABMAP_REALSENSE2
3931  return dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
3932 #endif
3933  return "NA";
3934 }
3935 
3936 void CameraRealSense2::setEmitterEnabled(bool enabled)
3937 {
3938 #ifdef RTABMAP_REALSENSE2
3939  emitterEnabled_ = enabled;
3940 #endif
3941 }
3942 
3943 void CameraRealSense2::setIRDepthFormat(bool enabled)
3944 {
3945 #ifdef RTABMAP_REALSENSE2
3946  irDepth_ = enabled;
3947 #endif
3948 }
3949 
3950 SensorData CameraRealSense2::captureImage(CameraInfo * info)
3951 {
3952  SensorData data;
3953 #ifdef RTABMAP_REALSENSE2
3954 
3955  try{
3956  auto frameset = syncer_->wait_for_frames(5000);
3957  UTimer timer;
3958  while (frameset.size() != 2 && timer.elapsed() < 2.0)
3959  {
3960  // maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
3961  frameset = syncer_->wait_for_frames(100);
3962  }
3963  if (frameset.size() == 2)
3964  {
3965  double stamp = UTimer::now();
3966  UDEBUG("Frameset arrived.");
3967  bool is_rgb_arrived = false;
3968  bool is_depth_arrived = false;
3969  rs2::frame rgb_frame;
3970  rs2::frame depth_frame;
3971  for (auto it = frameset.begin(); it != frameset.end(); ++it)
3972  {
3973  auto f = (*it);
3974  auto stream_type = f.get_profile().stream_type();
3975  if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
3976  {
3977  rgb_frame = f;
3978  is_rgb_arrived = true;
3979  }
3980  else if (stream_type == RS2_STREAM_DEPTH)
3981  {
3982  depth_frame = f;
3983  is_depth_arrived = true;
3984  }
3985  }
3986 
3987  if(is_rgb_arrived && is_depth_arrived)
3988  {
3989  auto from_image_frame = depth_frame.as<rs2::video_frame>();
3990  cv::Mat depth;
3991  if(irDepth_)
3992  {
3993  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
3994  }
3995  else
3996  {
3997  depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type());
3998  alignFrame(*depthIntrinsics_, *rgbIntrinsics_,
3999  depth_frame, from_image_frame.get_bytes_per_pixel(),
4000  *depthToRGBExtrinsics_, depth, depth_scale_meters_);
4001  }
4002 
4003  cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
4004  cv::Mat bgr;
4005  if(rgb.channels() == 3)
4006  {
4007  cv::cvtColor(rgb, bgr, CV_RGB2BGR);
4008  }
4009  else
4010  {
4011  bgr = rgb.clone();
4012  }
4013 
4014  data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
4015  }
4016  else
4017  {
4018  UERROR("Not received depth and rgb");
4019  }
4020  }
4021  else
4022  {
4023  UERROR("Missing frames (received %d)", (int)frameset.size());
4024  }
4025  }
4026  catch(const std::exception& ex)
4027  {
4028  UERROR("An error has occurred during frame callback: %s", ex.what());
4029  }
4030 #else
4031  UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
4032 #endif
4033  return data;
4034 }
4035 
4036 //
4037 // CameraRGBDImages
4038 //
4039 bool CameraRGBDImages::available()
4040 {
4041  return true;
4042 }
4043 
4044 CameraRGBDImages::CameraRGBDImages(
4045  const std::string & pathRGBImages,
4046  const std::string & pathDepthImages,
4047  float depthScaleFactor,
4048  float imageRate,
4049  const Transform & localTransform) :
4050  CameraImages(pathRGBImages, imageRate, localTransform)
4051 {
4052  UASSERT(depthScaleFactor >= 1.0);
4053  cameraDepth_.setPath(pathDepthImages);
4054  cameraDepth_.setDepth(true, depthScaleFactor);
4055 }
4056 
4057 CameraRGBDImages::~CameraRGBDImages()
4058 {
4059 }
4060 
4061 bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
4062 {
4063  bool success = false;
4064  if(CameraImages::init(calibrationFolder, cameraName) && cameraDepth_.init())
4065  {
4066  if(this->imagesCount() == cameraDepth_.imagesCount())
4067  {
4068  success = true;
4069  }
4070  else
4071  {
4072  UERROR("Cameras don't have the same number of images (%d vs %d)",
4073  this->imagesCount(), cameraDepth_.imagesCount());
4074  }
4075  }
4076 
4077  return success;
4078 }
4079 
4080 bool CameraRGBDImages::isCalibrated() const
4081 {
4082  return this->cameraModel().isValidForProjection();
4083 }
4084 
4085 std::string CameraRGBDImages::getSerial() const
4086 {
4087  return this->cameraModel().name();
4088 }
4089 
4090 SensorData CameraRGBDImages::captureImage(CameraInfo * info)
4091 {
4092  SensorData data;
4093 
4094  SensorData rgb, depth;
4095  rgb = CameraImages::captureImage(info);
4096  if(!rgb.imageRaw().empty())
4097  {
4098  depth = cameraDepth_.takeImage();
4099  if(!depth.depthRaw().empty())
4100  {
4101  data = SensorData(rgb.imageRaw(), depth.depthRaw(), rgb.cameraModels(), rgb.id(), rgb.stamp());
4102  data.setGroundTruth(rgb.groundTruth());
4103  }
4104  }
4105  return data;
4106 }
4107 
4108 
4109 } // namespace rtabmap
freenect_device
struct _freenect_device freenect_device
Definition: CameraRGBD.h:92
NULL
#define NULL
rtabmap::CameraImages::setPath
void setPath(const std::string &dir)
Definition: CameraRGB.h:70
rtabmap::CameraModel::K_raw
cv::Mat K_raw() const
Definition: CameraModel.h:101
USemaphore::release
void release(int n=1)
Definition: USemaphore.h:168
rtabmap::CameraInfo::odomCovariance
cv::Mat odomCovariance
Definition: CameraInfo.h:69
UTimer
Definition: UTimer.h:46
rtabmap::CameraOpenNI2::setAutoExposure
bool setAutoExposure(bool enabled)
Definition: CameraRGBD.cpp:451
glm::any
GLM_FUNC_DECL bool any(vecType< bool, P > const &v)
rtabmap::CameraOpenni::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:163
rtabmap::CameraOpenNICV::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:344
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
glm::float2
detail::tvec2< float, highp > float2
single-precision floating-point vector with 2 components. (From GLM_GTX_compatibility extension) ...
Definition: compatibility.hpp:122
rtabmap::CameraModel::name
const std::string & name() const
Definition: CameraModel.h:93
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
rtabmap::CameraOpenNI2::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:798
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:63
rtabmap::CameraRealSense::setRGBSource
void setRGBSource(RGBSource source)
Definition: CameraRGBD.cpp:2698
rtabmap::CameraRealSense::setDepthScaledToRGBSize
void setDepthScaledToRGBSize(bool enabled)
Definition: CameraRGBD.cpp:2693
rtabmap::CameraRGBDImages::cameraDepth_
CameraImages cameraDepth_
Definition: CameraRGBD.h:506
rtabmap::CameraRealSense::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:3373
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
rtabmap::CameraImages::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGB.cpp:496
rtabmap::CameraK4W2::cDepthWidth
static const int cDepthWidth
Definition: CameraRGBD.h:336
rtabmap::CameraRealSense2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:3744
rtabmap::CameraOpenNI2::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:793
glm::e
GLM_FUNC_DECL genType e()
rtabmap::CameraRealSense2::setEmitterEnabled
void setEmitterEnabled(bool enabled)
Definition: CameraRGBD.cpp:3936
config
config
rtabmap::CameraRealSense2::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:3950
glm::scale
GLM_FUNC_DECL detail::tmat4x4< T, P > scale(detail::tmat4x4< T, P > const &m, detail::tvec3< T, P > const &v)
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::CameraRealSense2::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:3923
rtabmap::CameraOpenNICV::_capture
cv::VideoCapture _capture
Definition: CameraRGBD.h:169
glm::byte
detail::uint8 byte
Definition: raw_data.hpp:55
getName
std::string getName(void *handle)
rs2
Definition: CameraRGBD.h:82
rtabmap::CameraOpenNICV::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:339
UMath.h
Basic mathematics functions.
rtabmap::CameraImages::imagesCount
unsigned int imagesCount() const
Definition: CameraRGB.cpp:478
UConversion.h
Some conversion functions.
rtabmap::CameraOpenNICV::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:290
rtabmap::CameraRGBDImages::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:4085
rtabmap::util2d::registerDepth
cv::Mat RTABMAP_EXP registerDepth(const cv::Mat &depth, const cv::Mat &depthK, const cv::Size &colorSize, const cv::Mat &colorK, const rtabmap::Transform &transform)
Definition: util2d.cpp:1357
USemaphore::acquire
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::CameraOpenni::dataReady_
USemaphore dataReady_
Definition: CameraRGBD.h:141
rtabmap::SensorData::groundTruth
const Transform & groundTruth() const
Definition: SensorData.h:232
rtabmap::CameraFreenect2::CameraFreenect2
CameraFreenect2(int deviceId=0, Type type=kTypeDepth2ColorSD, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity(), float minDepth=0.3f, float maxDepth=12.0f, bool bilateralFiltering=true, bool edgeAwareFiltering=true, bool noiseFiltering=true, const std::string &pipelineName="")
Definition: CameraRGBD.cpp:1371
rtabmap::CameraOpenNI2::available
static bool available()
Definition: CameraRGBD.cpp:382
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
rtabmap::CameraRealSense::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:3407
rtabmap::opticalRotation
static rtabmap::Transform opticalRotation(1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f)
glm::uint16_t
detail::uint16 uint16_t
Definition: fwd.hpp:912
rtabmap::CameraFreenect::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:1283
rtabmap::CameraOpenNICV::available
static bool available()
Definition: CameraRGBD.cpp:272
boost::shared_ptr
rtabmap::CameraModel::R
cv::Mat R() const
Definition: CameraModel.h:105
rtabmap::CameraOpenNI2::setOpenNI2StampsAndIDsUsed
void setOpenNI2StampsAndIDsUsed(bool used)
Definition: CameraRGBD.cpp:510
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
rtabmap::CameraOpenni::CameraOpenni
CameraOpenni(const std::string &deviceId="", float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:102
UASSERT
#define UASSERT(condition)
rtabmap::Camera::takeImage
SensorData takeImage(CameraInfo *info=0)
Definition: Camera.cpp:67
UStl.h
Wrappers of STL for convenient functions.
rtabmap::CameraOpenNI2::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:809
rtabmap::CameraK4W2::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:2293
rtabmap::CameraFreenect2::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:1643
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
true
#define true
Definition: ConvertUTF.c:57
rtabmap::CameraModel::load
bool load(const std::string &directory, const std::string &cameraName)
Definition: CameraModel.cpp:212
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::CameraRGBDImages::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:4080
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CameraFreenect2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:1505
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:96
UMutex
Definition: UMutex.h:54
rtabmap::CameraK4W2::cColorWidth
static const int cColorWidth
Definition: CameraRGBD.h:338
rtabmap::CameraFreenect2::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:1638
rtabmap::CameraOpenNICV::CameraOpenNICV
CameraOpenNICV(bool asus=false, float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:277
source
const char * source
Definition: lz4hc.h:181
rtabmap::CameraRealSense2::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:3928
glm::uint64_t
detail::uint64 uint64_t
Definition: fwd.hpp:920
rtabmap::CameraOpenni::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:230
rtabmap::CameraImages::cameraModel
const CameraModel & cameraModel() const
Definition: CameraRGB.h:68
rtabmap::CameraOpenNI2::setGain
bool setGain(int value)
Definition: CameraRGBD.cpp:481
rtabmap::CameraFreenect::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:1294
rtabmap::CameraOpenNI2::setExposure
bool setExposure(int value)
Definition: CameraRGBD.cpp:464
rtabmap::CameraFreenect::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:1278
rtabmap::CameraImages::setDepth
void setDepth(bool isDepth, float depthScaleFactor=1.0f)
Definition: CameraRGB.h:127
rtabmap::CameraOpenni::deviceId_
std::string deviceId_
Definition: CameraRGBD.h:135
rtabmap::CameraOpenNI2::setIRDepthShift
void setIRDepthShift(int horizontal, int vertical)
Definition: CameraRGBD.cpp:517
rtabmap::CameraOpenni::connection_
boost::signals2::connection connection_
Definition: CameraRGBD.h:136
rtabmap::CameraOpenni::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:219
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:152
rtabmap::CameraOpenNI2::setMirroring
bool setMirroring(bool enabled)
Definition: CameraRGBD.cpp:498
rtabmap::CameraK4W2::CameraK4W2
CameraK4W2(int deviceId=0, Type type=kTypeDepth2ColorSD, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:2074
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:154
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
rtabmap::util2d::fillRegisteredDepthHoles
void RTABMAP_EXP fillRegisteredDepthHoles(cv::Mat &depthRegistered, bool vertical, bool horizontal, bool fillDoubleHoles=false)
Definition: util2d.cpp:1593
rtabmap::CameraModel::project
void project(float u, float v, float depth, float &x, float &y, float &z) const
Definition: CameraModel.cpp:608
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:537
DepthSpacePoint
struct _DepthSpacePoint DepthSpacePoint
Definition: CameraRGBD.h:96
glm::uint32_t
detail::uint32 uint32_t
Definition: fwd.hpp:916
rtabmap::CameraOpenNI2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:527
rtabmap::CameraFreenect::CameraFreenect
CameraFreenect(int deviceId=0, Type type=kTypeColorDepth, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:1166
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:80
rtabmap::Camera::getNextSeqID
int getNextSeqID()
Definition: Camera.h:83
glm::tan
GLM_FUNC_DECL genType tan(genType const &angle)
rtabmap::CameraK4W2::cDepthHeight
static const int cDepthHeight
Definition: CameraRGBD.h:337
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
rtabmap::CameraK4W2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:2147
rtabmap::CameraK4W2::cColorHeight
static const int cColorHeight
Definition: CameraRGBD.h:339
rtabmap::CameraRealSense2::CameraRealSense2
CameraRealSense2(const std::string &deviceId="", float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:3637
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
freenect_context
struct _freenect_context freenect_context
Definition: CameraRGBD.h:91
rtabmap::CameraRGBDImages::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:4090
UERROR
#define UERROR(...)
rtabmap::util2d::fillDepthHoles
cv::Mat RTABMAP_EXP fillDepthHoles(const cv::Mat &depth, int maximumHoleSize=1, float errorRatio=0.02f)
Definition: util2d.cpp:1434
ULOGGER_WARN
#define ULOGGER_WARN(...)
Definition: ULogger.h:55
rtabmap::CameraImages::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGB.cpp:131
UWARN
#define UWARN(...)
rtabmap::CameraFreenect::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:1199
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
UTimer::now
static double now()
Definition: UTimer.cpp:73
rtabmap::CameraRealSense::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:3368
rtabmap::CameraRealSense::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:2805
rtabmap::CameraRealSense::odomProvided
virtual bool odomProvided() const
Definition: CameraRGBD.cpp:3388
rtabmap::CameraRealSense::CameraRealSense
CameraRealSense(int deviceId=0, int presetRGB=0, int presetDepth=0, bool computeOdometry=false, float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:2604
rtabmap::CameraK4W2::getSerial
virtual std::string getSerial() const
Definition: CameraRGBD.cpp:2275
RGBQUAD
struct tagRGBQUAD RGBQUAD
Definition: CameraRGBD.h:98
rtabmap::CameraOpenni::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:210
rtabmap::CameraRGBDImages::CameraRGBDImages
CameraRGBDImages(const std::string &pathRGBImages, const std::string &pathDepthImages, float depthScaleFactor=1.0f, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:4044
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:231
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
glm::float3
detail::tvec3< float, highp > float3
single-precision floating-point vector with 3 components. (From GLM_GTX_compatibility extension) ...
Definition: compatibility.hpp:123
rtabmap::CameraOpenNI2::CameraOpenNI2
CameraOpenNI2(const std::string &deviceId="", Type type=kTypeColorDepth, float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraRGBD.cpp:400
rtabmap::CameraK4W2::available
static bool available()
Definition: CameraRGBD.cpp:2065
rtabmap::CameraOpenNI2::exposureGainAvailable
static bool exposureGainAvailable()
Definition: CameraRGBD.cpp:391
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::CameraModel::D_raw
cv::Mat D_raw() const
Definition: CameraModel.h:102
rtabmap::CameraInfo::odomPose
Transform odomPose
Definition: CameraInfo.h:68
rtabmap::CameraRealSense2::setIRDepthFormat
void setIRDepthFormat(bool enabled)
Definition: CameraRGBD.cpp:3943
rtabmap::Transform::size
int size() const
Definition: Transform.h:89
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::CameraModel::setName
void setName(const std::string &name)
Definition: CameraModel.h:92
rtabmap::CameraRGBDImages::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraRGBD.cpp:4061
rtabmap::CameraK4W2::isCalibrated
virtual bool isCalibrated() const
Definition: CameraRGBD.cpp:2270
rtabmap::CameraFreenect2::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraRGBD.cpp:1654
rtabmap::CameraOpenNI2::setAutoWhiteBalance
bool setAutoWhiteBalance(bool enabled)
Definition: CameraRGBD.cpp:438
rtabmap::CameraModel::scaled
CameraModel scaled(double scale) const
Definition: CameraModel.cpp:452
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::CameraOpenni::available
static bool available()
Definition: CameraRGBD.cpp:110
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:172
rtabmap::CameraOpenni::interface_
pcl::Grabber * interface_
Definition: CameraRGBD.h:134
ColorSpacePoint
struct _ColorSpacePoint ColorSpacePoint
Definition: CameraRGBD.h:97
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:30