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::CreateNewMapPoints() { // Retrieve neighbor keyframes in covisibility graph // int nn = 10; // if(mbMonocular) int nn = 20; const vector<kfptr> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); ORBmatcher matcher(0.6, false); cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation(); cv::Mat Rwc1 = Rcw1.t(); cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation(); cv::Mat Tcw1(3, 4, CV_32F); Rcw1.copyTo(Tcw1.colRange(0, 3)); tcw1.copyTo(Tcw1.col(3)); cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter(); const float &fx1 = mpCurrentKeyFrame->fx; const float &fy1 = mpCurrentKeyFrame->fy; const float &cx1 = mpCurrentKeyFrame->cx; const float &cy1 = mpCurrentKeyFrame->cy; const float &invfx1 = mpCurrentKeyFrame->invfx; const float &invfy1 = mpCurrentKeyFrame->invfy; const float ratioFactor = 1.5f * mpCurrentKeyFrame->mfScaleFactor; int nnew = 0; // Search matches with epipolar restriction and triangulate for(size_t i = 0; i < vpNeighKFs.size(); i++) { if(i > 0 && CheckNewKeyFrames()) { return; } kfptr pKF2 = vpNeighKFs[i]; // Check first that baseline is not too short cv::Mat Ow2 = pKF2->GetCameraCenter(); cv::Mat vBaseline = Ow2 - Ow1; const float baseline = cv::norm(vBaseline); // if(!mbMonocular) // { // if(baseline<pKF2->mb) // continue; // } // else // { const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2); const float ratioBaselineDepth = baseline / medianDepthKF2; if(ratioBaselineDepth < 0.01) { continue; } // } // Compute Fundamental Matrix cv::Mat F12 = ComputeF12(mpCurrentKeyFrame, pKF2); // Search matches that fullfil epipolar constraint vector<pair<size_t, size_t> > vMatchedIndices; matcher.SearchForTriangulation(mpCurrentKeyFrame, pKF2, F12, vMatchedIndices); cv::Mat Rcw2 = pKF2->GetRotation(); cv::Mat Rwc2 = Rcw2.t(); cv::Mat tcw2 = pKF2->GetTranslation(); cv::Mat Tcw2(3, 4, CV_32F); Rcw2.copyTo(Tcw2.colRange(0, 3)); tcw2.copyTo(Tcw2.col(3)); const float &fx2 = pKF2->fx; const float &fy2 = pKF2->fy; const float &cx2 = pKF2->cx; const float &cy2 = pKF2->cy; const float &invfx2 = pKF2->invfx; const float &invfy2 = pKF2->invfy; // Triangulate each match const int nmatches = vMatchedIndices.size(); for(int ikp = 0; ikp < nmatches; ikp++) { const int &idx1 = vMatchedIndices[ikp].first; const int &idx2 = vMatchedIndices[ikp].second; const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1]; // const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1]; // bool bStereo1 = kp1_ur>=0; const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2]; // const float kp2_ur = pKF2->mvuRight[idx2]; // bool bStereo2 = kp2_ur>=0; // Check parallax between rays cv::Mat xn1 = (cv::Mat_<float>(3, 1) << (kp1.pt.x - cx1) * invfx1, (kp1.pt.y - cy1) * invfy1, 1.0); cv::Mat xn2 = (cv::Mat_<float>(3, 1) << (kp2.pt.x - cx2) * invfx2, (kp2.pt.y - cy2) * invfy2, 1.0); cv::Mat ray1 = Rwc1 * xn1; cv::Mat ray2 = Rwc2 * xn2; const float cosParallaxRays = ray1.dot(ray2) / (cv::norm(ray1) * cv::norm(ray2)); float cosParallaxStereo = cosParallaxRays + 1; // float cosParallaxStereo1 = cosParallaxStereo; // float cosParallaxStereo2 = cosParallaxStereo; // if(bStereo1) // cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1])); // else if(bStereo2) // cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2])); // cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2); cv::Mat x3D; if(cosParallaxRays < cosParallaxStereo && cosParallaxRays > 0 && (cosParallaxRays < 0.9998)) { //(bStereo1 || bStereo2 || cosParallaxRays<0.9998)) // Linear Triangulation Method cv::Mat A(4, 4, CV_32F); A.row(0) = xn1.at<float>(0) * Tcw1.row(2) - Tcw1.row(0); A.row(1) = xn1.at<float>(1) * Tcw1.row(2) - Tcw1.row(1); A.row(2) = xn2.at<float>(0) * Tcw2.row(2) - Tcw2.row(0); A.row(3) = xn2.at<float>(1) * Tcw2.row(2) - Tcw2.row(1); cv::Mat w, u, vt; cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV); x3D = vt.row(3).t(); if(x3D.at<float>(3) == 0) { continue; } // Euclidean coordinates x3D = x3D.rowRange(0, 3) / x3D.at<float>(3); } // else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2) // { // x3D = mpCurrentKeyFrame->UnprojectStereo(idx1); // } // else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1) // { // x3D = pKF2->UnprojectStereo(idx2); // } else { continue; //No stereo and very low parallax } cv::Mat x3Dt = x3D.t(); //Check triangulation in front of cameras float z1 = Rcw1.row(2).dot(x3Dt) + tcw1.at<float>(2); if(z1 <= 0) { continue; } float z2 = Rcw2.row(2).dot(x3Dt) + tcw2.at<float>(2); if(z2 <= 0) { continue; } //Check reprojection error in first keyframe const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave]; const float x1 = Rcw1.row(0).dot(x3Dt) + tcw1.at<float>(0); const float y1 = Rcw1.row(1).dot(x3Dt) + tcw1.at<float>(1); const float invz1 = 1.0 / z1; // if(!bStereo1) // { float u1 = fx1 * x1 * invz1 + cx1; float v1 = fy1 * y1 * invz1 + cy1; float errX1 = u1 - kp1.pt.x; float errY1 = v1 - kp1.pt.y; if((errX1 * errX1 + errY1 * errY1) > 5.991 * sigmaSquare1) { continue; } // } // else // { // float u1 = fx1*x1*invz1+cx1; // float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1; // float v1 = fy1*y1*invz1+cy1; // float errX1 = u1 - kp1.pt.x; // float errY1 = v1 - kp1.pt.y; // float errX1_r = u1_r - kp1_ur; // if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1) // continue; // } //Check reprojection error in second keyframe const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave]; const float x2 = Rcw2.row(0).dot(x3Dt) + tcw2.at<float>(0); const float y2 = Rcw2.row(1).dot(x3Dt) + tcw2.at<float>(1); const float invz2 = 1.0 / z2; // if(!bStereo2) // { float u2 = fx2 * x2 * invz2 + cx2; float v2 = fy2 * y2 * invz2 + cy2; float errX2 = u2 - kp2.pt.x; float errY2 = v2 - kp2.pt.y; if((errX2 * errX2 + errY2 * errY2) > 5.991 * sigmaSquare2) { continue; } // } // else // { // float u2 = fx2*x2*invz2+cx2; // float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2; // float v2 = fy2*y2*invz2+cy2; // float errX2 = u2 - kp2.pt.x; // float errY2 = v2 - kp2.pt.y; // float errX2_r = u2_r - kp2_ur; // if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2) // continue; // } //Check scale consistency cv::Mat normal1 = x3D - Ow1; float dist1 = cv::norm(normal1); cv::Mat normal2 = x3D - Ow2; float dist2 = cv::norm(normal2); if(dist1 == 0 || dist2 == 0) { continue; } const float ratioDist = dist2 / dist1; const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave] / pKF2->mvScaleFactors[kp2.octave]; /*if(fabs(ratioDist-ratioOctave)>ratioFactor) continue;*/ if(ratioDist * ratioFactor < ratioOctave || ratioDist > ratioOctave * ratioFactor) { continue; } // Triangulation is succesfull mpptr pMP{new MapPoint(x3D, mpCurrentKeyFrame, mpMap, mClientId, mpComm, mpCC->mSysState, -1)}; pMP->AddObservation(mpCurrentKeyFrame, idx1); pMP->AddObservation(pKF2, idx2); mpCurrentKeyFrame->AddMapPoint(pMP, idx1); pKF2->AddMapPoint(pMP, idx2); pMP->ComputeDistinctiveDescriptors(); pMP->UpdateNormalAndDepth(); mpMap->AddMapPoint(pMP); mlpRecentAddedMapPoints.push_back(pMP); nnew++; } } }
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)); } }