void LocalMapping::InsertKeyFrame(KeyFrame *pKF) { boost::mutex::scoped_lock lock(mMutexNewKFs); mlNewKeyFrames.push_back(pKF); mbAbortBA=true; SetAcceptKeyFrames(false); }
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)); } }