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

fixed optimization error ratio bug where comparison was done using not highest ratio link but on highest abs error link instead.

Browse Source
This commit is contained in:
matlabbe
2018-07-20 15:11:46 -04:00
parent 173bd49a26
commit e95cabb1fc
4 changed files with 73 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
tools/EurocDataset/main.cpp
View File

@@ -338,7 +338,7 @@ int main(int argc, char * argv[])
printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(440);
}
else if(seq.compare("MH_01_easy") == 0)
else if(seq.compare("MH_02_easy") == 0)
{
printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n");
((CameraStereoImages*)cameraThread.camera())->setStartIndex(525);
@@ -573,9 +573,12 @@ int main(int argc, char * argv[])
// Save trajectory
printf("Saving trajectory ...\n");
std::map<int, Transform> poses;
std::map<int, Transform> vo_poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures;
std::map<int, double> stamps;
rtabmap.getGraph(vo_poses, links, false, true);
links.clear();
rtabmap.getGraph(poses, links, true, true, &signatures);
for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
@@ -624,6 +627,25 @@ int main(int argc, char * argv[])
float rotational_std = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
// vo performance
graph::calcRMSE(
groundTruth,
vo_poses,
translational_rmse,
translational_mean,
translational_median,
translational_std,
translational_min,
translational_max,
rotational_rmse,
rotational_mean,
rotational_median,
rotational_std,
rotational_min,
rotational_max);
float translational_rmse_vo = translational_rmse;
float rotational_rmse_vo = rotational_rmse;
// SLAM performance
graph::calcRMSE(
groundTruth,
poses,
@@ -640,8 +662,8 @@ int main(int argc, char * argv[])
rotational_min,
rotational_max);
printf(" translational_rmse= %f m\n", translational_rmse);
printf(" rotational_rmse= %f deg\n", rotational_rmse);
printf(" translational_rmse= %f m (vo = %f m)\n", translational_rmse, translational_rmse_vo);
printf(" rotational_rmse= %f deg (vo = %f def)\n", rotational_rmse, rotational_rmse_vo);
FILE * pFile = 0;
std::string pathErrors = output+"/"+outputName+"_rmse.txt";

32
tools/Report/main.cpp
View File

@@ -293,6 +293,7 @@ int main(int argc, char * argv[])
float bestScale = 1.0f;
float bestRMSE = -1;
float bestRMSEAng = -1;
float bestVoRMSE = -1;
Transform bestGtToMap = Transform::getIdentity();
float kitti_t_err = 0.0f;
float kitti_r_err = 0.0f;
@@ -335,6 +336,8 @@ int main(int argc, char * argv[])
for(float scale=outputScaled?0.900f:1.0f; scale<1.100f; scale+=0.001)
{
std::map<int, Transform> scaledPoses;
std::map<int, Transform> scaledOdomPoses;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform t = iter->second.clone();
@@ -342,6 +345,13 @@ int main(int argc, char * argv[])
t.y() *= scale;
t.z() *= scale;
scaledPoses.insert(std::make_pair(iter->first, t));
UASSERT(posesOut.find(iter->first)!=posesOut.end());
t = posesOut.at(iter->first).clone();
t.x() *= scale;
t.y() *= scale;
t.z() *= scale;
scaledOdomPoses.insert(std::make_pair(iter->first, t));
}
// compute RMSE statistics
float translational_rmse = 0.0f;
@@ -356,6 +366,24 @@ int main(int argc, char * argv[])
float rotational_std = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
graph::calcRMSE(
groundTruth,
scaledOdomPoses,
translational_rmse,
translational_mean,
translational_median,
translational_std,
translational_min,
translational_max,
rotational_rmse,
rotational_mean,
rotational_median,
rotational_std,
rotational_min,
rotational_max);
float translational_rmse_vo = translational_rmse;
Transform gtToMap = graph::calcRMSE(
groundTruth,
scaledPoses,
@@ -377,6 +405,7 @@ int main(int argc, char * argv[])
break;
}
bestRMSE = translational_rmse;
bestVoRMSE = translational_rmse_vo;
bestRMSEAng = rotational_rmse;
bestScale = scale;
bestGtToMap = gtToMap;
@@ -461,12 +490,13 @@ int main(int argc, char * argv[])
}
}
}
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",
printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm, odom=%.3fm) ang=%.1fdeg%s, slam: avg=%dms (max=%dms) loops=%d, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
fileName.c_str(),
(int)ids.size(),
bestScale,
bestRMSE,
maxRMSE,
bestVoRMSE,
bestRMSEAng,
!outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
(int)uMean(slamTime), (int)uMax(slamTime),