void LoopClosing::Run()
{
//ros::Rate r(200);
//while(ros::ok())
while(true)
{
// Check if there are keyframes in the queue
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
{
// Compute similarity transformation [sR|t]
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
}
}
ResetIfRequested();
//r.sleep();
boost::this_thread::sleep(boost::posix_time::milliseconds(200));
}
}
void LocalMapping::RunServer() {
while(1) {
// unique_lock<mutex> lockMapping(mpCC->mMutexMapping);
if(mVerboseMode == -9) {
cout << "xxx Mapping --> Lock Mapping xxx" << endl;
}
if(mVerboseMode == -9) {
cout << "LockSleep: " << mpCC->mLockSleep << endl;
}
while(!mpCC->LockMapping()) {
usleep(mpCC->mLockSleep);
}
if(mVerboseMode == -9) {
cout << "xxx Mapping --> Mapping Locked xxx" << endl;
}
if(mpCC->mbOptActive) {
cout << "\033[1;31m!!!!! ERROR !!!!!\033[0m LocalMapping::Run(...): Optimization active - LocalMapping should be locked" << endl;
}
// Check if there are keyframes in the queue
if(CheckNewKeyFrames()) {
// unique_lock<mutex> lockMapUpdate(mpMap->mMutexMapUpdate);
if(mVerboseMode == -9) {
cout << "xxx Comm --> Lock MapUpdate xxx" << endl;
}
if(mVerboseMode == -9) {
cout << "LockSleep: " << mpCC->mLockSleep << endl;
}
while(!mpMap->LockMapUpdate()) {
usleep(mpCC->mLockSleep);
}
if(mVerboseMode == -9) {
cout << "xxx Mapping --> MapUpdate Locked xxx" << endl;
}
// pop KF from queue
ProcessNewKeyFrame();
//Local BA
// mbAbortBA=false;
// static size_t lbacount = 0;
// if(mpMap->KeyFramesInMap()>10 && (lbacount % 1)==0)
// {
//// cout << "lba start" << endl;
// Optimizer::LocalBundleAdjustmentClient(mpCurrentKeyFrame,&mbAbortBA, mpMap,mpComm,mClientId,eSystemState::SERVER);
//// cout << "lba end" << endl;
// }
// ++lbacount;
// if(CheckKfsForLBA())
// {
// kfptr pKfForLBA;
// {
// unique_lock<mutex> lock(mMutexKFsForLBA);
// pKfForLBA = mlKfsForLBA.front();
// mlKfsForLBA.pop_front();
// }
// if(mpMap->KeyFramesInMap()>10)
// Optimizer::LocalBundleAdjustmentClient(pKfForLBA,&mbAbortBA, mpMap,mpComm,mClientId,eSystemState::SERVER);
// }
// Check redundant local Keyframes
KeyFrameCulling();
// cout << "mpCurrentKF->mId" << mpCurrentKeyFrame->mId.first << "|" << mpCurrentKeyFrame->mId.second << " ->GetPose: " << mpCurrentKeyFrame->GetPose() << endl;
// cout << "mpCurrentKF->mId" << mpCurrentKeyFrame->mId.first << "|" << mpCurrentKeyFrame->mId.second << " ->GetPoseInverse: " << mpCurrentKeyFrame->GetPoseInverse() << endl;
// cout << "mpCurrentKF->mId" << mpCurrentKeyFrame->mId.first << "|" << mpCurrentKeyFrame->mId.second << " ->GetCameraCenter: " << mpCurrentKeyFrame->GetCameraCenter() << endl;
#ifndef MAPFUSION
mpLoopFinder->InsertKF(mpCurrentKeyFrame);
#endif
mpMapMatcher->InsertKF(mpCurrentKeyFrame);
mpKFDB->add(mpCurrentKeyFrame);
mpMap->UnLockMapUpdate();
}
// cout << "Culled KF IDs: " << endl;
// for(int idv = 0; idv < mCulledKfIds.size(); ++idv)
// cout << mCulledKfIds[idv];
ResetIfRequested();
mpCC->UnLockMapping();
usleep(mMappingRate);
}
}
void LocalMapping::RunClient() {
mbFinished = false;
while(1) {
// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(false);
// Check if there are keyframes in the queue
if(CheckNewKeyFrames()) {
// Get Communicator Mutex -> Comm cannot publish. Assure no publishing whilst changing data
// if(mpComm->mbStrictLock) unique_lock<mutex> lockComm(mpComm->mMutexForMapping);
if(mVerboseMode == -9) {
cout << "xxx Mapping --> Lock Mapping xxx" << endl;
}
if(mVerboseMode == -9) {
cout << "LockSleep: " << mpCC->mLockSleep << endl;
}
if(mpComm->mbStrictLock) while(!mpCC->LockMapping()) {
usleep(mpCC->mLockSleep);
}
if(mVerboseMode == -9) {
cout << "xxx Mapping --> Mapping Locked xxx" << endl;
}
// BoW conversion and insertion in Map
ProcessNewKeyFrame();
// Check recent MapPoints
MapPointCulling();
// Triangulate new MapPoints
CreateNewMapPoints();
if(!CheckNewKeyFrames()) {
// Find more matches in neighbor keyframes and fuse point duplications
SearchInNeighbors();
}
mbAbortBA = false;
if(!CheckNewKeyFrames() && !stopRequested()) {
// Local BA
if(mpMap->KeyFramesInMap() > 2) {
Optimizer::LocalBundleAdjustmentClient(mpCurrentKeyFrame, &mbAbortBA, mpMap, mpComm, mClientId);
}
// // Check redundant local Keyframes
// KeyFrameCulling();
}
if(mpComm->mbStrictLock) {
mpCC->UnLockMapping();
}
} else if(Stop()) {
// Safe area to stop
while(isStopped() && !CheckFinish()) {
usleep(mMappingRate);
}
if(CheckFinish()) {
break;
}
}
ResetIfRequested();
// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(true);
if(CheckFinish()) {
break;
}
usleep(mMappingRate);
}
SetFinish();
}
void LocalMapping::Run()
{
//ros::Rate r(500);
//while(ros::ok())
while(true)
{
// Check if there are keyframes in the queue
if(CheckNewKeyFrames())
{
// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(false);
// BoW conversion and insertion in Map
ProcessNewKeyFrame();
// Check recent MapPoints
MapPointCulling();
// Triangulate new MapPoints
CreateNewMapPoints();
// Find more matches in neighbor keyframes and fuse point duplications
SearchInNeighbors();
mbAbortBA = false;
if(!CheckNewKeyFrames() && !stopRequested())
{
// Local BA
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA);
// Check redundant local Keyframes
KeyFrameCulling();
mpMap->SetFlagAfterBA();
// Tracking will see Local Mapping idle
if(!CheckNewKeyFrames())
SetAcceptKeyFrames(true);
}
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
// Safe area to stop
if(stopRequested())
{
Stop();
//ros::Rate r2(1000);
while(isStopped())
{
//r2.sleep();
boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
}
SetAcceptKeyFrames(true);
}
ResetIfRequested();
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
}
}
