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

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

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())
{
@@ -735,40 +735,70 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
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)
if(type==2) // TORO
{
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
graph::TOROOptimizer::saveGraph(path, poses, constraints);
}
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
std::map<int, double> stamps;
if(type == 1)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform o;
int m, w;
std::string l;
double stamp = 0.0;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
stamps.insert(std::make_pair(iter->first, stamp));
}
UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
}
//get timestamps
std::map<int, double> stamps;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform o;
int m, w;
std::string l;
double stamp = 0.0;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
stamps.insert(std::make_pair(iter->first, stamp));
}
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();
this->dumpPoses(path, poses, stamps);
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
// Get poses in front of the robot, return optimized poses
std::map<int, Transform> Rtabmap::getForwardWMPoses(

View File

@@ -117,7 +117,7 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
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, Transform> poses;
@@ -127,11 +127,23 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
_rtabmap->get3DMap(signatures,
poses,
constraints,
optimized,
full);
if(graphOnly)
{
_rtabmap->getGraph(poses,
constraints,
optimized,
full,
&signatures);
}
else
{
_rtabmap->get3DMap(
signatures,
poses,
constraints,
optimized,
full);
}
this->post(new RtabmapEvent3DMap(
signatures,
@@ -139,29 +151,6 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
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,
constraints,
optimized,
full,
&signatures);
this->post(new RtabmapEvent3DMap(
signatures,
poses,
constraints));
}
void RtabmapThread::mainLoopKill()
{
this->clearBufferedData();
@@ -229,38 +218,27 @@ void RtabmapThread::mainLoop()
case kStateDumpingPrediction:
_rtabmap->dumpPrediction();
break;
case kStateGeneratingDOTGraph:
_rtabmap->generateDOTGraph(parameters.at("path"));
case kStateExportingDOTGraph:
_rtabmap->generateDOTGraph(
parameters.at("path"),
atoi(parameters.at("id").c_str()),
atoi(parameters.at("margin").c_str()));
break;
case kStateGeneratingDOTLocalGraph:
_rtabmap->generateDOTGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
break;
case kStateGeneratingTOROGraphLocal:
_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
break;
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);
case kStateExportingPoses:
_rtabmap->exportPoses(
parameters.at("path"),
uStr2Bool(parameters.at("optimized")),
uStr2Bool(parameters.at("global")),
atoi(parameters.at("type").c_str()));
break;
case kStateCleanDataBuffer:
this->clearBufferedData();
break;
case kStatePublishingMapLocal:
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false);
break;
case kStatePublishingMapGlobal:
this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
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);
case kStatePublishingMap:
this->publishMap(
uStr2Bool(parameters.at("optimized")),
uStr2Bool(parameters.at("global")),
uStr2Bool(parameters.at("graph_only")));
break;
case kStateTriggeringMap:
_rtabmap->triggerNewMap();
@@ -275,10 +253,10 @@ void RtabmapThread::mainLoop()
_rtabmap->setUserData(0, userData);
break;
case kStateSettingGoal:
id = atoi(parameters.at("goal_id").c_str());
if(id == 0 && !parameters.at("goal_label").empty() && _rtabmap->getMemory())
id = atoi(parameters.at("id").c_str());
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))
{
@@ -360,8 +338,8 @@ void RtabmapThread::handleEvent(UEvent* event)
{
ULOGGER_DEBUG("CMD_INIT");
ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
UASSERT(!rtabmapEvent->getStr().empty());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
UASSERT(rtabmapEvent->value1().isStr());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
pushNewState(kStateInit, parameters);
}
else if(cmd == RtabmapEventCmd::kCmdClose)
@@ -386,68 +364,30 @@ void RtabmapThread::handleEvent(UEvent* event)
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
{
UASSERT(!rtabmapEvent->getStr().empty());
ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isStr());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
pushNewState(kStateGeneratingDOTGraph, param);
param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingDOTGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTLocalGraph)
else if(cmd == RtabmapEventCmd::kCmdExportPoses)
{
std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
UASSERT(values.size() == 3);
ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH");
ULOGGER_DEBUG("CMD_EXPORT_POSES");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isStr());
UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("path", *values.begin()));
param.insert(ParametersPair("id", *(++values.begin())));
param.insert(ParametersPair("margin", *values.rbegin()));
pushNewState(kStateGeneratingDOTLocalGraph, 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);
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingPoses, param);
}
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
@@ -455,33 +395,17 @@ void RtabmapThread::handleEvent(UEvent* event)
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
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;
param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStatePublishingMapLocal, param);
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal)
{
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);
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
pushNewState(kStatePublishingMap, param);
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{
@@ -496,9 +420,10 @@ void RtabmapThread::handleEvent(UEvent* event)
else if(cmd == RtabmapEventCmd::kCmdGoal)
{
ULOGGER_DEBUG("CMD_GOAL");
UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
ParametersMap param;
param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
pushNewState(kStateSettingGoal, param);
}
else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
@@ -509,9 +434,10 @@ void RtabmapThread::handleEvent(UEvent* event)
else if(cmd == RtabmapEventCmd::kCmdLabel)
{
ULOGGER_DEBUG("CMD_LABEL");
UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->getStr()));
param.insert(ParametersPair("id", uNumber2Str(rtabmapEvent->getInt())));
param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
pushNewState(kStateLabelling, param);
}
else