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

RegistrationICP: fixed local scan transform issue

Browse Source
This commit is contained in:
matlabbe
2016-08-24 12:15:32 -04:00
parent 48f54cf2f2
commit edee909324
6 changed files with 66 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
guilib/src/MainWindow.cpp
View File

@@ -1036,7 +1036,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
cloud = util3d::laserScanToPointCloudNormal(scan, odom.data().laserScanInfo().localTransform()*pose);
cloud = util3d::laserScanToPointCloudNormal(scan, pose*odom.data().laserScanInfo().localTransform());
if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
{
cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
@@ -2700,7 +2700,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
//reconvert the voxelized cloud
scan = util3d::laserScanFromPointCloud(*cloud);
}
_createdScans.insert(std::make_pair(nodeId, scan));
else
{
scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
}
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
}
}
else
@@ -2725,16 +2729,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
{
//reconvert the voxelized cloud
if(scan.channels() == 2)
{
scan = util3d::laserScan2dFromPointCloud(*cloud);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
scan = util3d::laserScanFromPointCloud(*cloud);
}
_createdScans.insert(std::make_pair(nodeId, scan));
else
{
scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
}
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
}
}
_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));