From b5e62be664f7aea1ba6f99e29bce42d940e2611e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 5 May 2015 19:32:09 -0400 Subject: [PATCH 1/2] Freenect2: added new type Ir+Depth (RTAB-Map can be used without lightwith the Kinect v2) --- corelib/include/rtabmap/core/CameraRGBD.h | 1 + corelib/src/CameraRGBD.cpp | 238 +++++++++++++--------- guilib/src/ui/preferencesDialog.ui | 9 +- 3 files changed, 152 insertions(+), 96 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index bdbb3601..5762a92e 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -278,6 +278,7 @@ public: enum Type{ kTypeRGBDepthSD, kTypeRGBDepthHD, + kTypeIRDepth, kTypeRGBIR }; diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index d38b0677..f31f8bbb 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -1126,6 +1126,9 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const case kTypeRGBIR: listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir); break; + case kTypeIRDepth: + listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth); + break; case kTypeRGBDepthSD: case kTypeRGBDepthHD: default: @@ -1318,10 +1321,14 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f switch(type_) { - case kTypeRGBIR: + case kTypeRGBIR: //used for calibration rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0); irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0); break; + case kTypeIRDepth: + irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0); + depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0); + break; case kTypeRGBDepthSD: case kTypeRGBDepthHD: default: @@ -1330,128 +1337,171 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f break; } - cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data); - cv::flip(rgbMat, rgb, 1); - - if(stereoModel_.isValid()) + if(irFrame && depthFrame) { - //rectify color - rgb = stereoModel_.right().rectifyImage(rgb); - if(irFrame) + cv::Mat irMat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data); + //convert to gray scaled + float maxIr_ = 0x7FFF; + float minIr_ = 0x0; + const float factor = 255.0f / float((maxIr_ - minIr_)); + rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1); + for(int i=0; iheight, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1); - cv::flip(depth, depth, 1); + for(int j=0; j(i, j) = (unsigned char)std::min(float(std::max(irMat.at(i,j) - minIr_, 0.0f)) * factor, 255.0f); + } + } + + cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); + cv::flip(rgb, rgb, 1); + cv::flip(depth, depth, 1); + if(stereoModel_.isValid()) + { + //rectify + rgb = stereoModel_.left().rectifyImage(rgb); depth = stereoModel_.left().rectifyImage(depth); + fx = stereoModel_.left().fx(); + fy = stereoModel_.left().fy(); + cx = stereoModel_.left().cx(); + cy = stereoModel_.left().cy(); } else { - //rectify depth - cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); - cv::flip(depth, depth, 1); - depth = stereoModel_.left().rectifyDepth(depth); - - bool registered = true; - if(registered) - { - depth = util3d::registerDepth( - depth, - stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K - stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K - stereoModel_.transform()); - util3d::fillRegisteredDepthHoles(depth, true, false); - fx = stereoModel_.right().fx(); - fy = stereoModel_.right().fy(); - cx = stereoModel_.right().cx(); - cy = stereoModel_.right().cy(); - } - else - { - fx = stereoModel_.left().fx(); - fy = stereoModel_.left().fy(); - cx = stereoModel_.left().cx(); - cy = stereoModel_.left().cy(); - } + libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams(); + fx = params.fx; + fy = params.fy; + cx = params.cx; + cy = params.cy; } } else { - //use data from libfreenect2 - if(irFrame) + //rgb + ir or rgb + depth + + cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data); + cv::flip(rgbMat, rgb, 1); + + if(stereoModel_.isValid()) { - cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1); + //rectify color + rgb = stereoModel_.right().rectifyImage(rgb); + if(irFrame) + { + //rectify IR + cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1); + cv::flip(depth, depth, 1); + depth = stereoModel_.left().rectifyImage(depth); + } + else + { + //rectify depth + cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); + cv::flip(depth, depth, 1); + depth = stereoModel_.left().rectifyDepth(depth); + + bool registered = true; + if(registered) + { + depth = util3d::registerDepth( + depth, + stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K + stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K + stereoModel_.transform()); + util3d::fillRegisteredDepthHoles(depth, true, false); + fx = stereoModel_.right().fx(); + fy = stereoModel_.right().fy(); + cx = stereoModel_.right().cx(); + cy = stereoModel_.right().cy(); + } + else + { + fx = stereoModel_.left().fx(); + fy = stereoModel_.left().fy(); + cx = stereoModel_.left().cx(); + cy = stereoModel_.left().cy(); + } + } } else { - cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); - - //registration of the depth - if(reg_) + //use data from libfreenect2 + if(irFrame) { - if(type_ == kTypeRGBDepthSD) + cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1); + } + else + { + cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); + + //registration of the depth + if(reg_) { - cv::Mat tmp; - cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA); - rgb = tmp; - } - cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data); - depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U); - for(int dx=0; dx(dy,dx); - float dz1 = depthFrameMat.at(dy,dx+1); - float dz2 = depthFrameMat.at(dy+1,dx); - float dz3 = depthFrameMat.at(dy+1,dx+1); - if(dz && dz1 && dz2 && dz3) + cv::Mat tmp; + cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA); + rgb = tmp; + } + cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data); + depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U); + for(int dx=0; dx(dy,dx); + float dz1 = depthFrameMat.at(dy,dx+1); + float dz2 = depthFrameMat.at(dy+1,dx); + float dz3 = depthFrameMat.at(dy+1,dx+1); + if(dz && dz1 && dz2 && dz3) { - float cx=-1,cy=-1; - reg_->apply(dx, dy, dz, cx, cy); - if(type_==kTypeRGBDepthSD) + float avg = (dz + dz1 + dz2 + dz3) / 4; + float thres = 0.01 * avg; + if( fabs(dz - avg) < thres && + fabs(dz1 - avg) < thres && + fabs(dz2 - avg) < thres && + fabs(dz3 - avg) < thres) { - cx/=2.0f; - cy/=2.0f; - } - int rcx = cvRound(cx); - int rcy = cvRound(cy); - if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows)) - { - unsigned short & zReg = depth.at(rcy, rcx); - if(zReg == 0 || zReg > (unsigned short)dz) + float cx=-1,cy=-1; + reg_->apply(dx, dy, dz, cx, cy); + if(type_==kTypeRGBDepthSD) { - zReg = (unsigned short)dz; + cx/=2.0f; + cy/=2.0f; + } + int rcx = cvRound(cx); + int rcy = cvRound(cy); + if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows)) + { + unsigned short & zReg = depth.at(rcy, rcx); + if(zReg == 0 || zReg > (unsigned short)dz) + { + zReg = (unsigned short)dz; + } } } } } } + util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD); + util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass + libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams(); + fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f); + fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f); + cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f); + cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f); + } + else + { + libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams(); + fx = params.fx; + fy = params.fy; + cx = params.cx; + cy = params.cy; } - util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD); - util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass - libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams(); - fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f); - fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f); - cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f); - cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f); - } - else - { - libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams(); - fx = params.fx; - fy = params.fy; - cx = params.cx; - cy = params.cy; } + cv::flip(depth, depth, 1); } - cv::flip(depth, depth, 1); } listener_->release(frames); } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 1be8cd1a..212898b8 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -485 + -591 760 - 1132 + 1502 @@ -1974,6 +1974,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki RGB+Depth HD + + + IR+Depth + + From 0d500ff1bf0e95796014b759593b196159cdea37 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 5 May 2015 19:43:25 -0400 Subject: [PATCH 2/2] Calibration Freeenct2: switching images by default --- guilib/include/rtabmap/gui/CalibrationDialog.h | 3 ++- guilib/src/CalibrationDialog.cpp | 9 ++++++++- guilib/src/PreferencesDialog.cpp | 1 + tools/Calibration/main.cpp | 5 ++++- 4 files changed, 15 insertions(+), 3 deletions(-) diff --git a/guilib/include/rtabmap/gui/CalibrationDialog.h b/guilib/include/rtabmap/gui/CalibrationDialog.h index 37ff0044..bb617456 100644 --- a/guilib/include/rtabmap/gui/CalibrationDialog.h +++ b/guilib/include/rtabmap/gui/CalibrationDialog.h @@ -47,7 +47,7 @@ class RTABMAPGUI_EXP CalibrationDialog : public QDialog, public UEventsHandler Q_OBJECT; public: - CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", QWidget * parent = 0); + CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", bool switchImages = false, QWidget * parent = 0); virtual ~CalibrationDialog(); bool isCalibrated() const {return models_[0].isValid() && (stereo_?models_[1].isValid():true);} @@ -58,6 +58,7 @@ public: void saveSettings(QSettings & settings, const QString & group = "") const; void loadSettings(QSettings & settings, const QString & group = ""); + void setSwitchedImages(bool switched); void setStereoMode(bool stereo); void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;} diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index 796806c7..5a722b09 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -46,7 +46,7 @@ namespace rtabmap { #define COUNT_MIN 40 -CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, QWidget * parent) : +CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) : QDialog(parent), stereo_(stereo), savingDirectory_(savingDirectory), @@ -91,6 +91,8 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector ui_->radioButton_raw->setChecked(true); + ui_->checkBox_switchImages->setChecked(switchImages); + this->setStereoMode(stereo_); } @@ -136,6 +138,11 @@ void CalibrationDialog::loadSettings(QSettings & settings, const QString & group } } +void CalibrationDialog::setSwitchedImages(bool switched) +{ + ui_->checkBox_switchImages->setChecked(switched); +} + void CalibrationDialog::setStereoMode(bool stereo) { this->restart(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 99c646ee..d14b1f00 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -3523,6 +3523,7 @@ void PreferencesDialog::calibrate() } } _calibrationDialog->setStereoMode(true); + _calibrationDialog->setSwitchedImages(dynamic_cast(camera) != 0); _calibrationDialog->setSavingDirectory(this->getCameraInfoDir()); _calibrationDialog->registerToEventsManager(); diff --git a/tools/Calibration/main.cpp b/tools/Calibration/main.cpp index ffe82dfd..b93b1080 100644 --- a/tools/Calibration/main.cpp +++ b/tools/Calibration/main.cpp @@ -128,6 +128,8 @@ int main(int argc, char * argv[]) UINFO("Using device %d", device); UINFO("Stereo: %s", stereo?"true":"false"); + bool switchImages = false; + rtabmap::Camera * cameraUsb = 0; rtabmap::CameraRGBD * camera = 0; if(driver == -1) @@ -181,6 +183,7 @@ int main(int argc, char * argv[]) UERROR("Not built with Freenect2 support..."); exit(-1); } + switchImages = true; camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBIR); } else if(driver == 6) @@ -230,7 +233,7 @@ int main(int argc, char * argv[]) } QApplication app(argc, argv); - rtabmap::CalibrationDialog dialog(stereo); + rtabmap::CalibrationDialog dialog(stereo, ".", switchImages); dialog.registerToEventsManager(); dialog.show();