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

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

83
guilib/src/DatabaseViewer.cpp
View File

@@ -1556,27 +1556,72 @@ void DatabaseViewer::refineConstraint(int from, int to)
cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.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!"));
UERROR("ICP 3D cannot be done on stereo images!");
return;
cv::Mat leftMono;
cv::Mat left = rtabmap::util3d::uncompressImage(dataFrom.getImageCompressed());
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,
dataFrom.getDepthFx(), dataFrom.getDepthFy(), dataFrom.getDepthCx(), dataFrom.getDepthCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
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,
dataTo.getDepthFx(), dataTo.getDepthFy(), dataTo.getDepthCx(), dataTo.getDepthCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
currentLink.transform() * dataTo.getLocalTransform());
}
cloudA = util3d::getICPReadyCloud(depthA,
dataFrom.getDepthFx(), dataFrom.getDepthFy(), dataFrom.getDepthCx(), dataFrom.getDepthCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
dataFrom.getLocalTransform());
cloudB = util3d::getICPReadyCloud(depthB,
dataTo.getDepthFx(), dataTo.getDepthFy(), dataTo.getDepthCx(), dataTo.getDepthCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
currentLink.transform() * dataTo.getLocalTransform());
if(ui_->checkBox_icp_p2plane->isChecked())
{