diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index 196f9961..cf86a795 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -80,6 +80,7 @@ typedef struct _freenect_device freenect_device; typedef struct IKinectSensor IKinectSensor; typedef struct ICoordinateMapper ICoordinateMapper; typedef struct _DepthSpacePoint DepthSpacePoint; +typedef struct _ColorSpacePoint ColorSpacePoint; typedef struct tagRGBQUAD RGBQUAD; typedef struct IMultiSourceFrameReader IMultiSourceFrameReader; @@ -265,7 +266,7 @@ public: public: // default local transform z in, x right, y down)); CameraFreenect2(int deviceId= 0, - Type type = kTypeColor2DepthSD, + Type type = kTypeDepth2ColorSD, float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity(), float minDepth = 0.3f, @@ -311,6 +312,12 @@ class RTABMAP_EXP CameraK4W2 : public: static bool available(); + enum Type { + kTypeColor2DepthSD, + kTypeDepth2ColorSD, + kTypeDepth2ColorHD + }; + public: static const int cDepthWidth = 512; static const int cDepthHeight = 424; @@ -319,7 +326,8 @@ public: public: // default local transform z in, x right, y down)); - CameraK4W2(int deviceId = 0, + CameraK4W2(int deviceId = 0, // not used + Type type = kTypeDepth2ColorSD, float imageRate = 0.0f, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraK4W2(); @@ -331,14 +339,20 @@ public: protected: virtual SensorData captureImage(CameraInfo * info = 0); +private: + void close(); + private: #ifdef RTABMAP_K4W2 + Type type_; IKinectSensor* pKinectSensor_; ICoordinateMapper* pCoordinateMapper_; DepthSpacePoint* pDepthCoordinates_; + ColorSpacePoint* pColorCoordinates_; IMultiSourceFrameReader* pMultiSourceFrameReader_; RGBQUAD * pColorRGBX_; INT_PTR hMSEvent; + CameraModel colorCameraModel_; #endif }; diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 9ba2bb72..f1e90eb2 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -2073,14 +2073,17 @@ bool CameraK4W2::available() CameraK4W2::CameraK4W2( int deviceId, + Type type, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform) #ifdef RTABMAP_K4W2 , + type_(type), pKinectSensor_(NULL), pCoordinateMapper_(NULL), pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]), + pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]), pMultiSourceFrameReader_(NULL), pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]), hMSEvent(NULL) @@ -2097,12 +2100,25 @@ CameraK4W2::~CameraK4W2() pDepthCoordinates_ = NULL; } + if (pColorCoordinates_) + { + delete[] pColorCoordinates_; + pColorCoordinates_ = NULL; + } + if (pColorRGBX_) { delete[] pColorRGBX_; pColorRGBX_ = NULL; } + close(); +#endif +} + +void CameraK4W2::close() +{ +#ifdef RTABMAP_K4W2 if (pMultiSourceFrameReader_) { pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent); @@ -2123,6 +2139,8 @@ CameraK4W2::~CameraK4W2() } SafeRelease(pKinectSensor_); + + colorCameraModel_ = CameraModel(); #endif } @@ -2131,6 +2149,8 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & #ifdef RTABMAP_K4W2 HRESULT hr; + close(); + hr = GetDefaultKinectSensor(&pKinectSensor_); if (FAILED(hr)) { @@ -2141,22 +2161,22 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & { // Initialize the Kinect and get coordinate mapper and the frame reader - if (SUCCEEDED(hr)) - { - hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_); - } - hr = pKinectSensor_->Open(); if (SUCCEEDED(hr)) { - hr = pKinectSensor_->OpenMultiSourceFrameReader( - FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color, - &pMultiSourceFrameReader_); + hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_); if (SUCCEEDED(hr)) { - hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent); + hr = pKinectSensor_->OpenMultiSourceFrameReader( + FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color, + &pMultiSourceFrameReader_); + + if (SUCCEEDED(hr)) + { + hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent); + } } } } @@ -2164,9 +2184,82 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & if (!pKinectSensor_ || FAILED(hr)) { UERROR("No ready Kinect found!"); + close(); return false; } + // to query camera parameters, we should wait a little + uSleep(3000); + + // initialize color calibration if not set yet + CameraIntrinsics intrinsics; + hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics); + if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f) + { + // guess color intrinsics by comparing two reprojections + CameraModel depthModel( + intrinsics.FocalLengthX, + intrinsics.FocalLengthY, + intrinsics.PrincipalPointX, + intrinsics.PrincipalPointY); + + cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000; + hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_); + if (SUCCEEDED(hr)) + { + int firstIndex = -1; + int lastIndex = -1; + for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex) + { + ColorSpacePoint p = pColorCoordinates_[depthIndex]; + // Values that are negative infinity means it is an invalid color to depth mapping so we + // skip processing for this pixel + if (p.X != -std::numeric_limits::infinity() && p.Y != -std::numeric_limits::infinity()) + { + if (firstIndex == -1) + { + firstIndex = depthIndex; + } + lastIndex = depthIndex; + } + } + + UASSERT(firstIndex >= 0 && lastIndex >= 0); + float fx, fy, cx, cy; + float x1, y1, z1, x2, y2, z2; + depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1); + depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2); + ColorSpacePoint uv1 = pColorCoordinates_[firstIndex]; + ColorSpacePoint uv2 = pColorCoordinates_[lastIndex]; + fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1); + cx = uv1.X - (x1 / z1) * fx; + fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1); + cy = uv1.Y - (y1 / z1) * fy; + + colorCameraModel_ = CameraModel( + fx, + fy, + float(cColorWidth) - cx, + cy, + this->getLocalTransform(), + 0, + cv::Size(cColorWidth, cColorHeight)); + } + } + + if (!colorCameraModel_.isValidForProjection()) + { + UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD."); + close(); + return false; + } + + std::string serial = getSerial(); + if (!serial.empty()) + { + UINFO("Running kinect device \"%s\"", serial.c_str()); + } + return true; #else UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!"); @@ -2181,6 +2274,19 @@ bool CameraK4W2::isCalibrated() const std::string CameraK4W2::getSerial() const { +#ifdef RTABMAP_K4W2 + if (pKinectSensor_) + { + wchar_t uid[255] = { 0 }; + // It seems to fail every time!? + HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid); + if (SUCCEEDED(hr)) + { + std::wstring ws(uid); + return std::string(ws.begin(), ws.end()); + } + } +#endif return ""; } @@ -2240,7 +2346,6 @@ SensorData CameraK4W2::captureImage(CameraInfo * info) if (pDepthFrame && pColorFrame) { - INT64 nDepthTime = 0; IFrameDescription* pDepthFrameDescription = NULL; int nDepthWidth = 0; int nDepthHeight = 0; @@ -2255,49 +2360,24 @@ SensorData CameraK4W2::captureImage(CameraInfo * info) RGBQUAD *pColorBuffer = NULL; // get depth frame data - hr = pDepthFrame->get_RelativeTime(&nDepthTime); - if (SUCCEEDED(hr)) - { hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription); - } - if (SUCCEEDED(hr)) - { hr = pDepthFrameDescription->get_Width(&nDepthWidth); - } - if (SUCCEEDED(hr)) - { hr = pDepthFrameDescription->get_Height(&nDepthHeight); - } - if (SUCCEEDED(hr)) - { hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer); - } + // get color frame data - if (SUCCEEDED(hr)) - { hr = pColorFrame->get_FrameDescription(&pColorFrameDescription); - } - if (SUCCEEDED(hr)) - { hr = pColorFrameDescription->get_Width(&nColorWidth); - } - if (SUCCEEDED(hr)) - { hr = pColorFrameDescription->get_Height(&nColorHeight); - } - if (SUCCEEDED(hr)) - { hr = pColorFrame->get_RawColorImageFormat(&imageFormat); - } - if (SUCCEEDED(hr)) { if (imageFormat == ColorImageFormat_Bgra) @@ -2315,7 +2395,8 @@ SensorData CameraK4W2::captureImage(CameraInfo * info) hr = E_FAIL; } } - if (SUCCEEDED(hr)) + + if(SUCCEEDED(hr)) { //ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight, // pColorBuffer, nColorWidth, nColorHeight, @@ -2326,74 +2407,161 @@ SensorData CameraK4W2::captureImage(CameraInfo * info) pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) && pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight)) { - HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_); - if (SUCCEEDED(hr)) + if (type_ == kTypeColor2DepthSD) { - cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1); - cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3); - // loop over output pixels - for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex) + HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_); + if (SUCCEEDED(hr)) { - DepthSpacePoint p = pDepthCoordinates_[colorIndex]; - // Values that are negative infinity means it is an invalid color to depth mapping so we - // skip processing for this pixel - if (p.X != -std::numeric_limits::infinity() && p.Y != -std::numeric_limits::infinity()) + cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1); + cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3); + // loop over output pixels + for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex) { - // To avoid black lines caused by rounding pixel values, we should set 4 pixels - // At the same do mirror - int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h; - pixel_x_l = nDepthWidth-static_cast(p.X); - pixel_y_l = static_cast(p.Y); - pixel_x_h = pixel_x_l-1; - pixel_y_h = pixel_y_l+1; + DepthSpacePoint p = pDepthCoordinates_[colorIndex]; + // Values that are negative infinity means it is an invalid color to depth mapping so we + // skip processing for this pixel + if (p.X != -std::numeric_limits::infinity() && p.Y != -std::numeric_limits::infinity()) + { + // To avoid black lines caused by rounding pixel values, we should set 4 pixels + // At the same do mirror + int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h; + pixel_x_l = nDepthWidth - static_cast(p.X); + pixel_y_l = static_cast(p.Y); + pixel_x_h = pixel_x_l - 1; + pixel_y_h = pixel_y_l + 1; - const RGBQUAD* pSrc = pColorBuffer + colorIndex; - if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight)) - { - unsigned char * ptr = imageColorRegistered.ptr(pixel_y_l, pixel_x_l); - ptr[0] = pSrc->rgbBlue; - ptr[1] = pSrc->rgbGreen; - ptr[2] = pSrc->rgbRed; - depth.at(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth); - } - if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight)) - { - unsigned char * ptr = imageColorRegistered.ptr(pixel_y_h, pixel_x_l); - ptr[0] = pSrc->rgbBlue; - ptr[1] = pSrc->rgbGreen; - ptr[2] = pSrc->rgbRed; - depth.at(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth); - } - if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight)) - { - unsigned char * ptr = imageColorRegistered.ptr(pixel_y_l, pixel_x_h); - ptr[0] = pSrc->rgbBlue; - ptr[1] = pSrc->rgbGreen; - ptr[2] = pSrc->rgbRed; - depth.at(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth); - } - if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight)) - { - unsigned char * ptr = imageColorRegistered.ptr(pixel_y_h, pixel_x_h); - ptr[0] = pSrc->rgbBlue; - ptr[1] = pSrc->rgbGreen; - ptr[2] = pSrc->rgbRed; - depth.at(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth); + const RGBQUAD* pSrc = pColorBuffer + colorIndex; + if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_l, pixel_x_l); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth); + } + if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_h, pixel_x_l); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth); + } + if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_l, pixel_x_h); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth); + } + if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight)) + { + unsigned char * ptr = imageColorRegistered.ptr(pixel_y_h, pixel_x_h); + ptr[0] = pSrc->rgbBlue; + ptr[1] = pSrc->rgbGreen; + ptr[2] = pSrc->rgbRed; + depth.at(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth); + } } } - } - CameraIntrinsics intrinsics; - pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics); - CameraModel model( - intrinsics.FocalLengthX, - intrinsics.FocalLengthY, - intrinsics.PrincipalPointX, - intrinsics.PrincipalPointY, - this->getLocalTransform(), - 0, - depth.size()); - data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now()); + CameraIntrinsics intrinsics; + pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics); + CameraModel model( + intrinsics.FocalLengthX, + intrinsics.FocalLengthY, + intrinsics.PrincipalPointX, + intrinsics.PrincipalPointY, + this->getLocalTransform(), + 0, + depth.size()); + data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now()); + } + else + { + UERROR("Failed color to depth registration!"); + } + } + else //depthToColor + { + HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_); + if (SUCCEEDED(hr)) + { + cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer); + cv::Mat depthRegistered = cv::Mat::zeros( + type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight, + type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth, + CV_16UC1); + cv::Mat imageColor; + if(type_ == kTypeDepth2ColorSD) + { + cv::Mat tmp; + cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA); + cv::cvtColor(tmp, imageColor, CV_BGRA2BGR); + } + else + { + cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR); + } + // loop over output pixels + for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex) + { + ColorSpacePoint p = pColorCoordinates_[depthIndex]; + // Values that are negative infinity means it is an invalid color to depth mapping so we + // skip processing for this pixel + if (p.X != -std::numeric_limits::infinity() && p.Y != -std::numeric_limits::infinity()) + { + if (type_ == kTypeDepth2ColorSD) + { + p.X /= 2.0f; + p.Y /= 2.0f; + } + const unsigned short & depth_value = depthSource.at(0, depthIndex); + int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h; + // get the coordinate on image plane. + pixel_x_l = depthRegistered.cols - p.X; // flip depth + pixel_y_l = p.Y; + pixel_x_h = pixel_x_l - 1; + pixel_y_h = pixel_y_l + 1; + + if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols && + pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line + depth_value) + { + unsigned short & depthPixel = depthRegistered.at(pixel_y_l, pixel_x_l); + if (depthPixel == 0 || depthPixel > depth_value) + { + depthPixel = depth_value; + } + } + if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols && + pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line + depth_value) + { + unsigned short & depthPixel = depthRegistered.at(pixel_y_h, pixel_x_h); + if (depthPixel == 0 || depthPixel > depth_value) + { + depthPixel = depth_value; + } + } + } + } + + CameraModel model = colorCameraModel_; + if (type_ == kTypeDepth2ColorSD) + { + model = model.scaled(0.5); + } + util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD); + depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1); + cv::flip(imageColor, imageColor, 1); + data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now()); + } + else + { + UERROR("Failed depth to color registration!"); + } } } } diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index a09116c1..f36f81c1 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -52,7 +52,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : imageView_(new ImageView(this)), cloudView_(new CloudViewer(this)), processingImages_(false), - validDecimationValue_(1), + validDecimationValue_(2), parameters_(parameters) { qRegisterMetaType("rtabmap::SensorData"); @@ -68,7 +68,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : decimationSpin_ = new QSpinBox(this); decimationSpin_->setMinimum(1); decimationSpin_->setMaximum(16); - decimationSpin_->setValue(1); + decimationSpin_->setValue(2); pause_ = new QPushButton("Pause", this); pause_->setCheckable(true); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index a6ea16a0..f164a25f 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -527,6 +527,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_freenect2EdgeAwareFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_freenect2Pipeline, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_k4w2Format, SIGNAL(currentIndexChanged(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->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -1519,13 +1520,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->openni2_stampsIdsUsed->setChecked(false); _ui->openni2_hshift->setValue(0); _ui->openni2_vshift->setValue(0); - _ui->comboBox_freenect2Format->setCurrentIndex(0); + _ui->comboBox_freenect2Format->setCurrentIndex(1); _ui->doubleSpinBox_freenect2MinDepth->setValue(0.3); _ui->doubleSpinBox_freenect2MaxDepth->setValue(12.0); _ui->checkBox_freenect2BilateralFiltering->setChecked(true); _ui->checkBox_freenect2EdgeAwareFiltering->setChecked(true); _ui->checkBox_freenect2NoiseFiltering->setChecked(true); _ui->lineEdit_freenect2Pipeline->setText(""); + _ui->comboBox_k4w2Format->setCurrentIndex(1); _ui->comboBox_realsensePresetRGB->setCurrentIndex(0); _ui->comboBox_realsensePresetDepth->setCurrentIndex(2); _ui->checkbox_realsenseOdom->setChecked(false); @@ -1903,6 +1905,10 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->lineEdit_freenect2Pipeline->setText(settings.value("pipeline", _ui->lineEdit_freenect2Pipeline->text()).toString()); settings.endGroup(); // Freenect2 + settings.beginGroup("K4W2"); + _ui->comboBox_k4w2Format->setCurrentIndex(settings.value("format", _ui->comboBox_k4w2Format->currentIndex()).toInt()); + settings.endGroup(); // K4W2 + 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()); @@ -2301,6 +2307,10 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("pipeline", _ui->lineEdit_freenect2Pipeline->text()); settings.endGroup(); // Freenect2 + settings.beginGroup("K4W2"); + settings.setValue("format", _ui->comboBox_k4w2Format->currentIndex()); + settings.endGroup(); // K4W2 + settings.beginGroup("RealSense"); settings.setValue("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()); settings.setValue("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()); @@ -4138,6 +4148,7 @@ void PreferencesDialog::updateSourceGrpVisibility() _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_k4w2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - 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); @@ -4768,6 +4779,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) { camera = new CameraK4W2( this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()), + (CameraK4W2::Type)_ui->comboBox_k4w2Format->currentIndex(), this->getGeneralInputRate(), this->getSourceLocalTransform()); } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 69b5bc0a..254f3dfd 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -174 + -292 677 2682 @@ -2865,7 +2865,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 7 + 8 @@ -3640,7 +3640,70 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + + + + + + 0 + 0 + + + + Kinect for Windows SDK v2 + + + + + + QComboBox::AdjustToContents + + + + Registered Color to Depth SD + + + + + Registered Depth to Color SD + + + + + Registered Depth to Color HD + + + + + + + + Format. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + +