Reprocess: added --odom option to recompute odometry.

tools/Reprocess/main.cpp

@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Graph.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/OdometryInfo.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -68,6 +70,10 @@ void showUsage()
 			"                       from the database. If custom parameters are also set as \n"
 			"                       arguments, they overwrite those in config file and the database.\n"
 			"     -default    Input database's parameters are ignored, using default ones instead.\n"
+			"     -odom       Recompute odometry. See \"Odom/\" parameters with --params. If -skip option\n"
+			"                 is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n"
+			"                 cannot be detected in this mode (assuming the database contains continuous frames\n"
+			"                 of a single session).\n"
 			"     -start #    Start from this node ID.\n"
 			"     -stop #     Last node to process.\n"
 			"     -start_s #  Start from this map session ID.\n"
@@ -231,6 +237,7 @@ int main(int argc, char * argv[])
 	bool assemble3dOctoMap = false;
 	bool useDatabaseRate = false;
 	bool useDefaultParameters = false;
+	bool recomputeOdometry = false;
 	int startId = 0;
 	int stopId = 0;
 	int startMapId = 0;
@@ -281,6 +288,10 @@ int main(int argc, char * argv[])
 			useDefaultParameters = true;
 			printf("Using default parameters.\n");
 		}
+		else if(strcmp(argv[i], "-odom") == 0 || strcmp(argv[i], "--odom") == 0)
+		{
+			recomputeOdometry = true;
+		}
 		else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0)
 		{
 			++i;
@@ -770,6 +781,28 @@ int main(int argc, char * argv[])
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), linearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), angularUpdate);
 
+	Odometry * odometry = 0;
+	float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate();
+	double lastUpdateStamp = 0;
+	if(recomputeOdometry)
+	{
+		if(odometryIgnored)
+		{
+			printf("odom option is set but %s parameter is false, odometry won't be recomputed...\n", Parameters::kRGBDEnabled().c_str());
+			recomputeOdometry = false;
+		}
+		else
+		{
+			printf("Odometry will be recomputed (odom option is set)\n");
+			Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
+			if(rtabmapUpdateRate!=0)
+			{
+				rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
+			}
+			odometry = Odometry::create(parameters);
+		}
+	}
+
 	printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
 	std::map<std::string, float> globalMapStats;
 	int processed = 0;
@@ -786,6 +819,44 @@ int main(int argc, char * argv[])
 	bool inMotion = true;
 	while(data.isValid() && g_loopForever)
 	{
+		if(recomputeOdometry)
+		{
+			OdometryInfo odomInfo;
+			Transform pose = odometry->process(data, &odomInfo);
+			printf("Processed %d/%d frames (visual=%d/%d lidar=%f lost=%s)... odometry = %dms\n",
+					processed+1,
+					totalIds,
+					odomInfo.reg.inliers,
+					odomInfo.reg.matches,
+					odomInfo.reg.icpInliersRatio,
+					odomInfo.lost?"true":"false",
+					int(odomInfo.timeEstimation * 1000));
+			if(lastUpdateStamp > 0.0 && data.stamp() < lastUpdateStamp + rtabmapUpdateRate)
+			{
+				if(framesToSkip>0)
+				{
+					int skippedFrames = framesToSkip;
+					while(skippedFrames-- > 0)
+					{
+						++processed;
+						data = dbReader->takeImage();
+					}
+				}
+
+				data = dbReader->takeImage(&info);
+				if(scanFromDepth)
+				{
+					data.setLaserScan(LaserScan());
+				}
+				camThread.postUpdate(&data, &info);
+				++processed;
+				continue;
+			}
+			info.odomPose = pose;
+			info.odomCovariance = odomInfo.reg.covariance;
+			lastUpdateStamp = data.stamp();
+		}
+
 		UTimer iterationTime;
 		std::string status;
 		if(!odometryIgnored && info.odomPose.isNull())
@@ -999,11 +1070,12 @@ int main(int argc, char * argv[])
 
 		Transform odomPose = info.odomPose;
 
-		if(framesToSkip>0)
+		if(framesToSkip>0 && !recomputeOdometry)
 		{
 			int skippedFrames = framesToSkip;
 			while(skippedFrames-- > 0)
 			{
+				processed++;
 				data = dbReader->takeImage(&info);
 				if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
 				{
@@ -1071,6 +1143,8 @@ int main(int argc, char * argv[])
 	rtabmap.close(true);
 	printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
 
+	delete odometry;
+
 	if(assemble2dMap)
 	{
 		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";