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