Added --poses option to report tool. Reprocess tool: Export ground/obstacles when 3d map is created.

guilib/src/DatabaseViewer.cpp

@@ -696,6 +696,10 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			ui_->actionOpen_database->setEnabled(false);
 
 			pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
+			if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
+			{
+				pathDatabase_ = QDir::currentPath();
+			}
 			databaseFileName_ = UFile::getName(path.toStdString());
 			ui_->graphViewer->setWorkingDirectory(pathDatabase_);
 

guilib/src/GraphViewer.cpp

@@ -228,13 +228,13 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_root(0),
 		_graphRoot(0),
 		_globalPathRoot(0),
+		_nodeVisible(true),
 		_nodeRadius(0.01f),
 		_linkWidth(0),
 		_gridMap(0),
 		_referential(0),
 		_originReferential(0),
 		_gridCellSize(0.0f),
-		_nodeVisible(true),
 		_localRadius(0),
 		_loopClosureOutlierThr(0),
 		_maxLinkLength(0.02f),

tools/Report/main.cpp

@@ -41,7 +41,7 @@ using namespace rtabmap;
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-report [\"Statistic/Id\"] [--latex] [--kitti] [--scale] path\n"
+			"rtabmap-report [\"Statistic/Id\"] [--latex] [--kitti] [--scale] [--poses] path\n"
 			"  path               Directory containing rtabmap databases or path of a database.\n\n");
 	exit(1);
 }
@@ -57,6 +57,7 @@ int main(int argc, char * argv[])
 
 	bool outputLatex = false;
 	bool outputScaled = false;
+	bool outputPoses = false;
 	bool outputKittiError = false;
 	std::map<std::string, UPlot*> figures;
 	for(int i=1; i<argc-1; ++i)
@@ -73,6 +74,10 @@ int main(int argc, char * argv[])
 		{
 			outputScaled = true;
 		}
+		else if(strcmp(argv[i], "--poses") == 0)
+		{
+			outputPoses = true;
+		}
 		else
 		{
 			std::string figureTitle = argv[i];
@@ -168,7 +173,6 @@ int main(int argc, char * argv[])
 					slamTime.reserve(ids.size());
 					float rmse = -1;
 					float maxRMSE = -1;
-					float rmseAng = -1;
 					float maxOdomRAM = -1;
 					float maxMapRAM = -1;
 					std::map<std::string, UPlotCurve*> curves;
@@ -204,10 +208,6 @@ int main(int argc, char * argv[])
 										maxRMSE = rmse;
 									}
 								}
-								if(uContains(stat, Statistics::kGtRotational_rmse()))
-								{
-									rmseAng = stat.at(Statistics::kGtRotational_rmse());
-								}
 								if(uContains(stat, std::string("Camera/TotalTime/ms")))
 								{
 									cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
@@ -280,8 +280,9 @@ int main(int argc, char * argv[])
 
 					UERROR("");
 					float bestScale = 1.0f;
-					float bestRMSE = rmse;
+					float bestRMSE = -1;
-					float bestRMSEAng = rmseAng;
+					float bestRMSEAng = -1;
+					Transform bestGtToMap = Transform::getIdentity();
 					float kitti_t_err = 0.0f;
 					float kitti_r_err = 0.0f;
 					if(ids.size())
@@ -303,65 +304,67 @@ int main(int argc, char * argv[])
 							}
 						}
 
-						if(outputScaled)
+						for(float scale=outputScaled?0.900f:1.0f; scale<1.100f; scale+=0.001)
 						{
-							for(float scale=0.900f; scale<1.100f; scale+=0.001)
+							std::map<int, Transform> scaledPoses;
+							for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 							{
-								std::map<int, Transform> scaledPoses;
+								Transform t = iter->second.clone();
-								for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+								t.x() *= scale;
-								{
+								t.y() *= scale;
-									Transform t = iter->second.clone();
+								t.z() *= scale;
-									t.x() *= scale;
+								scaledPoses.insert(std::make_pair(iter->first, t));
-									t.y() *= scale;
+							}
-									t.z() *= scale;
+							// compute RMSE statistics
-									scaledPoses.insert(std::make_pair(iter->first, t));
+							float translational_rmse = 0.0f;
-								}
+							float translational_mean = 0.0f;
-								// compute RMSE statistics
+							float translational_median = 0.0f;
-								float translational_rmse = 0.0f;
+							float translational_std = 0.0f;
-								float translational_mean = 0.0f;
+							float translational_min = 0.0f;
-								float translational_median = 0.0f;
+							float translational_max = 0.0f;
-								float translational_std = 0.0f;
+							float rotational_rmse = 0.0f;
-								float translational_min = 0.0f;
+							float rotational_mean = 0.0f;
-								float translational_max = 0.0f;
+							float rotational_median = 0.0f;
-								float rotational_rmse = 0.0f;
+							float rotational_std = 0.0f;
-								float rotational_mean = 0.0f;
+							float rotational_min = 0.0f;
-								float rotational_median = 0.0f;
+							float rotational_max = 0.0f;
-								float rotational_std = 0.0f;
+							Transform gtToMap = graph::calcRMSE(
-								float rotational_min = 0.0f;
+									groundTruth,
-								float rotational_max = 0.0f;
+									scaledPoses,
-								graph::calcRMSE(
+									translational_rmse,
-										groundTruth,
+									translational_mean,
-										scaledPoses,
+									translational_median,
-										translational_rmse,
+									translational_std,
-										translational_mean,
+									translational_min,
-										translational_median,
+									translational_max,
-										translational_std,
+									rotational_rmse,
-										translational_min,
+									rotational_mean,
-										translational_max,
+									rotational_median,
-										rotational_rmse,
+									rotational_std,
-										rotational_mean,
+									rotational_min,
-										rotational_median,
+									rotational_max);
-										rotational_std,
-										rotational_min,
-										rotational_max);
 
-								if(scale!=0.900f && translational_rmse > bestRMSE)
+							if(bestRMSE!=-1 && translational_rmse > bestRMSE)
-								{
-									break;
-								}
-								bestRMSE = translational_rmse;
-								bestRMSEAng = rotational_rmse;
-								bestScale = scale;
-							}
-							if(bestScale!=1.0f)
 							{
-								for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+								break;
-								{
-									iter->second.x()*=bestScale;
-									iter->second.y()*=bestScale;
-									iter->second.z()*=bestScale;
-								}
 							}
+							bestRMSE = translational_rmse;
+							bestRMSEAng = rotational_rmse;
+							bestScale = scale;
+							bestGtToMap = gtToMap;
+							if(!outputScaled)
+							{
+								// just did iteration without any scale, then exit
+								break;
+							}
+						}
+
+						for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+						{
+							iter->second.x()*=bestScale;
+							iter->second.y()*=bestScale;
+							iter->second.z()*=bestScale;
+							iter->second = bestGtToMap * iter->second;
 						}
 
 						if(outputKittiError)
@@ -377,6 +380,26 @@ int main(int argc, char * argv[])
 										(int)poses.size(), (int)groundTruth.size());
 							}
 						}
+
+						if(outputPoses)
+						{
+							std::string dir = UDirectory::getDir(filePath);
+							std::string dbName = UFile::getName(filePath);
+							dbName = dbName.substr(0, dbName.size()-3); // remove db
+							std::string path = dir+UDirectory::separator()+dbName+"_poses.txt";
+							if(!graph::exportPoses(path, outputKittiError?2:0, poses))
+							{
+								printf("Could not export the poses to \"%s\"!?!\n", path.c_str());
+							}
+							if(groundTruth.size())
+							{
+								path = dir+UDirectory::separator()+dbName+"_gt.txt";
+								if(!graph::exportPoses(path, outputKittiError?2:0, groundTruth))
+								{
+									printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
+								}
+							}
+						}
 					}
 
 					printf("   %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm) ang=%.1fdeg%s, slam: avg=%dms (max=%dms) loops=%d, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",

tools/Reprocess/main.cpp

@@ -357,14 +357,26 @@ int main(int argc, char * argv[])
 	}
 	if(assemble3dMap)
 	{
-		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pcd";
+		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_obstacles.pcd";
 		if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0)
 		{
-			printf("Saving 3d cloud map \"%s\"... done!\n", outputPath.c_str());
+			printf("Saving 3d obstacles \"%s\"... done!\n", outputPath.c_str());
 		}
 		else
 		{
-			printf("Saving 3d cloud map \"%s\"... failed!\n", outputPath.c_str());
+			printf("Saving 3d obstacles \"%s\"... failed!\n", outputPath.c_str());
+		}
+		if(grid.getMapGround()->size())
+		{
+			outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_ground.pcd";
+			if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapGround()) == 0)
+			{
+				printf("Saving 3d ground \"%s\"... done!\n", outputPath.c_str());
+			}
+			else
+			{
+				printf("Saving 3d ground \"%s\"... failed!\n", outputPath.c_str());
+			}
 		}
 	}
 #ifdef RTABMAP_OCTOMAP
@@ -414,16 +426,28 @@ int main(int argc, char * argv[])
 	}
 	if(assemble3dOctoMap)
 	{
-		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pcd";
+		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_occupied.pcd";
-		std::vector<int> obstacles;
+		std::vector<int> obstacles, emptySpace;
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles, &emptySpace);
 		if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
 		{
-			printf("Saving octomap cloud \"%s\"... done!\n", outputPath.c_str());
+			printf("Saving obstacles cloud \"%s\"... done!\n", outputPath.c_str());
 		}
 		else
 		{
-			printf("Saving octomap cloud \"%s\"... failed!\n", outputPath.c_str());
+			printf("Saving obstacles cloud \"%s\"... failed!\n", outputPath.c_str());
+		}
+		if(emptySpace.size())
+		{
+			outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_empty.pcd";
+			if(pcl::io::savePCDFile(outputPath, *cloud, emptySpace, true) == 0)
+			{
+				printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
+			}
+			else
+			{
+				printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
+			}
 		}
 	}
 #endif