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

Added interpolate parameter to util3d::getDepth()

Browse Source 
Added Reset calibration button
Added Sense 3D scanner input source

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1625 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-30 17:42:58 +00:00
parent d3599f7937
commit 8e65812324
8 changed files with 199 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/util3d.h
View File

@@ -94,10 +94,12 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
pcl::PointXYZ RTABMAP_EXP getDepth(const cv::Mat & depthImage,
int x, int y,
pcl::PointXYZ RTABMAP_EXP getDepth(
const cv::Mat & depthImage,
float x, float y,
float cx, float cy,
float fx, float fy);
float fx, float fy,
bool interpolate);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

2
corelib/src/Features2d.cpp
View File

@@ -68,7 +68,7 @@ void filterKeypointsByDepth(
int oi=0;
for(unsigned int i=0; i<keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy);
pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy, true);
if(uIsFinite(pt.z) && pt.z < maxDepth)
{
output[oi++] = keypoints[i];

115
corelib/src/util3d.cpp
View File

@@ -312,12 +312,13 @@ std::multimap<int, pcl::PointXYZ> generateWords3(
{
pcl::PointXYZ pt = util3d::getDepth(
depth,
iter->second.pt.x+0.5f,
iter->second.pt.y+0.5f,
iter->second.pt.x,
iter->second.pt.y,
cx,
cy,
fx,
fy);
fy,
true);
if(!transform.isNull() && !transform.isIdentity())
{
@@ -414,14 +415,23 @@ void findCorrespondences(
inliers2.resize(oi);
}
pcl::PointXYZ getDepth(const cv::Mat & depthImage,
int x, int y,
pcl::PointXYZ getDepth(
const cv::Mat & depthImage,
float x, float y,
float cx, float cy,
float fx, float fy)
float fx, float fy,
bool interpolate)
{
UASSERT(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows);
pcl::PointXYZ pt;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
if(!(int(x) >=0 && int(x)<depthImage.cols && int(y) >=0 && int(y)<depthImage.rows))
{
UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f, y=%f, cols=%d, rows=%d)",
x,y,depthImage.cols, depthImage.rows);
pt.x = pt.y = pt.z = bad_point;
return pt;
}
// Use correct principal point from calibration
float center_x = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
@@ -433,31 +443,88 @@ pcl::PointXYZ getDepth(const cv::Mat & depthImage,
float unit_scaling = isInMM?0.001f:1.0f;
float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
float bad_point = std::numeric_limits<float>::quiet_NaN ();
float depth;
bool isValid;
if(isInMM)
float depth = 0.0f;
if(!interpolate || (int(x) < 1 || int(y) < 1 || int(x) >= depthImage.cols-1 || int(y) >= depthImage.rows-1))
{
depth = (float)depthImage.at<uint16_t>(y,x);
isValid = depth != 0.0f;
if(interpolate)
{
UERROR("Cannot interpolate for points on the image side. Falling back to no interpolation.");
}
// select directly to corresponding pixel
depth = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
}
else
{
depth = depthImage.at<float>(y,x);
isValid = uIsFinite(depth);
// Interpolate x axis
float depthX;
float first;
float second;
if(int(x) == int(x+0.5f))
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)-1):depthImage.at<float>(int(y),int(x)-1);
second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
}
else
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)+1):depthImage.at<float>(int(y),int(x)+1);
}
if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
{
// y = ax + b...
float a = second-first;
float b = first - a*(float(int(x))-0.5f);
depthX = a*(x) + b;
//UDEBUG("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthX);
}
if(depthX != 0.0f)
{
depth = depthX;
}
else
{
// Interpolate y axis
float depthY;
if(int(y) == int(y+0.5f))
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y)-1,int(x)):depthImage.at<float>(int(y)-1,int(x));
second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
}
else
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
second = isInMM?(float)depthImage.at<uint16_t>(int(y)+1,int(x)):depthImage.at<float>(int(y)+1,int(x));
}
if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
{
// y = ax + b...
float a = second-first;
float b = first - a*(float(int(y))-0.5f);
depthY = a*(y) + b;
//UWARN("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthY);
}
if(depthY != 0.0f)
{
depth = depthY;
}
}
}
// Check for invalid measurements
if (!isValid)
if (depth==0.0f || !uIsFinite(depth))
{
pt.x = pt.y = pt.z = bad_point;
}
else
{
// Fill in XYZ
pt.x = (float(x) - center_x) * depth * constant_x;
pt.y = (float(y) - center_y) * depth * constant_y;
pt.x = (x - center_x) * depth * constant_x;
pt.y = (y - center_y) * depth * constant_y;
pt.z = depth*unit_scaling;
}
return pt;
@@ -642,7 +709,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
{
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy);
pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -705,7 +772,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.r = v;
}
pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy);
pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -944,8 +1011,8 @@ void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2
iter!=correspondences.end();
++iter)
{
pcl::PointXYZ pt1 = getDepth(depthImage1, int(iter->first.x+0.5f), int(iter->first.y+0.5f), cx, cy, fx, fy);
pcl::PointXYZ pt2 = getDepth(depthImage2, int(iter->second.x+0.5f), int(iter->second.y+0.5f), cx, cy, fx, fy);
pcl::PointXYZ pt1 = getDepth(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
pcl::PointXYZ pt2 = getDepth(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
(maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
{
@@ -1470,7 +1537,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
for(unsigned int i=0; i<kpts.size(); ++i)
{
pcl::PointXYZ pt = getDepth(imageDepth, int(kpts[i].pt.x+0.5f), int(kpts[i].pt.y+0.5f), (float)image.cols/2, (float)image.rows/2, 1.0f/constant, 1.0f/constant);
pcl::PointXYZ pt = getDepth(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
{
points->push_back(pt);

1
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -228,6 +228,7 @@ private slots:
void cleanOdometryTest();
void testOdometry();
void calibrate();
void resetCalibration();
protected:
virtual void showEvent ( QShowEvent * event );

2
guilib/src/MainWindow.cpp
View File

@@ -297,6 +297,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
_ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
connect(_ui->actionOpenNI2_Sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
_ui->actionOpenNI2_Sense->setEnabled(CameraOpenNI2::available());
connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures()));

32
guilib/src/PreferencesDialog.cpp
View File

@@ -242,6 +242,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_openniCx, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_openniCy, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
connect(_ui->pushButton_calibrate_reset, SIGNAL(clicked()), this, SLOT(resetCalibration()));
//Rtabmap basic
@@ -1655,7 +1656,7 @@ void PreferencesDialog::selectSourceRGBD(Src src)
tr("Activate RGB-D SLAM?"),
tr("You've selected RGB-D camera as source input, "
"would you want to activate RGB-D SLAM mode?"),
QMessageBox::Yes | QMessageBox::No);
QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
if(button & QMessageBox::Yes)
{
_ui->general_checkBox_activateRGBD->setChecked(true);
@@ -1673,6 +1674,27 @@ void PreferencesDialog::selectSourceRGBD(Src src)
{
_ui->groupBox_sourceImage->setChecked(false);
_ui->groupBox_sourceDatabase->setChecked(false);
if(_ui->doubleSpinBox_openniFx->value() != 0 ||
_ui->doubleSpinBox_openniFy->value() != 0 ||
_ui->doubleSpinBox_openniCx->value() != 0 ||
_ui->doubleSpinBox_openniCy->value() != 0 )
{
int button = QMessageBox::information(this,
tr("Calibration detected"),
tr("Some calibration values (fx=%1, fy=%2, cx=%3, cy=%4) are set.\n"
"Do you want to reset them to factory defaults?")
.arg(_ui->doubleSpinBox_openniFx->value())
.arg(_ui->doubleSpinBox_openniFy->value())
.arg(_ui->doubleSpinBox_openniCx->value())
.arg(_ui->doubleSpinBox_openniCy->value()),
QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
if(button & QMessageBox::Yes)
{
this->resetCalibration();
}
}
if(validateForm())
{
this->writeSettings();
@@ -2988,4 +3010,12 @@ void PreferencesDialog::calibrate()
}
}
void PreferencesDialog::resetCalibration()
{
_ui->doubleSpinBox_openniFx->setValue(0);
_ui->doubleSpinBox_openniFy->setValue(0);
_ui->doubleSpinBox_openniCx->setValue(0);
_ui->doubleSpinBox_openniCy->setValue(0);
}
}

14
guilib/src/ui/mainWindow.ui
View File

@@ -27,7 +27,7 @@
<x>0</x>
<y>0</y>
<width>1012</width>
<height>25</height>
<height>21</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -110,8 +110,15 @@
<addaction name="actionOpenNI2"/>
<addaction name="actionOpenNI_CV_ASUS"/>
</widget>
<widget class="QMenu" name="menuSense_3D_scanner">
<property name="title">
<string>Sense 3D scanner</string>
</property>
<addaction name="actionOpenNI2_Sense"/>
</widget>
<addaction name="menuKinect_for_Xbox_360"/>
<addaction name="menuXtion_PRO_LIVE"/>
<addaction name="menuSense_3D_scanner"/>
</widget>
<addaction name="menuRGB_D_camera"/>
<addaction name="menuImage"/>
@@ -1056,6 +1063,11 @@
<string>View scans...</string>
</property>
</action>
<action name="actionOpenNI2_Sense">
<property name="text">
<string>OpenNI2</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>

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

@@ -64,8 +64,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>732</width>
<height>1198</height>
<width>736</width>
<height>751</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>1</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -287,14 +287,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<property name="title">
<string>Save/load settings</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_9">
<item>
<widget class="QPushButton" name="pushButton_saveConfig">
<property name="text">
<string>Save settings (*.ini) ...</string>
</property>
</widget>
</item>
<layout class="QHBoxLayout" name="horizontalLayout_9">
<item>
<widget class="QPushButton" name="pushButton_loadConfig">
<property name="text">
@@ -302,6 +295,13 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_saveConfig">
<property name="text">
<string>Save settings (*.ini) ...</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_resetConfig">
<property name="text">
@@ -309,6 +309,19 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
@@ -1820,6 +1833,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QPushButton" name="pushButton_calibrate">
<property name="text">
@@ -1827,6 +1842,28 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_calibrate_reset">
<property name="text">
<string>Reset</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
</item>