i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed Memory::computeScanMatchingTransform() (now named computeIcpTransformMulti())

Browse Source
This commit is contained in:
matlabbe
2015-11-26 16:03:03 -05:00
parent 991a603648
commit 148504dabd
4 changed files with 48 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/RegistrationIcp.cpp
+25 -25
View File
@@ -156,7 +156,7 @@ Transform RegistrationIcp::computeTransformation(
int correspondences = 0;
double variance = 1.0;
bool correspondencesComputed = false;
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(!_icp2D) // 3D ICP
{
if(_pointToPlane)
@@ -170,24 +170,24 @@ Transform RegistrationIcp::computeTransformation(
if(toCloudNormals->size() && fromCloudNormals->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
icpT = util3d::icpPointToPlane(
toCloudNormals,
fromCloudNormals,
toCloudNormals,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*newCloudNormalsRegistered);
*fromCloudNormalsRegistered);
if(!filtered &&
!icpT.isNull() &&
hasConverged)
{
util3d::computeVarianceAndCorrespondences(
newCloudNormalsRegistered,
fromCloudNormals,
_maxCorrespondenceDistance,
variance,
correspondences);
fromCloudNormals,
fromCloudNormalsRegistered,
_maxCorrespondenceDistance,
variance,
correspondences);
correspondencesComputed = true;
}
}
@@ -195,34 +195,34 @@ Transform RegistrationIcp::computeTransformation(
else
{
icpT = util3d::icp(
toCloudFiltered,
fromCloudFiltered,
toCloudFiltered,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*newCloudRegistered);
*fromCloudRegistered);
}
}
else // 2D ICP
{
icpT = util3d::icp2D(
toCloudFiltered,
fromCloudFiltered,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*newCloudRegistered);
fromCloudFiltered,
toCloudFiltered,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*fromCloudRegistered);
}
/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
pcl::io::savePCDFile("toCloud.pcd", *toCloud);
UWARN("saved fromCloud.pcd and toCloud.pcd");
if(!icpT.isNull())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudTmp = util3d::transformPointCloud(toCloud, icpT);
pcl::io::savePCDFile("newCloudFinal.pcd", *toCloudTmp);
UWARN("saved toCloudFinal.pcd");
}*/
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloud, icpT);
pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
UWARN("saved fromCloudFinal.pcd");
}
if(!icpT.isNull() &&
hasConverged)
@@ -256,12 +256,12 @@ Transform RegistrationIcp::computeTransformation(
}
else
{
fromCloud = newCloudRegistered;
fromCloud = fromCloudRegistered;
}
util3d::computeVarianceAndCorrespondences(
toCloud,
fromCloud,
toCloud,
_maxCorrespondenceDistance,
variance,
correspondences);
@@ -313,7 +313,7 @@ Transform RegistrationIcp::computeTransformation(
}
else
{
transform = guess*icpT;
transform = icpT.inverse()*guess;
}
}
}