i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraK4W2: added depth to color registration. For CameraFreenect2, kTypeColor2DepthSD is now used by default.

Browse Source
This commit is contained in:
matlabbe
2017-11-06 22:59:12 -05:00
parent 7dabfdc207
commit 8cdd138143
5 changed files with 363 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -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
};

364
corelib/src/CameraRGBD.cpp
View File

@@ -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<float>::infinity() && p.Y != -std::numeric_limits<float>::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<float>::infinity() && p.Y != -std::numeric_limits<float>::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<int>(p.X);
pixel_y_l = static_cast<int>(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<float>::infinity() && p.Y != -std::numeric_limits<float>::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<int>(p.X);
pixel_y_l = static_cast<int>(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<unsigned char>(pixel_y_l, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_h, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_l, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_h, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_l, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_h, pixel_x_l);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_l, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<unsigned char>(pixel_y_h, pixel_x_h);
ptr[0] = pSrc->rgbBlue;
ptr[1] = pSrc->rgbGreen;
ptr[2] = pSrc->rgbRed;
depth.at<unsigned short>(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<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
{
if (type_ == kTypeDepth2ColorSD)
{
p.X /= 2.0f;
p.Y /= 2.0f;
}
const unsigned short & depth_value = depthSource.at<unsigned short>(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<unsigned short>(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<unsigned short>(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!");
}
}
}
}

4
guilib/src/CameraViewer.cpp
View File

@@ -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>("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);

14
guilib/src/PreferencesDialog.cpp
View File

@@ -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());
}

69
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-174</y>
<y>-292</y>
<width>677</width>
<height>2682</height>
</rect>
@@ -2865,7 +2865,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_rgbd">
<property name="currentIndex">
<number>7</number>
<number>8</number>
</property>
<widget class="QWidget" name="page_32">
<layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3640,7 +3640,70 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</layout>
</widget>
<widget class="QWidget" name="page_72">
<layout class="QVBoxLayout" name="verticalLayout_127">
<layout class="QVBoxLayout" name="verticalLayout_127" stretch="1,0">
<item>
<widget class="QGroupBox" name="groupBox_k4w2">
<property name="sizePolicy">
<sizepolicy hsizetype="Ignored" vsizetype="Ignored">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>Kinect for Windows SDK v2</string>
</property>
<layout class="QGridLayout" name="gridLayout_96" columnstretch="0,1">
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_k4w2Format">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>Registered Color to Depth SD</string>
</property>
</item>
<item>
<property name="text">
<string>Registered Depth to Color SD</string>
</property>
</item>
<item>
<property name="text">
<string>Registered Depth to Color HD</string>
</property>
</item>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_440">
<property name="text">
<string>Format.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<spacer name="verticalSpacer_68">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_67">
<property name="orientation">