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