-Updated File menu with new actions "New database...", "Open database", "Close database" and "Edit database". Each new session will be automatically saved when closing the database. The "play" action cannot be activated until a new database is created or a previous session database is opened.

-  Memory usage reviewed for the GUI: we don't uncompress data for downloaded nodes from "Download all clouds" action.
- Odometry: added optional argument "initialPose" to reset() method

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1930 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-28 01:20:57 +00:00
parent 457c068e0f
commit b56e03b463
30 changed files with 632 additions and 424 deletions
+1 -1
View File
@@ -91,7 +91,7 @@ public:
// Load objects
void load(VWDictionary * dictionary) const;
void loadLastNodes(std::list<Signature *> & signatures) const;
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
// Specific queries...
+2 -1
View File
@@ -69,7 +69,7 @@ public:
bool init(const std::string & dbUrl,
bool dbOverwritten = false,
const ParametersMap & parameters = ParametersMap(),
bool postInitEvents = true);
bool postInitClosingEvents = false);
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
int incrementMapId();
@@ -215,6 +215,7 @@ private:
int _lastGlobalLoopClosureChildId;
bool _memoryChanged; // False by default, become true when Memory::update() is called.
int _signaturesAdded;
bool _postInitClosingEvents;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id
+4 -4
View File
@@ -57,7 +57,7 @@ class RTABMAP_EXP Odometry
public:
virtual ~Odometry() {}
Transform process(SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0);
virtual void reset();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
bool isLargeEnoughTransform(const Transform & transform);
@@ -104,7 +104,7 @@ public:
OdometryBOW(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryBOW();
virtual void reset();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const std::multimap<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
const Memory * getMemory() const {return _memory;}
@@ -125,7 +125,7 @@ public:
OdometryOpticalFlow(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryOpticalFlow();
virtual void reset();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const cv::Mat & getLastFrame() const {return lastFrame_;}
const std::vector<cv::Point2f> & getLastCorners() const {return lastCorners_;}
@@ -168,7 +168,7 @@ public:
float maxFitness = 0.01f,
bool pointToPlane = true,
const ParametersMap & odometryParameter = rtabmap::ParametersMap());
void reset();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
@@ -139,7 +139,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Rtabmap, DetectionRate, float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
RTABMAP_PARAM(Rtabmap, ImageBufferSize, int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory.");
RTABMAP_PARAM_STR(Rtabmap, DatabasePath, Parameters::getDefaultDatabasePath(), "Database path.");
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
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.");
@@ -354,7 +353,6 @@ public:
private:
Parameters();
static std::string getDefaultWorkingDirectory();
static std::string getDefaultDatabasePath();
private:
static ParametersMap parameters_;
+3 -5
View File
@@ -65,13 +65,12 @@ public:
bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
bool process(const SensorData & data); // for convenience
void init(const ParametersMap & param, bool deleteMemory = true);
void init(const std::string & configFile = "", bool deleteMemory = true);
void init(const ParametersMap & parameters, const std::string & databasePath = "");
void init(const std::string & configFile = "", const std::string & databasePath = "");
void close();
const std::string & getWorkingDir() const {return _wDir;}
std::string getDatabasePath() const;
int getLoopClosureId() const;
int getRetrievedId() const;
int getLastLocationId() const;
@@ -98,12 +97,11 @@ public:
void triggerNewMap();
void generateDOTGraph(const std::string & path, int id=0, int margin=5);
void generateTOROGraph(const std::string & path, bool optimized, bool global);
void resetMemory(bool dbOverwritten = false);
void resetMemory();
void dumpPrediction() const;
void dumpData() const;
void parseParameters(const ParametersMap & parameters);
void setWorkingDirectory(std::string path);
void setDatabasePath(const std::string & path);
void deleteLocation(int locationId); // Only nodes in STM can be deleted
void rejectLoopClosure(int oldId, int newId);
void get3DMap(std::map<int, Signature> & signatures,
+11 -18
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UEvent.h>
#include "rtabmap/core/Statistics.h"
#include "rtabmap/core/Parameters.h"
namespace rtabmap
{
@@ -57,14 +58,15 @@ class RtabmapEventCmd : public UEvent
public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
enum Cmd {
kCmdInit,
kCmdResetMemory,
kCmdClose,
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateDOTGraph, // params: path
kCmdGenerateDOTLocalGraph, // params: path, id, margin
kCmdGenerateTOROGraphLocal, // params: path, optimized
kCmdGenerateTOROGraphGlobal, // params: path, optimized
kCmdDeleteMemory, // params: path [optional]
kCmdCleanDataBuffer,
kCmdPublish3DMapLocal, // params: optimized
kCmdPublish3DMapGlobal, // params: optimized
@@ -73,26 +75,12 @@ public:
kCmdTriggerNewMap,
kCmdPause};
public:
RtabmapEventCmd(Cmd cmd) :
UEvent(0),
_cmd(cmd),
_strValue(""),
_intValue(0){}
RtabmapEventCmd(Cmd cmd, int value) :
UEvent(0),
_cmd(cmd),
_strValue(""),
_intValue(value){}
RtabmapEventCmd(Cmd cmd, const std::string & value) :
UEvent(0),
_cmd(cmd),
_strValue(value),
_intValue(0){}
RtabmapEventCmd(Cmd cmd, const std::string & strValue, int intValue) :
RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
UEvent(0),
_cmd(cmd),
_strValue(strValue),
_intValue(intValue){}
_intValue(intValue),
_parameters(parameters){}
virtual ~RtabmapEventCmd() {}
Cmd getCmd() const {return _cmd;}
@@ -103,12 +91,15 @@ public:
void setInt(int v) {_intValue = v;}
int getInt() const {return _intValue;}
const ParametersMap & getParameters() const {return _parameters;}
virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
private:
Cmd _cmd;
std::string _strValue;
int _intValue;
ParametersMap _parameters;
};
class RtabmapEventInit : public UEvent
@@ -118,6 +109,8 @@ public:
enum Status {
kInitializing,
kInitialized,
kClosing,
kClosed,
kInfo,
kError
};
+2 -3
View File
@@ -53,9 +53,10 @@ class RTABMAP_EXP RtabmapThread :
{
public:
enum State {
kStateIdle,
kStateInit,
kStateDetecting,
kStateReseting,
kStateClose,
kStateChangingParameters,
kStateDumpingMemory,
kStateDumpingPrediction,
@@ -63,13 +64,11 @@ public:
kStateGeneratingDOTLocalGraph,
kStateGeneratingTOROGraphLocal,
kStateGeneratingTOROGraphGlobal,
kStateDeletingMemory,
kStateCleanDataBuffer,
kStatePublishingMapLocal,
kStatePublishingMapGlobal,
kStatePublishingTOROGraphLocal,
kStatePublishingTOROGraphGlobal,
Graph,
kStateTriggeringMap
};
+1
View File
@@ -140,6 +140,7 @@ public:
SensorData toSensorData();
void uncompressData();
void uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const;
private:
int _id;
+2 -1
View File
@@ -71,12 +71,13 @@ class RTABMAP_EXP CompressionThread : public UThread
public:
// format : ".png" ".jpg" "" (empty is general)
CompressionThread(const cv::Mat & mat, const std::string & format = "");
CompressionThread(const std::vector<unsigned char> & bytes, bool isImage);
CompressionThread(const std::vector<unsigned char> * bytes, bool isImage);
const std::vector<unsigned char> & getCompressedData() const {return compressedData_;}
cv::Mat & getUncompressedData() {return uncompressedData_;}
protected:
virtual void mainLoop();
private:
const std::vector<unsigned char> * constCompressedData_;
std::vector<unsigned char> compressedData_;
cv::Mat uncompressedData_;
std::string format_;
+7 -1
View File
@@ -295,7 +295,9 @@ void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::loadSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
void DBDriver::loadSignatures(const std::list<int> & signIds,
std::list<Signature *> & signatures,
std::set<int> * loadedFromTrash)
{
UDEBUG("");
// look up in the trash before the database
@@ -326,6 +328,10 @@ void DBDriver::loadSignatures(const std::list<int> & signIds, std::list<Signatur
}
if(valueFound)
{
if(loadedFromTrash)
{
loadedFromTrash->insert(*iter);
}
iter = ids.erase(iter);
}
else
+3 -3
View File
@@ -220,9 +220,9 @@ void DBReader::getNextImage(
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctDepth2D(depth2dBytes, false);
util3d::CompressionThread ctImage(&imageBytes, true);
util3d::CompressionThread ctDepth(&depthBytes, true);
util3d::CompressionThread ctDepth2D(&depth2dBytes, false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
+48 -31
View File
@@ -71,6 +71,7 @@ Memory::Memory(const ParametersMap & parameters) :
_lastGlobalLoopClosureChildId(0),
_memoryChanged(false),
_signaturesAdded(0),
_postInitClosingEvents(false),
_feature2D(new SURF(parameters)),
_featureType(Feature2D::kFeatureSurf),
@@ -114,16 +115,17 @@ Memory::Memory(const ParametersMap & parameters) :
this->parseParameters(parameters);
}
bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitEvents)
bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitClosingEvents)
{
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
_postInitClosingEvents = postInitClosingEvents;
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
UDEBUG("");
this->parseParameters(parameters);
bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
Parameters::parse(parameters, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
DBDriver * tmpDriver = 0;
if(!_memoryChanged)
{
@@ -134,7 +136,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
}
}
this->clear();
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
if(tmpDriver)
{
@@ -143,9 +145,9 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
if(_dbDriver)
{
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
_dbDriver->closeConnection();
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
}
if(_dbDriver == 0 && !dbUrl.empty())
@@ -157,18 +159,18 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
if(_dbDriver)
{
success = false;
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
if(_dbDriver->openConnection(dbUrl, dbOverwritten))
{
success = true;
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
// Load the last working memory...
std::list<Signature*> dbSignatures;
if(loadAllNodesInWM)
{
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
std::set<int> ids;
_dbDriver->getAllNodeIds(ids, true);
_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
@@ -176,7 +178,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
else
{
// load previous session working memory
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
_dbDriver->loadLastNodes(dbSignatures);
}
for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
@@ -199,7 +201,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
delete *iter;
}
}
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
// Assign the last signature
if(_stMem.size()>0)
@@ -217,7 +219,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
}
else
{
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
}
}
else
@@ -235,7 +237,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
// Now load the dictionary if we have a connection
if(_dbDriver && _dbDriver->isConnected())
{
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
if(loadAllNodesInWM)
{
// load all referenced words in working memory
@@ -268,10 +270,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
}
UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
_vwd->update();
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
}
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
// Enable loaded signatures
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
@@ -290,7 +292,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
s->setEnabled(true);
}
}
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
if(_vwd->getUnusedWordsSize())
{
@@ -298,34 +300,47 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
}
UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
return success;
}
Memory::~Memory()
{
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
UDEBUG("");
if(!_memoryChanged)
{
UDEBUG("");
if(_dbDriver)
{
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
_dbDriver->closeConnection();
delete _dbDriver;
_dbDriver = 0;
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
}
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
this->clear();
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
}
else
{
UDEBUG("");
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
this->clear();
if(_dbDriver)
{
_dbDriver->emptyTrashes();
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
_dbDriver->closeConnection();
delete _dbDriver;
_dbDriver = 0;
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
}
else
{
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
}
}
@@ -337,6 +352,7 @@ Memory::~Memory()
{
delete _vwd;
}
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
}
void Memory::parseParameters(const ParametersMap & parameters)
@@ -2557,6 +2573,7 @@ std::vector<unsigned char> Memory::getImage(int signatureId) const
Signature Memory::getSignatureData(int locationId, bool uncompressedData)
{
UDEBUG("");
Signature r;
Signature * s = this->_getSignature(locationId);
if(s && s->getImage().size())
@@ -2578,7 +2595,8 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
std::list<int> ids;
ids.push_back(locationId);
std::list<Signature*> signatures;
_dbDriver->loadSignatures(ids, signatures);
std::set<int> loadedFromTrash;
_dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
if(signatures.size())
{
Signature * sTmp = signatures.front();
@@ -2587,10 +2605,19 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
}
r = *sTmp;
this->moveToTrash(s);
if(loadedFromTrash.size())
{
//put it back to trash
_dbDriver->asyncSave(sTmp);
}
else
{
delete sTmp;
}
}
}
}
UDEBUG("");
if(uncompressedData && r.getImageRaw().empty() && r.getImage().size())
{
@@ -2604,20 +2631,10 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
}
else
{
util3d::CompressionThread ctImage(r.getImage(), true);
util3d::CompressionThread ctDepth(r.getDepth(), true);
util3d::CompressionThread ctDepth2D(r.getDepth2D(), false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
ctImage.join();
ctDepth.join();
ctDepth2D.join();
r.setImageRaw(ctImage.getUncompressedData());
r.setDepthRaw(ctDepth.getUncompressedData());
r.setDepth2DRaw(ctDepth2D.getUncompressedData());
r.uncompressData();
}
}
UDEBUG("");
return r;
}
+10 -10
View File
@@ -84,10 +84,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
}
void Odometry::reset()
void Odometry::reset(const Transform & initialPose)
{
_resetCurrentCount = 0;
_pose = Transform::getIdentity();
_pose = initialPose;
}
bool Odometry::isLargeEnoughTransform(const Transform & transform)
@@ -193,7 +193,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
}
_memory = new Memory(customParameters);
if(!_memory->init("", false, ParametersMap(), false))
if(!_memory->init("", false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
@@ -205,10 +205,10 @@ OdometryBOW::~OdometryBOW()
}
void OdometryBOW::reset()
void OdometryBOW::reset(const Transform & initialPose)
{
Odometry::reset();
_memory->init("", false, ParametersMap(), false);
Odometry::reset(initialPose);
_memory->init("", false, ParametersMap());
localMap_.clear();
}
@@ -471,9 +471,9 @@ OdometryOpticalFlow::~OdometryOpticalFlow()
}
void OdometryOpticalFlow::reset()
void OdometryOpticalFlow::reset(const Transform & initialPose)
{
Odometry::reset();
Odometry::reset(initialPose);
lastFrame_ = cv::Mat();
lastCorners_.clear();
lastCorners3D_->clear();
@@ -1063,9 +1063,9 @@ OdometryICP::OdometryICP(int decimation,
{
}
void OdometryICP::reset()
void OdometryICP::reset(const Transform & initialPose)
{
Odometry::reset();
Odometry::reset(initialPose);
_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
}
-5
View File
@@ -65,11 +65,6 @@ std::string Parameters::getDefaultWorkingDirectory()
return path;
}
std::string Parameters::getDefaultDatabasePath()
{
return getDefaultWorkingDirectory() + getDefaultDatabaseName();
}
std::string Parameters::getDefaultDatabaseName()
{
return "rtabmap.db";
+30 -58
View File
@@ -246,31 +246,25 @@ void Rtabmap::flushStatisticLogs()
}
}
void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
void Rtabmap::init(const ParametersMap & parameters, const std::string & databasePath)
{
if(deleteMemory)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
// Set the working directory and database path before
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
this->setWorkingDirectory(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapDatabasePath())) != parameters.end())
{
this->setDatabasePath(iter->second.c_str());
}
Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
this->resetMemory(true);
this->setWorkingDirectory(iter->second.c_str());
}
_databasePath = databasePath;
if(_databasePath.empty())
{
_databasePath = _wDir + Parameters::getDefaultDatabaseName();
}
UINFO("Using database \"%s\".", _databasePath.c_str());
this->parseParameters(parameters);
setupLogFiles();
}
void Rtabmap::init(const std::string & configFile, bool deleteMemory)
void Rtabmap::init(const std::string & configFile, const std::string & databasePath)
{
// fill ctrl struct with values from the configuration file
ParametersMap param;// = Parameters::defaultParameters;
@@ -281,7 +275,7 @@ void Rtabmap::init(const std::string & configFile, bool deleteMemory)
this->readParameters(configFile, param);
}
this->init(param, deleteMemory);
this->init(param, databasePath);
}
void Rtabmap::close()
@@ -313,20 +307,23 @@ void Rtabmap::close()
delete _bayesFilter;
_bayesFilter = 0;
}
_databasePath.clear();
}
void Rtabmap::parseParameters(const ParametersMap & parameters)
{
if(_databasePath.empty())
{
UERROR("RTAB-Map is not initialized. Call rtabmap::init() instead.");
return;
}
ULOGGER_DEBUG("");
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
this->setWorkingDirectory(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapDatabasePath())) != parameters.end())
{
this->setDatabasePath(iter->second.c_str());
}
Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
@@ -380,7 +377,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
if(!_memory)
{
_memory = new Memory(parameters);
_memory->init(getDatabasePath(), false, parameters);
_memory->init(_databasePath, false, parameters, true);
//generate map
if(_rgbdSlamMode && _memory->getLastWorkingSignature())
@@ -429,11 +426,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
}
}
std::string Rtabmap::getDatabasePath() const
{
return _databasePath.empty()?_wDir + Parameters::getDefaultDatabaseName():_databasePath;
}
int Rtabmap::getLoopClosureId() const
{
return _lcHypothesisId;
@@ -647,7 +639,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
}
}
void Rtabmap::resetMemory(bool dbOverwritten)
void Rtabmap::resetMemory()
{
_retrievedId = 0;
_lcHypothesisValue = 0;
@@ -660,7 +652,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
if(_memory)
{
_memory->init(getDatabasePath(), dbOverwritten);
_memory->init(_databasePath, true, ParametersMap(), true);
if(_memory->getLastWorkingSignature())
{
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
@@ -670,13 +662,11 @@ void Rtabmap::resetMemory(bool dbOverwritten)
_bayesFilter->reset();
}
}
else if(dbOverwritten)
else
{
// May be memory should be already created here, and use init above...
UINFO("Erasing file : \"%s\"", getDatabasePath().c_str());
UFile::erase(getDatabasePath());
UERROR("RTAB-Map is not initialized. No memory to reset...");
}
this->setupLogFiles(dbOverwritten);
this->setupLogFiles(true);
}
//============================================================
@@ -1803,28 +1793,8 @@ void Rtabmap::setWorkingDirectory(std::string path)
}
}
void Rtabmap::setDatabasePath(const std::string & path)
{
if(!path.empty())
{
ULOGGER_DEBUG("Comparing new database path \"%s\" with \"%s\"", path.c_str(), _databasePath.c_str());
if(path.compare(_databasePath) != 0)
{
UDEBUG("Set new database path to \"%s\"", _databasePath.c_str());
_databasePath = path;
if(_memory)
{
UDEBUG("Reset memory!");
this->resetMemory();
}
}
}
else
{
ULOGGER_ERROR("Cannot set null database path!");
}
}
void Rtabmap::deleteLocation(int locationId)
{
if(_memory)
@@ -2238,10 +2208,12 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
UDEBUG("");
Signature data = _memory->getSignatureData(*iter);
UDEBUG("");
if(data.id() != Memory::kIdInvalid)
{
signatures.insert(std::make_pair(*iter, data));
signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
}
}
+38 -21
View File
@@ -168,6 +168,10 @@ void RtabmapThread::mainLoop()
case kStateDetecting:
this->process();
break;
case kStateInit:
UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
_rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath"));
break;
case kStateChangingParameters:
Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
@@ -179,6 +183,14 @@ void RtabmapThread::mainLoop()
_rtabmap->resetMemory();
this->clearBufferedData();
break;
case kStateClose:
if(_dataBuffer.size())
{
UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
this->clearBufferedData();
}
_rtabmap->close();
break;
case kStateDumpingMemory:
_rtabmap->dumpData();
break;
@@ -197,14 +209,6 @@ void RtabmapThread::mainLoop()
case kStateGeneratingTOROGraphGlobal:
_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStateDeletingMemory:
if(!parameters.at("path").empty())
{
_rtabmap->setDatabasePath(parameters.at("path"));
}
_rtabmap->resetMemory(true);
this->clearBufferedData();
break;
case kStateCleanDataBuffer:
this->clearBufferedData();
break;
@@ -254,7 +258,20 @@ void RtabmapThread::handleEvent(UEvent* event)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
if(cmd == RtabmapEventCmd::kCmdResetMemory)
if(cmd == RtabmapEventCmd::kCmdInit)
{
ULOGGER_DEBUG("CMD_INIT");
ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
UASSERT(!rtabmapEvent->getStr().empty());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
pushNewState(kStateInit, parameters);
}
else if(cmd == RtabmapEventCmd::kCmdClose)
{
ULOGGER_DEBUG("CMD_CLOSE");
pushNewState(kStateClose);
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
pushNewState(kStateReseting);
@@ -313,13 +330,6 @@ void RtabmapThread::handleEvent(UEvent* event)
pushNewState(kStateGeneratingTOROGraphGlobal, param);
}
else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
{
ULOGGER_DEBUG("CMD_DELETE_MEMORY");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
pushNewState(kStateDeletingMemory, param);
}
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
@@ -384,12 +394,19 @@ void RtabmapThread::process()
getData(data);
if(data.isValid())
{
_rtabmap->process(data);
if(_rtabmap->getMemory())
{
_rtabmap->process(data);
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
ULOGGER_DEBUG("posting statistics_ event...");
this->post(new RtabmapEvent(stats));
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
ULOGGER_DEBUG("posting statistics_ event...");
this->post(new RtabmapEvent(stats));
}
else
{
UERROR("RTAB-Map is not initialized! Ignoring received data...");
}
}
}
+55 -4
View File
@@ -88,7 +88,7 @@ Signature::Signature(
Signature::~Signature()
{
ULOGGER_DEBUG("id=%d", _id);
//ULOGGER_DEBUG("id=%d", _id);
}
void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
@@ -247,9 +247,9 @@ void Signature::uncompressData()
if(_imageRaw.empty() && _image.size())
{
//uncompress data
util3d::CompressionThread ctImage(_image, true);
util3d::CompressionThread ctDepth(_depth, true);
util3d::CompressionThread ctDepth2D(_depth2D, false);
util3d::CompressionThread ctImage(&_image, true);
util3d::CompressionThread ctDepth(&_depth, true);
util3d::CompressionThread ctDepth2D(&_depth2D, false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
@@ -262,4 +262,55 @@ void Signature::uncompressData()
}
}
void Signature::uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const
{
if(image)
{
*image = _imageRaw;
}
if(depth)
{
*depth = _depthRaw;
}
if(depth2D)
{
*depth2D = _depth2DRaw;
}
if( (image && image->empty()) ||
(depth && depth->empty()) ||
(depth2D && depth2D->empty()))
{
util3d::CompressionThread ctImage(&_image, true);
util3d::CompressionThread ctDepth(&_depth, true);
util3d::CompressionThread ctDepth2D(&_depth2D, false);
if(image && image->empty())
{
ctImage.start();
}
if(depth && depth->empty())
{
ctDepth.start();
}
if(depth2D && depth2D->empty())
{
ctDepth2D.start();
}
ctImage.join();
ctDepth.join();
ctDepth2D.join();
if(image)
{
*image = ctImage.getUncompressedData();
}
if(depth)
{
*depth = ctDepth.getUncompressedData();
}
if(depth2D)
{
*depth2D = ctDepth2D.getUncompressedData();
}
}
}
} //namespace rtabmap
+6 -5
View File
@@ -66,6 +66,7 @@ namespace util3d
// format : ".png" ".jpg" "" (empty is general)
CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
constCompressedData_(0),
uncompressedData_(mat),
format_(format),
image_(!format.empty()),
@@ -74,8 +75,8 @@ CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & fo
UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
}
// assume image
CompressionThread::CompressionThread(const std::vector<unsigned char> & bytes, bool isImage) :
compressedData_(bytes),
CompressionThread::CompressionThread(const std::vector<unsigned char> * bytes, bool isImage) :
constCompressedData_(bytes),
image_(isImage),
compressMode_(false)
{}
@@ -97,15 +98,15 @@ void CompressionThread::mainLoop()
}
else // uncompress
{
if(!compressedData_.empty())
if(constCompressedData_ && constCompressedData_->size())
{
if(image_)
{
uncompressedData_ = uncompressImage(compressedData_);
uncompressedData_ = uncompressImage(*constCompressedData_);
}
else
{
uncompressedData_ = uncompressData(compressedData_);
uncompressedData_ = uncompressData(*constCompressedData_);
}
}
}
+2 -1
View File
@@ -1,5 +1,6 @@
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/utilite/include
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
@@ -12,7 +13,7 @@ SET(LIBRARIES
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(bow_mapping main.cpp)
TARGET_LINK_LIBRARIES(bow_mapping rtabmap_core ${LIBRARIES})
TARGET_LINK_LIBRARIES(bow_mapping rtabmap_core rtabmap_utilite ${LIBRARIES})
SET_TARGET_PROPERTIES( bow_mapping
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping)
+8 -1
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Camera.h"
#include <opencv2/core/core.hpp>
#include "rtabmap/utilite/UFile.h"
#include <stdio.h>
void showUsage()
@@ -96,15 +97,21 @@ int main(int argc, char * argv[])
// Appearance-based only, disable RGB-D mode
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false"));
std::string databasePath = rtabmap::Parameters::defaultRtabmapWorkingDirectory()+rtabmap::Parameters::getDefaultDatabaseName();
if(localizationMode)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));
}
else
{
// delete previous database if there's one...
UFile::erase(databasePath);
}
// Initialize rtabmap: delete/create database...
rtabmap.init(parameters, !localizationMode);
rtabmap.init(parameters, databasePath);
// Process each image of the directory...
printf("\nProcessing images... from directory \"%s\"\n", path.c_str());
+10 -2
View File
@@ -72,6 +72,10 @@ class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler
public:
enum State {
kIdle,
kInitializing,
kInitialized,
kApplicationClosing,
kClosing,
kStartingDetection,
kDetecting,
kPaused,
@@ -108,6 +112,10 @@ protected:
private slots:
void changeState(MainWindow::State state);
void beep();
void newDatabase();
void openDatabase();
void closeDatabase();
void editDatabase();
void startDetection();
void pauseDetection();
void stopDetection();
@@ -139,7 +147,6 @@ private slots:
void takeScreenshot();
void updateElapsedTime();
void processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize);
void applyAllPrefSettings();
void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
void applyPrefSettings(const rtabmap::ParametersMap & parameters);
void processRtabmapEventInit(int status, const QString & info);
@@ -185,7 +192,7 @@ signals:
private:
void update3DMapVisibility(bool cloudsShown, bool scansShown);
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose, const std::multimap<int, Link> & constraints);
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose, const std::multimap<int, Link> & constraints, bool verboseProgress = false);
void createAndAddCloudToMap(int nodeId, const Transform & pose);
void createAndAddScanToMap(int nodeId, const Transform & pose);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
@@ -247,6 +254,7 @@ private:
int _lastId;
bool _processingStatistics;
bool _odometryReceived;
QString _openedDatabasePath;
QMap<int, Signature> _cachedSignatures;
QMap<int, int> _mapIds;
@@ -150,7 +150,6 @@ public:
double getGridMapOpacity() const;
QString getWorkingDirectory() const;
QString getDatabasePath() const;
// source panel
double getGeneralInputRate() const;
@@ -228,7 +227,6 @@ private slots:
void addParameter(const QString & value);
void updatePredictionPlot();
void updateKpROI();
void changeDatabasePath();
void changeWorkingDirectory();
void changeDictionaryPath();
void readSettingsEnd();
+1 -1
View File
@@ -66,7 +66,7 @@ bool DataRecorder::init(const QString & path, bool recordInRAM)
customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // to keep images
}
memory_ = new Memory();
if(!memory_->init(path.toStdString(), true, customParameters, false))
if(!memory_->init(path.toStdString(), true, customParameters))
{
delete memory_;
memory_ = 0;
+9
View File
@@ -268,6 +268,15 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
{
event->accept();
}
if(event->isAccepted())
{
if(memory_)
{
delete memory_;
memory_ = 0;
}
}
}
void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
+2 -1
View File
@@ -80,7 +80,7 @@ void LoopClosureViewer::updateView(const Transform & transform)
}
UDEBUG("decimation = %d", decimation);
UDEBUG("maxDepth = %d", maxDepth);
UDEBUG("maxDepth = %f", maxDepth);
UDEBUG("samples = %d", samples);
Transform t;
@@ -194,6 +194,7 @@ void LoopClosureViewer::updateView(const Transform & transform)
}
else
{
UERROR("loop transform is null !?!?");
ui_->cloudViewerTransform->removeAllClouds();
}
ui_->cloudViewerTransform->render();
+317 -106
View File
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/ImageView.h"
#include "rtabmap/gui/KeypointItem.h"
#include "rtabmap/gui/DataRecorder.h"
#include "rtabmap/gui/DatabaseViewer.h"
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/ULogger.h>
@@ -114,6 +115,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_lastId(0),
_processingStatistics(false),
_odometryReceived(false),
_openedDatabasePath(""),
_odometryCorrection(Transform::getIdentity()),
_lastOdometryProcessed(true),
_oneSecondTimer(0),
@@ -241,6 +243,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(a, SIGNAL(triggered(bool)), _initProgressDialog, SLOT(show()));
// connect actions with custom slots
connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
connect(_ui->actionEdit_database, SIGNAL(triggered()), this, SLOT(editDatabase()));
connect(_ui->actionStart, SIGNAL(triggered()), this, SLOT(startDetection()));
connect(_ui->actionPause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
@@ -329,7 +335,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
connect(_ui->actionApply_settings_to_the_detector, SIGNAL(triggered()), this, SLOT(applyAllPrefSettings()));
// more connects...
connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
@@ -356,6 +361,26 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
{
const std::map<std::string, float> & statistics = Statistics::defaultData();
for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
{
_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), 0, (*iter).second);
}
}
// update loop closure viewer parameters
ParametersMap parameters = _preferencesDialog->getAllParameters();
_ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
_ui->widget_loopClosureViewer->setMaxDepth(atof(parameters.at(Parameters::kLccIcp3MaxDepth()).c_str()));
_ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
//update ui
_ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
_ui->doubleSpinBox_stats_timeLimit->setValue(_preferencesDialog->getTimeLimit());
_ui->actionSLAM_mode->setChecked(_preferencesDialog->isSLAMMode());
splash.close();
this->setFocus();
@@ -364,10 +389,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
MainWindow::~MainWindow()
{
UDEBUG("");
if(_state != kIdle)
{
this->stopDetection();
}
this->stopDetection();
delete _ui;
delete _elapsedTime;
}
@@ -399,6 +421,11 @@ void MainWindow::closeEvent(QCloseEvent* event)
if(_state != kIdle && _state != kMonitoring && _state != kMonitoringPaused)
{
this->stopDetection();
if(_state == kInitialized)
{
this->closeDatabase();
this->changeState(kApplicationClosing);
}
if(_state != kIdle)
{
processStopped = false;
@@ -698,8 +725,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
// update cache
Signature & signature = *_cachedSignatures.insert(stat.getSignature().id(), stat.getSignature());
signature.uncompressData(); // make sure data are already uncompressed
Signature signature = stat.getSignature();
signature.uncompressData(); // make sure data are uncompressed
_cachedSignatures.insert(stat.getSignature().id(), signature);
UDEBUG("");
// map ids
@@ -736,6 +764,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
int matchId = 0;
cv::Mat loopImage;
cv::Mat loopDepth;
cv::Mat loopDepth2D;
int shownLoopId = 0;
if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
{
@@ -782,8 +811,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
if(iter != _cachedSignatures.end())
{
loopImage = iter.value().getImageRaw();
loopDepth = iter.value().getDepthRaw();
iter.value().uncompressData(&loopImage, &loopDepth, &loopDepth2D);
}
}
}
@@ -933,14 +961,20 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
loopOldId = stat.localLoopClosureId();
}
int loopNewId = stat.refImageId();
QMap<int, Signature>::iterator newIter = _cachedSignatures.find(loopNewId);
QMap<int, Signature>::iterator oldIter = _cachedSignatures.find(loopOldId);
if(newIter!=_cachedSignatures.end() && oldIter!=_cachedSignatures.end())
if(oldIter!=_cachedSignatures.end())
{
_ui->widget_loopClosureViewer->setData(*oldIter, *newIter);
Signature old = oldIter.value();
if(old.getImageRaw().empty())
{
old.setImageRaw(loopImage);
old.setDepthRaw(loopDepth);
old.setDepth2DRaw(loopDepth2D);
}
signature.setPose(loopClosureTransform);
_ui->widget_loopClosureViewer->setData(*oldIter, signature);
if(_ui->dockWidget_loopClosureViewer->isVisible())
{
UTimer loopTimer;
@@ -973,7 +1007,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
void MainWindow::updateMapCloud(
const std::map<int, Transform> & posesIn,
const Transform & currentPose,
const std::multimap<int, Link> & constraints)
const std::multimap<int, Link> & constraints,
bool verboseProgress)
{
if(posesIn.size())
{
@@ -1028,6 +1063,7 @@ void MainWindow::updateMapCloud(
// Map updated! regenerate the assembled cloud, last pose is the new one
UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str());
QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
int i=1;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(!iter->second.isNull())
@@ -1056,7 +1092,7 @@ void MainWindow::updateMapCloud(
else if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(!jter->getImageRaw().empty() && !jter->getDepthRaw().empty())
if(jter->getImage().size() && jter->getDepth().size())
{
this->createAndAddCloudToMap(iter->first, iter->second);
}
@@ -1091,7 +1127,7 @@ void MainWindow::updateMapCloud(
else if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(!jter->getDepth2DRaw().empty())
if(jter->getDepth2D().size())
{
this->createAndAddScanToMap(iter->first, iter->second);
}
@@ -1107,11 +1143,20 @@ void MainWindow::updateMapCloud(
UDEBUG("Hide scan %s", scanName.c_str());
_ui->widget_cloudViewer->setCloudVisibility(scanName.c_str(), false);
}
if(verboseProgress)
{
_initProgressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
}
}
else
{
UERROR("transform is null!?");
}
++i;
}
//remove not used clouds
for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
@@ -1224,10 +1269,13 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
return;
}
cv::Mat image, depth;
iter->uncompressData(&image, &depth, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(nodeId,
iter->getImageRaw(),
iter->getDepthRaw(),
image,
depth,
iter->getDepthFx(),
iter->getDepthFy(),
iter->getDepthCx(),
@@ -1327,9 +1375,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
UERROR("Node %d is not in the cache.", nodeId);
return;
}
cv::Mat depth2D;
iter->uncompressData(0, 0, &depth2D);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::depth2DToPointCloud(iter->getDepth2DRaw());
cloud = util3d::depth2DToPointCloud(depth2D);
QColor color = Qt::red;
int mapId = _mapIds.value(nodeId, -1);
if(mapId >= 0)
@@ -1399,10 +1449,50 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
this->changeState(MainWindow::kInitializing);
}
else if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitialized)
{
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
this->changeState(MainWindow::kInitialized);
}
else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosing)
{
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
if(_state!=kApplicationClosing)
{
this->changeState(MainWindow::kClosing);
}
}
else if((RtabmapEventInit::Status)status == RtabmapEventInit::kClosed)
{
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
if(_openedDatabasePath.compare(_preferencesDialog->getWorkingDirectory()+"/"+Parameters::getDefaultDatabaseName().c_str()) == 0)
{
// Temp database used, automatically backup with unique name (timestamp)
QString newName = _preferencesDialog->getWorkingDirectory()+(QString("/rtabmap_%1.db").arg(QDateTime::currentDateTime().toString("yyMMddhhmmss")));
if(QFile::rename(_openedDatabasePath, newName))
{
std::string msg = uFormat("Database saved to \"%s\".", newName.toStdString().c_str());
UINFO(msg.c_str());
QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
}
else
{
std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".", _openedDatabasePath.toStdString().c_str(), newName.toStdString().c_str());
UERROR(msg.c_str());
QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
}
}
_openedDatabasePath.clear();
bool applicationClosing = _state == kApplicationClosing;
this->changeState(MainWindow::kIdle);
if(applicationClosing)
{
this->close();
}
}
else
{
@@ -1410,10 +1500,16 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
QString msg(info);
if((RtabmapEventInit::Status)status == RtabmapEventInit::kError)
{
_openedDatabasePath.clear();
_initProgressDialog->setAutoClose(false);
msg.prepend(tr("[ERROR] "));
_initProgressDialog->appendText(msg);
this->changeState(MainWindow::kIdle);
}
else
{
_initProgressDialog->appendText(msg);
}
_initProgressDialog->appendText(msg);
}
}
@@ -1437,8 +1533,9 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
UINFO(" poses = %d", event.getPoses().size());
UINFO(" constraints = %d", event.getConstraints().size());
_initProgressDialog->setMaximumSteps(event.getSignatures().size());
_initProgressDialog->setMaximumSteps(event.getSignatures().size()+event.getPoses().size()+1);
_initProgressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
QApplication::processEvents();
int addedSignatures = 0;
for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
@@ -1447,17 +1544,15 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
{
if(!_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::iterator inserted = _cachedSignatures.insert(iter->first, iter->second);
//uncompress data if required
if(inserted->getImageRaw().empty() && inserted->getImage().size())
{
inserted->uncompressData();
++addedSignatures;
}
_cachedSignatures.insert(iter->first, iter->second);
++addedSignatures;
}
_initProgressDialog->incrementStep();
QApplication::processEvents();
}
_initProgressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
_initProgressDialog->incrementStep();
QApplication::processEvents();
for(std::map<int, int>::const_iterator iter = event.getMapIds().begin();
iter!=event.getMapIds().end();
@@ -1467,6 +1562,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
}
_initProgressDialog->appendText(tr("Inserted %1 map ids").arg(_mapIds.size()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
_initProgressDialog->appendText("Inserting data in the cache... done.");
@@ -1475,7 +1571,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_initProgressDialog->appendText("Updating the 3D map cloud...");
_initProgressDialog->incrementStep();
QApplication::processEvents();
this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints());
this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), true);
_initProgressDialog->appendText("Updating the 3D map cloud... done.");
}
else
@@ -1489,27 +1585,6 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
}
void MainWindow::applyAllPrefSettings()
{
ULOGGER_DEBUG("");
//This will update the statistics toolbox
_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
{
const std::map<std::string, float> & statistics = Statistics::defaultData();
for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
{
_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), 0, (*iter).second);
}
}
_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
this->applyPrefSettings(PreferencesDialog::kPanelAll);
this->applyPrefSettings(_preferencesDialog->getAllParameters());
}
void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
{
ULOGGER_DEBUG("");
@@ -1539,7 +1614,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
{
_dbReader->setFrameRate(_preferencesDialog->getGeneralInputRate());
}
}
}//This will update the statistics toolbox
if(flags & PreferencesDialog::kPanelGeneral)
{
@@ -1579,7 +1654,7 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
rtabmap::ParametersMap parametersModified = parameters;
if(_state != kIdle)
if(_state != kIdle && parametersModified.size())
{
if(parametersModified.erase(Parameters::kRtabmapWorkingDirectory()))
{
@@ -1592,23 +1667,15 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
QMessageBox::information(this, tr("Working memory changed"), tr("The working directory can't be changed while the detector is running. This will be applied when the detector will stop."));
}
}
if(parametersModified.erase(Parameters::kRtabmapDatabasePath()))
{
if(_state == kMonitoring || _state == kMonitoringPaused)
{
QMessageBox::information(this, tr("Database path changed"), tr("The remote database path can't be changed while the interface is in monitoring mode."));
}
else
{
QMessageBox::information(this, tr("Database path changed"), tr("The database path can't be changed while the detector is running. This will be applied when the detector will stop."));
}
}
this->post(new ParamEvent(parametersModified));
}
else if(parametersModified.size() == _preferencesDialog->getAllParameters().size())
if(_state != kMonitoring && _state != kMonitoringPaused &&
uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
{
// Send only if all parameters are sent
this->post(new ParamEvent(parametersModified));
_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
}
// update loop closure viewer parameters
@@ -1884,6 +1951,83 @@ void MainWindow::beep()
}
//ACTIONS
void MainWindow::newDatabase()
{
if(_state != MainWindow::kIdle)
{
UERROR("This method can be called only in IDLE state.");
return;
}
_openedDatabasePath.clear();
ULOGGER_DEBUG("");
this->clearTheCache();
std::string databasePath = _preferencesDialog->getWorkingDirectory().toStdString()+"/"+Parameters::getDefaultDatabaseName();
if(QFile::exists(databasePath.c_str()))
{
if(QFile::remove(databasePath.c_str()))
{
UINFO("Deleted database \"%s\".", databasePath.c_str());
}
else
{
UERROR("Cannot create a new database because the temporary database \"%s\" cannot be deleted.", databasePath.c_str());
return;
}
}
_openedDatabasePath = databasePath.c_str();
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, 0, _preferencesDialog->getAllParameters()));
}
void MainWindow::openDatabase()
{
if(_state != MainWindow::kIdle)
{
UERROR("This method can be called only in IDLE state.");
return;
}
_openedDatabasePath.clear();
QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
if(!path.isEmpty())
{
this->clearTheCache();
_openedDatabasePath = path;
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, path.toStdString(), 0, _preferencesDialog->getAllParameters()));
}
}
void MainWindow::closeDatabase()
{
if(_state != MainWindow::kInitialized)
{
UERROR("This method can be called only in INITIALIZED state.");
return;
}
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose));
}
void MainWindow::editDatabase()
{
if(_state != MainWindow::kIdle)
{
UERROR("This method can be called only in IDLE state.");
return;
}
QString path = QFileDialog::getOpenFileName(this, tr("Edit database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
if(!path.isEmpty())
{
DatabaseViewer * viewer = new DatabaseViewer(this);
viewer->setWindowModality(Qt::WindowModal);
if(viewer->openDatabase(path))
{
viewer->show();
}
else
{
delete viewer;
}
}
}
void MainWindow::startDetection()
{
ParametersMap parameters = _preferencesDialog->getAllParameters();
@@ -1945,7 +2089,7 @@ void MainWindow::startDetection()
tr("RTAB-Map"),
tr("A camera is running, stop it first."));
UWARN("_camera is not null... it must be stopped first");
emit stateChanged(kIdle);
emit stateChanged(kInitialized);
return;
}
if(_dbReader != 0)
@@ -1954,7 +2098,7 @@ void MainWindow::startDetection()
tr("RTAB-Map"),
tr("A database reader is running, stop it first."));
UWARN("_dbReader is not null... it must be stopped first");
emit stateChanged(kIdle);
emit stateChanged(kInitialized);
return;
}
@@ -1967,7 +2111,7 @@ void MainWindow::startDetection()
tr("RTAB-Map"),
tr("No sources are selected. See Preferences->Source panel."));
UWARN("No sources are selected. See Preferences->Source panel.");
emit stateChanged(kIdle);
emit stateChanged(kInitialized);
return;
}
@@ -2052,7 +2196,7 @@ void MainWindow::startDetection()
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Camera initialization failed..."));
emit stateChanged(kIdle);
emit stateChanged(kInitialized);
delete camera;
camera = 0;
if(_odomThread)
@@ -2105,7 +2249,7 @@ void MainWindow::startDetection()
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Database reader initialization failed..."));
emit stateChanged(kIdle);
emit stateChanged(kInitialized);
delete _dbReader;
_dbReader = 0;
if(_odomThread)
@@ -2161,7 +2305,7 @@ void MainWindow::startDetection()
QMessageBox::warning(this,
tr("RTAB-Map"),
tr("Camera initialization failed..."));
emit stateChanged(kIdle);
emit stateChanged(kInitialized);
delete camera;
camera = 0;
return;
@@ -2174,7 +2318,6 @@ void MainWindow::startDetection()
_lastOdomPose.setNull();
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCleanDataBuffer)); // clean sensors buffer
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdTriggerNewMap)); // Trigger a new map
this->applyAllPrefSettings();
if(_odomThread)
{
@@ -2229,7 +2372,7 @@ void MainWindow::pauseDetection()
void MainWindow::stopDetection()
{
if(_state == kIdle || (!_camera && !_dbReader))
if(!_camera && !_dbReader && !_odomThread)
{
return;
}
@@ -2280,7 +2423,7 @@ void MainWindow::stopDetection()
delete _odomThread;
_odomThread = 0;
}
emit stateChanged(kIdle);
emit stateChanged(kInitialized);
}
void MainWindow::printLoopClosureIds()
@@ -2424,12 +2567,11 @@ void MainWindow::generateTOROMap()
void MainWindow::deleteMemory()
{
QMessageBox::StandardButton button;
QString dbPath = _preferencesDialog->getDatabasePath();
if(_state == kMonitoring || _state == kMonitoringPaused)
{
button = QMessageBox::question(this,
tr("Deleting memory..."),
tr("The remote database file \"%1\" and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)").arg(dbPath),
tr("The remote database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
QMessageBox::Yes|QMessageBox::No,
QMessageBox::No);
}
@@ -2437,7 +2579,7 @@ void MainWindow::deleteMemory()
{
button = QMessageBox::question(this,
tr("Deleting memory..."),
tr("The database file \"%1\" (%2 MB) and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)").arg(dbPath).arg(UFile::length(dbPath.toStdString())/1000000),
tr("The current database and log files will be deleted. Are you sure you want to continue? (This cannot be reverted)"),
QMessageBox::Yes|QMessageBox::No,
QMessageBox::No);
}
@@ -2447,7 +2589,7 @@ void MainWindow::deleteMemory()
return;
}
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDeleteMemory, dbPath.toStdString()));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
this->clearTheCache();
}
@@ -2588,11 +2730,11 @@ void MainWindow::downloadAllClouds()
.arg(global?"true":"false").arg(optimized?"true":"false"));
if(global)
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapGlobal, optimized?1:0));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapGlobal, "", optimized?1:0));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapLocal, optimized?1:0));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapLocal, "", optimized?1:0));
}
}
}
@@ -2640,11 +2782,11 @@ void MainWindow::downloadPoseGraph()
.arg(global?"true":"false").arg(optimized?"true":"false"));
if(global)
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphGlobal, optimized?1:0));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphGlobal, "", optimized?1:0));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphLocal, optimized?1:0));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphLocal, "", optimized?1:0));
}
}
}
@@ -3721,18 +3863,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
{
if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::const_iterator jter = _cachedSignatures.find(iter->first);
const Signature & s = _cachedSignatures.find(iter->first).value();
cv::Mat image, depth;
s.uncompressData(&image, &depth, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(regenerateClouds)
{
cloud = createCloud(iter->first,
jter->getImageRaw(),
jter->getDepthRaw(),
jter->getDepthFx(),
jter->getDepthFy(),
jter->getDepthCx(),
jter->getDepthCy(),
jter->getLocalTransform(),
image,
depth,
s.getDepthFx(),
s.getDepthFy(),
s.getDepthCx(),
s.getDepthCy(),
s.getLocalTransform(),
iter->second,
regenerateVoxelSize,
regenerateDecimation,
@@ -3805,18 +3949,20 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
{
if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::const_iterator jter = _cachedSignatures.find(iter->first);
const Signature & s = _cachedSignatures.find(iter->first).value();
cv::Mat image, depth;
s.uncompressData(&image, &depth, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(regenerateClouds)
{
cloud = createCloud(iter->first,
jter->getImageRaw(),
jter->getDepthRaw(),
jter->getDepthFx(),
jter->getDepthFy(),
jter->getDepthCx(),
jter->getDepthCy(),
jter->getLocalTransform(),
image,
depth,
s.getDepthFx(),
s.getDepthFy(),
s.getDepthCx(),
s.getDepthCy(),
s.getLocalTransform(),
Transform::getIdentity(),
regenerateVoxelSize,
regenerateDecimation,
@@ -3872,7 +4018,57 @@ void MainWindow::changeState(MainWindow::State newState)
// TODO : To protect with mutex ?
switch (newState)
{
case kIdle:
case kIdle: // RTAB-Map is not initialized yet
_ui->actionNew_database->setEnabled(true);
_ui->actionOpen_database->setEnabled(true);
_ui->actionClose_database->setEnabled(false);
_ui->actionEdit_database->setEnabled(true);
_ui->actionStart->setEnabled(false);
_ui->actionPause->setEnabled(false);
_ui->actionPause->setChecked(false);
_ui->actionPause->setToolTip(tr("Pause"));
_ui->actionStop->setEnabled(false);
_ui->actionPause_on_match->setEnabled(true);
_ui->actionPause_on_local_loop_detection->setEnabled(true);
_ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
_ui->actionDump_the_memory->setEnabled(false);
_ui->actionDump_the_prediction_matrix->setEnabled(false);
_ui->actionDelete_memory->setEnabled(false);
_ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionData_recorder->setEnabled(false);
_ui->actionOpen_working_directory->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
_ui->actionTrigger_a_new_map->setEnabled(false);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
_ui->statusbar->clearMessage();
_state = newState;
_oneSecondTimer->stop();
break;
case kApplicationClosing:
case kClosing:
_ui->actionStart->setEnabled(false);
_ui->actionPause->setEnabled(false);
_ui->actionStop->setEnabled(false);
_state = newState;
break;
case kInitializing:
_ui->actionNew_database->setEnabled(false);
_ui->actionOpen_database->setEnabled(false);
_ui->actionEdit_database->setEnabled(false);
_state = newState;
break;
case kInitialized:
_ui->actionNew_database->setEnabled(false);
_ui->actionOpen_database->setEnabled(false);
_ui->actionClose_database->setEnabled(true);
_ui->actionEdit_database->setEnabled(false);
_ui->actionStart->setEnabled(true);
_ui->actionPause->setEnabled(false);
_ui->actionPause->setChecked(false);
@@ -3889,9 +4085,10 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
_ui->actionData_recorder->setEnabled(false);
_ui->actionOpen_working_directory->setEnabled(true);
_ui->actionApply_settings_to_the_detector->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setEnabled(true);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
_ui->statusbar->clearMessage();
_state = newState;
@@ -3900,6 +4097,7 @@ void MainWindow::changeState(MainWindow::State newState)
case kStartingDetection:
_ui->actionStart->setEnabled(false);
_state = newState;
break;
case kDetecting:
@@ -3919,9 +4117,10 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionData_recorder->setEnabled(true);
_ui->actionOpen_working_directory->setEnabled(true);
_ui->actionApply_settings_to_the_detector->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
_ui->statusbar->showMessage(tr("Detecting..."));
_state = newState;
@@ -3953,6 +4152,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_local_map->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_state = kDetecting;
_elapsedTime->start();
_oneSecondTimer->start();
@@ -3979,6 +4179,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_local_map->setEnabled(true);
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_state = kPaused;
_oneSecondTimer->stop();
@@ -3995,6 +4196,10 @@ void MainWindow::changeState(MainWindow::State newState)
}
break;
case kMonitoring:
_ui->actionNew_database->setVisible(false);
_ui->actionOpen_database->setVisible(false);
_ui->actionClose_database->setVisible(false);
_ui->actionEdit_database->setVisible(false);
_ui->actionStart->setVisible(false);
_ui->actionPause->setEnabled(true);
_ui->actionPause->setChecked(false);
@@ -4012,18 +4217,23 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_TORO_graph_graph->setVisible(false);
_ui->actionData_recorder->setVisible(false);
_ui->actionOpen_working_directory->setEnabled(false);
_ui->actionApply_settings_to_the_detector->setVisible(false);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setVisible(false);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setVisible(false);
_ui->doubleSpinBox_stats_imgRate_label->setVisible(false);
_ui->statusbar->showMessage(tr("Monitoring..."));
_state = newState;
_elapsedTime->start();
_oneSecondTimer->start();
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, 0));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 0));
break;
case kMonitoringPaused:
_ui->actionNew_database->setVisible(false);
_ui->actionOpen_database->setVisible(false);
_ui->actionClose_database->setVisible(false);
_ui->actionEdit_database->setVisible(false);
_ui->actionStart->setVisible(false);
_ui->actionPause->setToolTip(tr("Continue"));
_ui->actionPause->setChecked(true);
@@ -4041,15 +4251,16 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_TORO_graph_graph->setVisible(false);
_ui->actionData_recorder->setVisible(false);
_ui->actionOpen_working_directory->setEnabled(false);
_ui->actionApply_settings_to_the_detector->setVisible(false);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setVisible(false);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setVisible(false);
_ui->doubleSpinBox_stats_imgRate_label->setVisible(false);
_ui->statusbar->showMessage(tr("Monitoring paused..."));
_state = newState;
_oneSecondTimer->stop();
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, 1));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 1));
break;
default:
break;
+3 -1
View File
@@ -70,8 +70,10 @@ void PdfPlotItem::showDescription(bool shown)
{
QImage img;
QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
if(iter != _signaturesRef->constEnd() && !iter.value().getImageRaw().empty())
if(iter != _signaturesRef->constEnd() && !iter.value().getImage().empty())
{
cv::Mat image;
iter.value().uncompressData(&image, 0, 0);
img = uCvMat2QImage(iter.value().getImageRaw());
QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
_img = new QGraphicsPixmapItem(scaled, this);
-51
View File
@@ -268,8 +268,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->general_spinBox_imagesBufferSize, SIGNAL(valueChanged(int)), _ui->general_spinBox_imagesBufferSize_2, SLOT(setValue(int)));
connect(_ui->general_spinBox_maxStMemSize, SIGNAL(valueChanged(int)), _ui->general_spinBox_maxStMemSize_2, SLOT(setValue(int)));
connect(_ui->lineEdit_databasePath, SIGNAL(textChanged(const QString &)), _ui->lineEdit_databasePath_2, SLOT(setText(const QString &)));
connect(_ui->general_doubleSpinBox_timeThr_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->general_doubleSpinBox_hardThr_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->doubleSpinBox_similarityThreshold_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
@@ -283,9 +281,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->general_checkBox_SLAM_mode, SIGNAL(stateChanged(int)), this, SLOT(updateBasicParameter()));
connect(_ui->general_checkBox_SLAM_mode_2, SIGNAL(stateChanged(int)), this, SLOT(updateBasicParameter()));
connect(_ui->lineEdit_databasePath_2, SIGNAL(textChanged(const QString &)), _ui->lineEdit_databasePath, SLOT(setText(const QString &)));
connect(_ui->toolButton_databasePath_2, SIGNAL(clicked()), this, SLOT(changeDatabasePath()));
// Map objects name with the corresponding parameter key, needed for the addParameter() slots
//Rtabmap
_ui->groupBox_publishing->setObjectName(Parameters::kRtabmapPublishStats().c_str());
@@ -301,9 +296,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_spinBox_imagesBufferSize->setObjectName(Parameters::kRtabmapImageBufferSize().c_str());
_ui->general_spinBox_maxRetrieved->setObjectName(Parameters::kRtabmapMaxRetrieved().c_str());
_ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
_ui->lineEdit_databasePath->setObjectName(Parameters::kRtabmapDatabasePath().c_str());
_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
connect(_ui->toolButton_databasePath, SIGNAL(clicked()), this, SLOT(changeDatabasePath()));
connect(_ui->toolButton_workingDirectory, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));
// Memory
@@ -868,7 +861,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->general_checkBox_publishStats_2->setChecked(Parameters::defaultRtabmapPublishStats());
_ui->general_checkBox_activateRGBD_2->setChecked(Parameters::defaultRGBDEnabled());
_ui->general_checkBox_SLAM_mode_2->setChecked(Parameters::defaultMemIncrementalMemory());
_ui->lineEdit_databasePath_2->setText(Parameters::defaultRtabmapDatabasePath().c_str());
_ui->general_doubleSpinBox_detectionRate_2->setValue(Parameters::defaultRtabmapDetectionRate());
// match the advanced (spin and doubleSpin boxes)
_ui->general_doubleSpinBox_timeThr->setValue(Parameters::defaultRtabmapTimeThr());
@@ -944,11 +936,6 @@ QString PreferencesDialog::getWorkingDirectory() const
return _ui->lineEdit_workingDirectory->text();
}
QString PreferencesDialog::getDatabasePath() const
{
return _ui->lineEdit_databasePath->text();
}
QString PreferencesDialog::getIniFilePath() const
{
QString privatePath = QDir::homePath() + "/.rtabmap";
@@ -1492,13 +1479,6 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
if(_monitoringState)
{
// In monitoring state, you cannot change remote paths
_ui->lineEdit_databasePath->setEnabled(false);
_ui->lineEdit_databasePath_2->setEnabled(false);
_ui->toolButton_databasePath->setEnabled(false);
_ui->toolButton_databasePath_2->setEnabled(false);
_ui->label_databasePath->setEnabled(false);
_ui->label_databasePath_2->setEnabled(false);
_ui->lineEdit_workingDirectory->setEnabled(false);
_ui->toolButton_workingDirectory->setEnabled(false);
_ui->label_workingDirectory->setEnabled(false);
@@ -1514,13 +1494,6 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
}
else
{
_ui->lineEdit_databasePath->setEnabled(true);
_ui->lineEdit_databasePath_2->setEnabled(true);
_ui->toolButton_databasePath->setEnabled(true);
_ui->toolButton_databasePath_2->setEnabled(true);
_ui->label_databasePath->setEnabled(true);
_ui->label_databasePath_2->setEnabled(true);
_ui->lineEdit_workingDirectory->setEnabled(true);
_ui->toolButton_workingDirectory->setEnabled(true);
_ui->label_workingDirectory->setEnabled(true);
@@ -2468,30 +2441,6 @@ void PreferencesDialog::updateKpROI()
_ui->lineEdit_kp_roi->setText(strings.join(" "));
}
void PreferencesDialog::changeDatabasePath()
{
QFileDialog fileDialog;
fileDialog.setFileMode(QFileDialog::AnyFile);
fileDialog.setNameFilter(tr("RTAB-Map database files (*.db)"));
fileDialog.setDefaultSuffix("db");
fileDialog.setDirectory(QFileInfo(_ui->lineEdit_databasePath->text()).dir());
fileDialog.selectFile (_ui->lineEdit_databasePath->text());
QStringList fileNames;
QString path;
if(fileDialog.exec())
{
if(!fileDialog.selectedFiles().empty())
{
path = fileDialog.selectedFiles().first();
}
}
if(!path.isEmpty())
{
_ui->lineEdit_databasePath->setText(path);
}
}
void PreferencesDialog::changeWorkingDirectory()
{
QString directory = QFileDialog::getExistingDirectory(this, tr("Working directory"), _ui->lineEdit_workingDirectory->text());
+29 -9
View File
@@ -34,32 +34,37 @@
<property name="title">
<string>File</string>
</property>
<addaction name="actionNew_database"/>
<addaction name="actionOpen_database"/>
<addaction name="separator"/>
<addaction name="actionSave_point_cloud"/>
<addaction name="actionExport_2D_scans_ply_pcd"/>
<addaction name="actionExport_2D_Grid_map_bmp_png"/>
<addaction name="separator"/>
<addaction name="actionClose_database"/>
<addaction name="separator"/>
<addaction name="actionEdit_database"/>
<addaction name="separator"/>
<addaction name="actionExit"/>
</widget>
<widget class="QMenu" name="menuEdit">
<property name="title">
<string>Edit</string>
</property>
<addaction name="actionApply_settings_to_the_detector"/>
<addaction name="separator"/>
<addaction name="actionDownload_all_clouds"/>
<addaction name="actionDownload_graph"/>
<addaction name="actionClear_cache"/>
<addaction name="separator"/>
<addaction name="actionOpen_working_directory"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="separator"/>
<addaction name="actionDelete_memory"/>
<addaction name="actionClear_cache"/>
<addaction name="separator"/>
<addaction name="actionDump_the_memory"/>
<addaction name="actionDump_the_prediction_matrix"/>
<addaction name="actionGenerate_map"/>
<addaction name="actionGenerate_local_map"/>
<addaction name="actionGenerate_TORO_graph_graph"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="separator"/>
<addaction name="actionData_recorder"/>
<addaction name="separator"/>
@@ -777,11 +782,6 @@
<string>Stop</string>
</property>
</action>
<action name="actionApply_settings_to_the_detector">
<property name="text">
<string>Apply settings to detector</string>
</property>
</action>
<action name="actionDump_the_memory">
<property name="text">
<string>Dump the memory</string>
@@ -1068,6 +1068,26 @@
<string>OpenNI2</string>
</property>
</action>
<action name="actionOpen_database">
<property name="text">
<string>Open database...</string>
</property>
</action>
<action name="actionNew_database">
<property name="text">
<string>New database...</string>
</property>
</action>
<action name="actionClose_database">
<property name="text">
<string>Close database</string>
</property>
</action>
<action name="actionEdit_database">
<property name="text">
<string>Edit database...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
+28 -76
View File
@@ -65,7 +65,7 @@
<x>0</x>
<y>0</y>
<width>744</width>
<height>1047</height>
<height>905</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>1</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1948,7 +1948,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="title">
<string>RTAB-Map settings</string>
</property>
<layout class="QGridLayout" name="gridLayout_3" columnstretch="0,0,1">
<layout class="QGridLayout" name="gridLayout_3" columnstretch="0,1">
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate_2">
<property name="suffix">
<string> Hz</string>
</property>
<property name="decimals">
<number>1</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_2">
<property name="text">
@@ -1959,7 +1972,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="0" column="2">
<item row="0" column="1">
<widget class="QLabel" name="label_79">
<property name="text">
<string>SLAM mode.
@@ -1980,7 +1993,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="2">
<item row="1" column="1">
<widget class="QLabel" name="label_activateRGBD_2">
<property name="text">
<string>Activate metric RGB-D SLAM.
@@ -2001,7 +2014,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="2">
<item row="2" column="1">
<widget class="QLabel" name="label_publishStat">
<property name="text">
<string>Publish statistics.</string>
@@ -2011,20 +2024,7 @@ 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="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate_2">
<property name="suffix">
<string> Hz</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="2">
<item row="3" column="1">
<widget class="QLabel" name="label_134">
<property name="text">
<string>Detection rate (0 means inf). RTAB-Map will filter input images to satisfy this rate.</string>
@@ -2044,7 +2044,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="2">
<item row="4" column="1">
<widget class="QLabel" name="label_112">
<property name="text">
<string>Images buffer size (0 means inf).</string>
@@ -2073,7 +2073,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="2">
<item row="5" column="1">
<widget class="QLabel" name="label_timeLimit_2">
<property name="text">
<string>T_time : Max time (ms) allowed (0 means inf).</string>
@@ -2096,7 +2096,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="2">
<item row="6" column="1">
<widget class="QLabel" name="label_121">
<property name="text">
<string>T_loop : Loop closure threshold.</string>
@@ -2116,7 +2116,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="2">
<item row="7" column="1">
<widget class="QLabel" name="label_similarity_2">
<property name="text">
<string>T_similarity : Similarity threshold (for Rehearsal/Weight Update).</string>
@@ -2139,7 +2139,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="2">
<item row="8" column="1">
<widget class="QLabel" name="label_126">
<property name="text">
<string>STM size : Short-term memory size.</string>
@@ -2149,30 +2149,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_databasePath_2">
<property name="text">
<string>Database path.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QToolButton" name="toolButton_databasePath_2">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="9" column="2">
<widget class="QLineEdit" name="lineEdit_databasePath_2">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -2519,32 +2495,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_4" columnstretch="0,0,1">
<layout class="QGridLayout" name="gridLayout_4" columnstretch="0,0,0">
<item row="0" column="0">
<widget class="QLabel" name="label_databasePath">
<property name="text">
<string>Database path.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QToolButton" name="toolButton_databasePath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLineEdit" name="lineEdit_databasePath">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_workingDirectory">
<property name="text">
<string>Working directory (where to put logs/dump stuff).</string>
@@ -2554,14 +2506,14 @@ 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="0" column="1">
<widget class="QToolButton" name="toolButton_workingDirectory">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="1" column="2">
<item row="0" column="2">
<widget class="QLineEdit" name="lineEdit_workingDirectory">
<property name="readOnly">
<bool>true</bool>