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

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

Browse Source
This commit is contained in:
matlabbe
2018-02-06 11:31:39 -05:00
parent 56df87e60c
commit 42199eefd2
4 changed files with 122 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
guilib/src/DatabaseViewer.cpp
View File

@@ -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_);

2
guilib/src/GraphViewer.cpp
View File

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

147
tools/Report/main.cpp
View File

@@ -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 bestRMSEAng = rmseAng;
float bestRMSE = -1;
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;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform t = iter->second.clone();
t.x() *= scale;
t.y() *= scale;
t.z() *= scale;
scaledPoses.insert(std::make_pair(iter->first, t));
}
// compute RMSE statistics
float translational_rmse = 0.0f;
float translational_mean = 0.0f;
float translational_median = 0.0f;
float translational_std = 0.0f;
float translational_min = 0.0f;
float translational_max = 0.0f;
float rotational_rmse = 0.0f;
float rotational_mean = 0.0f;
float rotational_median = 0.0f;
float rotational_std = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
graph::calcRMSE(
groundTruth,
scaledPoses,
translational_rmse,
translational_mean,
translational_median,
translational_std,
translational_min,
translational_max,
rotational_rmse,
rotational_mean,
rotational_median,
rotational_std,
rotational_min,
rotational_max);
Transform t = iter->second.clone();
t.x() *= scale;
t.y() *= scale;
t.z() *= scale;
scaledPoses.insert(std::make_pair(iter->first, t));
}
// compute RMSE statistics
float translational_rmse = 0.0f;
float translational_mean = 0.0f;
float translational_median = 0.0f;
float translational_std = 0.0f;
float translational_min = 0.0f;
float translational_max = 0.0f;
float rotational_rmse = 0.0f;
float rotational_mean = 0.0f;
float rotational_median = 0.0f;
float rotational_std = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
Transform gtToMap = graph::calcRMSE(
groundTruth,
scaledPoses,
translational_rmse,
translational_mean,
translational_median,
translational_std,
translational_min,
translational_max,
rotational_rmse,
rotational_mean,
rotational_median,
rotational_std,
rotational_min,
rotational_max);
if(scale!=0.900f && translational_rmse > bestRMSE)
{
break;
}
bestRMSE = translational_rmse;
bestRMSEAng = rotational_rmse;
bestScale = scale;
}
if(bestScale!=1.0f)
if(bestRMSE!=-1 && translational_rmse > bestRMSE)
{
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;
}
break;
}
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",

40
tools/Reprocess/main.cpp
View File

@@ -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::vector<int> obstacles;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles);
std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_occupied.pcd";
std::vector<int> obstacles, emptySpace;
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