From 9d09c509abdb3df8848305ce5b4229151c53314a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 Jul 2016 18:39:34 -0400 Subject: [PATCH] Added librealsense support #100 --- CMakeLists.txt | 21 ++ Version.h.in | 1 + cmake_modules/FindRealSense.cmake | 34 +++ corelib/include/rtabmap/core/CameraRGBD.h | 40 +++ corelib/src/CMakeLists.txt | 11 + corelib/src/CameraRGBD.cpp | 211 ++++++++++++++ guilib/include/rtabmap/gui/MainWindow.h | 1 + .../include/rtabmap/gui/PreferencesDialog.h | 3 +- guilib/src/AboutDialog.cpp | 1 + guilib/src/ImageView.cpp | 5 +- guilib/src/MainWindow.cpp | 8 + guilib/src/PreferencesDialog.cpp | 29 ++ guilib/src/ui/aboutDialog.ui | 259 ++++++++++-------- guilib/src/ui/mainWindow.ui | 138 +++++++++- guilib/src/ui/preferencesDialog.ui | 194 ++++++++++++- tools/CameraRGBD/main.cpp | 18 +- tools/ImagesJoiner/CMakeLists.txt | 2 - 17 files changed, 821 insertions(+), 155 deletions(-) create mode 100644 cmake_modules/FindRealSense.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ca62441..8948044b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,6 +138,7 @@ option(WITH_VERTIGO "Include Vertigo support" ON) option(WITH_CVSBA "Include cvsba support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) option(WITH_ZED "Include ZED sdk support" ON) +option(WITH_REALSENSE "Include RealSense support" ON) option(WITH_OCTOMAP "Include Octomap support" ON) option(PCL_OMP "With PCL OMP implementations" ON) @@ -285,6 +286,13 @@ IF(WITH_ZED) ENDIF(ZED_FOUND) ENDIF(WITH_ZED) +IF(WITH_REALSENSE) + FIND_PACKAGE(RealSense QUIET) + IF(RealSense_FOUND) + MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}") + ENDIF(RealSense_FOUND) +ENDIF(WITH_REALSENSE) + IF(WITH_OCTOMAP) FIND_PACKAGE(OCTOMAP QUIET) IF(OCTOMAP_FOUND) @@ -396,6 +404,11 @@ IF(NOT ZED_FOUND) ELSE() SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) ENDIF() +IF(NOT RealSense_FOUND) + SET(REALSENSE "//") +ELSE() + SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) +ENDIF() IF(NOT OCTOMAP_FOUND) SET(OCTOMAP "//") ELSE() @@ -687,6 +700,14 @@ ELSE() MESSAGE(STATUS " With ZED = NO (ZED sdk not found)") ENDIF() +IF(RealSense_FOUND) +MESSAGE(STATUS " With RealSense = YES (License: Apache-2)") +ELSEIF(NOT WITH_REALSENSE) +MESSAGE(STATUS " With RealSense = NO (WITH_REALSENSE=OFF)") +ELSE() +MESSAGE(STATUS " With RealSense = NO (librealsense not found)") +ENDIF() + IF(OCTOMAP_FOUND) MESSAGE(STATUS " With OCTOMAP = YES (License: BSD)") ELSEIF(NOT WITH_OCTOMAP) diff --git a/Version.h.in b/Version.h.in index 06a6bafd..93343a25 100644 --- a/Version.h.in +++ b/Version.h.in @@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @DC1394@#define RTABMAP_DC1394 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2 @ZED@#define RTABMAP_ZED +@REALSENSE@#define RTABMAP_REALSENSE @OCTOMAP@#define RTABMAP_OCTOMAP #endif /* VERSION_H_ */ diff --git a/cmake_modules/FindRealSense.cmake b/cmake_modules/FindRealSense.cmake new file mode 100644 index 00000000..87377508 --- /dev/null +++ b/cmake_modules/FindRealSense.cmake @@ -0,0 +1,34 @@ +# - Find librealsense (https://github.com/IntelRealSense/librealsense) +# +# RealSense_ROOT_DIR environment variable can be set to find the library. +# +# It sets the following variables: +# RealSense_FOUND - Set to false, or undefined, if RealSense isn't found. +# RealSense_INCLUDE_DIRS - The RealSense include directory. +# RealSense_LIBRARIES - The RealSense library to link against. + +#RealSense library +find_path(RealSense_INCLUDE_DIRS NAMES librealsense/rs.hpp PATHS $ENV{RealSense_ROOT_DIR}/include) +if(CMAKE_CL_64) +find_library(RealSense_LIBRARY NAMES realsense PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64) +else() +find_library(RealSense_LIBRARY NAMES realsense PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32) +endif() + +IF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY) + SET(RealSense_FOUND TRUE) +ENDIF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY) + +IF (RealSense_FOUND) + # show which RealSense was found only if not quiet + SET(RealSense_LIBRARIES ${RealSense_LIBRARY}) + IF (NOT RealSense_FIND_QUIETLY) + MESSAGE(STATUS "Found RealSense: ${RealSense_LIBRARIES}") + ENDIF (NOT RealSense_FIND_QUIETLY) +ELSE (RealSense_FOUND) + # fatal error if RealSense is required but not found + IF (RealSense_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find RealSense (librealsense)") + ENDIF (RealSense_FIND_REQUIRED) +ENDIF (RealSense_FOUND) + diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index 6fb5af12..6b058eab 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -64,6 +64,12 @@ class Registration; class PacketPipeline; } +namespace rs +{ + class context; + class device; +} + typedef struct _freenect_context freenect_context; typedef struct _freenect_device freenect_device; @@ -266,6 +272,40 @@ private: bool noiseFiltering_; }; +///////////////////////// +// CameraOpenNIPCL +///////////////////////// +class RTABMAP_EXP CameraRealSense : + public Camera +{ +public: + static bool available(); + +public: + // default local transform z in, x right, y down)); + CameraRealSense( + int deviceId = 0, + int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate + int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate + float imageRate = 0, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraRealSense(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual SensorData captureImage(CameraInfo * info = 0); + +private: + rs::context * ctx_; + rs::device * dev_; + int deviceId_; + int presetRGB_; + int presetDepth_; +}; + ///////////////////////// // CameraRGBDImages diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 176a338b..3b82efb7 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -131,6 +131,17 @@ IF(freenect2_FOUND) ) ENDIF(freenect2_FOUND) +IF(RealSense_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${RealSense_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${RealSense_LIBRARIES} + ) +ENDIF(RealSense_FOUND) + IF(DC1394_FOUND) SET(INCLUDE_DIRS ${INCLUDE_DIRS} diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 914984c4..9133f58f 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -64,6 +64,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef RTABMAP_REALSENSE +#include +#endif + #ifdef RTABMAP_OPENNI2 #include #include @@ -1667,6 +1671,213 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info) return data; } +///////////////////////// +// CameraRealSense +///////////////////////// +bool CameraRealSense::available() +{ +#ifdef RTABMAP_REALSENSE + return true; +#else + return false; +#endif +} + +CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, float imageRate, const rtabmap::Transform & localTransform) : + Camera(imageRate, localTransform), + ctx_(0), + dev_(0), + deviceId_(device), + presetRGB_(presetRGB), + presetDepth_(presetDepth) +{ +} + +CameraRealSense::~CameraRealSense() +{ +#ifdef RTABMAP_REALSENSE + if (ctx_) + { + delete ctx_; + } +#endif +} + +bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName) +{ +#ifdef RTABMAP_REALSENSE + if (ctx_ == 0) + { + ctx_ = new rs::context(); + } + if (ctx_->get_device_count() == 0) + { + UERROR("No RealSense device detected!"); + return false; + } + + dev_ = ctx_->get_device(deviceId_); + if (dev_ == 0) + { + UERROR("Cannot connect to device %d", deviceId_); + return false; + } + UINFO("Using device %d, an %s", deviceId_, dev_->get_name()); + UINFO(" Serial number: %s", dev_->get_serial()); + UINFO(" Firmware version: %s", dev_->get_firmware_version()); + UINFO(" Preset RGB: %d", presetRGB_); + UINFO(" Preset Depth: %d", presetDepth_); + + // Configure depth and color to run with the device's preferred settings + UINFO("Enabling streams..."); + // R200: + // 0=640x480 vs 480x360 + // 1=1920x1080 vs 640x480 + // 2=640x480 vs 320x240 + dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_); + dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_); + + rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); + rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color); + UINFO(" RGB: %dx%d", color_intrin.width, color_intrin.height); + UINFO(" Depth: %dx%d", depth_intrin.width, depth_intrin.height); + dev_->start(); + + dev_->wait_for_frames(); + uSleep(1000); // ignore the first frames + UINFO("Enabling streams...done!"); + + return true; + +#else + UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!"); + return false; +#endif +} + +bool CameraRealSense::isCalibrated() const +{ + return true; +} + +std::string CameraRealSense::getSerial() const +{ +#ifdef RTABMAP_REALSENSE + if (dev_) + { + return dev_->get_serial(); + } + else + { + UERROR("Cannot get a serial before initialization. Call init() before."); + } +#endif + return "NA"; +} + +SensorData CameraRealSense::captureImage(CameraInfo * info) +{ + SensorData data; +#ifdef RTABMAP_REALSENSE + if (dev_) + { + dev_->wait_for_frames(); + + // Retrieve our images + const uint16_t * depth_image = (const uint16_t *)dev_->get_frame_data(rs::stream::depth); + const uint8_t * color_image = (const uint8_t *)dev_->get_frame_data(rs::stream::color); + + // Retrieve camera parameters for mapping between depth and color + rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); + rs::extrinsics depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::color); + rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color); + float scale = dev_->get_depth_scale(); + + // factory registration... + cv::Mat rgb = cv::Mat(cv::Size(color_intrin.width, color_intrin.height), CV_8UC3, (void*)color_image); + cv::Mat bgr; + cv::cvtColor(rgb, bgr, CV_RGB2BGR); + + CameraModel model( + color_intrin.fx, //fx + color_intrin.fy, //fy + color_intrin.ppx, //cx + color_intrin.ppy, //cy + this->getLocalTransform(), + 0, + bgr.size()); + + cv::Mat depth; + if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 && + depth_intrin.width < color_intrin.width && + depth_intrin.height < color_intrin.height) + { + //we can keep the depth image size as is + depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1); + float scaleX = float(depth_intrin.width) / float(color_intrin.width); + float scaleY = float(depth_intrin.height) / float(color_intrin.height); + color_intrin.fx *= scaleX; + color_intrin.fy *= scaleY; + color_intrin.ppx *= scaleX; + color_intrin.ppy *= scaleY; + color_intrin.height = depth_intrin.height; + color_intrin.width = depth_intrin.width; + } + else + { + //depth to color + depth = cv::Mat::zeros(bgr.size(), CV_16UC1); + } + for (int dy = 0; dy < depth_intrin.height; ++dy) + { + for (int dx = 0; dx < depth_intrin.width; ++dx) + { + // Retrieve the 16-bit depth value and map it into a depth in meters + uint16_t depth_value = depth_image[dy * depth_intrin.width + dx]; + float depth_in_meters = depth_value * scale; + + // Skip over pixels with a depth value of zero, which is used to indicate no data + if (depth_value == 0 || depth_in_meters>10.0f) continue; + + // Map from pixel coordinates in the depth image to pixel coordinates in the color image + rs::float2 depth_pixel = { (float)dx, (float)dy }; + rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters); + rs::float3 color_point = depth_to_color.transform(depth_point); + rs::float2 color_pixel = color_intrin.project(color_point); + + int pdx = color_pixel.x; + int pdy = color_pixel.y; + if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows)) + { + depth.at(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm + } + } + } + + if (color_intrin.width > depth_intrin.width) + { + // Fill holes + UTimer time; + util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2); + util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass + UDEBUG("Filling depth holes: %fs", time.ticks()); + } + + if (!bgr.empty() && !depth.empty()) + { + data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now()); + } + } + else + { + ULOGGER_WARN("The camera must be initialized before requesting an image."); + } +#else + UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!"); +#endif + return data; +} + // // CameraRGBDImages // diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index eb93b6d6..cf83bbdf 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -150,6 +150,7 @@ private slots: void selectOpenniCvAsus(); void selectOpenni2(); void selectFreenect2(); + void selectRealSense(); void selectStereoDC1394(); void selectStereoFlyCapture2(); void selectStereoZed(); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index ca45855a..289b8f18 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -89,7 +89,8 @@ public: kSrcOpenNI_CV_ASUS = 3, kSrcOpenNI2 = 4, kSrcFreenect2 = 5, - kSrcRGBDImages = 6, + kSrcRealSense = 6, + kSrcRGBDImages = 7, kSrcStereo = 100, kSrcDC1394 = 100, diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 2bce6c1c..fade0fb8 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -62,6 +62,7 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No"); _ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No"); _ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No"); + _ui->label_realsense->setText(CameraRealSense::available() ? "Yes" : "No"); _ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No"); _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); _ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No"); diff --git a/guilib/src/ImageView.cpp b/guilib/src/ImageView.cpp index d6de30b3..591e877d 100644 --- a/guilib/src/ImageView.cpp +++ b/guilib/src/ImageView.cpp @@ -745,11 +745,10 @@ void ImageView::setImageDepth(const QImage & imageDepth) _image.width() > _imageDepth.width() && _image.height() > _imageDepth.height() && _image.width() % _imageDepth.width() == 0 && - _image.height() % _imageDepth.height() == 0 && - _image.width() / _imageDepth.width() == _image.height() / _imageDepth.height()) + _image.height() % _imageDepth.height() == 0) { // scale depth to rgb - _imageDepth = _imageDepth.scaledToWidth(_image.width()); + _imageDepth = _imageDepth.scaled(_image.size()); } if(_graphicsView->isVisible()) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index d831a606..4b196e37 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -405,6 +405,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2())); connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2())); connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2())); + connect(_ui->actionRealSense, SIGNAL(triggered()), this, SLOT(selectRealSense())); connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394())); connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2())); connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed())); @@ -416,6 +417,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available()); _ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available()); _ui->actionFreenect2->setEnabled(CameraFreenect2::available()); + _ui->actionRealSense->setEnabled(CameraRealSense::available()); _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available()); _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available()); _ui->actionStereoZed->setEnabled(CameraStereoZed::available()); @@ -3585,6 +3587,7 @@ void MainWindow::updateSelectSourceMenu() _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2); + _ui->actionRealSense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense); _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394); _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2); _ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed); @@ -4756,6 +4759,11 @@ void MainWindow::selectFreenect2() _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2); } +void MainWindow::selectRealSense() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense); +} + void MainWindow::selectStereoDC1394() { _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index d02901e8..75e19db4 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -235,6 +235,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : { _ui->comboBox_cameraRGBD->setItemData(5, 0, Qt::UserRole - 1); } + if (!CameraRealSense::available()) + { + _ui->comboBox_cameraRGBD->setItemData(6, 0, Qt::UserRole - 1); + } if(!CameraStereoDC1394::available()) { _ui->comboBox_cameraStereo->setItemData(0, 0, Qt::UserRole - 1); @@ -459,6 +463,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_freenect2BilateralFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_freenect2EdgeAwareFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_realsensePresetRGB, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->toolButton_cameraImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceImagesStamps())); connect(_ui->lineEdit_cameraImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); @@ -1306,6 +1312,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_freenect2BilateralFiltering->setChecked(true); _ui->checkBox_freenect2EdgeAwareFiltering->setChecked(true); _ui->checkBox_freenect2NoiseFiltering->setChecked(true); + _ui->comboBox_realsensePresetRGB->setCurrentIndex(0); + _ui->comboBox_realsensePresetDepth->setCurrentIndex(2); _ui->lineEdit_openniOniPath->clear(); _ui->lineEdit_openni2OniPath->clear(); _ui->lineEdit_cameraRGBDImages_path_rgb->setText(""); @@ -1648,6 +1656,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->checkBox_freenect2NoiseFiltering->setChecked(settings.value("noiseFiltering", _ui->checkBox_freenect2NoiseFiltering->isChecked()).toBool()); settings.endGroup(); // Freenect2 + settings.beginGroup("RealSense"); + _ui->comboBox_realsensePresetRGB->setCurrentIndex(settings.value("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()).toInt()); + _ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt()); + settings.endGroup(); // RealSense + settings.beginGroup("RGBDImages"); _ui->lineEdit_cameraRGBDImages_path_rgb->setText(settings.value("path_rgb", _ui->lineEdit_cameraRGBDImages_path_rgb->text()).toString()); _ui->lineEdit_cameraRGBDImages_path_depth->setText(settings.value("path_depth", _ui->lineEdit_cameraRGBDImages_path_depth->text()).toString()); @@ -2061,6 +2074,11 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("noiseFiltering", _ui->checkBox_freenect2NoiseFiltering->isChecked()); settings.endGroup(); // Freenect2 + settings.beginGroup("RealSense"); + settings.setValue("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()); + settings.setValue("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()); + settings.endGroup(); // RealSense + settings.beginGroup("RGBDImages"); settings.setValue("path_rgb", _ui->lineEdit_cameraRGBDImages_path_rgb->text()); settings.setValue("path_depth", _ui->lineEdit_cameraRGBDImages_path_depth->text()); @@ -3573,10 +3591,12 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && (_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD || + _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD)); _ui->groupBox_openni2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD); _ui->groupBox_freenect2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD); + _ui->groupBox_realsense->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD); _ui->groupBox_cameraRGBDImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD); _ui->groupBox_openni->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD); @@ -4081,6 +4101,15 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) _ui->checkBox_freenect2EdgeAwareFiltering->isChecked(), _ui->checkBox_freenect2NoiseFiltering->isChecked()); } + else if (driver == kSrcRealSense) + { + camera = new CameraRealSense( + this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()), + _ui->comboBox_realsensePresetRGB->currentIndex(), + _ui->comboBox_realsensePresetDepth->currentIndex(), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } else if(driver == kSrcRGBDImages) { camera = new CameraRGBDImages( diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 91b24758..1493afec 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -82,23 +82,6 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - PCL version : - - - @@ -109,41 +92,6 @@ p, li { white-space: pre-wrap; } - - - - With Freenect : - - - - - - - With OpenNI2 : - - - - - - - With stereo dc1394 : - - - - - - - Version : - - - - - - - Links : - - - @@ -151,33 +99,6 @@ p, li { white-space: pre-wrap; } - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><a href="http://github.com/introlab/rtabmap"><span style=" text-decoration: underline; color:#0000ff;">Source page</span></a></p></body></html> - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - Mathieu Labbé, matlabbe@gmail.com - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - @@ -205,6 +126,13 @@ p, li { white-space: pre-wrap; } + + + + With Freenect : + + + @@ -212,13 +140,27 @@ p, li { white-space: pre-wrap; } - + + + + Links : + + + + With g2o : + + + + With stereo dc1394 : + + + @@ -239,7 +181,7 @@ p, li { white-space: pre-wrap; } - + @@ -256,13 +198,71 @@ p, li { white-space: pre-wrap; } - + With stereo FlyCapture2 : + + + + Mathieu Labbé, matlabbe@gmail.com + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + PCL version : + + + + + + + With OpenNI2 : + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><a href="http://github.com/introlab/rtabmap"><span style=" text-decoration: underline; color:#0000ff;">Source page</span></a></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + Version : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + @@ -273,34 +273,7 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - With cvsba : - - - - + @@ -310,14 +283,48 @@ p, li { white-space: pre-wrap; } - + + + + With stereo Zed : + + + + With GTSAM : - + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + With Octomap : + + + + @@ -328,14 +335,14 @@ p, li { white-space: pre-wrap; } - + - With Octomap : + With cvsba : - - + + @@ -344,15 +351,25 @@ p, li { white-space: pre-wrap; } - - + + - With stereo Zed : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - + + + + With RealSense : + + + + + diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index c0a40d9e..9474df67 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 25 + 21 @@ -167,10 +167,17 @@ + + + RealSense R200 + + + + @@ -298,7 +305,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -336,7 +352,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -579,7 +604,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -600,7 +634,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -621,7 +664,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -642,7 +694,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -660,7 +721,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -681,7 +751,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -702,7 +781,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -726,7 +814,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -796,7 +893,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -1326,6 +1432,14 @@ Export octomap... + + + true + + + RealSense + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index adf70f37..b6af4aa9 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,15 +64,24 @@ 0 0 - 686 - 2023 + 685 + 1896 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -86,7 +95,7 @@ QFrame::Raised - 0 + 5 @@ -2401,7 +2410,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 1 + 0 @@ -2464,6 +2473,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Freenect2 + + + RealSense + + Images @@ -2506,7 +2520,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 + 6 @@ -2986,6 +3000,108 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + 0 + 0 + + + + RealSense + + + + + + Preset for RGB stream. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + QComboBox::AdjustToContents + + + + Best Quality + + + + + Largest Image + + + + + Highest Framerate + + + + + + + + Preset for depth stream. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Best Quality + + + + + Largest Image + + + + + Highest Framerate + + + + + + + + + @@ -4071,7 +4187,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - + + 0 + + + 0 + + + 0 + + 0 @@ -9248,7 +9373,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + 0 + + + 0 + + + 0 + + 0 @@ -9388,7 +9522,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + 0 + + + 0 + + + 0 + + 0 @@ -9546,7 +9689,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -9626,7 +9778,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -9738,7 +9899,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 7e0e714d..47846f26 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -54,6 +54,7 @@ void showUsage() " 6=DC1394 (Bumblebee2)\n" " 7=FlyCapture2 (Bumblebee2)\n" " 8=ZED stereo\n" + " 9=RealSense\n" " Options:\n" " -rate #.# Input rate Hz (default 0=inf)\n" " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n" @@ -150,9 +151,9 @@ int main(int argc, char * argv[]) // last driver = atoi(argv[i]); - if(driver < 0 || driver > 8) + if(driver < 0 || driver > 9) { - UERROR("driver should be between 0 and 8."); + UERROR("driver should be between 0 and 9."); showUsage(); } } @@ -245,6 +246,15 @@ int main(int argc, char * argv[]) } camera = new rtabmap::CameraStereoZed(0); } + else if (driver == 9) + { + if (!rtabmap::CameraRealSense::available()) + { + UERROR("Not built with RealSense support..."); + exit(-1); + } + camera = new rtabmap::CameraRealSense(0); + } else { UFATAL(""); @@ -258,7 +268,7 @@ int main(int argc, char * argv[]) } rtabmap::SensorData data = camera->takeImage(); - if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows) + if(data.imageRaw().cols % data.depthOrRightRaw().cols != 0 || data.imageRaw().rows % data.depthOrRightRaw().rows != 0) { UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.", data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows); @@ -336,7 +346,7 @@ int main(int argc, char * argv[]) depth = rtabmap::util2d::cvtDepthFromFloat(depth); } - if(rgb.cols == depth.cols && rgb.rows == depth.rows && + if(!rgb.empty() && rgb.cols % depth.cols == 0 && rgb.rows % depth.rows == 0 && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) { diff --git a/tools/ImagesJoiner/CMakeLists.txt b/tools/ImagesJoiner/CMakeLists.txt index c3821c34..c6979bfe 100644 --- a/tools/ImagesJoiner/CMakeLists.txt +++ b/tools/ImagesJoiner/CMakeLists.txt @@ -9,8 +9,6 @@ SET(INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS} ) -MESSAGE(STATUS "OpenCV_INCLUDE_DIRS=${OpenCV_INCLUDE_DIRS}") - SET(LIBRARIES ${OpenCV_LIBRARIES} )