i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 06:20:19 +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
CMakeLists.txt
+1 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 20)
SET(RTABMAP_PATCH_VERSION 21)
SET(RTABMAP_PATCH_VERSION 22)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/Parameters.h
+2 -1
View File
@@ -183,7 +183,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Rtabmap, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes, bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, "", "Working directory.");
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum nodes retrieved at the same time from LTM.");
RTABMAP_PARAM(Rtabmap, MaxRepublished, unsigned int, 2, uFormat("Maximum nodes republished when requesting missing data. When %s=false, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if %s=false.", kRGBDEnabled().c_str(), kRtabmapPublishLastSignature().c_str()));
RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
RTABMAP_PARAM(Rtabmap, StatisticLogged, bool, false, "Logging enabled.");
RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders, bool, true, "Add column header description to log files.");
corelib/include/rtabmap/core/Rtabmap.h
+4
View File
@@ -223,6 +223,7 @@ public:
int refineLinks();
bool addLink(const Link & link);
cv::Mat getInformation(const cv::Mat & covariance) const;
void addNodesToRepublish(const std::vector<int> & ids);
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -284,6 +285,7 @@ private:
bool _verifyLoopClosureHypothesis;
unsigned int _maxRetrieved;
unsigned int _maxLocalRetrieved;
unsigned int _maxRepublished;
bool _rawDataKept;
bool _statisticLogsBufferedInRAM;
bool _statisticLogged;
@@ -368,6 +370,8 @@ private:
std::vector<float> _odomCorrectionAcc;
std::map<int, Transform> _markerPriors;
std::set<int> _nodesToRepublish;
// Planning stuff
int _pathStatus;
std::vector<std::pair<int,Transform> > _path;
corelib/include/rtabmap/core/RtabmapEvent.h
+3
View File
@@ -59,15 +59,18 @@ class RtabmapEventCmd : public UEvent
public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
enum Cmd {
kCmdUndef,
kCmdInit, // params: [string] database path + ParametersMap
kCmdResetMemory,
kCmdClose, // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
kCmdUpdateParams, // params: ParametersMap
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
kCmdCleanDataBuffer,
kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly
kCmdRepublishData, // params: [vector<int>] ids
kCmdTriggerNewMap,
kCmdPause,
kCmdResume,
corelib/include/rtabmap/core/RtabmapThread.h
+3 -17
View File
@@ -54,22 +54,8 @@ class RTABMAP_EXP RtabmapThread :
{
public:
enum State {
kStateInit,
kStateDetecting,
kStateReseting,
kStateClose,
kStateChangingParameters,
kStateDumpingMemory,
kStateDumpingPrediction,
kStateExportingDOTGraph,
kStateExportingPoses,
kStateCleanDataBuffer,
kStatePublishingMap,
kStateTriggeringMap,
kStateSettingGoal,
kStateCancellingGoal,
kStateLabelling,
kStateRemovingLabel
kStateProcessCommand
};
public:
@@ -105,13 +91,13 @@ private:
void process();
void addData(const OdometryEvent & odomEvent);
bool getData(OdometryEvent & data);
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void pushNewState(State newState, const RtabmapEventCmd & cmdEvent = RtabmapEventCmd(RtabmapEventCmd::kCmdUndef));
void publishMap(bool optimized, bool full, bool graphOnly) const;
private:
UMutex _stateMutex;
std::queue<State> _state;
std::queue<ParametersMap> _stateParam;
std::queue<RtabmapEventCmd> _stateParam;
std::list<OdometryEvent> _dataBuffer;
std::list<double> _newMapEvents;
corelib/include/rtabmap/core/Statistics.h
+7 -3
View File
@@ -241,7 +241,9 @@ public:
void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
void setStamp(double stamp) {_stamp = stamp;}
void setLastSignatureData(const Signature & data) {_lastSignatureData = data;}
RTABMAP_DEPRECATED(void setLastSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}, "Use addSignatureData() instead.");
void addSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}
void setSignaturesData(const std::map<int, Signature> & data) {_signaturesData = data;}
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -270,7 +272,8 @@ public:
int proximityDetectionMapId() const {return _proximiyDetectionMapId;}
double stamp() const {return _stamp;}
const Signature & getLastSignatureData() const {return _lastSignatureData;}
const Signature & getLastSignatureData() const {return _signaturesData.empty()?_dummyEmptyData:_signaturesData.rbegin()->second;}
const std::map<int, Signature> & getSignaturesData() const {return _signaturesData;}
const std::map<int, Transform> & poses() const {return _poses;}
const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -302,7 +305,8 @@ private:
int _proximiyDetectionMapId;
double _stamp;
Signature _lastSignatureData;
std::map<int, Signature> _signaturesData;
Signature _dummyEmptyData;
std::map<int, Transform> _poses;
std::multimap<int, Link> _constraints;
corelib/src/OccupancyGrid.cpp
+191 -185
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
}
}
}
}
corelib/src/Odometry.cpp
+1
View File
@@ -202,6 +202,7 @@ Odometry::~Odometry()
void Odometry::reset(const Transform & initialPose)
{
UDEBUG("");
UASSERT(!initialPose.isNull());
previousVelocities_.clear();
velocityGuess_.setNull();
corelib/src/Rtabmap.cpp
+98 -3
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);
corelib/src/RtabmapThread.cpp
+144 -208
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;
corelib/src/camera/CameraDepthAI.cpp
+2 -4
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)
{
guilib/include/rtabmap/gui/MainWindow.h
+1
View File
@@ -193,6 +193,7 @@ protected Q_SLOTS:
void dumpThePrediction();
void sendGoal();
void sendWaypoints();
void postGoal();
void postGoal(const QString & goal);
void cancelGoal();
void label();
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -286,6 +286,7 @@ public:
//
bool isImagesKept() const;
bool isMissingCacheRepublished() const;
bool isCloudsKept() const;
float getTimeLimit() const;
float getDetectionRate() const;
guilib/src/MainWindow.cpp
+63 -2
View File
@@ -1997,6 +1997,33 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
}
// Add data
for(std::map<int, Signature>::const_iterator iter = stat.getSignaturesData().begin();
iter!=stat.getSignaturesData().end();
++iter)
{
if(signature.id() != iter->first &&
(!_cachedSignatures.contains(iter->first) ||
(_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty())))
{
_cachedSignatures.insert(iter->first, iter->second);
_cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
unsigned int count = 0;
if(!iter->second.getWords3().empty())
{
for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
{
if(util3d::isFinite(iter->second.getWords3()[jter->second]))
{
++count;
}
}
}
_cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
UINFO("Added %d node data to cache", iter->first);
}
}
// For intermediate empty nodes, keep latest image shown
if(signature.getWeight() >= 0)
{
@@ -2573,6 +2600,33 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_cachedMemoryUsage += s.sensorData().getMemoryUsed();
}
// Check missing cache
if(stat.getSignaturesData().size() <= 1)
{
if(_preferencesDialog->isMissingCacheRepublished() &&
_preferencesDialog->isImagesKept() &&
atoi(_preferencesDialog->getParameter(Parameters::kRtabmapMaxRepublished()).c_str()) > 0)
{
std::vector<int> missingIds;
for(std::map<int, Transform>::const_iterator iter=stat.poses().begin(); iter!=stat.poses().end(); ++iter)
{
QMap<int, Signature>::iterator ster = _cachedSignatures.find(iter->first);
if(ster == _cachedSignatures.end() ||
(ster.value().getWeight() >=0 && // ignore intermediate nodes
ster.value().sensorData().imageCompressed().empty() &&
ster.value().sensorData().depthOrRightCompressed().empty() &&
ster.value().sensorData().laserScanCompressed().empty()))
{
missingIds.push_back(iter->first);
}
}
if(!missingIds.empty())
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdRepublishData, UVariant(missingIds)));
}
}
}
UDEBUG("time= %d ms (update cache)", time.restart());
}
else if(!stat.extended() && stat.loopClosureId()>0)
@@ -4651,8 +4705,7 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
else if(event.getPoses().empty() && _waypoints.size())
{
// resend the same goal
uSleep(1000);
this->postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
QTimer::singleShot(1000, this, SLOT(postGoal()));
}
}
@@ -7045,6 +7098,14 @@ void MainWindow::sendWaypoints()
}
}
void MainWindow::postGoal()
{
if(!_waypoints.isEmpty())
{
postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
}
}
void MainWindow::postGoal(const QString & goal)
{
if(!goal.isEmpty())
guilib/src/PreferencesDialog.cpp
+9
View File
@@ -450,6 +450,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// General panel
connect(_ui->general_checkBox_imagesKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->general_checkBox_missing_cache_republished, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->general_checkBox_cloudsKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->checkBox_verticalLayoutUsed, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->checkBox_imageRejectedShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
@@ -888,6 +889,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_spinBox_imagesBufferSize->setObjectName(Parameters::kRtabmapImageBufferSize().c_str());
_ui->general_checkBox_createIntermediateNodes->setObjectName(Parameters::kRtabmapCreateIntermediateNodes().c_str());
_ui->general_spinBox_maxRetrieved->setObjectName(Parameters::kRtabmapMaxRetrieved().c_str());
_ui->general_spinBox_max_republished->setObjectName(Parameters::kRtabmapMaxRepublished().c_str());
_ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
_ui->general_checkBox_startNewMapOnGoodSignature->setObjectName(Parameters::kRtabmapStartNewMapOnGoodSignature().c_str());
_ui->general_checkBox_imagesAlreadyRectified->setObjectName(Parameters::kRtabmapImagesAlreadyRectified().c_str());
@@ -1792,6 +1794,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
if(groupBox->objectName() == _ui->groupBox_generalSettingsGui0->objectName())
{
_ui->general_checkBox_imagesKept->setChecked(true);
_ui->general_checkBox_missing_cache_republished->setChecked(true);
_ui->general_checkBox_cloudsKept->setChecked(true);
_ui->checkBox_beep->setChecked(false);
_ui->checkBox_stamps->setChecked(true);
@@ -2272,6 +2275,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
settings.beginGroup("Gui");
settings.beginGroup("General");
_ui->general_checkBox_imagesKept->setChecked(settings.value("imagesKept", _ui->general_checkBox_imagesKept->isChecked()).toBool());
_ui->general_checkBox_missing_cache_republished->setChecked(settings.value("missingRepublished", _ui->general_checkBox_missing_cache_republished->isChecked()).toBool());
_ui->general_checkBox_cloudsKept->setChecked(settings.value("cloudsKept", _ui->general_checkBox_cloudsKept->isChecked()).toBool());
_ui->comboBox_loggerLevel->setCurrentIndex(settings.value("loggerLevel", _ui->comboBox_loggerLevel->currentIndex()).toInt());
_ui->comboBox_loggerEventLevel->setCurrentIndex(settings.value("loggerEventLevel", _ui->comboBox_loggerEventLevel->currentIndex()).toInt());
@@ -2801,6 +2805,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.beginGroup("General");
settings.remove("");
settings.setValue("imagesKept", _ui->general_checkBox_imagesKept->isChecked());
settings.setValue("missingRepublished", _ui->general_checkBox_missing_cache_republished->isChecked());
settings.setValue("cloudsKept", _ui->general_checkBox_cloudsKept->isChecked());
settings.setValue("loggerLevel", _ui->comboBox_loggerLevel->currentIndex());
settings.setValue("loggerEventLevel", _ui->comboBox_loggerEventLevel->currentIndex());
@@ -6482,6 +6487,10 @@ bool PreferencesDialog::isImagesKept() const
{
return _ui->general_checkBox_imagesKept->isChecked();
}
bool PreferencesDialog::isMissingCacheRepublished() const
{
return _ui->general_checkBox_missing_cache_republished->isChecked();
}
bool PreferencesDialog::isCloudsKept() const
{
return _ui->general_checkBox_cloudsKept->isChecked();
guilib/src/ui/preferencesDialog.ui
+232 -186
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-543</y>
<y>0</y>
<width>756</width>
<height>3657</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -107,15 +107,8 @@
<layout class="QVBoxLayout" name="verticalLayout_28">
<item>
<layout class="QGridLayout" name="gridLayout_39" columnstretch="0,1">
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_beep">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
<item row="0" column="0">
<widget class="QCheckBox" name="general_checkBox_imagesKept">
<property name="text">
<string/>
</property>
@@ -124,7 +117,7 @@
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_247">
<property name="text">
<string>Use time for figures' x-axis. Otherwise, sensor data IDs are used for camera/odometry info and for all mapping statistics, node IDs are used.</string>
@@ -137,7 +130,24 @@
</property>
</widget>
</item>
<item row="1" column="1">
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_cacheStatistics">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_313">
<property name="text">
<string>Insert created clouds in the GUI cache to avoid regenerating clouds on export.</string>
@@ -150,47 +160,14 @@
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_102">
<property name="text">
<string>Notify when a new global path is received.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="general_checkBox_imagesKept">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="general_checkBox_cloudsKept">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_stamps">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_342">
<property name="text">
<string>Cache statistics.</string>
@@ -203,7 +180,40 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_beep">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_102">
<property name="text">
<string>Notify when a new global path is received.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_239">
<property name="text">
<string>Disable odometry. This option only makes sense in localization mode with a pre-built map and that odometry cannot be computed from the source selected (e.g. RGB source).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_88">
<property name="text">
<string>Beep! on special events (finished processing the data set, an error has occured, ...).</string>
@@ -229,34 +239,14 @@
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QCheckBox" name="checkbox_odomDisabled">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_239">
<property name="text">
<string>Disable odometry. This option only makes sense in localization mode with a pre-built map and that odometry cannot be computed from the source selected (e.g. RGB source).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_cacheStatistics">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_347">
<property name="text">
<string>When a ground truth is provided, align it with the map.</string>
@@ -269,13 +259,46 @@
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QCheckBox" name="checkbox_groundTruthAlign">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="general_checkBox_cloudsKept">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_651">
<property name="text">
<string>Republish nodes not in GUI cache.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="general_checkBox_missing_cache_republished">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -8307,19 +8330,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_15">
<item>
<layout class="QGridLayout" name="gridLayout_43" columnstretch="0,1">
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
<property name="suffix">
<string> Hz</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="general_checkBox_SLAM_mode">
<property name="text">
@@ -8344,13 +8354,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="general_checkBox_startNewMapOnLoopClosure">
<property name="text">
<string/>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
<property name="suffix">
<string> Hz</string>
</property>
<property name="checked">
<bool>true</bool>
<property name="decimals">
<number>3</number>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
@@ -8377,29 +8390,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="general_checkBox_createIntermediateNodes">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_185">
<property name="text">
<string>Start a new map only if there is a global loop closure detected first with a previous map. If there is no map in memory, a new map is still created.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_84">
<property name="text">
@@ -8413,6 +8403,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="general_checkBox_createIntermediateNodes">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_165">
<property name="text">
@@ -8426,10 +8426,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_467">
<item row="4" column="0">
<widget class="QCheckBox" name="general_checkBox_startNewMapOnLoopClosure">
<property name="text">
<string>Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.</string>
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_185">
<property name="text">
<string>Start a new map only if there is a global loop closure detected first with a previous map. If there is no map in memory, a new map is still created.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -8439,8 +8449,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="general_checkBox_imagesAlreadyRectified">
<item row="5" column="0">
<widget class="QCheckBox" name="general_checkBox_startNewMapOnGoodSignature">
<property name="text">
<string/>
</property>
@@ -8462,8 +8472,31 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="general_checkBox_startNewMapOnGoodSignature">
<item row="6" column="0">
<widget class="QCheckBox" name="general_checkBox_imagesAlreadyRectified">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_467">
<property name="text">
<string>Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="general_checkBox_rectifyOnlyFeatures">
<property name="text">
<string/>
</property>
@@ -8485,8 +8518,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="general_checkBox_rectifyOnlyFeatures">
<item row="8" column="0">
<widget class="QCheckBox" name="general_checkBox_saveLocalizationData">
<property name="text">
<string/>
</property>
@@ -8508,16 +8541,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="general_checkBox_saveLocalizationData">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -8645,7 +8668,27 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<bool>true</bool>
</property>
<layout class="QGridLayout" name="gridLayout_45" columnstretch="0,1">
<item row="1" column="0">
<item row="4" column="0">
<widget class="QCheckBox" name="general_checkBox_publishRMSE">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="general_checkBox_saveWMState">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="general_checkBox_publishPdf">
<property name="text">
<string/>
@@ -8655,7 +8698,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_106">
<property name="text">
<string>Publish loop closure hypotheses (pdf).</string>
@@ -8665,17 +8708,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="general_checkBox_publishLikelihood">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_462">
<property name="text">
<string>Save working memory state after each update.</string>
@@ -8685,6 +8718,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_116">
<property name="text">
<string>Publish loop closure likelihood.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_452">
<property name="text">
<string>Publish RAM usage.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_437">
<property name="text">
<string>Compute RMSE when ground truth is available.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="general_checkBox_publishRawData">
<property name="text">
@@ -8695,6 +8758,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="general_checkBox_publishLikelihood">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_91">
<property name="text">
@@ -8705,27 +8778,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_116">
<property name="text">
<string>Publish loop closure likelihood.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_452">
<property name="text">
<string>Publish RAM usage.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QCheckBox" name="general_checkBox_publishRAM">
<property name="text">
<string/>
@@ -8735,33 +8788,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_437">
<item row="1" column="1">
<widget class="QLabel" name="label_652">
<property name="text">
<string>Compute RMSE when ground truth is available.</string>
<string>Maximum nodes republished when requesting missing data. When RGB-D SLAM mode is disabled, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if &quot;Publish signature data&quot; is unchecked.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="general_checkBox_publishRMSE">
<property name="text">
<string/>
<item row="1" column="0">
<widget class="QSpinBox" name="general_spinBox_max_republished">
<property name="maximum">
<number>999</number>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="general_checkBox_saveWMState">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
utilite/include/rtabmap/utilite/UVariant.h
+32
View File
@@ -41,6 +41,14 @@ public:
kFloat,
kDouble,
kStr,
kCharArray,
kUCharArray,
kShortArray,
kUShortArray,
kIntArray,
kUIntArray,
kFloatArray,
kDoubleArray,
kUndef
};
public:
@@ -56,6 +64,14 @@ public:
UVariant(const double & value);
UVariant(const char * value);
UVariant(const std::string & value);
UVariant(const std::vector<char> & value);
UVariant(const std::vector<unsigned char> & value);
UVariant(const std::vector<short> & value);
UVariant(const std::vector<unsigned short> & value);
UVariant(const std::vector<int> & value);
UVariant(const std::vector<unsigned int> & value);
UVariant(const std::vector<float> & value);
UVariant(const std::vector<double> & value);
Type type() const {return type_;}
@@ -70,6 +86,14 @@ public:
bool isFloat() const {return type_ == kFloat;}
bool isDouble() const {return type_ == kDouble;}
bool isStr() const {return type_ == kStr;}
bool isCharArray() const {return type_ == kCharArray;}
bool isUCharArray() const {return type_ == kUCharArray;}
bool isShortArray() const {return type_ == kShortArray;}
bool isUShortArray() const {return type_ == kUShortArray;}
bool isIntArray() const {return type_ == kIntArray;}
bool isUIntArray() const {return type_ == kUIntArray;}
bool isFloatArray() const {return type_ == kFloatArray;}
bool isDoubleArray() const {return type_ == kDoubleArray;}
bool toBool() const;
char toChar(bool * ok = 0) const;
@@ -81,6 +105,14 @@ public:
float toFloat(bool * ok = 0) const;
double toDouble(bool * ok = 0) const;
std::string toStr(bool * ok = 0) const;
std::vector<char> toCharArray(bool * ok = 0) const;
std::vector<unsigned char> toUCharArray(bool * ok = 0) const;
std::vector<short> toShortArray(bool * ok = 0) const;
std::vector<unsigned short> toUShortArray(bool * ok = 0) const;
std::vector<int> toIntArray(bool * ok = 0) const;
std::vector<unsigned int> toUIntArray(bool * ok = 0) const;
std::vector<float> toFloatArray(bool * ok = 0) const;
std::vector<double> toDoubleArray(bool * ok = 0) const;
virtual ~UVariant() {}
utilite/src/UVariant.cpp
+227
View File
@@ -93,6 +93,54 @@ UVariant::UVariant(const std::string & value) :
{
memcpy(data_.data(), value.data(), value.size()+1);
}
UVariant::UVariant(const std::vector<char> & value) :
type_(kCharArray),
data_(sizeof(char)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(char)*value.size());
}
UVariant::UVariant(const std::vector<unsigned char> & value) :
type_(kUCharArray),
data_(sizeof(unsigned char)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(unsigned char)*value.size());
}
UVariant::UVariant(const std::vector<short> & value) :
type_(kShortArray),
data_(sizeof(short)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(short)*value.size());
}
UVariant::UVariant(const std::vector<unsigned short> & value) :
type_(kUShortArray),
data_(sizeof(unsigned short)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(unsigned short)*value.size());
}
UVariant::UVariant(const std::vector<int> & value) :
type_(kIntArray),
data_(sizeof(int)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(int)*value.size());
}
UVariant::UVariant(const std::vector<unsigned int> & value) :
type_(kUIntArray),
data_(sizeof(unsigned int)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(unsigned int)*value.size());
}
UVariant::UVariant(const std::vector<float> & value) :
type_(kFloatArray),
data_(sizeof(float)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(float)*value.size());
}
UVariant::UVariant(const std::vector<double> & value) :
type_(kDoubleArray),
data_(sizeof(double)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(double)*value.size());
}
bool UVariant::toBool() const
{
@@ -641,3 +689,182 @@ std::string UVariant::toStr(bool * ok) const
}
return v;
}
std::vector<char> UVariant::toCharArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<char> v;
if(type_ == kCharArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(char));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<unsigned char> UVariant::toUCharArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<unsigned char> v;
if(type_ == kUCharArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(unsigned char));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<short> UVariant::toShortArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<short> v;
if(type_ == kShortArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(short));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<unsigned short> UVariant::toUShortArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<unsigned short> v;
if(type_ == kUShortArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(unsigned short));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<int> UVariant::toIntArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<int> v;
if(type_ == kIntArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(int));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<unsigned int> UVariant::toUIntArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<unsigned int> v;
if(type_ == kUIntArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(unsigned int));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<float> UVariant::toFloatArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<float> v;
if(type_ == kFloatArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(float));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<double> UVariant::toDoubleArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<double> v;
if(type_ == kDoubleArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(double));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}