i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +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
corelib/include/rtabmap/core/Graph.h
+13
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).
corelib/src/Graph.cpp
+42
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,
tools/Report/main.cpp
+36 -1
View File
@@ -46,6 +46,7 @@ void showUsage()
" path Directory containing rtabmap databases or path of a database.\n"
" --latex Print table formatted in LaTeX with results.\n"
" --kitti Compute error based on KITTI benchmark.\n"
" --relative Compute relative motion error between poses.\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"
@@ -69,6 +70,7 @@ int main(int argc, char * argv[])
bool outputScaled = false;
bool outputPoses = false;
bool outputKittiError = false;
bool outputRelativeError = false;
std::map<std::string, UPlot*> figures;
for(int i=1; i<argc-1; ++i)
{
@@ -80,6 +82,10 @@ int main(int argc, char * argv[])
{
outputKittiError = true;
}
else if(strcmp(argv[i], "--relative") == 0)
{
outputRelativeError = true;
}
else if(strcmp(argv[i], "--scale") == 0)
{
outputScaled = true;
@@ -303,6 +309,8 @@ int main(int argc, char * argv[])
Transform bestGtToMap = Transform::getIdentity();
float kitti_t_err = 0.0f;
float kitti_r_err = 0.0f;
float relative_t_err = 0.0f;
float relative_r_err = 0.0f;
if(ids.size())
{
std::map<int, Transform> posesOut;
@@ -438,6 +446,32 @@ int main(int argc, char * argv[])
iter->second = bestGtToMap * iter->second;
}
if(outputRelativeError)
{
if(groundTruth.size() == poses.size())
{
// compute Motion statistics
graph::calcRelativeErrors(uValues(groundTruth), uValues(poses), relative_t_err, relative_r_err);
}
else
{
std::vector<Transform> gtPoses;
std::vector<Transform> rPoses;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(groundTruth.find(iter->first) != groundTruth.end())
{
gtPoses.push_back(groundTruth.at(iter->first));
rPoses.push_back(poses.at(iter->first));
}
}
if(!gtPoses.empty())
{
graph::calcRelativeErrors(gtPoses, rPoses, relative_t_err, relative_r_err);
}
}
}
if(outputKittiError)
{
if(groundTruth.size() == poses.size())
@@ -504,7 +538,7 @@ int main(int argc, char * argv[])
}
}
}
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",
printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm, odom=%.3fm) ang=%.1fdeg%s%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,
@@ -513,6 +547,7 @@ int main(int argc, char * argv[])
bestVoRMSE,
bestRMSEAng,
!outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
!outputRelativeError?"":uFormat(", Relative: t_err=%.3fm r_err=%.2f deg", relative_t_err, relative_r_err).c_str(),
(int)uMean(slamTime), (int)uMax(slamTime),
(int)loopClosureLinks.size(),
(int)uMean(odomTime), (int)uMax(odomTime),
tools/RgbdDataset/main.cpp
+28 -2
View File
@@ -57,6 +57,7 @@ void showUsage()
" \"groundtruth.txt\" is found in the sequence folder, they will be saved in the database.\n"
" --output Output directory. By default, results are saved in \"path\".\n"
" --output_name Output database name (default \"rtabmap\").\n"
" --skip # Skip X frames.\n"
" --quiet Don't show log messages and iteration updates.\n"
"%s\n"
"Example:\n\n"
@@ -90,6 +91,7 @@ int main(int argc, char * argv[])
std::string path;
std::string output;
std::string outputName = "rtabmap";
int skipFrames = 0;
bool quiet = false;
if(argc < 2)
{
@@ -107,6 +109,11 @@ int main(int argc, char * argv[])
{
outputName = argv[++i];
}
else if(std::strcmp(argv[i], "--skip") == 0)
{
skipFrames = atoi(argv[++i]);
UASSERT(skipFrames > 0);
}
else if(std::strcmp(argv[i], "--quiet") == 0)
{
quiet = true;
@@ -150,13 +157,15 @@ int main(int argc, char * argv[])
" RGB path: %s\n"
" Depth path: %s\n"
" Output: %s\n"
" Output name: %s\n",
" Output name: %s\n"
" Skip frames: %d\n",
seq.c_str(),
path.c_str(),
pathRgbImages.c_str(),
pathDepthImages.c_str(),
output.c_str(),
outputName.c_str());
outputName.c_str(),
skipFrames);
if(!pathGt.empty())
{
printf(" groundtruth.txt: %s\n", pathGt.c_str());
@@ -216,6 +225,11 @@ int main(int argc, char * argv[])
{
int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
if(skipFrames>0)
{
totalImages /= skipFrames+1;
}
printf("Processing %d images...\n", totalImages);
ParametersMap odomParameters = parameters;
@@ -236,8 +250,20 @@ int main(int argc, char * argv[])
cv::Mat covariance;
int odomKeyFrames = 0;
double previousStamp = 0.0;
int skipCount = 0;
while(data.isValid() && g_forever)
{
if(skipCount < skipFrames)
{
++skipCount;
cameraInfo = CameraInfo();
timer.restart();
data = cameraThread.camera()->takeImage(&cameraInfo);
continue;
}
skipCount = 0;
cameraThread.postUpdate(&data, &cameraInfo);
cameraInfo.timeTotal = timer.ticks();