i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rtabmap::detectMoreLoopClosures(): publish more progression messages for every loop closure found. rtabmap-detectMoreLoopClosures: we can do ctrl-c to interrupt the processing safely.

Browse Source
This commit is contained in:
matlabbe
2019-03-27 15:39:46 -04:00
parent 80ef0e1a75
commit 8c92ed25e5
2 changed files with 34 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
corelib/src/Rtabmap.cpp
View File

@@ -4333,13 +4333,22 @@ int Rtabmap::detectMoreLoopClosures(
if(updateConstraints) if(updateConstraints)
{ {
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from); addedLinks.insert(from);
addedLinks.insert(to); addedLinks.insert(to);
cv::Mat inf = getInformation(info.covariance); cv::Mat inf = getInformation(info.covariance);
links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf))); links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
loopClosuresAdded.push_back(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) 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()); UINFO(msg.c_str());
if(!processState->callback(msg)) if(!processState->callback(msg))
{ {
@@ -4358,7 +4367,7 @@ int Rtabmap::detectMoreLoopClosures(
} }
else 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) if(addedLinks.size() == 0)

23
tools/DetectMoreLoopClosures/main.cpp
View File

@@ -57,6 +57,14 @@ void showUsage()
exit(1); 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 class PrintProgressState : public ProgressState
{ {
public: public:
@@ -64,12 +72,16 @@ public:
{ {
if(!msg.empty()) if(!msg.empty())
printf("%s \n", msg.c_str()); printf("%s \n", msg.c_str());
return true; return g_loopForever;
} }
}; };
int main(int argc, char * argv[]) int main(int argc, char * argv[])
{ {
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
ULogger::setType(ULogger::kTypeConsole); ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kError); ULogger::setLevel(ULogger::kError);
@@ -183,7 +195,14 @@ int main(int argc, char * argv[])
int detected = rtabmap.detectMoreLoopClosures(clusterRadius, clusterAngle, iterations, intraSession, interSession, &progress); int detected = rtabmap.detectMoreLoopClosures(clusterRadius, clusterAngle, iterations, intraSession, interSession, &progress);
if(detected < 0) 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(); rtabmap.close();