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

Fixed bug with proximity detection (with combined scans) giving wrong transform sometimes if RGBD/ProximityPathFilteringRadius was used (default true). Fixed util3d::computeNormalsComplexity() when scan has intensity.

Browse Source
This commit is contained in:
matlabbe
2019-02-07 11:19:55 -05:00
parent 6fc884b575
commit 1752b55678
6 changed files with 131 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
guilib/src/DatabaseViewer.cpp
View File

@@ -195,6 +195,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopCovLimited()));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDProximityPathFilteringRadius()));
ui_->parameters_toolbox->setupUi(parameters);
exportDialog_->setObjectName("ExportCloudsDialog");
restoreDefaultSettings();
@@ -6191,6 +6193,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
int maxPoints = fromScan.size();
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
{
if(iter->first != currentLink.from())
@@ -6203,13 +6207,31 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
data.uncompressData(0, 0, &scan);
if(!scan.isEmpty() && fromScan.format() == scan.format())
{
if(scan.hasNormals())
if(scan.hasIntensity())
{
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan, toPoseInv * iter->second * scan.localTransform());
if(scan.hasNormals())
{
*assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
toPoseInv * iter->second * scan.localTransform());
}
else
{
*assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
toPoseInv * iter->second * scan.localTransform());
}
}
else
{
*assembledToClouds += *util3d::laserScanToPointCloud(scan, toPoseInv * iter->second * scan.localTransform());
if(scan.hasNormals())
{
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
toPoseInv * iter->second * scan.localTransform());
}
else
{
*assembledToClouds += *util3d::laserScanToPointCloud(scan,
toPoseInv * iter->second * scan.localTransform());
}
}
if(scan.size() > maxPoints)
@@ -6234,6 +6256,14 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
{
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
}
else if(assembledToNormalIClouds->size())
{
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
}
else if(assembledToIClouds->size())
{
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
}
SensorData assembledData;
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScanRaw(LaserScan(