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

Landmark poses can be exported with format=11. Report: added option poses_raw to export poses not aligned with ground truth (default it is).

Browse Source
This commit is contained in:
matlabbe
2024-09-22 19:53:27 -07:00
parent 21abdddbff
commit 8c0e57bc52
2 changed files with 85 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
corelib/src/Graph.cpp
View File

@@ -104,7 +104,20 @@ bool exportPoses(
#endif #endif
if(fout) if(fout)
{ {
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) std::list<std::pair<int, Transform> > posesList;
for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter)
{
posesList.push_back(*iter);
}
if(format == 11)
{
// Put landmarks at the end
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter)
{
posesList.push_back(*iter);
}
}
for(std::list<std::pair<int, Transform> >::const_iterator iter=posesList.begin(); iter!=posesList.end(); ++iter)
{ {
if(format == 1 || format == 10 || format == 11) // rgbd-slam format if(format == 1 || format == 10 || format == 11) // rgbd-slam format
{ {
@@ -125,7 +138,7 @@ bool exportPoses(
// Format: stamp x y z qx qy qz qw // Format: stamp x y z qx qy qz qw
Eigen::Quaternionf q = pose.getQuaternionf(); Eigen::Quaternionf q = pose.getQuaternionf();
if(iter == poses.begin()) if(iter == posesList.begin())
{ {
// header // header
if(format == 11) if(format == 11)

82
tools/Report/main.cpp
View File

@@ -60,8 +60,9 @@ void showUsage()
" --loop Compute relative motion error of loop closures.\n" " --loop Compute relative motion error of loop closures.\n"
" --scale Find the best scale for the map against the ground truth\n" " --scale Find the best scale for the map against the ground truth\n"
" and compute error based on the scaled path.\n" " and compute error based on the scaled path.\n"
" --poses Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n" " --poses Export odometry to [path]_odom.txt, optimized graph to [path]_slam.txt \n"
" and valid ground truth indices to [path]_indices.txt \n" " and ground truth to [path]_gt.txt in TUM RGB-D format.\n"
" --poses_raw Same as --poses, but poses are not aligned to gt."
" --gt FILE.txt Use this file as ground truth (TUM RGB-D format). It will\n" " --gt FILE.txt Use this file as ground truth (TUM RGB-D format). It will\n"
" override the ground truth set in database if there is one.\n" " override the ground truth set in database if there is one.\n"
" If extension is *.db, the optimized poses of that database will\n" " If extension is *.db, the optimized poses of that database will\n"
@@ -144,6 +145,7 @@ int main(int argc, char * argv[])
bool outputRelativeError = false; bool outputRelativeError = false;
bool outputReport = false; bool outputReport = false;
bool outputLoopAccuracy = false; bool outputLoopAccuracy = false;
bool outputPosesAlignedToGt = true;
bool incrementalOptimization = false; bool incrementalOptimization = false;
bool showAvailableStats = false; bool showAvailableStats = false;
bool invertFigures = false; bool invertFigures = false;
@@ -191,6 +193,11 @@ int main(int argc, char * argv[])
{ {
outputPoses = true; outputPoses = true;
} }
else if(strcmp(argv[i], "--poses_raw") == 0)
{
outputPoses = true;
outputPosesAlignedToGt = false;
}
else if(strcmp(argv[i], "--loop") == 0) else if(strcmp(argv[i], "--loop") == 0)
{ {
outputLoopAccuracy = true; outputLoopAccuracy = true;
@@ -886,6 +893,7 @@ int main(int argc, char * argv[])
float bestRMSEAng = -1; float bestRMSEAng = -1;
float bestVoRMSE = -1; float bestVoRMSE = -1;
Transform bestGtToMap = Transform::getIdentity(); Transform bestGtToMap = Transform::getIdentity();
Transform bestGtToOdom = Transform::getIdentity();
float kitti_t_err = 0.0f; float kitti_t_err = 0.0f;
float kitti_r_err = 0.0f; float kitti_r_err = 0.0f;
float relative_t_err = 0.0f; float relative_t_err = 0.0f;
@@ -944,10 +952,20 @@ int main(int argc, char * argv[])
{ {
//remove landmarks //remove landmarks
std::map<int, Transform>::iterator iter=poses.begin(); std::map<int, Transform>::iterator iter=poses.begin();
std::map<int, Transform> optimizedLandmarks;
while(iter!=poses.end() && iter->first < 0) while(iter!=poses.end() && iter->first < 0)
{ {
optimizedLandmarks.insert(*iter);
poses.erase(iter++); poses.erase(iter++);
} }
if(outputKittiError) {
// remove landmarks
std::map<int, Transform>::iterator iter=posesOut.begin();
while(iter!=posesOut.end() && iter->first < 0)
{
posesOut.erase(iter++);
}
}
std::map<int, Transform> groundTruth; std::map<int, Transform> groundTruth;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -993,7 +1011,7 @@ int main(int argc, char * argv[])
float rotational_min = 0.0f; float rotational_min = 0.0f;
float rotational_max = 0.0f; float rotational_max = 0.0f;
graph::calcRMSE( Transform gtToOdom = graph::calcRMSE(
groundTruth, groundTruth,
scaledOdomPoses, scaledOdomPoses,
translational_rmse, translational_rmse,
@@ -1035,6 +1053,7 @@ int main(int argc, char * argv[])
bestRMSEAng = rotational_rmse; bestRMSEAng = rotational_rmse;
bestScale = scale; bestScale = scale;
bestGtToMap = gtToMap; bestGtToMap = gtToMap;
bestGtToOdom = gtToOdom;
if(!outputScaled) if(!outputScaled)
{ {
// just did iteration without any scale, then exit // just did iteration without any scale, then exit
@@ -1042,12 +1061,32 @@ int main(int argc, char * argv[])
} }
} }
// Scale/align slam poses
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
iter->second.x()*=bestScale; iter->second.x()*=bestScale;
iter->second.y()*=bestScale; iter->second.y()*=bestScale;
iter->second.z()*=bestScale; iter->second.z()*=bestScale;
iter->second = bestGtToMap * iter->second; if(outputPosesAlignedToGt)
iter->second = bestGtToMap * iter->second;
}
for(std::map<int, Transform>::iterator iter=optimizedLandmarks.begin(); iter!=optimizedLandmarks.end(); ++iter)
{
iter->second.x()*=bestScale;
iter->second.y()*=bestScale;
iter->second.z()*=bestScale;
if(outputPosesAlignedToGt)
iter->second = bestGtToMap * iter->second;
}
// Scale/align odom poses
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
{
iter->second.x()*=bestScale;
iter->second.y()*=bestScale;
iter->second.z()*=bestScale;
if(outputPosesAlignedToGt)
iter->second = bestGtToOdom * iter->second;
} }
if(outputRelativeError) if(outputRelativeError)
@@ -1099,14 +1138,26 @@ int main(int argc, char * argv[])
std::multimap<int, Link> dummyLinks; std::multimap<int, Link> dummyLinks;
std::map<int, double> stamps; std::map<int, double> stamps;
if(!outputKittiError) if(!outputKittiError)
{
// re-add landmarks
uInsert(poses, optimizedLandmarks);
}
if(!outputKittiError)
{ {
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
UASSERT(odomStamps.find(iter->first) != odomStamps.end()); if(iter->first < 0)
stamps.insert(*odomStamps.find(iter->first)); {
stamps.insert(std::make_pair(iter->first, 0));
}
else
{
UASSERT(odomStamps.find(iter->first) != odomStamps.end());
stamps.insert(*odomStamps.find(iter->first));
}
} }
} }
if(!graph::exportPoses(path, outputKittiError?2:10, poses, dummyLinks, stamps)) if(!graph::exportPoses(path, outputKittiError?2:11, 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());
} }
@@ -1116,13 +1167,20 @@ int main(int argc, char * argv[])
stamps.clear(); stamps.clear();
if(!outputKittiError) if(!outputKittiError)
{ {
for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter) for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
{ {
UASSERT(odomStamps.find(iter->first) != odomStamps.end()); if(iter->first < 0)
stamps.insert(*odomStamps.find(iter->first)); {
stamps.insert(std::make_pair(iter->first, 0));
}
else
{
UASSERT(odomStamps.find(iter->first) != odomStamps.end());
stamps.insert(*odomStamps.find(iter->first));
}
} }
} }
if(!graph::exportPoses(path, outputKittiError?2:10, odomPoses, dummyLinks, stamps)) if(!graph::exportPoses(path, outputKittiError?2:11, posesOut, dummyLinks, stamps))
{ {
printf("Could not export the odometry to \"%s\"!?!\n", path.c_str()); printf("Could not export the odometry to \"%s\"!?!\n", path.c_str());
} }
@@ -1140,7 +1198,7 @@ int main(int argc, char * argv[])
stamps.insert(*odomStamps.find(iter->first)); stamps.insert(*odomStamps.find(iter->first));
} }
} }
if(!graph::exportPoses(path, outputKittiError?2:10, groundTruth, dummyLinks, stamps)) if(!graph::exportPoses(path, outputKittiError?2:11, groundTruth, dummyLinks, stamps))
{ {
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());
} }