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

corelib/include/rtabmap/core/Graph.h

@@ -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

@@ -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

@@ -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

@@ -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();