Updated rtabmap-kitti tool

corelib/src/Parameters.cpp

@@ -136,7 +136,8 @@ ParametersMap Parameters::deserialize(const std::string & parameters)
 
 			}
 
-			if(Parameters::getDefaultParameters().find(key) == Parameters::getDefaultParameters().end())
+			if(oldIter==Parameters::getRemovedParameters().end() &&
+			   Parameters::getDefaultParameters().find(key) == Parameters::getDefaultParameters().end())
 			{
 				UWARN("Unknown parameter \"%s\"=\"%s\"! The parameter is still added to output map.", key.c_str(), value.c_str());
 			}

tools/Report/main.cpp

@@ -43,7 +43,13 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-report [\"Statistic/Id\"] [--latex] [--kitti] [--scale] [--poses] path\n"
-			"  path               Directory containing rtabmap databases or path of a database.\n\n");
+			"  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"
+			"  --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\n");
 	exit(1);
 }
 
@@ -54,6 +60,9 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
 	QApplication app(argc, argv);
 
 	bool outputLatex = false;
@@ -161,7 +170,9 @@ int main(int argc, char * argv[])
 				ParametersMap params;
 				if(driver->openConnection(filePath))
 				{
+					ULogger::setLevel(ULogger::kError); // to suppress parameter warnings
 					params = driver->getLastParameters();
+					ULogger::setLevel(ULogger::kWarning);
 					std::set<int> ids;
 					driver->getAllNodeIds(ids);
 					std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
@@ -294,7 +305,24 @@ int main(int argc, char * argv[])
 						optimizer->getConnectedGraph(firstId, odomPoses, graph::filterDuplicateLinks(links), posesOut, linksOut);
 
 						std::map<int, Transform> poses = optimizer->optimize(firstId, posesOut, linksOut);
+						if(poses.empty())
+						{
+							// try incremental optimization
+							UWARN("Optimization failed! Try incremental optimization...");
+							poses = optimizer->optimizeIncremental(firstId, posesOut, linksOut);
+							if(poses.empty())
+							{
+								UERROR("Incremental optimization also failed! Only original RMSE will be shown.");
+								bestRMSE = rmse;
+							}
+							else
+							{
+								UWARN("Incremental optimization succeeded!");
+							}
+						}
 
+						if(poses.size())
+						{
 							std::map<int, Transform> groundTruth;
 							for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 							{
@@ -393,15 +421,46 @@ int main(int argc, char * argv[])
 								}
 								if(groundTruth.size())
 								{
+									// For missing ground truth poses, set them to null
+									std::vector<int> validIndices(poses.size(), 1);
+									int i=0;
+									for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter, ++i)
+									{
+										if(groundTruth.find(iter->first) == groundTruth.end())
+										{
+											groundTruth.insert(std::make_pair(iter->first, Transform()));
+											validIndices[i] = 0;
+										}
+									}
 									path = dir+UDirectory::separator()+dbName+"_gt.txt";
 									if(!graph::exportPoses(path, outputKittiError?2:0, groundTruth))
 									{
 										printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
 									}
+									else
+									{
+										// save valid indices
+										path = dir+UDirectory::separator()+dbName+"_indices.txt";
+										FILE * file = 0;
+	#ifdef _MSC_VER
+										fopen_s(&file, path.c_str(), "w");
+	#else
+										file = fopen(path.c_str(), "w");
+	#endif
+										if(file)
+										{
+											// VERTEX3 id x y z phi theta psi
+											for(unsigned int k=0; k<validIndices.size(); ++k)
+											{
+												fprintf(file, "%d\n", validIndices[k]);
+											}
+											fclose(file);
+										}
+									}
+								}
 							}
 						}
 					}
-
 					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",
 							fileName.c_str(),
 							(int)ids.size(),