i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -181,7 +181,7 @@ public:
Transform computeVisualTransform(int fromId, int toId, std::string * rejectedMsg = 0, int * inliers = 0, float * variance = 0);
Transform computeIcpTransform(int fromId, int toId, Transform guess, std::string * rejectedMsg = 0, int * correspondences = 0, float * variance = 0, float * correspondencesRatio = 0);
Transform computeScanMatchingTransform(
Transform computeIcpTransformMulti(
int newId,
int oldId,
const std::map<int, Transform> & poses,

42
corelib/src/Memory.cpp
View File

@@ -2160,19 +2160,19 @@ Transform Memory::computeIcpTransform(
return t;
}
// poses of newId and oldId must be in "poses"
Transform Memory::computeScanMatchingTransform(
int newId,
int oldId,
// compute transform fromId -> multiple toId
Transform Memory::computeIcpTransformMulti(
int fromId,
int toId,
const std::map<int, Transform> & poses,
std::string * rejectedMsg,
int * inliers,
float * variance)
{
UASSERT(uContains(poses, newId) && uContains(_signatures, newId));
UASSERT(uContains(poses, oldId) && uContains(_signatures, oldId));
UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
UDEBUG("Guess=%s", (poses.at(newId).inverse() * poses.at(oldId)).prettyPrint().c_str());
UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
// make sure that all laser scans are loaded
std::list<Signature*> depthToLoad;
@@ -2190,29 +2190,29 @@ Transform Memory::computeScanMatchingTransform(
_dbDriver->loadNodeData(depthToLoad);
}
Signature * newS = _getSignature(newId);
cv::Mat newScan;
newS->sensorData().uncompressData(0, 0, &newScan);
Signature * fromS = _getSignature(fromId);
cv::Mat fromScan;
fromS->sensorData().uncompressData(0, 0, &fromScan);
Transform t;
if(!newScan.empty())
if(!fromScan.empty())
{
// Create a fake signature with all scans merged in oldId referential
SensorData assembledData;
Transform oldPose = poses.at(oldId);
Transform toPose = poses.at(toId);
std::string msg;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledOldClouds(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first != newId)
if(iter->first != fromId)
{
Signature * s = this->_getSignature(iter->first);
if(!s->sensorData().laserScanCompressed().empty())
{
cv::Mat scan;
s->sensorData().uncompressData(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, oldPose.inverse() * iter->second);
*assembledOldClouds += *cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, toPose.inverse() * iter->second);
*assembledToClouds += *cloud;
}
else
{
@@ -2220,14 +2220,14 @@ Transform Memory::computeScanMatchingTransform(
}
}
}
if(assembledOldClouds->size())
if(assembledToClouds->size())
{
assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledOldClouds, Transform()), 0, 0);
assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts(), fromS->sensorData().laserScanMaxRange());
}
Transform guess = poses.at(newId).inverse() * poses.at(oldId);
Signature oldS(0, 0, 0, 0, "", oldPose, assembledData);
t = _registrationIcp->computeTransformation(*newS, oldS, guess, rejectedMsg, inliers, variance);
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
Signature toS(0, 0, 0, 0, "", toPose, assembledData);
t = _registrationIcp->computeTransformation(*fromS, toS, guess, rejectedMsg, inliers, variance);
}
return t;

50
corelib/src/RegistrationIcp.cpp
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;
}
}
}

2
corelib/src/Rtabmap.cpp
View File

@@ -1916,7 +1916,7 @@ bool Rtabmap::process(
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
float variance = 1.0f;
Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, 0, 0, &variance);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)