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

databaseViewer: enabled refineICP for stereo images

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1960 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-03 18:44:21 +00:00
parent 4140e883b5
commit 61ce7c4274
1 changed files with 64 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
guilib/src/DatabaseViewer.cpp
View File

@@ -1556,13 +1556,31 @@ void DatabaseViewer::refineConstraint(int from, int to)
cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepthCompressed()); cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepthCompressed());
cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepthCompressed()); cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepthCompressed());
if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1) if(depthA.type() == CV_8UC1)
{ {
QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!")); cv::Mat leftMono;
UERROR("ICP 3D cannot be done on stereo images!"); cv::Mat left = rtabmap::util3d::uncompressImage(dataFrom.getImageCompressed());
return; if(left.channels() > 1)
{
cv::cvtColor(left, leftMono, CV_BGR2GRAY);
} }
else
{
leftMono = left;
}
cloudA = util3d::cloudFromDisparity(util3d::disparityFromStereoImages(leftMono, depthA), dataFrom.getDepthCx(), dataFrom.getDepthCy(), dataFrom.getDepthFx(), dataFrom.getDepthFy(), ui_->spinBox_icp_decimation->value());
if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
{
cloudA = util3d::passThrough<pcl::PointXYZ>(cloudA, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
}
if(ui_->doubleSpinBox_icp_voxel->value() > 0)
{
cloudA = util3d::voxelize<pcl::PointXYZ>(cloudA, ui_->doubleSpinBox_icp_voxel->value());
}
cloudA = util3d::transformPointCloud<pcl::PointXYZ>(cloudA, dataFrom.getLocalTransform());
}
else
{
cloudA = util3d::getICPReadyCloud(depthA, cloudA = util3d::getICPReadyCloud(depthA,
dataFrom.getDepthFx(), dataFrom.getDepthFy(), dataFrom.getDepthCx(), dataFrom.getDepthCy(), dataFrom.getDepthFx(), dataFrom.getDepthFy(), dataFrom.getDepthCx(), dataFrom.getDepthCy(),
ui_->spinBox_icp_decimation->value(), ui_->spinBox_icp_decimation->value(),
@@ -1570,6 +1588,32 @@ void DatabaseViewer::refineConstraint(int from, int to)
ui_->doubleSpinBox_icp_voxel->value(), ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling 0, // no sampling
dataFrom.getLocalTransform()); dataFrom.getLocalTransform());
}
if(depthB.type() == CV_8UC1)
{
cv::Mat leftMono;
cv::Mat left = rtabmap::util3d::uncompressImage(dataTo.getImageCompressed());
if(left.channels() > 1)
{
cv::cvtColor(left, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = left;
}
cloudB = util3d::cloudFromDisparity(util3d::disparityFromStereoImages(leftMono, depthB), dataTo.getDepthCx(), dataTo.getDepthCy(), dataTo.getDepthFx(), dataTo.getDepthFy(), ui_->spinBox_icp_decimation->value());
if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
{
cloudB = util3d::passThrough<pcl::PointXYZ>(cloudB, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
}
if(ui_->doubleSpinBox_icp_voxel->value() > 0)
{
cloudB = util3d::voxelize<pcl::PointXYZ>(cloudB, ui_->doubleSpinBox_icp_voxel->value());
}
cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, currentLink.transform() * dataTo.getLocalTransform());
}
else
{
cloudB = util3d::getICPReadyCloud(depthB, cloudB = util3d::getICPReadyCloud(depthB,
dataTo.getDepthFx(), dataTo.getDepthFy(), dataTo.getDepthCx(), dataTo.getDepthCy(), dataTo.getDepthFx(), dataTo.getDepthFy(), dataTo.getDepthCx(), dataTo.getDepthCy(),
ui_->spinBox_icp_decimation->value(), ui_->spinBox_icp_decimation->value(),
@@ -1577,6 +1621,7 @@ void DatabaseViewer::refineConstraint(int from, int to)
ui_->doubleSpinBox_icp_voxel->value(), ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling 0, // no sampling
currentLink.transform() * dataTo.getLocalTransform()); currentLink.transform() * dataTo.getLocalTransform());
}
if(ui_->checkBox_icp_p2plane->isChecked()) if(ui_->checkBox_icp_p2plane->isChecked())
{ {