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

Added graph::calcRelativeErrors() and option --relative for report. Added skip frames option on rgbd_dataset tool.

Browse Source
This commit is contained in:
matlabbe
2019-04-27 20:39:40 -04:00
parent 195f6147ad
commit 125d532e95
4 changed files with 119 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/include/rtabmap/core/Graph.h
View File

@@ -79,6 +79,19 @@ void RTABMAP_EXP calcKittiSequenceErrors(
float & t_err,
float & r_err);
/**
* Compute average of translation and rotation errors between each poses.
* @param poses_gt, Ground Truth poses
* @param poses_result, Estimated poses
* @param t_err, Output translation error (m)
* @param r_err, Output rotation error (deg)
*/
void RTABMAP_EXP calcRelativeErrors (
const std::vector<Transform> &poses_gt,
const std::vector<Transform> &poses_result,
float & t_err,
float & r_err);
/**
* Compute root-mean-square error (RMSE) like the TUM RGBD
* dataset's evaluation tool (absolute trajectory error).

42
corelib/src/Graph.cpp
View File

@@ -690,6 +690,48 @@ void calcKittiSequenceErrors (
}
// KITTI evaluation end
void calcRelativeErrors (
const std::vector<Transform> &poses_gt,
const std::vector<Transform> &poses_result,
float & t_err,
float & r_err) {
UASSERT(poses_gt.size() == poses_result.size());
// error vector
std::vector<errors> err;
// for all start positions do
for (unsigned int i=0; i<poses_gt.size()-1; ++i)
{
// compute rotational and translational errors
Transform pose_delta_gt = poses_gt[i].inverse()*poses_gt[i+1];
Transform pose_delta_result = poses_result[i].inverse()*poses_result[i+1];
Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
float r_err = pose_error.getAngle();
float t_err = pose_error.getNorm();
// write to file
err.push_back(errors(i,r_err,t_err,0,0));
}
t_err = 0;
r_err = 0;
// for all errors do => compute sum of t_err, r_err
for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
{
t_err += it->t_err;
r_err += it->r_err;
}
// save errors
float num = err.size();
t_err /= num;
r_err /= num;
r_err *= 180/CV_PI; // Rotation error (deg)
}
Transform calcRMSE (
const std::map<int, Transform> & groundTruth,
const std::map<int, Transform> & poses,