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