i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated how RtabmapCmdEvent should be called (using UVariant). MainWindow: added new actions to export poses (KITTI, RGBD-SLAM and TORO formats)

Browse Source
This commit is contained in:
matlabbe
2015-08-01 23:42:36 -04:00
parent 2877a14360
commit 554b8978a0
14 changed files with 990 additions and 478 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Rtabmap.h
+6 -6
View File
@@ -108,15 +108,15 @@ public:
bool labelLocation(int id, const std::string & label); bool labelLocation(int id, const std::string & label);
bool setUserData(int id, const cv::Mat & data); bool setUserData(int id, const cv::Mat & data);
void generateDOTGraph(const std::string & path, int id=0, int margin=5); void generateDOTGraph(const std::string & path, int id=0, int margin=5);
void generateTOROGraph(const std::string & path, bool optimized, bool global); void exportPoses(
void exportPoses(const std::string & path, bool optimized, bool global); const std::string & path,
bool optimized,
bool global,
int type // 0=raw/KITTI format, 1=rgbd-slam format, 2=TORO
);
void resetMemory(); void resetMemory();
void dumpPrediction() const; void dumpPrediction() const;
void dumpData() const; void dumpData() const;
void dumpPoses(
const std::string & path,
const std::map<int, Transform> & poses,
const std::map<int, double> & stamps = std::map<int, double>()) const;
void parseParameters(const ParametersMap & parameters); void parseParameters(const ParametersMap & parameters);
void setWorkingDirectory(std::string path); void setWorkingDirectory(std::string path);
void rejectLoopClosure(int oldId, int newId); void rejectLoopClosure(int oldId, int newId);
corelib/include/rtabmap/core/RtabmapEvent.h
+49 -29
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UEvent.h> #include <rtabmap/utilite/UEvent.h>
#include <rtabmap/utilite/UVariant.h>
#include "rtabmap/core/Statistics.h" #include "rtabmap/core/Statistics.h"
#include "rtabmap/core/Parameters.h" #include "rtabmap/core/Parameters.h"
@@ -58,53 +59,72 @@ class RtabmapEventCmd : public UEvent
public: public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?! enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
enum Cmd { enum Cmd {
kCmdInit, kCmdInit, // params: [string] database path + ParametersMap
kCmdResetMemory, kCmdResetMemory,
kCmdClose, kCmdClose,
kCmdDumpMemory, kCmdDumpMemory,
kCmdDumpPrediction, kCmdDumpPrediction,
kCmdGenerateDOTGraph, // params: path kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
kCmdGenerateDOTLocalGraph, // params: path, id, margin kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO)
kCmdGenerateTOROGraphLocal, // params: path, optimized
kCmdGenerateTOROGraphGlobal, // params: path, optimized
kCmdExportPosesGlobal,
kCmdExportPosesLocal,
kCmdCleanDataBuffer, kCmdCleanDataBuffer,
kCmdPublish3DMapLocal, // params: optimized kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly
kCmdPublish3DMapGlobal, // params: optimized
kCmdPublishTOROGraphGlobal, // params: optimized
kCmdPublishTOROGraphLocal, // params: optimized
kCmdTriggerNewMap, kCmdTriggerNewMap,
kCmdPause, kCmdPause,
kCmdGoal, // params: label or location ID kCmdGoal, // params: [string] label or [int] location ID
kCmdCancelGoal, kCmdCancelGoal,
kCmdLabel}; // // params: label or location ID kCmdLabel // params: [string] label or [int] location ID
};
public: public:
RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) : RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :
UEvent(0), UEvent(0),
_cmd(cmd), cmd_(cmd),
_strValue(strValue), parameters_(parameters){}
_intValue(intValue), RtabmapEventCmd(Cmd cmd, const UVariant & value1, const ParametersMap & parameters = ParametersMap()) :
_parameters(parameters){} UEvent(0),
cmd_(cmd),
value1_(value1),
parameters_(parameters){}
RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const ParametersMap & parameters = ParametersMap()) :
UEvent(0),
cmd_(cmd),
value1_(value1),
value2_(value2),
parameters_(parameters){}
RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const ParametersMap & parameters = ParametersMap()) :
UEvent(0),
cmd_(cmd),
value1_(value1),
value2_(value2),
value3_(value3),
parameters_(parameters){}
RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const UVariant & value4, const ParametersMap & parameters = ParametersMap()) :
UEvent(0),
cmd_(cmd),
value1_(value1),
value2_(value2),
value3_(value3),
value4_(value4),
parameters_(parameters){}
virtual ~RtabmapEventCmd() {} virtual ~RtabmapEventCmd() {}
Cmd getCmd() const {return _cmd;} Cmd getCmd() const {return cmd_;}
void setStr(const std::string & str) {_strValue = str;} const UVariant & value1() const {return value1_;}
const std::string & getStr() const {return _strValue;} const UVariant & value2() const {return value2_;}
const UVariant & value3() const {return value3_;}
const UVariant & value4() const {return value4_;}
void setInt(int v) {_intValue = v;} const ParametersMap & getParameters() const {return parameters_;}
int getInt() const {return _intValue;}
const ParametersMap & getParameters() const {return _parameters;}
virtual std::string getClassName() const {return std::string("RtabmapEventCmd");} virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
private: private:
Cmd _cmd; Cmd cmd_;
std::string _strValue; UVariant value1_;
int _intValue; UVariant value2_;
ParametersMap _parameters; UVariant value3_;
UVariant value4_;
ParametersMap parameters_;
}; };
class RtabmapEventInit : public UEvent class RtabmapEventInit : public UEvent
corelib/include/rtabmap/core/RtabmapThread.h
+4 -12
View File
@@ -61,17 +61,10 @@ public:
kStateChangingParameters, kStateChangingParameters,
kStateDumpingMemory, kStateDumpingMemory,
kStateDumpingPrediction, kStateDumpingPrediction,
kStateGeneratingDOTGraph, kStateExportingDOTGraph,
kStateGeneratingDOTLocalGraph, kStateExportingPoses,
kStateGeneratingTOROGraphLocal,
kStateGeneratingTOROGraphGlobal,
kStateExportingPosesLocal,
kStateExportingPosesGlobal,
kStateCleanDataBuffer, kStateCleanDataBuffer,
kStatePublishingMapLocal, kStatePublishingMap,
kStatePublishingMapGlobal,
kStatePublishingTOROGraphLocal,
kStatePublishingTOROGraphGlobal,
kStateTriggeringMap, kStateTriggeringMap,
kStateAddingUserData, kStateAddingUserData,
kStateSettingGoal, kStateSettingGoal,
@@ -99,8 +92,7 @@ private:
void addData(const OdometryEvent & odomEvent); void addData(const OdometryEvent & odomEvent);
bool getData(OdometryEvent & data); bool getData(OdometryEvent & data);
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap()); void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void publishMap(bool optimized, bool full) const; void publishMap(bool optimized, bool full, bool graphOnly) const;
void publishGraph(bool optimized, bool full) const;
private: private:
UMutex _stateMutex; UMutex _stateMutex;
corelib/src/Rtabmap.cpp
+49 -59
View File
@@ -718,7 +718,7 @@ void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
} }
} }
void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool global) void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int type)
{ {
if(_memory && _memory->getLastWorkingSignature()) if(_memory && _memory->getLastWorkingSignature())
{ {
@@ -735,29 +735,16 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
} }
if(type==2) // TORO
{
graph::TOROOptimizer::saveGraph(path, poses, constraints); graph::TOROOptimizer::saveGraph(path, poses, constraints);
} }
}
void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global)
{
if(_memory && _memory->getLastWorkingSignature())
{
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
if(optimized)
{
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
}
else else
{ {
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
//get timestamps //get timestamps
std::map<int, double> stamps; std::map<int, double> stamps;
if(type == 1)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
Transform o; Transform o;
@@ -767,8 +754,51 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global)
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true); _memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
stamps.insert(std::make_pair(iter->first, stamp)); stamps.insert(std::make_pair(iter->first, stamp));
} }
UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
}
this->dumpPoses(path, poses, stamps); FILE* fout = 0;
#ifdef _MSC_VER
fopen_s(&fout, path.c_str(), "w");
#else
fout = fopen(path.c_str(), "w");
#endif
if(fout)
{
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(type == 1) // rgbd-slam format
{
// Format: stamp x y z qw qx qy qz
Eigen::Quaternionf q = (*iter).second.getQuaternionf();
UASSERT(uContains(stamps, iter->first));
fprintf(fout, "%f %f %f %f %f %f %f %f\n",
stamps.at(iter->first),
(*iter).second.x(),
(*iter).second.y(),
(*iter).second.z(),
q.w(),
q.x(),
q.y(),
q.z());
}
else // default / KITTI format
{
// Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
const float * p = (const float *)(*iter).second.data();
fprintf(fout, "%f", p[0]);
for(int i=1; i<(*iter).second.size(); i++)
{
fprintf(fout, " %f", p[i]);
}
fprintf(fout, "\n");
}
}
fclose(fout);
}
}
} }
} }
@@ -2476,46 +2506,6 @@ void Rtabmap::dumpData() const
} }
} }
void Rtabmap::dumpPoses(
const std::string & path,
const std::map<int, Transform> & poses,
const std::map<int, double> & stamps) const
{
UDEBUG("");
UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
FILE* fout = 0;
#ifdef _MSC_VER
fopen_s(&fout, path.c_str(), "w");
#else
fout = fopen(path.c_str(), "w");
#endif
if(fout)
{
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
// in camera frame
const float * p = (const float *)(*iter).second.data();
int index = 0;
if(stamps.size() == poses.size())
{
UASSERT(uContains(stamps, iter->first));
fprintf(fout, "%f", stamps.at(iter->first));
}
else
{
fprintf(fout, "%f", p[index++]);
}
for(int i=index; i<(*iter).second.size(); i++)
{
fprintf(fout, " %f", p[i]);
}
fprintf(fout, "\n");
}
fclose(fout);
}
}
// fromId must be in _memory and in _optimizedPoses // fromId must be in _memory and in _optimizedPoses
// Get poses in front of the robot, return optimized poses // Get poses in front of the robot, return optimized poses
std::map<int, Transform> Rtabmap::getForwardWMPoses( std::map<int, Transform> Rtabmap::getForwardWMPoses(
corelib/src/RtabmapThread.cpp
+67 -141
View File
@@ -117,7 +117,7 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
enabled = _createIntermediateNodes; enabled = _createIntermediateNodes;
} }
void RtabmapThread::publishMap(bool optimized, bool full) const void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
{ {
std::map<int, Signature> signatures; std::map<int, Signature> signatures;
std::map<int, Transform> poses; std::map<int, Transform> poses;
@@ -127,33 +127,23 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
std::map<int, std::string> labels; std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas; std::map<int, std::vector<unsigned char> > userDatas;
_rtabmap->get3DMap(signatures, if(graphOnly)
poses,
constraints,
optimized,
full);
this->post(new RtabmapEvent3DMap(
signatures,
poses,
constraints));
}
void RtabmapThread::publishGraph(bool optimized, bool full) const
{ {
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
_rtabmap->getGraph(poses, _rtabmap->getGraph(poses,
constraints, constraints,
optimized, optimized,
full, full,
&signatures); &signatures);
}
else
{
_rtabmap->get3DMap(
signatures,
poses,
constraints,
optimized,
full);
}
this->post(new RtabmapEvent3DMap( this->post(new RtabmapEvent3DMap(
signatures, signatures,
@@ -161,7 +151,6 @@ void RtabmapThread::publishGraph(bool optimized, bool full) const
constraints)); constraints));
} }
void RtabmapThread::mainLoopKill() void RtabmapThread::mainLoopKill()
{ {
this->clearBufferedData(); this->clearBufferedData();
@@ -229,38 +218,27 @@ void RtabmapThread::mainLoop()
case kStateDumpingPrediction: case kStateDumpingPrediction:
_rtabmap->dumpPrediction(); _rtabmap->dumpPrediction();
break; break;
case kStateGeneratingDOTGraph: case kStateExportingDOTGraph:
_rtabmap->generateDOTGraph(parameters.at("path")); _rtabmap->generateDOTGraph(
parameters.at("path"),
atoi(parameters.at("id").c_str()),
atoi(parameters.at("margin").c_str()));
break; break;
case kStateGeneratingDOTLocalGraph: case kStateExportingPoses:
_rtabmap->generateDOTGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str())); _rtabmap->exportPoses(
break; parameters.at("path"),
case kStateGeneratingTOROGraphLocal: uStr2Bool(parameters.at("optimized")),
_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false); uStr2Bool(parameters.at("global")),
break; atoi(parameters.at("type").c_str()));
case kStateGeneratingTOROGraphGlobal:
_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
break;
case kStateExportingPosesLocal:
_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
break;
case kStateExportingPosesGlobal:
_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
break; break;
case kStateCleanDataBuffer: case kStateCleanDataBuffer:
this->clearBufferedData(); this->clearBufferedData();
break; break;
case kStatePublishingMapLocal: case kStatePublishingMap:
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false); this->publishMap(
break; uStr2Bool(parameters.at("optimized")),
case kStatePublishingMapGlobal: uStr2Bool(parameters.at("global")),
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true); uStr2Bool(parameters.at("graph_only")));
break;
case kStatePublishingTOROGraphLocal:
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
break;
case kStatePublishingTOROGraphGlobal:
this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
break; break;
case kStateTriggeringMap: case kStateTriggeringMap:
_rtabmap->triggerNewMap(); _rtabmap->triggerNewMap();
@@ -275,10 +253,10 @@ void RtabmapThread::mainLoop()
_rtabmap->setUserData(0, userData); _rtabmap->setUserData(0, userData);
break; break;
case kStateSettingGoal: case kStateSettingGoal:
id = atoi(parameters.at("goal_id").c_str()); id = atoi(parameters.at("id").c_str());
if(id == 0 && !parameters.at("goal_label").empty() && _rtabmap->getMemory()) if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
{ {
id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label")); id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
} }
if(id <= 0 || !_rtabmap->computePath(id, true)) if(id <= 0 || !_rtabmap->computePath(id, true))
{ {
@@ -360,8 +338,8 @@ void RtabmapThread::handleEvent(UEvent* event)
{ {
ULOGGER_DEBUG("CMD_INIT"); ULOGGER_DEBUG("CMD_INIT");
ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters(); ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
UASSERT(!rtabmapEvent->getStr().empty()); UASSERT(rtabmapEvent->value1().isStr());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second); UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
pushNewState(kStateInit, parameters); pushNewState(kStateInit, parameters);
} }
else if(cmd == RtabmapEventCmd::kCmdClose) else if(cmd == RtabmapEventCmd::kCmdClose)
@@ -386,68 +364,30 @@ void RtabmapThread::handleEvent(UEvent* event)
} }
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph) else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
{ {
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH"); 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; ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr())); param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
pushNewState(kStateGeneratingDOTGraph, param); 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::kCmdGenerateDOTLocalGraph) else if(cmd == RtabmapEventCmd::kCmdExportPoses)
{ {
std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';'); ULOGGER_DEBUG("CMD_EXPORT_POSES");
UASSERT(values.size() == 3); UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH"); UASSERT(rtabmapEvent->value3().isStr());
UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param; ParametersMap param;
param.insert(ParametersPair("path", *values.begin())); param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("id", *(++values.begin()))); param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("margin", *values.rbegin())); param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
pushNewState(kStateGeneratingDOTLocalGraph, param); param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingPoses, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphLocal)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_LOCAL");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateGeneratingTOROGraphLocal, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphGlobal)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_GLOBAL");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateGeneratingTOROGraphGlobal, param);
}
else if(cmd == RtabmapEventCmd::kCmdExportPosesLocal)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_EXPORT_POSES_LOCAL");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateExportingPosesLocal, param);
}
else if(cmd == RtabmapEventCmd::kCmdExportPosesGlobal)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_EXPORT_POSES_GLOBAL");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateExportingPosesGlobal, param);
} }
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer) else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
@@ -455,33 +395,17 @@ void RtabmapThread::handleEvent(UEvent* event)
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER"); ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
pushNewState(kStateCleanDataBuffer); pushNewState(kStateCleanDataBuffer);
} }
else if(cmd == RtabmapEventCmd::kCmdPublish3DMapLocal) else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
{ {
ULOGGER_DEBUG("CMD_PUBLISH_MAP_LOCAL"); ULOGGER_DEBUG("CMD_PUBLISH_MAP");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isBool());
ParametersMap param; ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
pushNewState(kStatePublishingMapLocal, param); param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
} param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal) pushNewState(kStatePublishingMap, param);
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP_GLOBAL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingMapGlobal, param);
}
else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphLocal)
{
ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_LOCAL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingTOROGraphLocal, param);
}
else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
{
ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_GLOBAL");
ParametersMap param;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingTOROGraphGlobal, param);
} }
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap) else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{ {
@@ -496,9 +420,10 @@ void RtabmapThread::handleEvent(UEvent* event)
else if(cmd == RtabmapEventCmd::kCmdGoal) else if(cmd == RtabmapEventCmd::kCmdGoal)
{ {
ULOGGER_DEBUG("CMD_GOAL"); ULOGGER_DEBUG("CMD_GOAL");
UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
ParametersMap param; ParametersMap param;
param.insert(ParametersPair("goal_label", rtabmapEvent->getStr())); param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt()))); param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
pushNewState(kStateSettingGoal, param); pushNewState(kStateSettingGoal, param);
} }
else if(cmd == RtabmapEventCmd::kCmdCancelGoal) else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
@@ -509,9 +434,10 @@ void RtabmapThread::handleEvent(UEvent* event)
else if(cmd == RtabmapEventCmd::kCmdLabel) else if(cmd == RtabmapEventCmd::kCmdLabel)
{ {
ULOGGER_DEBUG("CMD_LABEL"); ULOGGER_DEBUG("CMD_LABEL");
UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
ParametersMap param; ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->getStr())); param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
param.insert(ParametersPair("id", uNumber2Str(rtabmapEvent->getInt()))); param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
pushNewState(kStateLabelling, param); pushNewState(kStateLabelling, param);
} }
else else
guilib/include/rtabmap/gui/MainWindow.h
+6 -6
View File
@@ -126,10 +126,10 @@ private slots:
void stopDetection(); void stopDetection();
void notifyNoMoreImages(); void notifyNoMoreImages();
void printLoopClosureIds(); void printLoopClosureIds();
void generateMap(); void generateGraphDOT();
void generateLocalMap(); void exportPosesKITTI();
void generateTOROMap(); void exportPosesRGBDSLAM();
void exportPoses(); void exportPosesTORO();
void postProcessing(); void postProcessing();
void deleteMemory(); void deleteMemory();
void openWorkingDirectory(); void openWorkingDirectory();
@@ -223,6 +223,7 @@ private:
void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent); void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
void saveFigures(); void saveFigures();
void loadFigures(); void loadFigures();
void exportPoses(int format);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud( pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,
@@ -296,8 +297,7 @@ private:
DetailedProgressDialog * _initProgressDialog; DetailedProgressDialog * _initProgressDialog;
QString _graphSavingFileName; QString _graphSavingFileName;
QString _toroSavingFileName; QMap<int, QString> _exportPosesFileName;
QString _posesSavingFileName;
bool _autoScreenCaptureOdomSync; bool _autoScreenCaptureOdomSync;
QVector<int> _refIds; QVector<int> _refIds;
guilib/src/MainWindow.cpp
+54 -150
View File
@@ -299,10 +299,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache())); connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec())); connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds())); connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateMap())); connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
connect(_ui->actionGenerate_local_map, SIGNAL(triggered()), this, SLOT(generateLocalMap())); connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
connect(_ui->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this , SLOT(generateTOROMap())); connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
connect(_ui->actionExport_poses_txt, SIGNAL(triggered()), this , SLOT(exportPoses())); connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory())); connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds())); connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph())); connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
@@ -2643,7 +2643,7 @@ void MainWindow::newDatabase()
} }
} }
_newDatabasePath = databasePath.c_str(); _newDatabasePath = databasePath.c_str();
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, 0, _preferencesDialog->getAllParameters())); this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, _preferencesDialog->getAllParameters()));
applyPrefSettings(_preferencesDialog->getAllParameters(), false); applyPrefSettings(_preferencesDialog->getAllParameters(), false);
} }
@@ -3115,57 +3115,30 @@ void MainWindow::printLoopClosureIds()
_ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop)); _ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop));
} }
void MainWindow::generateMap() void MainWindow::generateGraphDOT()
{
if(_graphSavingFileName.isEmpty())
{
_graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
}
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
if(!path.isEmpty())
{
_graphSavingFileName = path;
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, path.toStdString())); // The event is automatically deleted by the EventsManager...
_ui->dockWidget_console->show();
_ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName).arg(_graphSavingFileName));
}
}
void MainWindow::generateLocalMap()
{ {
if(_graphSavingFileName.isEmpty()) if(_graphSavingFileName.isEmpty())
{ {
_graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot"; _graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
} }
bool ok = false; bool ok;
int loopId = 1; int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID (0=full map)"), 0, 0, 999999, 0, &ok);
if(_ui->label_matchId->text().size())
{
std::list<std::string> values = uSplitNumChar(_ui->label_matchId->text().toStdString());
if(values.size() > 1)
{
int val = QString((++values.begin())->c_str()).toInt(&ok);
if(ok) if(ok)
{ {
loopId = val; int margin = 0;
} if(id > 0)
ok = false;
}
}
int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), loopId, 1, 999999, 1, &ok);
if(ok)
{ {
int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok); margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
}
if(ok) if(ok)
{ {
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)")); QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
if(!path.isEmpty()) if(!path.isEmpty())
{ {
_graphSavingFileName = path; _graphSavingFileName = path;
QString str = path + QString(";") + QString::number(id) + QString(";") + QString::number(margin); this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, false, path.toStdString(), id, margin));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTLocalGraph, str.toStdString())); // The event is automatically deleted by the EventsManager...
_ui->dockWidget_console->show(); _ui->dockWidget_console->show();
_ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName).arg(_graphSavingFileName)); _ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName).arg(_graphSavingFileName));
@@ -3174,13 +3147,21 @@ void MainWindow::generateLocalMap()
} }
} }
void MainWindow::generateTOROMap() void MainWindow::exportPosesKITTI()
{ {
if(_toroSavingFileName.isEmpty()) exportPoses(0);
}
void MainWindow::exportPosesRGBDSLAM()
{ {
_toroSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "toro.graph"; exportPoses(1);
}
void MainWindow::exportPosesTORO()
{
exportPoses(2);
} }
void MainWindow::exportPoses(int format)
{
QStringList items; QStringList items;
items.append("Local map optimized"); items.append("Local map optimized");
items.append("Local map not optimized"); items.append("Local map not optimized");
@@ -3213,82 +3194,30 @@ void MainWindow::generateTOROMap()
UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str()); UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
} }
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _toroSavingFileName, tr("TORO file (*.graph)")); if(_exportPosesFileName[format].isEmpty())
{
_exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==2?"toro.graph":"poses.txt");
}
QString path = QFileDialog::getSaveFileName(
this,
tr("Save File"),
_exportPosesFileName[format],
format == 2?tr("TORO file (*.graph)"):tr("Text file (*.txt)"));
if(!path.isEmpty()) if(!path.isEmpty())
{ {
_toroSavingFileName = path; _exportPosesFileName[format] = path;
if(global)
{ this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPoses, global, optimized, path.toStdString(), format));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraphGlobal, path.toStdString(), optimized?1:0));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraphLocal, path.toStdString(), optimized?1:0));
}
_ui->dockWidget_console->show(); _ui->dockWidget_console->show();
_ui->widget_console->appendMsg(QString("TORO Graph saved (global=%1, optimized=%2)... %3") _ui->widget_console->appendMsg(
.arg(global?"true":"false").arg(optimized?"true":"false").arg(_toroSavingFileName)); QString("%1 saved (global=%2, optimized=%3)... %4")
} .arg(format == 2?"TORO graph":"Poses")
.arg(global?"true":"false")
} .arg(optimized?"true":"false")
} .arg(_exportPosesFileName[format]));
void MainWindow::exportPoses()
{
if(_posesSavingFileName.isEmpty())
{
_posesSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "poses.txt";
}
QStringList items;
items.append("Local map optimized");
items.append("Local map not optimized");
items.append("Global map optimized");
items.append("Global map not optimized");
bool ok;
QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 2, false, &ok);
if(ok)
{
bool optimized=false, global=false;
if(item.compare("Local map optimized") == 0)
{
optimized = true;
}
else if(item.compare("Local map not optimized") == 0)
{
}
else if(item.compare("Global map optimized") == 0)
{
global=true;
optimized=true;
}
else if(item.compare("Global map not optimized") == 0)
{
global=true;
}
else
{
UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
}
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _posesSavingFileName, tr("Text file (*.txt)"));
if(!path.isEmpty())
{
_posesSavingFileName = path;
if(global)
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesGlobal, path.toStdString(), optimized?1:0));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesLocal, path.toStdString(), optimized?1:0));
}
_ui->dockWidget_console->show();
_ui->widget_console->appendMsg(QString("Poses saved (global=%1, optimized=%2)... %3")
.arg(global?"true":"false").arg(optimized?"true":"false").arg(_posesSavingFileName));
} }
} }
@@ -3957,14 +3886,7 @@ void MainWindow::downloadAllClouds()
_initProgressDialog->show(); _initProgressDialog->show();
_initProgressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...") _initProgressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...")
.arg(global?"true":"false").arg(optimized?"true":"false")); .arg(global?"true":"false").arg(optimized?"true":"false"));
if(global) this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, false));
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapGlobal, "", optimized?1:0));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapLocal, "", optimized?1:0));
}
} }
} }
@@ -4008,14 +3930,8 @@ void MainWindow::downloadPoseGraph()
_initProgressDialog->show(); _initProgressDialog->show();
_initProgressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...") _initProgressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...")
.arg(global?"true":"false").arg(optimized?"true":"false")); .arg(global?"true":"false").arg(optimized?"true":"false"));
if(global)
{ this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, true));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphGlobal, "", optimized?1:0));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphLocal, "", optimized?1:0));
}
} }
} }
@@ -5261,9 +5177,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDump_the_memory->setVisible(!monitoring); _ui->actionDump_the_memory->setVisible(!monitoring);
_ui->actionDump_the_prediction_matrix->setVisible(!monitoring); _ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
_ui->actionGenerate_map->setVisible(!monitoring); _ui->actionGenerate_map->setVisible(!monitoring);
_ui->actionGenerate_local_map->setVisible(!monitoring); _ui->menuExport_poses->setEnabled(!monitoring);
_ui->actionGenerate_TORO_graph_graph->setVisible(!monitoring);
_ui->actionExport_poses_txt->setVisible(!monitoring);
_ui->actionOpen_working_directory->setVisible(!monitoring); _ui->actionOpen_working_directory->setVisible(!monitoring);
_ui->actionData_recorder->setVisible(!monitoring); _ui->actionData_recorder->setVisible(!monitoring);
_ui->menuSelect_source->menuAction()->setVisible(!monitoring); _ui->menuSelect_source->menuAction()->setVisible(!monitoring);
@@ -5345,9 +5259,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDelete_memory->setEnabled(false); _ui->actionDelete_memory->setEnabled(false);
_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1); _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
_ui->actionGenerate_map->setEnabled(false); _ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false); _ui->menuExport_poses->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionExport_poses_txt->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false); _ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false); _ui->menuSelect_source->setEnabled(false);
@@ -5393,9 +5305,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDelete_memory->setEnabled(true); _ui->actionDelete_memory->setEnabled(true);
_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1); _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
_ui->actionGenerate_map->setEnabled(true); _ui->actionGenerate_map->setEnabled(true);
_ui->actionGenerate_local_map->setEnabled(true); _ui->menuExport_poses->setEnabled(true);
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
_ui->actionExport_poses_txt->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true); _ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true); _ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setEnabled(true); _ui->menuSelect_source->setEnabled(true);
@@ -5430,9 +5340,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDelete_memory->setEnabled(false); _ui->actionDelete_memory->setEnabled(false);
_ui->actionPost_processing->setEnabled(false); _ui->actionPost_processing->setEnabled(false);
_ui->actionGenerate_map->setEnabled(false); _ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false); _ui->menuExport_poses->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionExport_poses_txt->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false); _ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false); _ui->menuSelect_source->setEnabled(false);
@@ -5469,9 +5377,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDelete_memory->setEnabled(false); _ui->actionDelete_memory->setEnabled(false);
_ui->actionPost_processing->setEnabled(false); _ui->actionPost_processing->setEnabled(false);
_ui->actionGenerate_map->setEnabled(false); _ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false); _ui->menuExport_poses->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionExport_poses_txt->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false); _ui->actionDownload_graph->setEnabled(false);
_state = kDetecting; _state = kDetecting;
@@ -5498,9 +5404,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionDelete_memory->setEnabled(false); _ui->actionDelete_memory->setEnabled(false);
_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1); _ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
_ui->actionGenerate_map->setEnabled(true); _ui->actionGenerate_map->setEnabled(true);
_ui->actionGenerate_local_map->setEnabled(true); _ui->menuExport_poses->setEnabled(true);
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
_ui->actionExport_poses_txt->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true); _ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true); _ui->actionDownload_graph->setEnabled(true);
_state = kPaused; _state = kPaused;
guilib/src/ui/mainWindow.ui
+26 -20
View File
@@ -27,7 +27,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>1012</width> <width>1012</width>
<height>25</height> <height>22</height>
</rect> </rect>
</property> </property>
<widget class="QMenu" name="menuFile"> <widget class="QMenu" name="menuFile">
@@ -55,13 +55,19 @@
<property name="title"> <property name="title">
<string>Advanced</string> <string>Advanced</string>
</property> </property>
<widget class="QMenu" name="menuExport_poses">
<property name="title">
<string>Export poses...</string>
</property>
<addaction name="actionKITTI_format_txt"/>
<addaction name="actionRGBD_SLAM_format_txt"/>
<addaction name="actionTORO_graph"/>
</widget>
<addaction name="actionOpen_working_directory"/> <addaction name="actionOpen_working_directory"/>
<addaction name="actionDump_the_memory"/> <addaction name="actionDump_the_memory"/>
<addaction name="actionDump_the_prediction_matrix"/> <addaction name="actionDump_the_prediction_matrix"/>
<addaction name="actionGenerate_map"/> <addaction name="actionGenerate_map"/>
<addaction name="actionGenerate_local_map"/> <addaction name="menuExport_poses"/>
<addaction name="actionGenerate_TORO_graph_graph"/>
<addaction name="actionExport_poses_txt"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/> <addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="separator"/> <addaction name="separator"/>
<addaction name="actionSend_goal"/> <addaction name="actionSend_goal"/>
@@ -908,7 +914,7 @@
</action> </action>
<action name="actionGenerate_map"> <action name="actionGenerate_map">
<property name="text"> <property name="text">
<string>Generate graph map (*.dot)...</string> <string>Generate graph (*.dot)...</string>
</property> </property>
</action> </action>
<action name="actionDelete_memory"> <action name="actionDelete_memory">
@@ -973,11 +979,6 @@
<string>Usb camera</string> <string>Usb camera</string>
</property> </property>
</action> </action>
<action name="actionGenerate_local_map">
<property name="text">
<string>Generate graph local map (*.dot)...</string>
</property>
</action>
<action name="actionPrint_loop_closure_IDs_to_console"> <action name="actionPrint_loop_closure_IDs_to_console">
<property name="text"> <property name="text">
<string>Print loop closure IDs to console</string> <string>Print loop closure IDs to console</string>
@@ -1043,11 +1044,6 @@
<string>Trigger a new map</string> <string>Trigger a new map</string>
</property> </property>
</action> </action>
<action name="actionGenerate_TORO_graph_graph">
<property name="text">
<string>Generate TORO graph (*.graph)...</string>
</property>
</action>
<action name="actionDownload_graph"> <action name="actionDownload_graph">
<property name="text"> <property name="text">
<string>Download graph only</string> <string>Download graph only</string>
@@ -1213,11 +1209,6 @@
<string>Send a goal...</string> <string>Send a goal...</string>
</property> </property>
</action> </action>
<action name="actionExport_poses_txt">
<property name="text">
<string>Export poses (*.txt)...</string>
</property>
</action>
<action name="actionCancel_goal"> <action name="actionCancel_goal">
<property name="text"> <property name="text">
<string>Cancel goal</string> <string>Cancel goal</string>
@@ -1241,6 +1232,21 @@
<string>Label current location...</string> <string>Label current location...</string>
</property> </property>
</action> </action>
<action name="actionKITTI_format_txt">
<property name="text">
<string>KITTI format (*.txt)</string>
</property>
</action>
<action name="actionRGBD_SLAM_format_txt">
<property name="text">
<string>RGBD-SLAM format (*.txt)</string>
</property>
</action>
<action name="actionTORO_graph">
<property name="text">
<string>TORO (*.graph)</string>
</property>
</action>
</widget> </widget>
<customwidgets> <customwidgets>
<customwidget> <customwidget>
guilib/src/ui/preferencesDialog.ui
+6 -6
View File
@@ -63,9 +63,9 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-399</y> <y>0</y>
<width>760</width> <width>755</width>
<height>1570</height> <height>1591</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>3</number> <number>4</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29"> <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2996,7 +2996,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string> Hz</string> <string> Hz</string>
</property> </property>
<property name="decimals"> <property name="decimals">
<number>1</number> <number>3</number>
</property> </property>
<property name="value"> <property name="value">
<double>1.000000000000000</double> <double>1.000000000000000</double>
@@ -3334,7 +3334,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string> Hz</string> <string> Hz</string>
</property> </property>
<property name="decimals"> <property name="decimals">
<number>1</number> <number>3</number>
</property> </property>
<property name="value"> <property name="value">
<double>1.000000000000000</double> <double>1.000000000000000</double>
utilite/include/rtabmap/utilite/UConversion.h
+1
View File
@@ -148,6 +148,7 @@ std::string UTILITE_EXP uBool2Str(bool boolean);
* @return the boolean * @return the boolean
*/ */
bool UTILITE_EXP uStr2Bool(const char * str); bool UTILITE_EXP uStr2Bool(const char * str);
bool UTILITE_EXP uStr2Bool(const std::string & str);
/** /**
* Convert a string to an array of bytes including the null character ('\0'). * Convert a string to an array of bytes including the null character ('\0').
utilite/include/rtabmap/utilite/UVariant.h
+58 -32
View File
@@ -20,47 +20,73 @@
#ifndef UVARIANT_H #ifndef UVARIANT_H
#define UVARIANT_H #define UVARIANT_H
#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
#include <string> #include <string>
#include <vector>
/** /**
* Experimental class... * Experimental class...
*/ */
class UVariant class UTILITE_EXP UVariant
{ {
public: public:
enum Type{
kBool,
kChar,
kUChar,
kShort,
kUShort,
kInt,
kUInt,
kFloat,
kDouble,
kStr,
kUndef
};
public:
UVariant();
UVariant(const bool & value);
UVariant(const char & value);
UVariant(const unsigned char & value);
UVariant(const short & value);
UVariant(const unsigned short & value);
UVariant(const int & value);
UVariant(const unsigned int & value);
UVariant(const float & value);
UVariant(const double & value);
UVariant(const char * value);
UVariant(const std::string & value);
Type type() const {return type_;}
bool isUndef() const {return type_ == kUndef;}
bool isBool() const {return type_ == kBool;}
bool isChar() const {return type_ == kChar;}
bool isUChar() const {return type_ == kUChar;}
bool isShort() const {return type_ == kShort;}
bool isUShort() const {return type_ == kUShort;}
bool isInt() const {return type_ == kInt;}
bool isUInt() const {return type_ == kUInt;}
bool isFloat() const {return type_ == kFloat;}
bool isDouble() const {return type_ == kDouble;}
bool isStr() const {return type_ == kStr;}
bool toBool() const;
char toChar(bool * ok = 0) const;
unsigned char toUChar(bool * ok = 0) const;
short toShort(bool * ok = 0) const;
unsigned short toUShort(bool * ok = 0) const;
int toInt(bool * ok = 0) const;
unsigned int toUInt(bool * ok = 0) const;
float toFloat(bool * ok = 0) const;
double toDouble(bool * ok = 0) const;
std::string toStr(bool * ok = 0) const;
virtual ~UVariant() {} virtual ~UVariant() {}
virtual std::string className() const = 0; private:
Type type_;
template<class T> std::vector<unsigned char> data_;
const T * data() const {
if(data_)
return (T*)data_;
return (const T*)constData_;
}
template<class T>
T * takeDataOwnership() {
T * data = (T*)data_;
constData_ = 0;
data_=0;
return data;
}
protected:
UVariant(void * data) :
data_(data),
constData_(0)
{}
UVariant(const void * data) :
data_(0),
constData_(data)
{}
protected:
void * data_;
const void * constData_;
}; };
#endif /* UVARIANT_H */ #endif /* UVARIANT_H */
utilite/src/CMakeLists.txt
+1
View File
@@ -10,6 +10,7 @@ SET(SRC_FILES
UThread.cpp UThread.cpp
UTimer.cpp UTimer.cpp
UProcessInfo.cpp UProcessInfo.cpp
UVariant.cpp
) )
SET(INCLUDE_DIRS SET(INCLUDE_DIRS
utilite/src/UConversion.cpp
+5
View File
@@ -150,6 +150,11 @@ bool uStr2Bool(const char * str)
return !(str && (strcmp(str, "false") == 0 || strcmp(str, "FALSE") == 0 || strcmp(str, "0") == 0)); return !(str && (strcmp(str, "false") == 0 || strcmp(str, "FALSE") == 0 || strcmp(str, "0") == 0));
} }
bool uStr2Bool(const std::string & str)
{
return !(str.compare("false") == 0 || str.compare("FALSE") == 0 || str.compare("0") == 0);
}
std::vector<unsigned char> uStr2Bytes(const std::string & str) std::vector<unsigned char> uStr2Bytes(const std::string & str)
{ {
std::vector<unsigned char> bytes(str.size()+1); std::vector<unsigned char> bytes(str.size()+1);
utilite/src/UVariant.cpp
+641
View File
@@ -0,0 +1,641 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UVariant.h"
#include "rtabmap/utilite/UConversion.h"
UVariant::UVariant() :
type_(kUndef)
{
}
UVariant::UVariant(const bool & value) :
type_(kBool),
data_(1)
{
data_[0] = value?1:0;
}
UVariant::UVariant(const char & value) :
type_(kChar),
data_(sizeof(char))
{
memcpy(data_.data(), &value, sizeof(char));
}
UVariant::UVariant(const unsigned char & value) :
type_(kUChar),
data_(sizeof(unsigned char))
{
memcpy(data_.data(), &value, sizeof(unsigned char));
}
UVariant::UVariant(const short & value) :
type_(kShort),
data_(sizeof(short))
{
memcpy(data_.data(), &value, sizeof(short));
}
UVariant::UVariant(const unsigned short & value) :
type_(kUShort),
data_(sizeof(unsigned short))
{
memcpy(data_.data(), &value, sizeof(unsigned short));
}
UVariant::UVariant(const int & value) :
type_(kInt),
data_(sizeof(int))
{
memcpy(data_.data(), &value, sizeof(int));
}
UVariant::UVariant(const unsigned int & value) :
type_(kUInt),
data_(sizeof(unsigned int))
{
memcpy(data_.data(), &value, sizeof(unsigned int));
}
UVariant::UVariant(const float & value) :
type_(kFloat),
data_(sizeof(float))
{
memcpy(data_.data(), &value, sizeof(float));
}
UVariant::UVariant(const double & value) :
type_(kDouble),
data_(sizeof(double))
{
memcpy(data_.data(), &value, sizeof(double));
}
UVariant::UVariant(const char * value) :
type_(kStr)
{
std::string str(value);
data_.resize(str.size()+1);
memcpy(data_.data(), str.data(), str.size()+1);
}
UVariant::UVariant(const std::string & value) :
type_(kStr),
data_(value.size()+1) // with null character
{
memcpy(data_.data(), value.data(), value.size()+1);
}
bool UVariant::toBool() const
{
if(type_ ==kStr)
{
return uStr2Bool(toStr().c_str());
}
else if(data_.size())
{
return memcmp(data_.data(), std::vector<unsigned char>(data_.size(), 0).data(), data_.size()) != 0;
}
return false;
}
char UVariant::toChar(bool * ok) const
{
if(ok)
{
*ok = false;
}
char v = 0;
if(type_ == kChar)
{
memcpy(&v, data_.data(), sizeof(char));
if(ok)
{
*ok = true;
}
}
else if(type_ == kUChar)
{
unsigned char tmp = toUChar();
if(tmp <= std::numeric_limits<char>::max())
{
v = (char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= std::numeric_limits<char>::min() && tmp <= std::numeric_limits<char>::max())
{
v = (char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUShort)
{
unsigned short tmp = toUShort();
if(tmp <= std::numeric_limits<char>::max())
{
v = (char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<char>::min() && tmp <= std::numeric_limits<char>::max())
{
v = (char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= std::numeric_limits<char>::max())
{
v = (char)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
unsigned char UVariant::toUChar(bool * ok) const
{
if(ok)
{
*ok = false;
}
unsigned char v = 0;
if(type_ == kUChar)
{
memcpy(&v, data_.data(), sizeof(unsigned char));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
char tmp = toChar();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUShort)
{
unsigned short tmp = toUShort();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
short UVariant::toShort(bool * ok) const
{
if(ok)
{
*ok = false;
}
short v = 0;
if(type_ == kShort)
{
memcpy(&v, data_.data(), sizeof(short));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
v = (short)toChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUChar)
{
v = (short)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUShort)
{
unsigned short tmp = toUShort();
if(tmp <= std::numeric_limits<short>::max())
{
v = (short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<short>::min() && tmp <= std::numeric_limits<short>::max())
{
v = (short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= std::numeric_limits<short>::max())
{
v = (short)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
unsigned short UVariant::toUShort(bool * ok) const
{
if(ok)
{
*ok = false;
}
unsigned short v = 0;
if(type_ == kUShort)
{
memcpy(&v, data_.data(), sizeof(unsigned short));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
char tmp = toChar();
if(tmp >= 0)
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUChar)
{
v = (unsigned short)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
int UVariant::toInt(bool * ok) const
{
if(ok)
{
*ok = false;
}
int v = 0;
if(type_ == kInt)
{
memcpy(&v, data_.data(), sizeof(int));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
v = (int)toChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUChar)
{
v = (int)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kShort)
{
v = (int)toShort();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUShort)
{
v = (int)toUShort();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= std::numeric_limits<int>::max())
{
v = (int)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
unsigned int UVariant::toUInt(bool * ok) const
{
if(ok)
{
*ok = false;
}
unsigned int v = 0;
if(type_ == kUInt)
{
memcpy(&v, data_.data(), sizeof(unsigned int));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
char tmp = toChar();
if(tmp >= 0)
{
v = (unsigned int)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUChar)
{
v = (unsigned int)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= 0)
{
v = (unsigned int)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUShort)
{
v = (unsigned int)toUShort();
if(ok)
{
*ok = true;
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= 0)
{
v = (unsigned int)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
float UVariant::toFloat(bool * ok) const
{
if(ok)
{
*ok = false;
}
float v = 0;
if(type_ == kFloat)
{
memcpy(&v, data_.data(), sizeof(float));
if(ok)
{
*ok = true;
}
}
else if(type_ == kDouble)
{
double tmp = toDouble();
if(tmp >= std::numeric_limits<float>::min() && tmp <= std::numeric_limits<float>::max())
{
v = (float)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
double UVariant::toDouble(bool * ok) const
{
if(ok)
{
*ok = false;
}
double v = 0;
if(type_ == kDouble)
{
memcpy(&v, data_.data(), sizeof(double));
if(ok)
{
*ok = true;
}
}
else if(type_ == kFloat)
{
v = (double)toFloat(ok);
}
return v;
}
std::string UVariant::toStr(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::string v;
if(type_ == kStr)
{
v = std::string((const char *)data_.data());
if(ok)
{
*ok = true;
}
}
else if(type_ == kBool)
{
v = toBool()?"true":"false";
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
v = " ";
v.at(0) = toChar(ok);
}
else if(type_ == kUChar)
{
v = uNumber2Str(toUChar(ok));
}
else if(type_ == kShort)
{
v = uNumber2Str(toShort(ok));
}
else if(type_ == kUShort)
{
v = uNumber2Str(toUShort(ok));
}
else if(type_ == kInt)
{
v = uNumber2Str(toInt(ok));
}
else if(type_ == kUInt)
{
v = uNumber2Str(toUInt(ok));
}
else if(type_ == kFloat)
{
v = uNumber2Str(toFloat(ok));
}
else if(type_ == kDouble)
{
v = uNumber2Str(toDouble(ok));
}
return v;
}