i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

6
guilib/src/DatabaseViewer.cpp
View File

@@ -1705,7 +1705,7 @@ void DatabaseViewer::view3DLaserScans()
{
scan = util3d::downsample(scan, downsamplingStepSize);
}
cloud = util3d::laserScanToPointCloud(scan);
cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
if(cloud->size())
{
@@ -1947,7 +1947,7 @@ void DatabaseViewer::generate3DLaserScans()
{
scan = util3d::downsample(scan, downsamplingStepSize);
}
cloud = util3d::laserScanToPointCloud(scan);
cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
if(cloud->size())
{
@@ -3280,7 +3280,7 @@ void DatabaseViewer::updateConstraintView(
data.uncompressDataConst(0, 0, &scan, 0);
if(!scan.empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
if(assembledScans->size() == 0)
{
assembledScans = util3d::transformPointCloud(scanCloud, iter->second);

1
guilib/src/ExportScansDialog.cpp
View File

@@ -402,6 +402,7 @@ std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> ExportScansDialog::getScan
scan = util3d::downsample(scan, _ui->spinBox_decimation->value());
}
}
scan = util3d::transformLaserScan(scan, s.sensorData().laserScanInfo().localTransform());
}
else
{

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));