Skip to content

Commit

Permalink
Landmark poses can be exported with format=11. Report: added option p…
Browse files Browse the repository at this point in the history 
…oses_raw to export poses not aligned with ground truth (default it is).
  • Loading branch information
@matlabbe
matlabbe committed Sep 23, 2024
1 parent 21abddd commit 8c0e57b
Show file tree
Hide file tree
Showing 2 changed files with 85 additions and 14 deletions.
17 changes: 15 additions & 2 deletions corelib/src/Graph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,20 @@ bool exportPoses(
#endif
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
{
Expand All @@ -125,7 +138,7 @@ bool exportPoses(
// Format: stamp x y z qx qy qz qw
Eigen::Quaternionf q = pose.getQuaternionf();

if(iter == poses.begin())
if(iter == posesList.begin())
{
// header
if(format == 11)
Expand Down
82 changes: 70 additions & 12 deletions tools/Report/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,9 @@ void showUsage()
" --loop Compute relative motion error of loop closures.\n"
" --scale Find the best scale for the map against the ground truth\n"
" and compute error based on the scaled path.\n"
" --poses Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n"
" and valid ground truth indices to [path]_indices.txt \n"
" --poses Export odometry to [path]_odom.txt, optimized graph to [path]_slam.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"
" override the ground truth set in database if there is one.\n"
" If extension is *.db, the optimized poses of that database will\n"
Expand Down Expand Up @@ -144,6 +145,7 @@ int main(int argc, char * argv[])
bool outputRelativeError = false;
bool outputReport = false;
bool outputLoopAccuracy = false;
bool outputPosesAlignedToGt = true;
bool incrementalOptimization = false;
bool showAvailableStats = false;
bool invertFigures = false;
Expand Down Expand Up @@ -191,6 +193,11 @@ int main(int argc, char * argv[])
{
outputPoses = true;
}
else if(strcmp(argv[i], "--poses_raw") == 0)
{
outputPoses = true;
outputPosesAlignedToGt = false;
}
else if(strcmp(argv[i], "--loop") == 0)
{
outputLoopAccuracy = true;
Expand Down Expand Up @@ -886,6 +893,7 @@ int main(int argc, char * argv[])
float bestRMSEAng = -1;
float bestVoRMSE = -1;
Transform bestGtToMap = Transform::getIdentity();
Transform bestGtToOdom = Transform::getIdentity();
float kitti_t_err = 0.0f;
float kitti_r_err = 0.0f;
float relative_t_err = 0.0f;
Expand Down Expand Up @@ -944,10 +952,20 @@ int main(int argc, char * argv[])
{
//remove landmarks
std::map<int, Transform>::iterator iter=poses.begin();
std::map<int, Transform> optimizedLandmarks;
while(iter!=poses.end() && iter->first < 0)
{
optimizedLandmarks.insert(*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;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
Expand Down Expand Up @@ -993,7 +1011,7 @@ int main(int argc, char * argv[])
float rotational_min = 0.0f;
float rotational_max = 0.0f;

graph::calcRMSE(
Transform gtToOdom = graph::calcRMSE(
groundTruth,
scaledOdomPoses,
translational_rmse,
Expand Down Expand Up @@ -1035,19 +1053,40 @@ int main(int argc, char * argv[])
bestRMSEAng = rotational_rmse;
bestScale = scale;
bestGtToMap = gtToMap;
bestGtToOdom = gtToOdom;
if(!outputScaled)
{
// just did iteration without any scale, then exit
break;
}
}

// Scale/align slam poses
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(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)
Expand Down Expand Up @@ -1099,14 +1138,26 @@ int main(int argc, char * argv[])
std::multimap<int, Link> dummyLinks;
std::map<int, double> stamps;
if(!outputKittiError)
{
// re-add landmarks
uInsert(poses, optimizedLandmarks);
}
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(iter->first < 0)
{
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());
}
Expand All @@ -1116,13 +1167,20 @@ int main(int argc, char * argv[])
stamps.clear();
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());
stamps.insert(*odomStamps.find(iter->first));
if(iter->first < 0)
{
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());
}
Expand All @@ -1140,7 +1198,7 @@ int main(int argc, char * argv[])
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());
}
Expand Down

0 comments on commit 8c0e57b

Please sign in to comment.