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

ProximitySpace: extracting all paths inside local radius up to max graph depth, no length limit of the proximity space links. Fixed local scan matching assembling bug when laser local transform is set. DbViewer: we can now refine proximity detection by space (laser scan matching).

Browse Source
This commit is contained in:
matlabbe
2017-10-09 18:45:42 -04:00
parent 007d23308a
commit 400952b327
6 changed files with 337 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
corelib/src/Memory.cpp
View File

@@ -1065,6 +1065,7 @@ std::map<int, int> Memory::getNeighborsId(
bool incrementMarginOnLoop, // default false
bool ignoreLoopIds, // default false
bool ignoreIntermediateNodes, // default false
const std::set<int> & nodesSet,
double * dbAccessTime
) const
{
@@ -1094,7 +1095,7 @@ std::map<int, int> Memory::getNeighborsId(
for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
{
if(ids.find(*jter) == ids.end())
if(ids.find(*jter) == ids.end() && (nodesSet.empty() || nodesSet.find(*jter) != nodesSet.end()))
{
//UDEBUG("Added %d with margin %d", *jter, m);
// Look up in STM/WM if all ids are here, if not... load them from the database
@@ -1184,6 +1185,7 @@ std::map<int, float> Memory::getNeighborsIdRadius(
UASSERT(uContains(optimizedPoses, signatureId));
UASSERT(signatureId > 0);
std::map<int, float> ids;
std::map<int, float> checkedIds;
std::list<int> curentMarginList;
std::set<int> currentMargin;
std::set<int> nextMargin;
@@ -1192,8 +1194,6 @@ std::map<int, float> Memory::getNeighborsIdRadius(
Transform referential = optimizedPoses.at(signatureId);
UASSERT(!referential.isNull());
float radiusSqrd = radius*radius;
std::map<int, float> savedRadius;
savedRadius.insert(std::make_pair(signatureId, 0));
while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
{
curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
@@ -1201,7 +1201,7 @@ std::map<int, float> Memory::getNeighborsIdRadius(
for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
{
if(ids.find(*jter) == ids.end())
if(checkedIds.find(*jter) == checkedIds.end())
{
//UDEBUG("Added %d with margin %d", *jter, m);
// Look up in STM/WM if all ids are here, if not... load them from the database
@@ -1210,7 +1210,13 @@ std::map<int, float> Memory::getNeighborsIdRadius(
const std::map<int, Link> * links = &tmpLinks;
if(s)
{
ids.insert(std::pair<int, float>(*jter, savedRadius.at(*jter)));
const Transform & t = optimizedPoses.at(*jter);
UASSERT(!t.isNull());
float distanceSqrd = referential.getDistanceSquared(t);
if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
{
ids.insert(std::pair<int, float>(*jter,distanceSqrd));
}
links = &s->getLinks();
}
@@ -1222,15 +1228,7 @@ std::map<int, float> Memory::getNeighborsIdRadius(
uContains(optimizedPoses, iter->first) &&
iter->second.type()!=Link::kVirtualClosure)
{
const Transform & t = optimizedPoses.at(iter->first);
UASSERT(!t.isNull());
float distanceSqrd = referential.getDistanceSquared(t);
if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
{
savedRadius.insert(std::make_pair(iter->first, distanceSqrd));
nextMargin.insert(iter->first);
}
nextMargin.insert(iter->first);
}
}
}
@@ -2478,7 +2476,7 @@ Transform Memory::computeIcpTransformMulti(
{
// Create a fake signature with all scans merged in oldId referential
SensorData assembledData;
Transform toPose = poses.at(toId);
Transform toPoseInv = poses.at(toId).inverse();
std::string msg;
int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2502,17 +2500,15 @@ Transform Memory::computeIcpTransformMulti(
if(scan.channels() >= 5)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormal = util3d::laserScanToPointCloudNormal(
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
*assembledToNormalClouds += *cloudNormal;
toPoseInv * iter->second * s->sensorData().laserScanInfo().localTransform());
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
*assembledToClouds += *util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
*assembledToClouds += *cloud;
toPoseInv * iter->second * s->sensorData().laserScanInfo().localTransform());
}
if(scan.cols > maxPoints)
@@ -2545,7 +2541,6 @@ Transform Memory::computeIcpTransformMulti(
is2D?Transform(0,0,fromS->sensorData().laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);
}

100
corelib/src/Rtabmap.cpp
View File

@@ -1504,6 +1504,7 @@ bool Rtabmap::process(
true,
true,
false,
std::set<int>(),
&timeGetNeighborsTimeDb);
ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
//Priority to locations near in time (direct neighbor) then by space (loop closure)
@@ -1558,6 +1559,7 @@ bool Rtabmap::process(
true,
false,
false,
std::set<int>(),
&timeGetNeighborsSpaceDb);
ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
firstPassDone = false;
@@ -1914,7 +1916,7 @@ bool Rtabmap::process(
//
UDEBUG("Proximity detection (local loop closure in SPACE using matching images)");
std::map<int, float> nearestIds;
if(_memory->isIncremental())
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
{
nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
}
@@ -1992,7 +1994,7 @@ bool Rtabmap::process(
else
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)",
"transform is too large!? (%fm > %fm)",
nearestId, transform.getNorm(), _proximityFilteringRadius);
}
}
@@ -2012,13 +2014,6 @@ bool Rtabmap::process(
// 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() &&
@@ -2035,10 +2030,25 @@ bool Rtabmap::process(
//UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
// nearest pose must be close and not linked to current location
if(!signature->hasLink(nearestId) &&
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
if(!signature->hasLink(nearestId))
{
if(_proximityMaxNeighbors < _proximityMaxGraphDepth || _proximityMaxGraphDepth == 0)
{
std::map<int, Transform> filteredPath;
int i=0;
std::map<int, Transform>::iterator nearestIdIter = path.find(nearestId);
for(std::map<int, Transform>::iterator iter=nearestIdIter; iter!=path.end() && i<=_proximityMaxNeighbors; ++iter, ++i)
{
filteredPath.insert(*iter);
}
i=1;
for(std::map<int, Transform>::reverse_iterator iter(nearestIdIter); iter!=path.rend() && i<=_proximityMaxNeighbors; ++iter, ++i)
{
filteredPath.insert(*iter);
}
path = filteredPath;
}
// Assemble scans in the path and do ICP only
if(_proximityRawPosesUsed)
{
@@ -2073,49 +2083,40 @@ bool Rtabmap::process(
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
{
UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
cv::Mat scanMatchingIds;
if(_scanMatchingIdsSavedInLinks)
cv::Mat scanMatchingIds;
if(_scanMatchingIdsSavedInLinks)
{
std::stringstream stream;
stream << "SCANS:";
for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
{
std::stringstream stream;
stream << "SCANS:";
for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
if(iter != path.begin())
{
if(iter != path.begin())
{
stream << ";";
}
stream << uNumber2Str(iter->first);
stream << ";";
}
std::string scansStr = stream.str();
scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
scanMatchingIds = compressData2(scanMatchingIds); // compressed
}
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, (info.covariance*100.0).inv(), scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++proximityDetectionsAddedByICPOnly;
// no local loop closure added visually
if(proximityDetectionsAddedVisually == 0 && _loopClosureHypothesis.first == 0)
{
lastProximitySpaceClosureId = nearestId;
stream << uNumber2Str(iter->first);
}
std::string scansStr = stream.str();
scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
scanMatchingIds = compressData2(scanMatchingIds); // compressed
}
else
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, (info.covariance*100.0).inv(), scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++proximityDetectionsAddedByICPOnly;
// no local loop closure added visually
if(proximityDetectionsAddedVisually == 0 && _loopClosureHypothesis.first == 0)
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _proximityFilteringRadius);
lastProximitySpaceClosureId = nearestId;
}
}
else
@@ -3003,13 +3004,14 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
std::map<int, std::map<int, Transform> > paths;
if(_memory && poses.size() && !target.isNull())
{
std::set<int> nodesSet = uKeysSet(poses);
// Segment poses connected only by neighbor links
while(poses.size())
{
std::map<int, Transform> path;
// select nearest pose and iterate neighbors from there
int nearestId = rtabmap::graph::findNearestNode(poses, target);
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true);
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, nodesSet);
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{

8
corelib/src/util3d_mapping.cpp
View File

@@ -224,7 +224,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
const float * vi = pair.first.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i);
cv::Point3f vt;
if(pair.first.channels() > 2)
if(pair.first.channels() != 2 && pair.first.channels() != 5)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
@@ -260,7 +260,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
const float * vi = pair.second.ptr<float>(0,i);
float * vo = obstacles.ptr<float>(0,i);
cv::Point3f vt;
if(pair.second.channels() > 2)
if(pair.second.channels() != 2 && pair.second.channels() != 5)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
@@ -320,6 +320,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
{
float * ptf = iter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
uFormat("id=%d, map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
@@ -357,6 +359,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
{
float * ptf = jter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
UASSERT_MSG(pt.y>0 && pt.y<map.rows && pt.x>0 && pt.x<map.cols,
uFormat("id=%d: map min=(%f, %f) max=(%f,%f) map=%dx%d pt=(%d,%d)", kter->first, xMin, yMin, xMax, yMax, map.cols, map.rows, pt.x, pt.y).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{