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 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 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 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 newId,
int oldId, int oldId,
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,

42
corelib/src/Memory.cpp
View File

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

50
corelib/src/RegistrationIcp.cpp
View File

@@ -156,7 +156,7 @@ Transform RegistrationIcp::computeTransformation(
int correspondences = 0; int correspondences = 0;
double variance = 1.0; double variance = 1.0;
bool correspondencesComputed = false; 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(!_icp2D) // 3D ICP
{ {
if(_pointToPlane) if(_pointToPlane)
@@ -170,24 +170,24 @@ Transform RegistrationIcp::computeTransformation(
if(toCloudNormals->size() && fromCloudNormals->size()) 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( icpT = util3d::icpPointToPlane(
toCloudNormals,
fromCloudNormals, fromCloudNormals,
toCloudNormals,
_maxCorrespondenceDistance, _maxCorrespondenceDistance,
_maxIterations, _maxIterations,
hasConverged, hasConverged,
*newCloudNormalsRegistered); *fromCloudNormalsRegistered);
if(!filtered && if(!filtered &&
!icpT.isNull() && !icpT.isNull() &&
hasConverged) hasConverged)
{ {
util3d::computeVarianceAndCorrespondences( util3d::computeVarianceAndCorrespondences(
newCloudNormalsRegistered, fromCloudNormals,
fromCloudNormals, fromCloudNormalsRegistered,
_maxCorrespondenceDistance, _maxCorrespondenceDistance,
variance, variance,
correspondences); correspondences);
correspondencesComputed = true; correspondencesComputed = true;
} }
} }
@@ -195,34 +195,34 @@ Transform RegistrationIcp::computeTransformation(
else else
{ {
icpT = util3d::icp( icpT = util3d::icp(
toCloudFiltered,
fromCloudFiltered, fromCloudFiltered,
toCloudFiltered,
_maxCorrespondenceDistance, _maxCorrespondenceDistance,
_maxIterations, _maxIterations,
hasConverged, hasConverged,
*newCloudRegistered); *fromCloudRegistered);
} }
} }
else // 2D ICP else // 2D ICP
{ {
icpT = util3d::icp2D( icpT = util3d::icp2D(
toCloudFiltered, fromCloudFiltered,
fromCloudFiltered, toCloudFiltered,
_maxCorrespondenceDistance, _maxCorrespondenceDistance,
_maxIterations, _maxIterations,
hasConverged, hasConverged,
*newCloudRegistered); *fromCloudRegistered);
} }
/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud); pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
pcl::io::savePCDFile("toCloud.pcd", *toCloud); pcl::io::savePCDFile("toCloud.pcd", *toCloud);
UWARN("saved fromCloud.pcd and toCloud.pcd"); UWARN("saved fromCloud.pcd and toCloud.pcd");
if(!icpT.isNull()) if(!icpT.isNull())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudTmp = util3d::transformPointCloud(toCloud, icpT); pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloud, icpT);
pcl::io::savePCDFile("newCloudFinal.pcd", *toCloudTmp); pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
UWARN("saved toCloudFinal.pcd"); UWARN("saved fromCloudFinal.pcd");
}*/ }
if(!icpT.isNull() && if(!icpT.isNull() &&
hasConverged) hasConverged)
@@ -256,12 +256,12 @@ Transform RegistrationIcp::computeTransformation(
} }
else else
{ {
fromCloud = newCloudRegistered; fromCloud = fromCloudRegistered;
} }
util3d::computeVarianceAndCorrespondences( util3d::computeVarianceAndCorrespondences(
toCloud,
fromCloud, fromCloud,
toCloud,
_maxCorrespondenceDistance, _maxCorrespondenceDistance,
variance, variance,
correspondences); correspondences);
@@ -313,7 +313,7 @@ Transform RegistrationIcp::computeTransformation(
} }
else 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()) if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{ {
float variance = 1.0f; 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(!transform.isNull())
{ {
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius) if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)