Added special export poses for the KITTI dataset

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -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

@@ -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]);
 						}

guilib/include/rtabmap/gui/MainWindow.h

@@ -128,8 +128,9 @@ private slots:
 	void notifyNoMoreImages();
 	void printLoopClosureIds();
 	void generateGraphDOT();
-	void exportPosesKITTI();
+	void exportPosesRaw();
 	void exportPosesRGBDSLAM();
+	void exportPosesKITTI();
 	void exportPosesTORO();
 	void postProcessing();
 	void deleteMemory();

guilib/src/MainWindow.cpp

@@ -304,8 +304,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
 	connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
 	connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
-	connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
+	connect(_ui->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
 	connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
+	connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
 	connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
 	connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
 	connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
@@ -3155,7 +3156,7 @@ void MainWindow::generateGraphDOT()
 	}
 }
 
-void MainWindow::exportPosesKITTI()
+void MainWindow::exportPosesRaw()
 {
 	exportPoses(0);
 }
@@ -3163,10 +3164,14 @@ void MainWindow::exportPosesRGBDSLAM()
 {
 	exportPoses(1);
 }
-void MainWindow::exportPosesTORO()
+void MainWindow::exportPosesKITTI()
 {
 	exportPoses(2);
 }
+void MainWindow::exportPosesTORO()
+{
+	exportPoses(3);
+}
 
 void MainWindow::exportPoses(int format)
 {
@@ -3204,14 +3209,14 @@ void MainWindow::exportPoses(int format)
 
 		if(_exportPosesFileName[format].isEmpty())
 		{
-			_exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==2?"toro.graph":"poses.txt");
+			_exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":"poses.txt");
 		}
 
 		QString path = QFileDialog::getSaveFileName(
 				this,
 				tr("Save File"),
 				_exportPosesFileName[format],
-				format == 2?tr("TORO file (*.graph)"):tr("Text file (*.txt)"));
+				format == 3?tr("TORO file (*.graph)"):tr("Text file (*.txt)"));
 
 		if(!path.isEmpty())
 		{
@@ -3222,7 +3227,7 @@ void MainWindow::exportPoses(int format)
 			_ui->dockWidget_console->show();
 			_ui->widget_console->appendMsg(
 					QString("%1 saved (global=%2, optimized=%3)... %4")
-					.arg(format == 2?"TORO graph":"Poses")
+					.arg(format == 3?"TORO graph":"Poses")
 					.arg(global?"true":"false")
 					.arg(optimized?"true":"false")
 					.arg(_exportPosesFileName[format]));

guilib/src/ui/mainWindow.ui

@@ -60,8 +60,9 @@
       <property name="title">
        <string>Export poses...</string>
       </property>
-      <addaction name="actionKITTI_format_txt"/>
+      <addaction name="actionRaw_format_txt"/>
       <addaction name="actionRGBD_SLAM_format_txt"/>
+      <addaction name="actionKITTI_format_txt"/>
       <addaction name="actionTORO_graph"/>
      </widget>
      <addaction name="actionOpen_working_directory"/>
@@ -1253,6 +1254,11 @@
     <string>Export cameras in Bundler format (*.out)...</string>
    </property>
   </action>
+  <action name="actionRaw_format_txt">
+   <property name="text">
+    <string>Raw format (*.txt)</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>