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

Tango: Added ICP refining option, mesh vs texture vs point cloud options, show/hide grid option, drift correction option

Browse Source
This commit is contained in:
matlabbe
2016-09-02 22:37:12 -04:00
parent 4bd9e4f847
commit e5074afdeb
17 changed files with 350 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -93,8 +93,7 @@ public:
void emptyTrash();
void joinTrashThread();
bool addLink(const Link & link, bool addInDatabase = false);
void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
void updateLink(const Link & link, bool updateInDatabase = false);
void removeAllVirtualLinks();
void removeVirtualLinks(int signatureId);
std::map<int, int> getNeighborsId(

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -128,6 +128,7 @@ public:
bool global,
std::map<int, Signature> * signatures = 0);
int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1);
int refineLinks();
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success

4
corelib/include/rtabmap/core/util3d.h
View File

@@ -141,7 +141,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & parameters = ParametersMap(),
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
/**
@@ -164,7 +164,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & parameters = ParametersMap(),
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(

89
corelib/src/Memory.cpp
View File

@@ -2422,54 +2422,63 @@ bool Memory::addLink(const Link & link, bool addInDatabase)
return true;
}
void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)
void Memory::updateLink(const Link & link, bool updateInDatabase)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
Signature * fromS = this->_getSignature(link.from());
Signature * toS = this->_getSignature(link.to());
if(fromS->hasLink(toId) && toS->hasLink(fromId))
if(fromS && toS)
{
Link::Type type = fromS->getLinks().at(toId).type();
fromS->removeLink(toId);
toS->removeLink(fromId);
fromS->addLink(Link(fromId, toId, type, transform, rotVariance, transVariance));
toS->addLink(Link(toId, fromId, type, transform.inverse(), rotVariance, transVariance));
if(type!=Link::kVirtualClosure)
if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
{
_linksChanged = true;
Link::Type oldType = fromS->getLinks().at(link.to()).type();
fromS->removeLink(link.to());
toS->removeLink(link.from());
fromS->addLink(link);
toS->addLink(link.inverse());
if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
{
_linksChanged = true;
}
}
else
{
UERROR("fromId=%d and toId=%d are not linked!", link.from(), link.to());
}
}
else if(!updateInDatabase)
{
if(!fromS)
{
UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
}
if(!toS)
{
UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
}
}
else if(fromS)
{
UDEBUG("Update link between %d and %d (db)", link.from(), link.to());
fromS->removeLink(link.to());
fromS->addLink(link);
_dbDriver->updateLink(link.inverse());
}
else if(toS)
{
UDEBUG("Update link between %d (db) and %d", link.from(), link.to());
toS->removeLink(link.from());
toS->addLink(link.inverse());
_dbDriver->updateLink(link);
}
else
{
UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
}
}
void Memory::updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
if(fromS->hasLink(toId) && toS->hasLink(fromId))
{
Link::Type type = fromS->getLinks().at(toId).type();
fromS->removeLink(toId);
toS->removeLink(fromId);
cv::Mat infMatrix = covariance.inv();
fromS->addLink(Link(fromId, toId, type, transform, infMatrix));
toS->addLink(Link(toId, fromId, type, transform.inverse(), infMatrix));
if(type!=Link::kVirtualClosure)
{
_linksChanged = true;
}
}
else
{
UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
UDEBUG("Update link between %d (db) and %d (db)", link.from(), link.to());
_dbDriver->updateLink(link);
_dbDriver->updateLink(link.inverse());
}
}

64
corelib/src/Rtabmap.cpp
View File

@@ -1012,7 +1012,7 @@ bool Rtabmap::process(
{
// set small variance
UDEBUG("Set small variance. The robot is not moving.");
_memory->updateLink(oldId, signature->id(), guess, 0.0001, 0.0001);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
}
}
else
@@ -1034,7 +1034,7 @@ bool Rtabmap::process(
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.variance, info.variance));
if(_optimizeFromGraphEnd)
{
@@ -1058,7 +1058,7 @@ bool Rtabmap::process(
if(info.variance > 0)
{
double sqrtVar = sqrt(info.variance);
_memory->updateLink(oldId, signature->id(), guess, sqrtVar, sqrtVar);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrtVar, sqrtVar));
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1821,7 +1821,7 @@ bool Rtabmap::process(
UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
// segment poses by paths, only one detection per path
std::map<int, std::map<int, Transform> > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxNeighbors);
std::map<int, std::map<int, Transform> > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth);
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
@@ -1886,13 +1886,21 @@ bool Rtabmap::process(
// 2) compare locally with nearest locations by scan matching
//
UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
if( !signature->sensorData().laserScanCompressed().empty() &&
if( _proximityMaxNeighbors > 0 &&
!signature->sensorData().laserScanCompressed().empty() &&
(_memory->isIncremental() || lastProximitySpaceClosureId == 0))
{
// In localization mode, no need to check local loop
// closures if we are already localized by at least one
// local visual closure above.
// Parse again with if different (normally, maxNeighbors would be smaller than MaxGraphDepth)
if(_proximityMaxNeighbors != _proximityMaxGraphDepth)
{
nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxNeighbors);
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
}
proximitySpacePaths = (int)nearestPaths.size();
for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() &&
@@ -3412,6 +3420,52 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
return (int)loopClosuresAdded.size();
}
int Rtabmap::refineLinks()
{
if(!_rgbdSlamMode)
{
UERROR("Refining links can be done only in RGBD-SLAM mode.");
return -1;
}
std::list<Link> linksRefined;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures;
this->getGraph(poses, links, false, true, &signatures);
int i=0;
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!= links.end(); ++iter)
{
int from = iter->second.from();
int to = iter->second.to();
UASSERT(signatures.find(from) != signatures.end());
UASSERT(signatures.find(to) != signatures.end());
RegistrationInfo info;
// use signatures instead of IDs because some signatures may not be in WM
Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), iter->second.transform(), &info);
if(!t.isNull())
{
linksRefined.push_back(Link(from, to, iter->second.type(), t, info.variance, info.variance));
UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
}
}
UINFO("Total refined %d links.", (int)linksRefined.size());
if(linksRefined.size())
{
for(std::list<Link>::iterator iter=linksRefined.begin(); iter!=linksRefined.end(); ++iter)
{
_memory->updateLink(*iter, true);
}
}
return (int)linksRefined.size();
}
void Rtabmap::clearPath(int status)
{
UINFO("status=%d", status);

8
corelib/src/util3d.cpp
View File

@@ -699,7 +699,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & parameters,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -815,7 +815,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
}
cloud = cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), stereoParameters),
sensorData.stereoCameraModel(),
decimation,
maxDepth,
@@ -839,7 +839,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & parameters,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
UASSERT(!sensorData.imageRaw().empty());
@@ -961,7 +961,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
maxDepth,
minDepth,
validIndices,
parameters);
stereoParameters);
if(cloud->size())
{