i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Bump version 0.20.22. Refactored RtabmapThread commands handling. MainWindow/RtabmapThread/Statistics/UVariant: added new option to republish missing data on GUI side. MainWindow: fixed 1 sec lag when waypoints are used. Parameters: added Rtabmap/MaxRepublished (moved from rtabmap_ros). OccupancyGrid: fixed ground cells ignored if there are empty cells, also don't add pose in addedNodes if corresponding node was not in cache.

Browse Source
This commit is contained in:
matlabbe
2022-10-28 12:25:48 -07:00
parent 8e30c1c812
commit 1297714271
18 changed files with 1021 additions and 610 deletions
Download Patch File Download Diff File Expand all files Collapse all files

376
corelib/src/OccupancyGrid.cpp
View File

@@ -770,7 +770,7 @@ void OccupancyGrid::addToCache(
const cv::Mat & obstacles,
const cv::Mat & empty)
{
UDEBUG("nodeId=%d", nodeId);
UDEBUG("nodeId=%d (ground=%d obstacles=%d empty=%d)", nodeId, ground.cols, obstacles.cols, empty.cols);
if(nodeId < 0)
{
UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
@@ -1026,6 +1026,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
if(!cache_.empty())
{
UDEBUG("Updating from cache");
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(cache_, iter->first))
@@ -1035,14 +1036,18 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
//ground
cv::Mat ground;
if(pair.first.first.cols || pair.second.cols)
{
ground = cv::Mat(1, pair.first.first.cols+pair.second.cols, CV_32FC2);
}
if(pair.first.first.cols)
{
if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
{
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
}
cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
for(int i=0; i<ground.cols; ++i)
for(int i=0; i<pair.first.first.cols; ++i)
{
const float * vi = pair.first.first.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i);
@@ -1067,7 +1072,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
else if(maxY < vo[1])
maxY = vo[1];
}
uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
if(cloudAssembling_)
{
@@ -1083,11 +1087,10 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
{
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
}
cv::Mat ground(1, pair.second.cols, CV_32FC2);
for(int i=0; i<ground.cols; ++i)
for(int i=0; i<pair.second.cols; ++i)
{
const float * vi = pair.second.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i+pair.first.first.cols);
cv::Point3f vt;
if(pair.second.channels() != 2 && pair.second.channels() != 5)
{
@@ -1109,7 +1112,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
else if(maxY < vo[1])
maxY = vo[1];
}
uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
if(cloudAssembling_)
{
@@ -1117,6 +1119,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
assembledEmptyCellsUpdated = true;
}
}
uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
//obstacles
if(pair.first.second.cols)
@@ -1246,205 +1249,208 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
{
if(kter->first > 0)
{
uInsert(addedNodes_, *kter);
}
std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
if(cter == cellCount_.end() && kter->first > 0)
if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
{
cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
}
if(iter!=emptyLocalMaps.end())
{
for(int i=0; i<iter->second.cols; ++i)
if(kter->first > 0)
{
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("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2 && (!incrementalGraphUpdate || value==-1))
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
if(value != -1)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId > 0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.first+=1;
}
value = 0; // free space
// update odds
if(nodeId != kter->first)
{
info[3] += probMiss_;
if (info[3] < probClampingMin_)
{
info[3] = probClampingMin_;
}
if (info[3] > probClampingMax_)
{
info[3] = probClampingMax_;
}
}
}
uInsert(addedNodes_, *kter);
}
}
if(footprintRadius_ >= cellSize_*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
ptEnd.x = map.cols-1;
if(ptBegin.y < 0)
ptBegin.y = 0;
if(ptEnd.y >= map.rows)
ptEnd.y = map.rows-1;
for(int i=ptBegin.x; i<ptEnd.x; ++i)
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
if(cter == cellCount_.end() && kter->first > 0)
{
for(int j=ptBegin.y; j<ptEnd.y; ++j)
{
UASSERT(j < map.rows && i < map.cols);
char & value = map.at<char>(j, i);
float * info = mapInfo.ptr<float>(j, i);
int nodeId = (int)info[0];
if(value != -1)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId>0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = float(i) * cellSize_ + xMin;
info[2] = float(j) * cellSize_ + yMin;
info[3] = probClampingMin_;
cter->second.first+=1;
}
value = -2; // free space (footprint)
}
cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
}
}
if(jter!=occupiedLocalMaps.end())
{
for(int i=0; i<jter->second.cols; ++i)
if(iter!=emptyLocalMaps.end())
{
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,
for(int i=0; i<iter->second.cols; ++i)
{
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("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2 && (!incrementalGraphUpdate || value==-1))
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
if(value != -1)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId > 0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.first+=1;
}
value = 0; // free space
// update odds
if(nodeId != kter->first)
{
info[3] += probMiss_;
if (info[3] < probClampingMin_)
{
info[3] = probClampingMin_;
}
if (info[3] > probClampingMax_)
{
info[3] = probClampingMax_;
}
}
}
}
}
if(footprintRadius_ >= cellSize_*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
ptEnd.x = map.cols-1;
if(ptBegin.y < 0)
ptBegin.y = 0;
if(ptEnd.y >= map.rows)
ptEnd.y = map.rows-1;
for(int i=ptBegin.x; i<ptEnd.x; ++i)
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
if(value != -1)
for(int j=ptBegin.y; j<ptEnd.y; ++j)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
UASSERT(j < map.rows && i < map.cols);
char & value = map.at<char>(j, i);
float * info = mapInfo.ptr<float>(j, i);
int nodeId = (int)info[0];
if(value != -1)
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId>0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
eter->second.first -= 1;
// cannot rewrite on cells referred by more recent nodes
continue;
}
else if(value == 100)
if(nodeId>0)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.second += 1;
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.second+=1;
}
// update odds
if(nodeId != kter->first || value!=100)
{
info[3] += probHit_;
if (info[3] < probClampingMin_)
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = float(i) * cellSize_ + xMin;
info[2] = float(j) * cellSize_ + yMin;
info[3] = probClampingMin_;
cter->second.first+=1;
}
if (info[3] > probClampingMax_)
{
info[3] = probClampingMax_;
}
value = -2; // free space (footprint)
}
}
}
value = 100; // obstacles
if(jter!=occupiedLocalMaps.end())
{
for(int i=0; i<jter->second.cols; ++i)
{
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("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
if(value != -1)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId>0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.second += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.second+=1;
}
// update odds
if(nodeId != kter->first || value!=100)
{
info[3] += probHit_;
if (info[3] < probClampingMin_)
{
info[3] = probClampingMin_;
}
if (info[3] > probClampingMax_)
{
info[3] = probClampingMax_;
}
}
value = 100; // obstacles
}
}
}
}

1
corelib/src/Odometry.cpp
View File

@@ -202,6 +202,7 @@ Odometry::~Odometry()
void Odometry::reset(const Transform & initialPose)
{
UDEBUG("");
UASSERT(!initialPose.isNull());
previousVelocities_.clear();
velocityGuess_.setNull();

101
corelib/src/Rtabmap.cpp
View File

@@ -101,6 +101,7 @@ Rtabmap::Rtabmap() :
_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
_maxRepublished(Parameters::defaultRtabmapMaxRepublished()),
_rawDataKept(Parameters::defaultMemImageKept()),
_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
_statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
@@ -355,6 +356,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
_globalScanMapPoses.clear();
_odomCachePoses.clear();
_odomCacheConstraints.clear();
_nodesToRepublish.clear();
// Parse all parameters
this->parseParameters(allParameters);
@@ -473,6 +475,8 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_globalScanMap.clear();
_globalScanMapPoses.clear();
_nodesToRepublish.clear();
flushStatisticLogs();
if(_foutFloat)
{
@@ -559,6 +563,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
Parameters::parse(parameters, Parameters::kRtabmapMaxRepublished(), _maxRepublished);
if(_maxRepublished == 0 || !_publishLastSignatureData)
{
_nodesToRepublish.clear();
}
Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
@@ -1053,6 +1062,7 @@ void Rtabmap::resetMemory()
_optimizeFromGraphEndChanged = false;
_globalScanMap.clear();
_globalScanMapPoses.clear();
_nodesToRepublish.clear();
this->clearPath(0);
if(_memory)
@@ -3667,6 +3677,7 @@ bool Rtabmap::process(
// Posterior is empty if a bad signature is detected
float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
// prepare statistics
if(_loopClosureHypothesis.first || _publishStats)
@@ -3681,6 +3692,7 @@ bool Rtabmap::process(
statistics_.setLoopClosureMapId(_memory->getMapId(_loopClosureHypothesis.first));
ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
}
if(_publishStats)
{
ULOGGER_INFO("send all stats...");
@@ -3722,7 +3734,6 @@ bool Rtabmap::process(
statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
statistics_.addStatistic(Statistics::kLoopId(), loopId);
statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);
@@ -4187,7 +4198,71 @@ bool Rtabmap::process(
if(_publishLastSignatureData)
{
UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
statistics_.setLastSignatureData(lastSignatureData);
statistics_.addSignatureData(lastSignatureData);
if(_nodesToRepublish.size())
{
std::multimap<int, int> missingIds;
// priority to loopId
int tmpId = loopId>0?loopId:_highestHypothesis.first;
if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end())
{
missingIds.insert(std::make_pair(-1, tmpId));
}
if(!_lastLocalizationPose.isNull())
{
// Republish data from closest nodes of the current localization
std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose);
if(id>0)
{
std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true, false, true);
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(iter->first != loopId &&
_nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
{
missingIds.insert(std::make_pair(iter->second, iter->first));
}
}
if(_nodesToRepublish.size() != missingIds.size())
{
// remove requested nodes not anymore in the graph
for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
{
if(ids.find(*iter) == ids.end())
{
iter = _nodesToRepublish.erase(iter);
}
else
{
++iter;
}
}
}
}
}
int loaded = 0;
std::stringstream stream;
for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter)
{
statistics_.addSignatureData(_memory->getNodeData(iter->second, true, true, true, true));
_nodesToRepublish.erase(iter->second);
++loaded;
stream << iter->second << " ";
}
if(loaded)
{
UWARN("Republishing data of requested node(s) %s(%s=%d)",
stream.str().c_str(),
Parameters::kRtabmapMaxRepublished().c_str(),
_maxRepublished);
}
}
}
else
{
@@ -4207,7 +4282,7 @@ bool Rtabmap::process(
}
nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
statistics_.setLastSignatureData(nodeInfo);
statistics_.addSignatureData(nodeInfo);
}
UDEBUG("");
localGraphSize = (int)poses.size();
@@ -6016,6 +6091,26 @@ cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
return information;
}
void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
{
if(ids.empty())
{
_nodesToRepublish.clear();
}
else if(_maxRepublished > 0 && _publishLastSignatureData)
{
_nodesToRepublish.insert(ids.begin(), ids.end());
}
else if(_maxRepublished == 0)
{
UWARN("%s=0, so cannot republish the %d requested nodes.", Parameters::kRtabmapMaxRepublished().c_str(), (int)ids.size());
}
else //_publishLastSignatureData=false
{
UWARN("%s=false, so cannot republish the %d requested nodes.", Parameters::kRtabmapPublishLastSignature().c_str(), (int)ids.size());
}
}
void Rtabmap::clearPath(int status)
{
UINFO("status=%d", status);

352
corelib/src/RtabmapThread.cpp
View File

@@ -66,14 +66,14 @@ RtabmapThread::~RtabmapThread()
delete _frameRateTimer;
}
void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
{
ULOGGER_DEBUG("to %d", newState);
_stateMutex.lock();
{
_state.push(newState);
_stateParam.push(parameters);
_stateParam.push(cmdEvent);
}
_stateMutex.unlock();
@@ -180,7 +180,7 @@ void RtabmapThread::mainLoopKill()
void RtabmapThread::mainLoop()
{
State state = kStateDetecting;
ParametersMap parameters;
RtabmapEventCmd cmdEvent(RtabmapEventCmd::kCmdUndef);
_stateMutex.lock();
{
@@ -188,7 +188,7 @@ void RtabmapThread::mainLoop()
{
state = _state.front();
_state.pop();
parameters = _stateParam.front();
cmdEvent = _stateParam.front();
_stateParam.pop();
}
}
@@ -198,110 +198,161 @@ void RtabmapThread::mainLoop()
cv::Mat userData;
UTimer timer;
std::string str;
RtabmapEventCmd::Cmd cmd = cmdEvent.getCmd();
switch(state)
{
case kStateDetecting:
this->process();
break;
case kStateInit:
UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
str = parameters.at("RtabmapThread/DatabasePath");
parameters.erase("RtabmapThread/DatabasePath");
Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
UASSERT(_rate >= 0.0f);
_rtabmap->init(parameters, str);
break;
case kStateChangingParameters:
Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
UASSERT(_rate >= 0.0f);
_rtabmap->parseParameters(parameters);
break;
case kStateReseting:
_rtabmap->resetMemory();
this->clearBufferedData();
break;
case kStateClose:
if(_dataBuffer.size())
case kStateProcessCommand:
if(cmd == RtabmapEventCmd::kCmdInit)
{
UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
ULOGGER_DEBUG("CMD_INIT");
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
UASSERT(_rate >= 0.0f);
_rtabmap->init(cmdEvent.getParameters(), cmdEvent.value1().toStr());
}
else if(cmd == RtabmapEventCmd::kCmdClose)
{
ULOGGER_DEBUG("CMD_CLOSE");
if(_dataBuffer.size())
{
UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
this->clearBufferedData();
}
_rtabmap->close(cmdEvent.value1().toBool(), cmdEvent.value2().toStr());
}
else if(cmd == RtabmapEventCmd::kCmdUpdateParams)
{
ULOGGER_DEBUG("CMD_UPDATE_PARAMS");
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
UASSERT(_rate >= 0.0f);
_rtabmap->parseParameters(cmdEvent.getParameters());
break;
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
_rtabmap->resetMemory();
this->clearBufferedData();
}
_rtabmap->close(uStr2Bool(parameters.at("saved")), parameters.at("outputPath"));
break;
case kStateDumpingMemory:
_rtabmap->dumpData();
break;
case kStateDumpingPrediction:
_rtabmap->dumpPrediction();
break;
case kStateExportingDOTGraph:
_rtabmap->generateDOTGraph(
parameters.at("path"),
atoi(parameters.at("id").c_str()),
atoi(parameters.at("margin").c_str()));
break;
case kStateExportingPoses:
_rtabmap->exportPoses(
parameters.at("path"),
uStr2Bool(parameters.at("optimized")),
uStr2Bool(parameters.at("global")),
atoi(parameters.at("type").c_str()));
break;
case kStateCleanDataBuffer:
this->clearBufferedData();
break;
case kStatePublishingMap:
this->publishMap(
uStr2Bool(parameters.at("optimized")),
uStr2Bool(parameters.at("global")),
uStr2Bool(parameters.at("graph_only")));
break;
case kStateTriggeringMap:
_rtabmap->triggerNewMap();
break;
case kStateSettingGoal:
id = atoi(parameters.at("id").c_str());
if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
{
id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
if(id <= 0)
ULOGGER_DEBUG("CMD_DUMP_MEMORY");
_rtabmap->dumpData();
}
else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
{
ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
_rtabmap->dumpPrediction();
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
{
ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
_rtabmap->generateDOTGraph(
cmdEvent.value2().toStr(),
cmdEvent.value1().toBool()?0:cmdEvent.value3().toInt(),
cmdEvent.value1().toBool()?0:cmdEvent.value4().toInt());
}
else if(cmd == RtabmapEventCmd::kCmdExportPoses)
{
ULOGGER_DEBUG("CMD_EXPORT_POSES");
_rtabmap->exportPoses(
cmdEvent.value3().toStr(),
cmdEvent.value2().toBool(),
cmdEvent.value1().toBool(),
cmdEvent.value4().toInt());
}
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
this->clearBufferedData();
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP");
this->publishMap(
cmdEvent.value2().toBool(),
cmdEvent.value1().toBool(),
cmdEvent.value3().toBool());
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{
ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
_rtabmap->triggerNewMap();
}
else if(cmd == RtabmapEventCmd::kCmdPause)
{
ULOGGER_DEBUG("CMD_PAUSE");
_paused = !_paused;
}
else if(cmd == RtabmapEventCmd::kCmdGoal)
{
ULOGGER_DEBUG("CMD_GOAL");
if(cmdEvent.value1().isStr() && !cmdEvent.value1().toStr().empty() && _rtabmap->getMemory())
{
UERROR("Failed to find a node with label \"%s\".", parameters.at("label").c_str());
id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr());
if(id <= 0)
{
UERROR("Failed to find a node with label \"%s\".", cmdEvent.value1().toStr());
}
}
else if(cmdEvent.value1().isInt() || cmdEvent.value1().isUInt())
{
id = cmdEvent.value1().toInt();
}
if(id < 0)
{
UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
}
timer.start();
if(id > 0 && !_rtabmap->computePath(id, true))
{
UERROR("Failed to compute a path to goal %d.", id);
}
this->post(new RtabmapGlobalPathEvent(
id,
cmdEvent.value1().isStr()?cmdEvent.value1().toStr():"",
_rtabmap->getPath(),
timer.elapsed()));
break;
}
else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
{
ULOGGER_DEBUG("CMD_CANCEL_GOAL");
_rtabmap->clearPath(0);
}
else if(cmd == RtabmapEventCmd::kCmdLabel)
{
ULOGGER_DEBUG("CMD_LABEL");
if(!_rtabmap->labelLocation(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()))
{
this->post(new RtabmapLabelErrorEvent(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()));
}
}
else if(id < 0)
else if(cmd == RtabmapEventCmd::kCmdRemoveLabel)
{
UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
ULOGGER_DEBUG("CMD_REMOVE_LABEL");
id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr(), true);
if(id <= 0 || !_rtabmap->labelLocation(id, ""))
{
this->post(new RtabmapLabelErrorEvent(id, cmdEvent.value1().toStr()));
}
}
timer.start();
if(id > 0 && !_rtabmap->computePath(id, true))
else if(cmd == RtabmapEventCmd::kCmdRepublishData)
{
UERROR("Failed to compute a path to goal %d.", id);
ULOGGER_DEBUG("CMD_REPUBLISH_DATA");
_rtabmap->addNodesToRepublish(cmdEvent.value1().toIntArray());
}
this->post(new RtabmapGlobalPathEvent(
id,
parameters.at("label"),
_rtabmap->getPath(),
timer.elapsed()));
break;
case kStateCancellingGoal:
_rtabmap->clearPath(0);
break;
case kStateLabelling:
if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label")))
else
{
this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label")));
}
break;
case kStateRemovingLabel:
id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"), true);
if(!_rtabmap->labelLocation(id, ""))
{
this->post(new RtabmapLabelErrorEvent(id, parameters.at("label")));
UWARN("Cmd %d unknown!", cmd);
}
break;
default:
@@ -397,127 +448,12 @@ bool RtabmapThread::handleEvent(UEvent* event)
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
if(cmd == RtabmapEventCmd::kCmdInit)
{
ULOGGER_DEBUG("CMD_INIT");
ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
UASSERT(rtabmapEvent->value1().isStr());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
pushNewState(kStateInit, parameters);
}
else if(cmd == RtabmapEventCmd::kCmdClose)
{
ULOGGER_DEBUG("CMD_CLOSE");
UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
ParametersMap param;
param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
param.insert(ParametersPair("outputPath", rtabmapEvent->value2().toStr()));
pushNewState(kStateClose, param);
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
pushNewState(kStateReseting);
}
else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
{
ULOGGER_DEBUG("CMD_DUMP_MEMORY");
pushNewState(kStateDumpingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
{
ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
pushNewState(kStateDumpingPrediction);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
{
ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isStr());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingDOTGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdExportPoses)
{
ULOGGER_DEBUG("CMD_EXPORT_POSES");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isStr());
UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingPoses, param);
}
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
pushNewState(kStateCleanDataBuffer);
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isBool());
ParametersMap param;
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
pushNewState(kStatePublishingMap, param);
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{
ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
pushNewState(kStateTriggeringMap);
}
else if(cmd == RtabmapEventCmd::kCmdPause)
{
ULOGGER_DEBUG("CMD_PAUSE");
_paused = !_paused;
}
else if(cmd == RtabmapEventCmd::kCmdGoal)
{
ULOGGER_DEBUG("CMD_GOAL");
UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
pushNewState(kStateSettingGoal, param);
}
else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
{
ULOGGER_DEBUG("CMD_CANCEL_GOAL");
pushNewState(kStateCancellingGoal);
}
else if(cmd == RtabmapEventCmd::kCmdLabel)
{
ULOGGER_DEBUG("CMD_LABEL");
UASSERT(rtabmapEvent->value1().isStr());
UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
pushNewState(kStateLabelling, param);
}
else
{
UWARN("Cmd %d unknown!", cmd);
}
pushNewState(kStateProcessCommand, *rtabmapEvent);
}
else if(event->getClassName().compare("ParamEvent") == 0)
{
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
pushNewState(kStateProcessCommand, RtabmapEventCmd(RtabmapEventCmd::kCmdUpdateParams, ((ParamEvent*)event)->getParameters()));
}
}
return false;

6
corelib/src/camera/CameraDepthAI.cpp
View File

@@ -261,12 +261,12 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
UINFO("IMU disabled");
}
leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
if(imuPublished_)
{
imuQueue_ = device_->getOutputQueue("imu", 50, false);
}
leftQueue_ = device_->getOutputQueue("rectified_left", 1, false);
rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false);
uSleep(2000); // avoid bad frames on start
@@ -333,7 +333,6 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
}
//get imu
int added= 0;
double stampStart = UTimer::now();
while(imuPublished_ && imuQueue_.get())
{
@@ -360,7 +359,6 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
{
gyroBuffer_.erase(gyroBuffer_.begin());
}
++added;
}
if(accStamp >= stamp && gyroStamp >= stamp)
{