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

RegIcp: complexity, checking if second eigen value is also under Icp/PointToPlaneMinComplexity to limit to only one axis. DbViewer: fixed refine link with scans having RGB channel.

Browse Source
This commit is contained in:
matlabbe
2020-11-22 19:32:01 -05:00
parent bce7ae6acd
commit 80199f23b5
2 changed files with 37 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
guilib/src/DatabaseViewer.cpp
View File

@@ -7007,7 +7007,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
}
}
}
}
}
if(scanPoses.size()>1)
{
@@ -7065,6 +7065,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
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>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
@@ -7090,6 +7092,19 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
{
*assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
toPoseInv * iter->second * scan.localTransform());
}
}
else if(scan.hasRGB())
{
if(scan.hasNormals())
{
*assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
toPoseInv * iter->second * scan.localTransform());
}
else
{
*assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
toPoseInv * iter->second * scan.localTransform());
}
}
else
@@ -7138,6 +7153,16 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
else if(assembledToIClouds->size())
{
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
}
else if(assembledToNormalRGBClouds->size())
{
UASSERT(!fromScan.is2d());
assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
}
else if(assembledToRGBClouds->size())
{
UASSERT(!fromScan.is2d());
assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
}
else
@@ -7787,16 +7812,7 @@ void DatabaseViewer::resetConstraint()
{
linksRefined_.erase(iter);
this->updateGraphView();
}
iter = rtabmap::graph::findLink(links_, from, to);
if(iter != links_.end())
{
this->updateConstraintView(iter->second);
}
iter = rtabmap::graph::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
}
updateConstraintView();
}