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

Stereo! The memory can now handle directly stereo images. Disparity can be computed on the fly by keeping left and right images, so for features extraction (and for re-extraction on loop closure), we can compute 3D points precisely. New parameters can be found under "RGB-D Mapping->Stereo". Full image disparity is reconstructed in the GUI (not the core).

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1861 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-16 00:14:23 +00:00
parent 8f451029e2
commit fcd3301665
18 changed files with 1230 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
guilib/src/DatabaseViewer.cpp
View File

@@ -1410,19 +1410,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
if(depthA.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageA.channels() == 3)
{
cv::cvtColor(imageA, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageA;
}
cv::Mat disparity = util3d::disparityFromStereoImages(leftImg, depthA);
cloudA = rtabmap::util3d::cloudFromDisparityRGB(
cloudA = rtabmap::util3d::cloudFromStereoImages(
imageA,
disparity,
depthA,
cxA, cyA,
fxA, fyA,
1);
@@ -1443,18 +1433,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(depthB.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageB.channels() == 3)
{
cv::cvtColor(imageB, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageB;
}
cloudB = rtabmap::util3d::cloudFromDisparityRGB(
cloudB = rtabmap::util3d::cloudFromStereoImages(
imageB,
util3d::disparityFromStereoImages(leftImg, depthB),
depthB,
cxB, cyB,
fxB, fyB,
1);
@@ -1786,6 +1767,13 @@ void DatabaseViewer::refineConstraint(int from, int to)
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
{
QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
UERROR("ICP 3D cannot be done on stereo images!");
return;
}
cloudA = util3d::getICPReadyCloud(depthA,
fxA, fyA, cxA, cyA,
ui_->spinBox_icp_decimation->value(),

48
guilib/src/LoopClosureViewer.cpp
View File

@@ -147,12 +147,24 @@ void LoopClosureViewer::updateView(const Transform & transform)
cv::Mat depth2dB = ct2dB.getUncompressedData();
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
cloudA = util3d::cloudFromDepthRGB(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
sA_->getDepthFx(), sA_->getDepthFy(),
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
if(depthA.type() == CV_8UC1)
{
cloudA = util3d::cloudFromStereoImages(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
sA_->getDepthFx(), sA_->getDepthFy(),
decimation);
}
else
{
cloudA = util3d::cloudFromDepthRGB(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
sA_->getDepthFx(), sA_->getDepthFy(),
decimation);
}
cloudA = util3d::removeNaNFromPointCloud(cloudA);
@@ -167,12 +179,24 @@ void LoopClosureViewer::updateView(const Transform & transform)
}
cloudA = util3d::transformPointCloud(cloudA, sA_->getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
cloudB = util3d::cloudFromDepthRGB(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
sB_->getDepthFx(), sB_->getDepthFy(),
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(depthB.type() == CV_8UC1)
{
cloudB = util3d::cloudFromStereoImages(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
sB_->getDepthFx(), sB_->getDepthFy(),
decimation);
}
else
{
cloudB = util3d::cloudFromDepthRGB(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
sB_->getDepthFx(), sB_->getDepthFy(),
decimation);
}
cloudB = util3d::removeNaNFromPointCloud(cloudB);

25
guilib/src/MainWindow.cpp
View File

@@ -3766,12 +3766,25 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
float maxDepth) const
{
UTimer timer;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
}
else
{
cloud = util3d::cloudFromDepthRGB(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
}
if(cloud->size())
{

24
guilib/src/OdometryViewer.cpp
View File

@@ -98,12 +98,24 @@ void OdometryViewer::processData()
// visualization: buffering the clouds
// Create the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudFromDepthRGB(
data.image(),
data.depth(),
data.cx(), data.cy(),
data.fx(), data.fy(),
decimation_);
if(data.depth().type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(
data.image(),
data.depth(),
data.cx(), data.cy(),
data.fx(), data.fy(),
decimation_);
}
else
{
cloud = util3d::cloudFromDepthRGB(
data.image(),
data.depth(),
data.cx(), data.cy(),
data.fx(), data.fy(),
decimation_);
}
if(voxelSize_ > 0.0f)
{

9
guilib/src/PreferencesDialog.cpp
View File

@@ -482,6 +482,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_flow_subpix_iterations->setObjectName(Parameters::kOdomFlowSubPixIterations().c_str());
_ui->odom_flow_subpix_eps->setObjectName(Parameters::kOdomFlowSubPixEps().c_str());
//Stereo
_ui->stereo_winSize->setObjectName(Parameters::kStereoWinSize().c_str());
_ui->stereo_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
_ui->stereo_iterations->setObjectName(Parameters::kStereoIterations().c_str());
_ui->stereo_eps->setObjectName(Parameters::kStereoEps().c_str());
_ui->stereo_subpix_winSize->setObjectName(Parameters::kStereoSubPixWinSize().c_str());
_ui->stereo_subpix_iterations->setObjectName(Parameters::kStereoSubPixIterations().c_str());
_ui->stereo_subpix_eps->setObjectName(Parameters::kStereoSubPixEps().c_str());
setupSignals();
// custom signals
connect(_ui->doubleSpinBox_kp_roi0, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));

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

@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>9</number>
<number>22</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5833,6 +5833,248 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</layout>
</widget>
<widget class="QWidget" name="page_29">
<layout class="QVBoxLayout" name="verticalLayout_57">
<item>
<widget class="QGroupBox" name="groupBox_stereo2">
<property name="title">
<string>Stereo</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_56">
<item>
<widget class="QLabel" name="label_187">
<property name="text">
<string>These parameters are used to find the 3D positions of visual words using disparity between the left and right images. Optical flow is used to find corresponding corners in the right image of words extracted from the left image. Sub pixel corners may not be used for features like SURF/SIFT, which are already sub pixel.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_10">
<property name="title">
<string>calcOpticalFlowPyrLK()</string>
</property>
<layout class="QGridLayout" name="gridLayout_33" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="stereo_winSize">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>21</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_205">
<property name="text">
<string>Window size.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="stereo_iterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>30</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_206">
<property name="text">
<string>Iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="stereo_eps">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_207">
<property name="text">
<string>Epsilon.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_208">
<property name="text">
<string>Max level.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="stereo_maxLevel">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>3</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_9">
<property name="title">
<string>cornerSubPix()</string>
</property>
<layout class="QGridLayout" name="gridLayout_32" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="stereo_subpix_winSize">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>5</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_200">
<property name="text">
<string>Window size for sub pixel estimation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="stereo_subpix_iterations">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_203">
<property name="text">
<string>Iterations for sub pixel estimation. 0 disables sub pixel refining.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="stereo_subpix_eps">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.030000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_204">
<property name="text">
<string>Epsilon for sub pixel estimation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_29">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>464</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_12">
<layout class="QVBoxLayout" name="verticalLayout_32">
<item>