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

Fixed OdomF2M ICP local scan transform bug (was not in 0.11.8)

Browse Source
This commit is contained in:
matlabbe
2016-11-03 17:27:01 -04:00
parent a7971373a6
commit fe19d277ac
2 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/OdometryF2F.cpp
View File

@@ -133,7 +133,7 @@ Transform OdometryF2F::computeTransform(
info->words = newFrame.getWords(); info->words = newFrame.getWords();
info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().cols; info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().cols;
info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), tmpRefFrame.sensorData().laserScanInfo().localTransform()*t); info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), t*tmpRefFrame.sensorData().laserScanInfo().localTransform());
} }
} }
else else

4
corelib/src/OdometryF2M.cpp
View File

@@ -334,7 +334,7 @@ Transform OdometryF2M::computeTransform(
if(lastFrame_->sensorData().laserScanRaw().cols) if(lastFrame_->sensorData().laserScanRaw().cols)
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan); pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), lastFrame_->sensorData().laserScanInfo().localTransform() * newFramePose); pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>); pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
int newPoints; int newPoints;
@@ -534,7 +534,7 @@ Transform OdometryF2M::computeTransform(
frameValid = true; frameValid = true;
if (fixedMapPath_.empty()) if (fixedMapPath_.empty())
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), lastFrame_->sensorData().laserScanInfo().localTransform() * newFramePose); pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>))); scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0)); map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
} }