i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Export: added --poses_format option (default 10=RGBD-SLAM format). Added new pose format 11=RGBD-SLAM + ID.

Browse Source
This commit is contained in:
matlabbe
2021-04-13 15:49:04 -04:00
parent f4207979ad
commit 2fcef88016
10 changed files with 105 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Graph.h
+3 -3
View File
@@ -46,9 +46,9 @@ namespace graph {
// Graph utilities
////////////////////////////////////////////
bool RTABMAP_EXP exportPoses(
bool RTABMAP_EXP exportPoses(
const std::string & filePath,
int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame, 11=10+ID), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
const std::map<int, double> & stamps = std::map<int, double>(), // required for format 1
@@ -56,7 +56,7 @@ namespace graph {
bool RTABMAP_EXP importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
std::map<int, double> * stamps = 0); // optional for format 1 and 9
corelib/src/Graph.cpp
+24 -11
View File
@@ -53,7 +53,7 @@ namespace graph {
bool exportPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints, // required for formats 3 and 4
const std::map<int, double> & stamps, // required for format 1
@@ -86,11 +86,11 @@ bool exportPoses(
tmpPath+=".txt";
}
if(format == 1 || format == 10)
if(format == 1 || format == 10 || format == 11)
{
if(stamps.size() != poses.size())
{
UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size! stamps=%d psoes=%d",
UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size! stamps=%d poses=%d",
(int)stamps.size(), (int)poses.size());
return false;
}
@@ -106,7 +106,7 @@ bool exportPoses(
{
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(format == 1 || format == 10) // rgbd-slam format
if(format == 1 || format == 10 || format == 11) // rgbd-slam format
{
Transform pose = iter->second;
if(format == 1)
@@ -128,11 +128,18 @@ bool exportPoses(
if(iter == poses.begin())
{
// header
fprintf(fout, "# timestamp x y z qx qy qz qw\n");
if(format == 11)
{
fprintf(fout, "# timestamp x y z qx qy qz qw id\n");
}
else
{
fprintf(fout, "# timestamp x y z qx qy qz qw\n");
}
}
UASSERT(uContains(stamps, iter->first));
fprintf(fout, "%f %f %f %f %f %f %f %f\n",
fprintf(fout, "%f %f %f %f %f %f %f %f%s\n",
stamps.at(iter->first),
pose.x(),
pose.y(),
@@ -140,7 +147,8 @@ bool exportPoses(
q.x(),
q.y(),
q.z(),
q.w());
q.w(),
format == 11?(" "+uNumber2Str(iter->first)).c_str():"");
}
else // default / KITTI format
{
@@ -175,7 +183,7 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1 and 9
@@ -419,13 +427,18 @@ bool importPoses(
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
}
}
else if(format == 1 || format==10) // rgbd-slam format
else if(format == 1 || format==10 || format==11) // rgbd-slam format
{
std::list<std::string> strList = uSplit(str);
if(strList.size() == 8)
if((strList.size() == 8 && format!=11) || (strList.size() == 9 && format==11))
{
double stamp = uStr2Double(strList.front());
strList.pop_front();
if(format==11)
{
id = uStr2Int(strList.back());
strList.pop_back();
}
str = uJoin(strList, " ");
Transform pose = Transform::fromString(str);
if(pose.isNull())
@@ -456,7 +469,7 @@ bool importPoses(
}
else
{
UERROR("Error parsing \"%s\" with RGBD-SLAM format (should have 8 values: stamp x y z qw qx qy qz)", str.c_str());
UERROR("Error parsing \"%s\" with RGBD-SLAM format (should have 8 values (or 9 with id): stamp x y z qw qx qy qz [id])", str.c_str());
}
}
else // default / KITTI format
guilib/include/rtabmap/gui/DatabaseViewer.h
+1
View File
@@ -107,6 +107,7 @@ private Q_SLOTS:
void exportPosesRaw();
void exportPosesRGBDSLAMMotionCapture();
void exportPosesRGBDSLAM();
void exportPosesRGBDSLAMID();
void exportPosesKITTI();
void exportPosesTORO();
void exportPosesG2O();
guilib/include/rtabmap/gui/MainWindow.h
+1
View File
@@ -155,6 +155,7 @@ protected Q_SLOTS:
void exportPosesRaw();
void exportPosesRGBDSLAM();
void exportPosesRGBDSLAMMotionCapture();
void exportPosesRGBDSLAMID();
void exportPosesKITTI();
void exportPosesTORO();
void exportPosesG2O();
guilib/src/DatabaseViewer.cpp
+6 -1
View File
@@ -272,6 +272,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
connect(ui_->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
connect(ui_->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
connect(ui_->actionRGBD_SLAM_ID_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMID()));
connect(ui_->actionRGBD_SLAM_motion_capture_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMMotionCapture()));
connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
@@ -2276,6 +2277,10 @@ void DatabaseViewer::exportPosesRGBDSLAM()
{
exportPoses(10);
}
void DatabaseViewer::exportPosesRGBDSLAMID()
{
exportPoses(11);
}
void DatabaseViewer::exportPosesKITTI()
{
exportPoses(2);
@@ -2603,7 +2608,7 @@ void DatabaseViewer::exportPoses(int format)
}
std::map<int, double> stamps;
if(format == 1 || format == 10)
if(format == 1 || format == 10 || format == 11)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
guilib/src/MainWindow.cpp
+6 -1
View File
@@ -363,6 +363,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
connect(_ui->actionRGBD_SLAM_motion_capture_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMMotionCapture()));
connect(_ui->actionRGBD_SLAM_ID_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMID()));
connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
@@ -5765,6 +5766,10 @@ void MainWindow::exportPosesRGBDSLAM()
{
exportPoses(10);
}
void MainWindow::exportPosesRGBDSLAMID()
{
exportPoses(11);
}
void MainWindow::exportPosesKITTI()
{
exportPoses(2);
@@ -5886,7 +5891,7 @@ void MainWindow::exportPoses(int format)
}
std::map<int, double> stamps;
if(format == 1 || format == 10)
if(format == 1 || format == 10 || format == 11)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
guilib/src/ui/DatabaseViewer.ui
+12 -6
View File
@@ -61,7 +61,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>422</width>
<width>417</width>
<height>311</height>
</rect>
</property>
@@ -304,7 +304,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>421</width>
<width>416</width>
<height>311</height>
</rect>
</property>
@@ -694,6 +694,7 @@
</property>
<addaction name="actionRaw_format_txt"/>
<addaction name="actionRGBD_SLAM_format_txt"/>
<addaction name="actionRGBD_SLAM_ID_format_txt"/>
<addaction name="actionRGBD_SLAM_motion_capture_format_txt"/>
<addaction name="actionKITTI_format_txt"/>
<addaction name="actionTORO_graph"/>
@@ -1542,8 +1543,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>519</width>
<height>871</height>
<width>280</width>
<height>1006</height>
</rect>
</property>
<attribute name="label">
@@ -2116,8 +2117,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>312</width>
<height>243</height>
<width>298</width>
<height>192</height>
</rect>
</property>
<attribute name="label">
@@ -2989,6 +2990,11 @@
<string>Regenerate optimized 2D map...</string>
</property>
</action>
<action name="actionRGBD_SLAM_ID_format_txt">
<property name="text">
<string>RGBD-SLAM ID format (*.txt)</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/mainWindow.ui
+6
View File
@@ -40,6 +40,7 @@
</property>
<addaction name="actionRaw_format_txt"/>
<addaction name="actionRGBD_SLAM_format_txt"/>
<addaction name="actionRGBD_SLAM_ID_format_txt"/>
<addaction name="actionRGBD_SLAM_motion_capture_format_txt"/>
<addaction name="actionKITTI_format_txt"/>
<addaction name="actionTORO_graph"/>
@@ -1645,6 +1646,11 @@
<string>DepthAI</string>
</property>
</action>
<action name="actionRGBD_SLAM_ID_format_txt">
<property name="text">
<string>RGBD-SLAM + ID format (*.txt)</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/preferencesDialog.ui
+14 -4
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-120</y>
<y>-1483</y>
<width>686</width>
<height>3414</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -6643,7 +6643,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="9" column="1">
<widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
@@ -6703,6 +6703,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>RGBD-SLAM</string>
</property>
</item>
<item>
<property name="text">
<string>RGBD-SLAM + ID</string>
</property>
</item>
</widget>
</item>
<item row="11" column="0">
@@ -6783,7 +6788,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="7" column="1">
<widget class="QComboBox" name="comboBox_cameraImages_odomFormat">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;br/&gt;EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
@@ -6843,6 +6848,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>RGBD-SLAM</string>
</property>
</item>
<item>
<property name="text">
<string>RGBD-SLAM + ID</string>
</property>
</item>
</widget>
</item>
<item row="6" column="0">
tools/Export/main.cpp
+32 -10
View File
@@ -66,9 +66,17 @@ void showUsage()
" --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
" --texture_d2c Distance to camera policy.\n"
" --cam_projection Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
" --poses Export optimized poses of the robot frame (e.g., base_link) in RGB-D SLAM dataset format (stamp x y z qx qy qz qw).\n"
" --poses_camera Export optimized poses of the camera frame (e.g., optical frame) in RGB-D SLAM dataset format (stamp x y z qx qy qz qw).\n"
" --poses_scan Export optimized poses of the scan frame in RGB-D SLAM dataset format (stamp x y z qx qy qz qw).\n"
" --poses Export optimized poses of the robot frame (e.g., base_link).\n"
" --poses_camera Export optimized poses of the camera frame (e.g., optical frame).\n"
" --poses_scan Export optimized poses of the scan frame.\n"
" --poses_format # Format used for exported poses (default is 10):\n"
" 0=Raw 3x4 transformation matrix (r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz)\n"
" 1=RGBD-SLAM (in motion capture coordinate frame)\n"
" 2=KITTI (same as raw but in optical frame)\n"
" 3=TORO\n"
" 4=g2o\n"
" 10=RGBD-SLAM in ROS coordinate frame (stamp x y z qx qy qz qw)\n"
" 11=RGBD-SLAM in ROS coordinate frame + ID (stamp x y z qx qy qz qw id)\n"
" --images Export images with stamp as file name.\n"
" --images_id Export images with node id as file name.\n"
" --ba Do global bundle adjustment before assembling the clouds.\n"
@@ -138,6 +146,7 @@ int main(int argc, char * argv[])
bool exportPoses = false;
bool exportPosesCamera = false;
bool exportPosesScan = false;
int exportPosesFormat = 10;
bool exportImages = false;
bool exportImagesId = false;
std::string outputName;
@@ -238,6 +247,18 @@ int main(int argc, char * argv[])
{
exportPosesScan = true;
}
else if(std::strcmp(argv[i], "--poses_format") == 0)
{
++i;
if(i<argc-1)
{
exportPosesFormat = uStr2Int(argv[i]);
}
else
{
showUsage();
}
}
else if(std::strcmp(argv[i], "--images") == 0)
{
exportImages = true;
@@ -824,10 +845,11 @@ int main(int argc, char * argv[])
}
else
{
std::string posesExt = (exportPosesFormat==3?"toro":exportPosesFormat==4?"g2o":"txt");
if(exportPoses)
{
std::string outputPath=outputDirectory+"/"+baseName+"_poses.txt";
rtabmap::graph::exportPoses(outputPath, 10, robotPoses, std::multimap<int, Link>(), cameraStamps);
std::string outputPath=outputDirectory+"/"+baseName+"_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps);
printf("Poses exported to \"%s\".\n", outputPath.c_str());
}
if(exportPosesCamera)
@@ -836,17 +858,17 @@ int main(int argc, char * argv[])
{
std::string outputPath;
if(cameraPoses.size()==1)
outputPath = outputDirectory+"/"+baseName+"_camera_poses.txt";
outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
else
outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+".txt";
rtabmap::graph::exportPoses(outputPath, 10, cameraPoses[i], std::multimap<int, Link>(), cameraStamps);
outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps);
printf("Camera poses exported to \"%s\".\n", outputPath.c_str());
}
}
if(exportPosesScan)
{
std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses.txt";
rtabmap::graph::exportPoses(outputPath, 10, scanPoses, std::multimap<int, Link>(), cameraStamps);
std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), cameraStamps);
printf("Scan poses exported to \"%s\".\n", outputPath.c_str());
}
}