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

Added special export poses for the KITTI dataset

Browse Source
This commit is contained in:
matlabbe
2015-08-25 19:36:48 -04:00
parent dbc03024ef
commit 12bd8a1720
5 changed files with 35 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/RtabmapEvent.h
+1 -1
View File
@@ -65,7 +65,7 @@ public:
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO)
kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO)
kCmdCleanDataBuffer,
kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly
kCmdTriggerNewMap,
corelib/src/Rtabmap.cpp
+14 -3
View File
@@ -735,7 +735,7 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
if(type==2) // TORO
if(type==3) // TORO
{
graph::TOROOptimizer::saveGraph(path, poses, constraints);
}
@@ -785,11 +785,22 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
}
else // default / KITTI format
{
Transform pose = iter->second;
if(type == 2)
{
// for KITTI, we need to remove optical rotation
// z pointing front, x left, y down
Transform t( 0, 0, 1, 0,
-1, 0, 0, 0,
0,-1, 0, 0);
pose = t.inverse() * pose * t;
}
// Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
const float * p = (const float *)(*iter).second.data();
const float * p = (const float *)pose.data();
fprintf(fout, "%f", p[0]);
for(int i=1; i<(*iter).second.size(); i++)
for(int i=1; i<pose.size(); i++)
{
fprintf(fout, " %f", p[i]);
}