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

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

@@ -127,6 +127,7 @@ public:
bool incrementMarginOnLoop = false, bool incrementMarginOnLoop = false,
bool ignoreLoopIds = false, bool ignoreLoopIds = false,
bool ignoreIntermediateNodes = false, bool ignoreIntermediateNodes = false,
const std::set<int> & nodesSet = std::set<int>(),
double * dbAccessTime = 0) const; double * dbAccessTime = 0) const;
std::map<int, float> getNeighborsIdRadius( std::map<int, float> getNeighborsIdRadius(
int signatureId, int signatureId,

37
corelib/src/Memory.cpp
View File

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

46
corelib/src/Rtabmap.cpp
View File

@@ -1504,6 +1504,7 @@ bool Rtabmap::process(
true, true,
true, true,
false, false,
std::set<int>(),
&timeGetNeighborsTimeDb); &timeGetNeighborsTimeDb);
ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size()); 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) //Priority to locations near in time (direct neighbor) then by space (loop closure)
@@ -1558,6 +1559,7 @@ bool Rtabmap::process(
true, true,
false, false,
false, false,
std::set<int>(),
&timeGetNeighborsSpaceDb); &timeGetNeighborsSpaceDb);
ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size()); ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
firstPassDone = false; firstPassDone = false;
@@ -1914,7 +1916,7 @@ bool Rtabmap::process(
// //
UDEBUG("Proximity detection (local loop closure in SPACE using matching images)"); UDEBUG("Proximity detection (local loop closure in SPACE using matching images)");
std::map<int, float> nearestIds; std::map<int, float> nearestIds;
if(_memory->isIncremental()) if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
{ {
nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth); nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
} }
@@ -1992,7 +1994,7 @@ bool Rtabmap::process(
else else
{ {
UWARN("Ignoring local loop closure with %d because resulting " 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); nearestId, transform.getNorm(), _proximityFilteringRadius);
} }
} }
@@ -2012,13 +2014,6 @@ bool Rtabmap::process(
// closures if we are already localized by at least one // closures if we are already localized by at least one
// local visual closure above. // 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(); proximitySpacePaths = (int)nearestPaths.size();
for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin(); for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
iter!=nearestPaths.rend() && 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))); //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 // nearest pose must be close and not linked to current location
if(!signature->hasLink(nearestId) && if(!signature->hasLink(nearestId))
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{ {
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 // Assemble scans in the path and do ICP only
if(_proximityRawPosesUsed) if(_proximityRawPosesUsed)
{ {
@@ -2072,8 +2082,6 @@ bool Rtabmap::process(
RegistrationInfo info; RegistrationInfo info;
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info); Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
if(!transform.isNull()) if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
{ {
UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s", UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
signature->id(), signature->id(),
@@ -2112,13 +2120,6 @@ bool Rtabmap::process(
} }
} }
else else
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _proximityFilteringRadius);
}
}
else
{ {
UWARN("Local scan matching rejected: %s", info.rejectedMsg.c_str()); UWARN("Local scan matching rejected: %s", info.rejectedMsg.c_str());
} }
@@ -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; std::map<int, std::map<int, Transform> > paths;
if(_memory && poses.size() && !target.isNull()) if(_memory && poses.size() && !target.isNull())
{ {
std::set<int> nodesSet = uKeysSet(poses);
// Segment poses connected only by neighbor links // Segment poses connected only by neighbor links
while(poses.size()) while(poses.size())
{ {
std::map<int, Transform> path; std::map<int, Transform> path;
// select nearest pose and iterate neighbors from there // select nearest pose and iterate neighbors from there
int nearestId = rtabmap::graph::findNearestNode(poses, target); 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) 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); const float * vi = pair.first.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i); float * vo = ground.ptr<float>(0,i);
cv::Point3f vt; 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); 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); const float * vi = pair.second.ptr<float>(0,i);
float * vo = obstacles.ptr<float>(0,i); float * vo = obstacles.ptr<float>(0,i);
cv::Point3f vt; 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); 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); float * ptf = iter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize); 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); char & value = map.at<char>(pt.y, pt.x);
if(value != -2) if(value != -2)
{ {
@@ -357,6 +359,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
{ {
float * ptf = jter->second.ptr<float>(0, i); float * ptf = jter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize); 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); char & value = map.at<char>(pt.y, pt.x);
if(value != -2) if(value != -2)
{ {

2
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -185,7 +185,7 @@ private:
std::string databaseFileName_; std::string databaseFileName_;
std::list<std::map<int, rtabmap::Transform> > graphes_; std::list<std::map<int, rtabmap::Transform> > graphes_;
std::multimap<int, rtabmap::Link> graphLinks_; std::multimap<int, rtabmap::Link> graphLinks_;
std::map<int, rtabmap::Transform> poses_; std::map<int, rtabmap::Transform> odomPoses_;
std::map<int, rtabmap::Transform> groundTruthPoses_; std::map<int, rtabmap::Transform> groundTruthPoses_;
std::map<int, rtabmap::Transform> gpsPoses_; std::map<int, rtabmap::Transform> gpsPoses_;
std::map<int, GPS> gpsValues_; std::map<int, GPS> gpsValues_;

264
guilib/src/DatabaseViewer.cpp
View File

@@ -702,7 +702,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
loopLinks_.clear(); loopLinks_.clear();
graphes_.clear(); graphes_.clear();
graphLinks_.clear(); graphLinks_.clear();
poses_.clear(); odomPoses_.clear();
groundTruthPoses_.clear(); groundTruthPoses_.clear();
gpsPoses_.clear(); gpsPoses_.clear();
gpsValues_.clear(); gpsValues_.clear();
@@ -1314,7 +1314,7 @@ void DatabaseViewer::updateIds()
ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end())); ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
idToIndex_.clear(); idToIndex_.clear();
mapIds_.clear(); mapIds_.clear();
poses_.clear(); odomPoses_.clear();
groundTruthPoses_.clear(); groundTruthPoses_.clear();
gpsPoses_.clear(); gpsPoses_.clear();
gpsValues_.clear(); gpsValues_.clear();
@@ -1413,7 +1413,7 @@ void DatabaseViewer::updateIds()
} }
if(addPose) if(addPose)
{ {
poses_.insert(std::make_pair(ids_[i], p)); odomPoses_.insert(std::make_pair(ids_[i], p));
if(!g.isNull()) if(!g.isNull())
{ {
groundTruthPoses_.insert(std::make_pair(ids_[i], g)); groundTruthPoses_.insert(std::make_pair(ids_[i], g));
@@ -1463,7 +1463,7 @@ void DatabaseViewer::updateIds()
ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty()); ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty());
} }
UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)poses_.size(), (int)links_.size()); UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
if(ids_.size() && ui_->toolBox_statistics->isVisible()) if(ids_.size() && ui_->toolBox_statistics->isVisible())
{ {
@@ -1486,7 +1486,7 @@ void DatabaseViewer::updateIds()
ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(totalTime*1000).toUTC().toString("hh:mm:ss.zzz"))); ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(totalTime*1000).toUTC().toString("hh:mm:ss.zzz")));
ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words").arg(ids.size()).arg(dbDriver_->getTotalDictionarySize())); ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words").arg(ids.size()).arg(dbDriver_->getTotalDictionarySize()));
ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize())); ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(poses_.size()).arg(links_.size())); ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(odomPoses_.size()).arg(links_.size()));
ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size())); ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size())); ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
ui_->textEdit_info->append(""); ui_->textEdit_info->append("");
@@ -1551,10 +1551,10 @@ void DatabaseViewer::updateIds()
if(ids.size()) if(ids.size())
{ {
if(poses_.size()) if(odomPoses_.size())
{ {
bool nullPoses = poses_.begin()->second.isNull(); bool nullPoses = odomPoses_.begin()->second.isNull();
for(std::map<int,Transform>::iterator iter=poses_.begin(); iter!=poses_.end(); ++iter) for(std::map<int,Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
{ {
if((!iter->second.isNull() && nullPoses) || if((!iter->second.isNull() && nullPoses) ||
(iter->second.isNull() && !nullPoses)) (iter->second.isNull() && !nullPoses))
@@ -1564,22 +1564,22 @@ void DatabaseViewer::updateIds()
UWARN("Pose %d is null!", iter->first); UWARN("Pose %d is null!", iter->first);
} }
UWARN("Mixed valid and null poses! Ignoring graph..."); UWARN("Mixed valid and null poses! Ignoring graph...");
poses_.clear(); odomPoses_.clear();
links_.clear(); links_.clear();
break; break;
} }
} }
if(nullPoses) if(nullPoses)
{ {
poses_.clear(); odomPoses_.clear();
links_.clear(); links_.clear();
} }
if(poses_.size()) if(odomPoses_.size())
{ {
ui_->spinBox_optimizationsFrom->setRange(poses_.begin()->first, poses_.rbegin()->first); ui_->spinBox_optimizationsFrom->setRange(odomPoses_.begin()->first, odomPoses_.rbegin()->first);
ui_->spinBox_optimizationsFrom->setValue(poses_.begin()->first); ui_->spinBox_optimizationsFrom->setValue(odomPoses_.begin()->first);
ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(poses_.begin()->first).arg(poses_.rbegin()->first)); ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(odomPoses_.begin()->first).arg(odomPoses_.rbegin()->first));
} }
} }
} }
@@ -3584,8 +3584,8 @@ void DatabaseViewer::updateConstraintView(
if(link.type() == Link::kNeighbor || if(link.type() == Link::kNeighbor ||
link.type() == Link::kNeighborMerged) link.type() == Link::kNeighborMerged)
{ {
Transform poseFrom = uValue(poses_, link.from(), Transform()); Transform poseFrom = uValue(odomPoses_, link.from(), Transform());
Transform poseTo = uValue(poses_, link.to(), Transform()); Transform poseTo = uValue(odomPoses_, link.to(), Transform());
if(!poseFrom.isNull() && !poseTo.isNull()) if(!poseFrom.isNull() && !poseTo.isNull())
{ {
// recompute raw odom transformation and // recompute raw odom transformation and
@@ -3863,10 +3863,10 @@ void DatabaseViewer::updateConstraintView(
} }
constraintsViewer_->removeCloud("scan2"); constraintsViewer_->removeCloud("scan2");
constraintsViewer_->removeCloud("scan2normals");
constraintsViewer_->removeGraph("scan2graph"); constraintsViewer_->removeGraph("scan2graph");
constraintsViewer_->removeCloud("scan0"); constraintsViewer_->removeCloud("scan0");
constraintsViewer_->removeCloud("scan1"); constraintsViewer_->removeCloud("scan1");
constraintsViewer_->removeCloud("scan2");
if(ui_->checkBox_show2DScans->isChecked()) if(ui_->checkBox_show2DScans->isChecked())
{ {
//cloud 2d //cloud 2d
@@ -3908,9 +3908,9 @@ void DatabaseViewer::updateConstraintView(
std::map<int, rtabmap::Transform> poses; std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<ids.size(); ++i) for(unsigned int i=0; i<ids.size(); ++i)
{ {
if(uContains(poses_, ids[i])) if(uContains(odomPoses_, ids[i]))
{ {
poses.insert(*poses_.find(ids[i])); poses.insert(*odomPoses_.find(ids[i]));
} }
else else
{ {
@@ -3955,6 +3955,7 @@ void DatabaseViewer::updateConstraintView(
// transform local poses in loop referential // transform local poses in loop referential
Transform u = t * finalPoses.at(link.to()).inverse(); Transform u = t * finalPoses.at(link.to()).inverse();
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter) for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
{ {
@@ -3968,20 +3969,23 @@ void DatabaseViewer::updateConstraintView(
data.uncompressDataConst(0, 0, &scan, 0); data.uncompressDataConst(0, 0, &scan, 0);
if(!scan.empty()) if(!scan.empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform()); if(scan.channels() >= 5 && ui_->doubleSpinBox_voxelSize->value() == 0.0)
if(assembledScans->size() == 0)
{ {
assembledScans = util3d::transformPointCloud(scanCloud, iter->second); *assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*data.laserScanInfo().localTransform());
} }
else else
{ {
*assembledScans += *util3d::transformPointCloud(scanCloud, iter->second); *assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*data.laserScanInfo().localTransform());
} }
} }
} }
graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z())); graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
} }
if(assembledNormalScans->size())
{
constraintsViewer_->addCloud("scan2normals", assembledNormalScans, pose, Qt::cyan);
}
if(assembledScans->size()) if(assembledScans->size())
{ {
if(ui_->doubleSpinBox_voxelSize->value() > 0.0) if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
@@ -4079,7 +4083,7 @@ void DatabaseViewer::updateConstraintButtons()
int from = ids_.at(ui_->horizontalSlider_A->value()); int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value()); int to = ids_.at(ui_->horizontalSlider_B->value());
if(from!=to && from && to && poses_.find(from) != poses_.end() && poses_.find(to) != poses_.end()) if(from!=to && from && to && odomPoses_.find(from) != odomPoses_.end() && odomPoses_.find(to) != odomPoses_.end())
{ {
if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) || if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
containsLink(linksRemoved_, from ,to)) containsLink(linksRemoved_, from ,to))
@@ -4548,22 +4552,22 @@ void DatabaseViewer::updateGraphView()
ui_->label_loopClosures->clear(); ui_->label_loopClosures->clear();
ui_->label_poses->clear(); ui_->label_poses->clear();
if(poses_.size()) if(odomPoses_.size())
{ {
int fromId = ui_->spinBox_optimizationsFrom->value(); int fromId = ui_->spinBox_optimizationsFrom->value();
if(!uContains(poses_, fromId)) if(!uContains(odomPoses_, fromId))
{ {
QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3") QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
.arg(fromId) .arg(fromId)
.arg(poses_.begin()->first) .arg(odomPoses_.begin()->first)
.arg(poses_.rbegin()->first)); .arg(odomPoses_.rbegin()->first));
return; return;
} }
graphes_.clear(); graphes_.clear();
graphLinks_.clear(); graphLinks_.clear();
std::map<int, rtabmap::Transform> poses = poses_; std::map<int, rtabmap::Transform> poses = odomPoses_;
// filter current map if not spanning to all maps // filter current map if not spanning to all maps
if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0) if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
@@ -4869,6 +4873,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
UERROR("Not found link! (%d->%d)", from, to); UERROR("Not found link! (%d->%d)", from, to);
return; return;
} }
UDEBUG("%d -> %d (type=%d)", from ,to, currentLink.type());
Transform t = currentLink.transform(); Transform t = currentLink.transform();
if(ui_->checkBox_showOptimized->isChecked() && if(ui_->checkBox_showOptimized->isChecked() &&
(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) && (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
@@ -4893,8 +4898,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
if(currentLink.type() == Link::kNeighbor || if(currentLink.type() == Link::kNeighbor ||
currentLink.type() == Link::kNeighborMerged) currentLink.type() == Link::kNeighborMerged)
{ {
Transform poseFrom = uValue(poses_, currentLink.from(), Transform()); Transform poseFrom = uValue(odomPoses_, currentLink.from(), Transform());
Transform poseTo = uValue(poses_, currentLink.to(), Transform()); Transform poseTo = uValue(odomPoses_, currentLink.to(), Transform());
if(!poseFrom.isNull() && !poseTo.isNull()) if(!poseFrom.isNull() && !poseTo.isNull())
{ {
t = poseFrom.inverse() * poseTo; // recompute raw odom transformation t = poseFrom.inverse() * poseTo; // recompute raw odom transformation
@@ -4904,15 +4909,173 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
Transform transform; Transform transform;
RegistrationInfo info; RegistrationInfo info;
Signature fromS;
Signature toS;
SensorData dataFrom, dataTo; SensorData dataFrom;
dbDriver_->getNodeData(currentLink.from(), dataFrom); dbDriver_->getNodeData(currentLink.from(), dataFrom);
dbDriver_->getNodeData(currentLink.to(), dataTo);
ParametersMap parameters = ui_->parameters_toolbox->getParameters(); ParametersMap parameters = ui_->parameters_toolbox->getParameters();
Registration * registration = Registration::create(parameters);
UTimer timer; UTimer timer;
// Is it a multi-scan proximity detection?
cv::Mat userData = currentLink.uncompressUserDataConst();
std::map<int, rtabmap::Transform> scanPoses;
if(currentLink.type() == Link::kLocalSpaceClosure &&
!currentLink.userDataCompressed().empty() &&
userData.type() == CV_8SC1 &&
userData.rows == 1 &&
userData.cols >= 8 && // including null str ending
userData.at<char>(userData.cols-1) == 0 &&
memcmp(userData.data, "SCANS:", 6) == 0 &&
currentLink.from() > currentLink.to())
{
std::string scansStr = (const char *)userData.data;
UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
if(!scansStr.empty())
{
std::list<std::string> strs = uSplit(scansStr, ':');
if(strs.size() == 2)
{
std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
{
int id = atoi(iter->c_str());
if(uContains(odomPoses_, id))
{
scanPoses.insert(*odomPoses_.find(id));
}
else
{
UERROR("Not found %d node!", id);
}
}
}
}
}
if(scanPoses.size())
{
//optimize the path's poses locally
Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
UASSERT(uContains(scanPoses, currentLink.to()));
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
optimizer->getConnectedGraph(
currentLink.to(),
scanPoses,
updateLinksWithModifications(links_),
posesOut,
linksOut);
if(scanPoses.size() != posesOut.size())
{
UWARN("Scan poses input and output are different! %d vs %d", (int)scanPoses.size(), (int)posesOut.size());
UWARN("Input poses: ");
for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
{
UWARN(" %d", iter->first);
}
UWARN("Input links: ");
std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
{
UWARN(" %d->%d", iter->second.from(), iter->second.to());
}
}
scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
delete optimizer;
std::map<int, Transform> filteredScanPoses = scanPoses;
float proximityFilteringRadius = 0.0f;
Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
{
// path filtering
filteredScanPoses = graph::radiusPosesFiltering(scanPoses, proximityFilteringRadius, 0, true);
// make sure the current pose is still here
filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
}
Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
cv::Mat fromScan;
dataFrom.uncompressData(0,0,&fromScan);
int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
bool is2D = true;
for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
{
if(iter->first != currentLink.from())
{
SensorData data;
dbDriver_->getNodeData(iter->first, data);
cv::Mat scan;
if(!data.laserScanCompressed().empty())
{
cv::Mat scan;
data.uncompressData(0, 0, &scan);
if(!scan.empty())
{
if(scan.channels() != 2 && scan.channels() != 5)
{
is2D = false;
}
if(scan.channels() >= 5)
{
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(
scan,
toPoseInv * iter->second * data.laserScanInfo().localTransform());
}
else
{
*assembledToClouds += *util3d::laserScanToPointCloud(
scan,
toPoseInv * iter->second * data.laserScanInfo().localTransform());
}
if(scan.cols > maxPoints)
{
maxPoints = scan.cols;
}
}
}
else
{
UWARN("Laser scan not found for signature %d", iter->first);
}
}
}
cv::Mat assembledScan;
if(assembledToNormalClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
}
else if(assembledToClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
}
SensorData assembledData;
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScanRaw(assembledScan,
LaserScanInfo(
dataFrom.laserScanInfo().maxPoints()?dataFrom.laserScanInfo().maxPoints():maxPoints,
dataFrom.laserScanInfo().maxRange(),
is2D?Transform(0,0,dataFrom.laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));
RegistrationIcp registrationIcp(parameters);
transform = registrationIcp.computeTransformation(dataFrom, assembledData, currentLink.transform(), &info);
}
else
{
SensorData dataTo;
dbDriver_->getNodeData(currentLink.to(), dataTo);
Registration * registration = Registration::create(parameters);
if(registration->isScanRequired()) if(registration->isScanRequired())
{ {
if(ui_->checkBox_icp_from_depth->isChecked()) if(ui_->checkBox_icp_from_depth->isChecked())
@@ -4962,18 +5125,12 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
UINFO("Uncompress time: %f s", timer.ticks()); UINFO("Uncompress time: %f s", timer.ticks());
Signature fromS(dataFrom); fromS = Signature(dataFrom);
Signature toS(dataTo); toS = Signature(dataTo);
transform = registration->computeTransformationMod(fromS, toS, t, &info); transform = registration->computeTransformationMod(fromS, toS, t, &info);
delete registration; delete registration;
UINFO("(%d ->%d) Registration time: %f s", from, to, timer.ticks());
if(!silent)
{
ui_->graphicsView_A->setFeatures(fromS.getWords(), dataFrom.depthRaw());
ui_->graphicsView_B->setFeatures(toS.getWords(), dataTo.depthRaw());
updateWordsMatching();
} }
UINFO("(%d ->%d) Registration time: %f s", from, to, timer.ticks());
if(!transform.isNull()) if(!transform.isNull())
{ {
@@ -4986,7 +5143,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
} }
} }
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv()); Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
bool updated = false; bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from()); std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -5012,8 +5169,19 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
} }
if(!silent && ui_->dockWidget_constraints->isVisible()) if(!silent && ui_->dockWidget_constraints->isVisible())
{
if(fromS.id() > 0 && toS.id() > 0)
{ {
this->updateConstraintView(newLink, true, fromS, toS); this->updateConstraintView(newLink, true, fromS, toS);
ui_->graphicsView_A->setFeatures(fromS.getWords(), fromS.sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(toS.getWords(), toS.sensorData().depthRaw());
updateWordsMatching();
}
else
{
this->updateConstraintView();
}
} }
} }
@@ -5184,10 +5352,10 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters()); Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::multimap<int, Link> links; std::multimap<int, Link> links;
UASSERT(poses_.find(fromId) != poses_.end()); UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
UASSERT_MSG(poses_.find(newLink.from()) != poses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str()); UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses_.find(newLink.to()) != poses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str()); UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
optimizer->getConnectedGraph(fromId, poses_, linksIn, poses, links); optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
UASSERT(poses.find(fromId) != poses.end()); UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(newLink.from()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str()); UASSERT_MSG(poses.find(newLink.from()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(newLink.to()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str()); UASSERT_MSG(poses.find(newLink.to()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());