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::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::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)); } }