Ejemplo n.º 1
0
 bool LoopClosing::ComputeSim3()
 {
     // For each consistent loop candidate we try to compute a Sim3
     
     const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
     
     // We compute first ORB matches for each candidate
     // If enough matches are found, we setup a Sim3Solver
     ORBmatcher matcher(0.75,true);
     
     vector<Sim3Solver*> vpSim3Solvers;
     vpSim3Solvers.resize(nInitialCandidates);
     
     vector<vector<MapPoint*> > vvpMapPointMatches;
     vvpMapPointMatches.resize(nInitialCandidates);
     
     vector<bool> vbDiscarded;
     vbDiscarded.resize(nInitialCandidates);
     
     int nCandidates=0; //candidates with enough matches
     
     for(int i=0; i<nInitialCandidates; i++)
     {
         KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
         
         // avoid that local mapping erase it while it is being processed in this thread
         pKF->SetNotErase();
         
         if(pKF->isBad())
         {
             vbDiscarded[i] = true;
             continue;
         }
         
         int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
         
         if(nmatches<20)
         {
             vbDiscarded[i] = true;
             continue;
         }
         else
         {
             Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i]);
             pSolver->SetRansacParameters(0.99,20,300);
             vpSim3Solvers[i] = pSolver;
         }
         
         nCandidates++;
     }
     
     bool bMatch = false;
     
     // Perform alternatively RANSAC iterations for each candidate
     // until one is succesful or all fail
     while(nCandidates>0 && !bMatch)
     {
         for(int i=0; i<nInitialCandidates; i++)
         {
             if(vbDiscarded[i])
                 continue;
             
             KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
             
             // Perform 5 Ransac Iterations
             vector<bool> vbInliers;
             int nInliers;
             bool bNoMore;
             
             Sim3Solver* pSolver = vpSim3Solvers[i];
             cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
             
             // If Ransac reachs max. iterations discard keyframe
             if(bNoMore)
             {
                 vbDiscarded[i]=true;
                 nCandidates--;
             }
             
             // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
             if(!Scm.empty())
             {
                 vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
                 for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
                 {
                     if(vbInliers[j])
                         vpMapPointMatches[j]=vvpMapPointMatches[i][j];
                 }
                 
                 cv::Mat R = pSolver->GetEstimatedRotation();
                 cv::Mat t = pSolver->GetEstimatedTranslation();
                 const float s = pSolver->GetEstimatedScale();
                 matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
                 
                 
                 g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
                 const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10);
                 
                 // If optimization is succesful stop ransacs and continue
                 if(nInliers>=20)
                 {
                     bMatch = true;
                     mpMatchedKF = pKF;
                     g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
                     mg2oScw = gScm*gSmw;
                     mScw = Converter::toCvMat(mg2oScw);
                     
                     mvpCurrentMatchedPoints = vpMapPointMatches;
                     break;
                 }
             }
         }
     }
     
     if(!bMatch)
     {
         for(int i=0; i<nInitialCandidates; i++)
             mvpEnoughConsistentCandidates[i]->SetErase();
         mpCurrentKF->SetErase();
         return false;
     }
     
     // Retrieve MapPoints seen in Loop Keyframe and neighbors
     vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
     vpLoopConnectedKFs.push_back(mpMatchedKF);
     mvpLoopMapPoints.clear();
     for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
     {
         KeyFrame* pKF = *vit;
         vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
         for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
         {
             MapPoint* pMP = vpMapPoints[i];
             if(pMP)
             {
                 if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
                 {
                     mvpLoopMapPoints.push_back(pMP);
                     pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                 }
             }
         }
     }
     
     // Find more matches projecting with the computed Sim3
     matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
     
     // If enough matches accept Loop
     int nTotalMatches = 0;
     for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
     {
         if(mvpCurrentMatchedPoints[i])
             nTotalMatches++;
     }
     
     if(nTotalMatches>=40)
     {
         for(int i=0; i<nInitialCandidates; i++)
             if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                 mvpEnoughConsistentCandidates[i]->SetErase();
         return true;
     }
     else
     {
         for(int i=0; i<nInitialCandidates; i++)
             mvpEnoughConsistentCandidates[i]->SetErase();
         mpCurrentKF->SetErase();
         return false;
     }
     
 }
Ejemplo n.º 2
0
int main(int argc, char** argv)
{
    VideoCapture cap(string(KAI_PATH).append("shield_vid.mp4"));
//    VideoCapture cap(string(KAI_PATH).append("indoor.mov"));
//    VideoCapture cap(string(MOHIT_PATH).append("indoor.avi"));
//    VideoCapture cap("/Users/MohitSridhar/Downloads/kitti_youtube.avi");
//    VideoCapture cap("/Users/MohitSridhar/Downloads/VID_20150530_120719.mp4");
    
    if (!cap.isOpened())
    {
        cout << "failed to open video file" << endl;
        return -1;
    }

    // Initialize visualizer and load initial map
    Ptr<VisualizerListener> visualizerListener = new VisualizerListener;
    InitializeVisualizer();
    RunVisualizationOnly();
    
    // Initialize SLAM
    Mat frame;
    Size size(640, 480);
    vslam::VSlam slam = vslam::VSlam();

    while (true) {
        cap >> frame;
        if (frame.empty()) {
            break;
        }
        
        resize(frame, frame, size);
        
        clock_t start = clock();
        slam.ProcessFrame(frame);
        clock_t end = clock();
        
        double processFrameDuration = (end - start) / (double) CLOCKS_PER_SEC;
        cout << "processFrameDuration: " << processFrameDuration << endl;
        
        if (waitKey(30) == 27) {
            break;
        }
        
        // Update visualizer
        visualizerListener->update(slam.GetKeyFrames(), slam.GetCameraRot().back(), slam.
                                   GetCameraPose().back());
        RunVisualizationOnly();
        
        // Draw translation and rotation information
        Augmentor augmentor;
        KeyFrame currKeyFrame = slam.GetCurrKeyFrame();
        Mat translationMatrix = currKeyFrame.GetTranslation();
        augmentor.DisplayTranslation(frame, translationMatrix);
        Mat rotationMatrix = currKeyFrame.GetRotation();
        augmentor.DisplayRotation(frame, rotationMatrix);
        
        // Draw keypoints
        KeypointArray keypoints = currKeyFrame.GetTrackedKeypoints();
        Mat trackedFeatures;
        Scalar kpColor = Scalar(255, 0, 0);
        drawKeypoints(frame, keypoints, trackedFeatures, kpColor);
        imshow("Tracked Features", trackedFeatures);
    }
    
    waitKey(0);

    WaitForVisualizationThread();
    return 0;
}