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

modified how correspondences ratio is computed (icp 2D and 3D), also fixed build with OpenCV3+Cuda

Browse Source
This commit is contained in:
Mathieu Labbé
2015-07-11 12:42:46 -04:00
parent bf295c4274
commit 2d7be6be48
12 changed files with 388 additions and 339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
guilib/src/DatabaseViewer.cpp
View File

@@ -2764,8 +2764,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanAVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanBVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
float correspondenceRatio = 0.0f;
if(ui_->checkBox_icp_2d->isChecked())
{
@@ -2776,6 +2776,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
if(!oldLaserScan.empty() && !newLaserScan.empty())
{
// 2D
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
scanA = util3d::cvMat2Cloud(oldLaserScan);
scanB = util3d::cvMat2Cloud(newLaserScan, t);
@@ -2785,21 +2787,40 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
scanA = util3d::voxelize(scanA, ui_->doubleSpinBox_icp_voxel->value());
scanB = util3d::voxelize(scanB, ui_->doubleSpinBox_icp_voxel->value());
}
else
{
scanAVoxelized = scanA;
scanBVoxelized = scanB;
}
if(scanB->size() && scanA->size())
{
transform = util3d::icp2D(scanB,
pcl::PointCloud<pcl::PointXYZ>::Ptr scanBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
transform = util3d::icp2D(
scanB,
scanA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
&variance,
&correspondences);
hasConverged,
*scanBRegistered);
if(!transform.isNull())
{
if(dataTo.laserScanMaxPts())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanBTransformed = scanBRegistered;
if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
{
scanBTransformed = util3d::transformPointCloud(scanB, transform);
}
util3d::computeVarianceAndCorrespondences(
scanBTransformed,
scanA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
variance,
correspondences);
correspondenceRatio = float(correspondences)/float(dataTo.laserScanMaxPts());
}
else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
@@ -2844,25 +2865,38 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
UWARN("removed nan normals...");
}
transform = util3d::icpPointToPlane(cloudBNormals,
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
transform = util3d::icpPointToPlane(
cloudBNormals,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
&variance,
&correspondences);
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
variance,
correspondences);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
transform = util3d::icp(cloudB,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
&variance,
&correspondences);
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
variance,
correspondences);
}
correspondenceRatio = float(correspondences)/float(dataFrom.imageRaw().total());
correspondenceRatio = float(correspondences)/float(cloudA->size()>cloudB->size()?cloudA->size():cloudB->size());
}
else
{
@@ -2913,8 +2947,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
if(ui_->dockWidget_constraints->isVisible())
{
cloudB = util3d::transformPointCloud(cloudB, transform);
scanB = util3d::transformPointCloud(scanB, transform);
this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB);
scanBVoxelized = util3d::transformPointCloud(scanBVoxelized, transform);
this->updateConstraintView(newLink, true, cloudA, cloudB, scanAVoxelized, scanBVoxelized);
}
}
}

48
guilib/src/MainWindow.cpp
View File

@@ -3505,22 +3505,22 @@ void MainWindow::postProcessing()
}
_initProgressDialog->appendText(tr("Refining links..."));
int decimation=8;
float maxDepth=2.0f;
float voxelSize=0.01f;
int samples = 0;
float maxCorrespondences = 0.05f;
float correspondenceRatio = 0.7f;
float icpIterations = 30;
int decimation=Parameters::defaultLccIcp3Decimation();
float maxDepth=Parameters::defaultLccIcp3MaxDepth();
float voxelSize=Parameters::defaultLccIcp3VoxelSize();
int samples = Parameters::defaultLccIcp3Samples();
float maxCorrespondenceDistance = Parameters::defaultLccIcp3MaxCorrespondenceDistance();
float correspondenceRatio = Parameters::defaultLccIcp3CorrespondenceRatio();
float icpIterations = Parameters::defaultLccIcp3Iterations();
Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation);
Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth);
Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize);
Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples);
Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio);
Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences);
Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
bool pointToPlane = false;
int pointToPlaneNormalNeighbors = 20;
bool pointToPlane = Parameters::defaultLccIcp3PointToPlane();
int pointToPlaneNormalNeighbors = Parameters::defaultLccIcp3PointToPlaneNormalNeighbors();
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane);
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors);
@@ -3605,24 +3605,36 @@ void MainWindow::postProcessing()
UWARN("removed nan normals...");
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
maxCorrespondences,
maxCorrespondenceDistance,
icpIterations,
&hasConverged,
&variance,
&correspondences);
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudANormals,
maxCorrespondenceDistance,
variance,
correspondences);
}
else
{
UDEBUG("");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
transform = util3d::icp(cloudB,
cloudA,
maxCorrespondences,
maxCorrespondenceDistance,
icpIterations,
&hasConverged,
&variance,
&correspondences);
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudA,
maxCorrespondenceDistance,
variance,
correspondences);
}
float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());