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

report tool: exporting poses in RGB-D dataset format by default, also exporting odom, slam and gt poses.

Browse Source
This commit is contained in:
matlabbe
2019-09-29 19:38:54 -04:00
parent 3fd5cdfb75
commit d360420204
2 changed files with 42 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/Graph.cpp
View File

@@ -85,11 +85,12 @@ bool exportPoses(
tmpPath+=".txt"; tmpPath+=".txt";
} }
if(format == 1) if(format == 1 || format == 10)
{ {
if(stamps.size() != poses.size()) if(stamps.size() != poses.size())
{ {
UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size!"); UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size! stamps=%d psoes=%d",
(int)stamps.size(), (int)poses.size());
return false; return false;
} }
} }

70
tools/Report/main.cpp
View File

@@ -183,6 +183,7 @@ int main(int argc, char * argv[])
driver->getAllNodeIds(ids); driver->getAllNodeIds(ids);
std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics(); std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
std::map<int, Transform> odomPoses, gtPoses; std::map<int, Transform> odomPoses, gtPoses;
std::map<int, double> odomStamps;
std::vector<float> cameraTime; std::vector<float> cameraTime;
cameraTime.reserve(ids.size()); cameraTime.reserve(ids.size());
std::vector<float> odomTime; std::vector<float> odomTime;
@@ -212,6 +213,7 @@ int main(int argc, char * argv[])
if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors)) if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors))
{ {
odomPoses.insert(std::make_pair(*iter, p)); odomPoses.insert(std::make_pair(*iter, p));
odomStamps.insert(std::make_pair(*iter, s));
if(!gt.isNull()) if(!gt.isNull())
{ {
gtPoses.insert(std::make_pair(*iter, gt)); gtPoses.insert(std::make_pair(*iter, gt));
@@ -493,49 +495,55 @@ int main(int argc, char * argv[])
std::string dir = UDirectory::getDir(filePath); std::string dir = UDirectory::getDir(filePath);
std::string dbName = UFile::getName(filePath); std::string dbName = UFile::getName(filePath);
dbName = dbName.substr(0, dbName.size()-3); // remove db dbName = dbName.substr(0, dbName.size()-3); // remove db
std::string path = dir+UDirectory::separator()+dbName+"_poses.txt"; std::string path = dir+UDirectory::separator()+dbName+"_slam.txt";
if(!graph::exportPoses(path, outputKittiError?2:0, poses)) std::multimap<int, Link> dummyLinks;
std::map<int, double> stamps;
if(!outputKittiError)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(odomStamps.find(iter->first) != odomStamps.end());
stamps.insert(*odomStamps.find(iter->first));
}
}
if(!graph::exportPoses(path, outputKittiError?2:10, poses, dummyLinks, stamps))
{ {
printf("Could not export the poses to \"%s\"!?!\n", path.c_str()); printf("Could not export the poses to \"%s\"!?!\n", path.c_str());
} }
//export odom
path = dir+UDirectory::separator()+dbName+"_odom.txt";
stamps.clear();
if(!outputKittiError)
{
for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
{
UASSERT(odomStamps.find(iter->first) != odomStamps.end());
stamps.insert(*odomStamps.find(iter->first));
}
}
if(!graph::exportPoses(path, outputKittiError?2:10, odomPoses, dummyLinks, stamps))
{
printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
}
//export ground truth
if(groundTruth.size()) if(groundTruth.size())
{ {
// For missing ground truth poses, set them to null path = dir+UDirectory::separator()+dbName+"_gt.txt";
std::vector<int> validIndices(poses.size(), 1); stamps.clear();
int i=0; if(!outputKittiError)
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter, ++i)
{ {
if(groundTruth.find(iter->first) == groundTruth.end()) for(std::map<int, Transform>::iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
{ {
groundTruth.insert(std::make_pair(iter->first, Transform())); UASSERT(odomStamps.find(iter->first) != odomStamps.end());
validIndices[i] = 0; stamps.insert(*odomStamps.find(iter->first));
} }
} }
path = dir+UDirectory::separator()+dbName+"_gt.txt"; if(!graph::exportPoses(path, outputKittiError?2:10, groundTruth, dummyLinks, stamps))
if(!graph::exportPoses(path, outputKittiError?2:0, groundTruth))
{ {
printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str()); printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
} }
else
{
// save valid indices
path = dir+UDirectory::separator()+dbName+"_indices.txt";
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, path.c_str(), "w");
#else
file = fopen(path.c_str(), "w");
#endif
if(file)
{
// VERTEX3 id x y z phi theta psi
for(unsigned int k=0; k<validIndices.size(); ++k)
{
fprintf(file, "%d\n", validIndices[k]);
}
fclose(file);
}
}
} }
} }
} }