From 8c92ed25e556f56f973b78e772592ef5c4c82ee3 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 27 Mar 2019 15:39:46 -0400
Subject: [PATCH] Rtabmap::detectMoreLoopClosures(): publish more progression
 messages for every loop closure found. rtabmap-detectMoreLoopClosures: we can
 do ctrl-c to interrupt the processing safely.

---
 corelib/src/Rtabmap.cpp               | 17 +++++++++++++----
 tools/DetectMoreLoopClosures/main.cpp | 23 +++++++++++++++++++++--
 2 files changed, 34 insertions(+), 6 deletions(-)

diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index 59e8196d..98fd70bf 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -4333,13 +4333,22 @@ int Rtabmap::detectMoreLoopClosures(
 
 							if(updateConstraints)
 							{
-								UINFO("Added new loop closure between %d and %d.", from, to);
 								addedLinks.insert(from);
 								addedLinks.insert(to);
 								cv::Mat inf = getInformation(info.covariance);
 								links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
 								loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
-								UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
+								std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
+								UINFO(msg.c_str());
+
+								if(processState)
+								{
+									UINFO(msg.c_str());
+									if(!processState->callback(msg))
+									{
+										return -1;
+									}
+								}
 							}
 						}
 					}
@@ -4349,7 +4358,7 @@ int Rtabmap::detectMoreLoopClosures(
 
 		if(processState)
 		{
-			std::string msg = uFormat("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
+			std::string msg = uFormat("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2);
 			UINFO(msg.c_str());
 			if(!processState->callback(msg))
 			{
@@ -4358,7 +4367,7 @@ int Rtabmap::detectMoreLoopClosures(
 		}
 		else
 		{
-			UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
+			UINFO("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2);
 		}
 
 		if(addedLinks.size() == 0)
diff --git a/tools/DetectMoreLoopClosures/main.cpp b/tools/DetectMoreLoopClosures/main.cpp
index acccbd0d..13a3e1af 100644
--- a/tools/DetectMoreLoopClosures/main.cpp
+++ b/tools/DetectMoreLoopClosures/main.cpp
@@ -57,6 +57,14 @@ void showUsage()
 	exit(1);
 }
 
+// catch ctrl-c
+bool g_loopForever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_loopForever = false;
+}
+
 class PrintProgressState : public ProgressState
 {
 public:
@@ -64,12 +72,16 @@ public:
 	{
 		if(!msg.empty())
 			printf("%s \n", msg.c_str());
-		return true;
+		return g_loopForever;
 	}
 };
 
 int main(int argc, char * argv[])
 {
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kError);
 
@@ -183,7 +195,14 @@ int main(int argc, char * argv[])
 	int detected = rtabmap.detectMoreLoopClosures(clusterRadius, clusterAngle, iterations, intraSession, interSession, &progress);
 	if(detected < 0)
 	{
-		printf("Loop closure detection failed!\n");
+		if(!g_loopForever)
+		{
+			printf("Detection interrupted. Loop closures found so far (if any) are not saved.\n");
+		}
+		else
+		{
+			printf("Loop closure detection failed!\n");
+		}
 	}
 
 	rtabmap.close();