diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc index 4bcb90f170..ca8d3afe82 100644 --- a/Examples/Monocular/mono_euroc.cc +++ b/Examples/Monocular/mono_euroc.cc @@ -18,15 +18,15 @@ * along with ORB-SLAM2. If not, see . */ +#include +#include +#include +#include +#include "unistd.h" -#include -#include -#include -#include +#include -#include - -#include +#include using namespace std; @@ -35,9 +35,10 @@ void LoadImages(const string &strImagePath, const string &strPathTimes, int main(int argc, char **argv) { - if(argc != 5) + if (argc != 5) { - cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_image_folder path_to_times_file" << endl; + cerr << endl + << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_image_folder path_to_times_file" << endl; return 1; } @@ -48,35 +49,38 @@ int main(int argc, char **argv) int nImages = vstrImageFilenames.size(); - if(nImages<=0) + if (nImages <= 0) { cerr << "ERROR: Failed to load images" << endl; return 1; } // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true); // Vector for tracking time statistics vector vTimesTrack; vTimesTrack.resize(nImages); - cout << endl << "-------" << endl; + cout << endl + << "-------" << endl; cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; + cout << "Images in the sequence: " << nImages << endl + << endl; // Main loop cv::Mat im; - for(int ni=0; ni >(t2 - t1).count(); + double ttrack = std::chrono::duration_cast>(t2 - t1).count(); - vTimesTrack[ni]=ttrack; + vTimesTrack[ni] = ttrack; // Wait to load the next frame - double T=0; - if(ni0) - T = tframe-vTimestamps[ni-1]; - - if(ttrack 0) + T = tframe - vTimestamps[ni - 1]; + + if (ttrack < T) + usleep((T - ttrack) * 1e6); } // Stop all threads SLAM.Shutdown(); // Tracking time statistics - sort(vTimesTrack.begin(),vTimesTrack.end()); + sort(vTimesTrack.begin(), vTimesTrack.end()); float totaltime = 0; - for(int ni=0; ni> t; - vTimeStamps.push_back(t/1e9); - + vTimeStamps.push_back(t / 1e9); } } } diff --git a/Examples/Monocular/mono_kitti.cc b/Examples/Monocular/mono_kitti.cc index f2f7b3ebfd..d3368d609b 100644 --- a/Examples/Monocular/mono_kitti.cc +++ b/Examples/Monocular/mono_kitti.cc @@ -18,16 +18,16 @@ * along with ORB-SLAM2. If not, see . */ +#include +#include +#include +#include +#include +#include "unistd.h" -#include -#include -#include -#include -#include +#include -#include - -#include"System.h" +#include "System.h" using namespace std; @@ -36,9 +36,10 @@ void LoadImages(const string &strSequence, vector &vstrImageFilenames, int main(int argc, char **argv) { - if(argc != 4) + if (argc != 4) { - cerr << endl << "Usage: ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl; + cerr << endl + << "Usage: ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl; return 1; } @@ -50,27 +51,30 @@ int main(int argc, char **argv) int nImages = vstrImageFilenames.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true); // Vector for tracking time statistics vector vTimesTrack; vTimesTrack.resize(nImages); - cout << endl << "-------" << endl; + cout << endl + << "-------" << endl; cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; + cout << "Images in the sequence: " << nImages << endl + << endl; // Main loop cv::Mat im; - for(int ni=0; ni >(t2 - t1).count(); + double ttrack = std::chrono::duration_cast>(t2 - t1).count(); - vTimesTrack[ni]=ttrack; + vTimesTrack[ni] = ttrack; // Wait to load the next frame - double T=0; - if(ni0) - T = tframe-vTimestamps[ni-1]; - - if(ttrack 0) + T = tframe - vTimestamps[ni - 1]; + + if (ttrack < T) + usleep((T - ttrack) * 1e6); } // Stop all threads SLAM.Shutdown(); // Tracking time statistics - sort(vTimesTrack.begin(),vTimesTrack.end()); + sort(vTimesTrack.begin(), vTimesTrack.end()); float totaltime = 0; - for(int ni=0; ni &vstrImageFilena ifstream fTimes; string strPathTimeFile = strPathToSequence + "/times.txt"; fTimes.open(strPathTimeFile.c_str()); - while(!fTimes.eof()) + while (!fTimes.eof()) { string s; - getline(fTimes,s); - if(!s.empty()) + getline(fTimes, s); + if (!s.empty()) { stringstream ss; ss << s; @@ -148,7 +153,7 @@ void LoadImages(const string &strPathToSequence, vector &vstrImageFilena const int nTimes = vTimestamps.size(); vstrImageFilenames.resize(nTimes); - for(int i=0; i. */ +#include +#include +#include +#include +#include "unistd.h" -#include -#include -#include -#include +#include -#include - -#include +#include using namespace std; @@ -35,42 +35,46 @@ void LoadImages(const string &strFile, vector &vstrImageFilenames, int main(int argc, char **argv) { - if(argc != 4) + if (argc != 4) { - cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl; + cerr << endl + << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl; return 1; } // Retrieve paths to images vector vstrImageFilenames; vector vTimestamps; - string strFile = string(argv[3])+"/rgb.txt"; + string strFile = string(argv[3]) + "/rgb.txt"; LoadImages(strFile, vstrImageFilenames, vTimestamps); int nImages = vstrImageFilenames.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true); // Vector for tracking time statistics vector vTimesTrack; vTimesTrack.resize(nImages); - cout << endl << "-------" << endl; + cout << endl + << "-------" << endl; cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; + cout << "Images in the sequence: " << nImages << endl + << endl; // Main loop cv::Mat im; - for(int ni=0; ni >(t2 - t1).count(); + double ttrack = std::chrono::duration_cast>(t2 - t1).count(); - vTimesTrack[ni]=ttrack; + vTimesTrack[ni] = ttrack; // Wait to load the next frame - double T=0; - if(ni0) - T = tframe-vTimestamps[ni-1]; - - if(ttrack 0) + T = tframe - vTimestamps[ni - 1]; + + if (ttrack < T) + usleep((T - ttrack) * 1e6); } // Stop all threads SLAM.Shutdown(); // Tracking time statistics - sort(vTimesTrack.begin(),vTimesTrack.end()); + sort(vTimesTrack.begin(), vTimesTrack.end()); float totaltime = 0; - for(int ni=0; ni &vstrImageFilenames, vecto // skip first three lines string s0; - getline(f,s0); - getline(f,s0); - getline(f,s0); + getline(f, s0); + getline(f, s0); + getline(f, s0); - while(!f.eof()) + while (!f.eof()) { string s; - getline(f,s); - if(!s.empty()) + getline(f, s); + if (!s.empty()) { stringstream ss; ss << s; diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc index 001199d1aa..fd0aae8527 100644 --- a/Examples/RGB-D/rgbd_tum.cc +++ b/Examples/RGB-D/rgbd_tum.cc @@ -18,15 +18,15 @@ * along with ORB-SLAM2. If not, see . */ +#include +#include +#include +#include +#include "unistd.h" -#include -#include -#include -#include +#include -#include - -#include +#include using namespace std; @@ -35,9 +35,10 @@ void LoadImages(const string &strAssociationFilename, vector &vstrImageF int main(int argc, char **argv) { - if(argc != 5) + if (argc != 5) { - cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl; + cerr << endl + << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl; return 1; } @@ -50,40 +51,45 @@ int main(int argc, char **argv) // Check consistency in the number of images and depthmaps int nImages = vstrImageFilenamesRGB.size(); - if(vstrImageFilenamesRGB.empty()) + if (vstrImageFilenamesRGB.empty()) { - cerr << endl << "No images found in provided path." << endl; + cerr << endl + << "No images found in provided path." << endl; return 1; } - else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size()) + else if (vstrImageFilenamesD.size() != vstrImageFilenamesRGB.size()) { - cerr << endl << "Different number of images for rgb and depth." << endl; + cerr << endl + << "Different number of images for rgb and depth." << endl; return 1; } // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true); // Vector for tracking time statistics vector vTimesTrack; vTimesTrack.resize(nImages); - cout << endl << "-------" << endl; + cout << endl + << "-------" << endl; cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; + cout << "Images in the sequence: " << nImages << endl + << endl; // Main loop cv::Mat imRGB, imD; - for(int ni=0; ni >(t2 - t1).count(); + double ttrack = std::chrono::duration_cast>(t2 - t1).count(); - vTimesTrack[ni]=ttrack; + vTimesTrack[ni] = ttrack; // Wait to load the next frame - double T=0; - if(ni0) - T = tframe-vTimestamps[ni-1]; - - if(ttrack 0) + T = tframe - vTimestamps[ni - 1]; + + if (ttrack < T) + usleep((T - ttrack) * 1e6); } // Stop all threads SLAM.Shutdown(); // Tracking time statistics - sort(vTimesTrack.begin(),vTimesTrack.end()); + sort(vTimesTrack.begin(), vTimesTrack.end()); float totaltime = 0; - for(int ni=0; ni &vstrImageF { ifstream fAssociation; fAssociation.open(strAssociationFilename.c_str()); - while(!fAssociation.eof()) + while (!fAssociation.eof()) { string s; - getline(fAssociation,s); - if(!s.empty()) + getline(fAssociation, s); + if (!s.empty()) { stringstream ss; ss << s; @@ -161,7 +168,6 @@ void LoadImages(const string &strAssociationFilename, vector &vstrImageF ss >> t; ss >> sD; vstrImageFilenamesD.push_back(sD); - } } } diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt index 7cbf71c199..a003c103bd 100644 --- a/Examples/ROS/ORB_SLAM2/CMakeLists.txt +++ b/Examples/ROS/ORB_SLAM2/CMakeLists.txt @@ -48,13 +48,14 @@ ${PROJECT_SOURCE_DIR}/../../../include ${Pangolin_INCLUDE_DIRS} ) -set(LIBS -${OpenCV_LIBS} +set(LIBS +${OpenCV_LIBS} ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so +-lboost_system ) # Node for monocular camera diff --git a/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc index 9c548e87d1..721c739a61 100644 --- a/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc +++ b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc @@ -25,6 +25,7 @@ #include #include #include +#include "unistd.h" using namespace std; @@ -35,38 +36,38 @@ const float eps = 1e-4; cv::Mat ExpSO3(const float &x, const float &y, const float &z) { - cv::Mat I = cv::Mat::eye(3,3,CV_32F); - const float d2 = x*x+y*y+z*z; + cv::Mat I = cv::Mat::eye(3, 3, CV_32F); + const float d2 = x * x + y * y + z * z; const float d = sqrt(d2); - cv::Mat W = (cv::Mat_(3,3) << 0, -z, y, + cv::Mat W = (cv::Mat_(3, 3) << 0, -z, y, z, 0, -x, - -y, x, 0); - if(d(0),v.at(1),v.at(2)); + return ExpSO3(v.at(0), v.at(1), v.at(2)); } -ViewerAR::ViewerAR(){} +ViewerAR::ViewerAR() {} void ViewerAR::Run() { - int w,h,wui; + int w, h, wui; cv::Mat im, Tcw; int status; vector vKeys; - vector vMPs; + vector vMPs; - while(1) + while (1) { - GetImagePose(im,Tcw,status,vKeys,vMPs); - if(im.empty()) + GetImagePose(im, Tcw, status, vKeys, vMPs); + if (im.empty()) cv::waitKey(mT); else { @@ -76,46 +77,46 @@ void ViewerAR::Run() } } - wui=200; + wui = 200; - pangolin::CreateWindowAndBind("Viewer",w+wui,h); + pangolin::CreateWindowAndBind("Viewer", w + wui, h); glEnable(GL_DEPTH_TEST); - glEnable (GL_BLEND); - - pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(wui)); - pangolin::Var menu_detectplane("menu.Insert Cube",false,false); - pangolin::Var menu_clear("menu.Clear All",false,false); - pangolin::Var menu_drawim("menu.Draw Image",true,true); - pangolin::Var menu_drawcube("menu.Draw Cube",true,true); - pangolin::Var menu_cubesize("menu. Cube Size",0.05,0.01,0.3); - pangolin::Var menu_drawgrid("menu.Draw Grid",true,true); - pangolin::Var menu_ngrid("menu. Grid Elements",3,1,10); - pangolin::Var menu_sizegrid("menu. Element Size",0.05,0.01,0.3); - pangolin::Var menu_drawpoints("menu.Draw Points",false,true); - - pangolin::Var menu_LocalizationMode("menu.Localization Mode",false,true); + glEnable(GL_BLEND); + + pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(wui)); + pangolin::Var menu_detectplane("menu.Insert Cube", false, false); + pangolin::Var menu_clear("menu.Clear All", false, false); + pangolin::Var menu_drawim("menu.Draw Image", true, true); + pangolin::Var menu_drawcube("menu.Draw Cube", true, true); + pangolin::Var menu_cubesize("menu. Cube Size", 0.05, 0.01, 0.3); + pangolin::Var menu_drawgrid("menu.Draw Grid", true, true); + pangolin::Var menu_ngrid("menu. Grid Elements", 3, 1, 10); + pangolin::Var menu_sizegrid("menu. Element Size", 0.05, 0.01, 0.3); + pangolin::Var menu_drawpoints("menu.Draw Points", false, true); + + pangolin::Var menu_LocalizationMode("menu.Localization Mode", false, true); bool bLocalizationMode = false; - pangolin::View& d_image = pangolin::Display("image") - .SetBounds(0,1.0f,pangolin::Attach::Pix(wui),1.0f,(float)w/h) - .SetLock(pangolin::LockLeft, pangolin::LockTop); + pangolin::View &d_image = pangolin::Display("image") + .SetBounds(0, 1.0f, pangolin::Attach::Pix(wui), 1.0f, (float)w / h) + .SetLock(pangolin::LockLeft, pangolin::LockTop); - pangolin::GlTexture imageTexture(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE); + pangolin::GlTexture imageTexture(w, h, GL_RGB, false, 0, GL_RGB, GL_UNSIGNED_BYTE); - pangolin::OpenGlMatrixSpec P = pangolin::ProjectionMatrixRDF_TopLeft(w,h,fx,fy,cx,cy,0.001,1000); + pangolin::OpenGlMatrixSpec P = pangolin::ProjectionMatrixRDF_TopLeft(w, h, fx, fy, cx, cy, 0.001, 1000); - vector vpPlane; + vector vpPlane; - while(1) + while (1) { - if(menu_LocalizationMode && !bLocalizationMode) + if (menu_LocalizationMode && !bLocalizationMode) { mpSystem->ActivateLocalizationMode(); bLocalizationMode = true; } - else if(!menu_LocalizationMode && bLocalizationMode) + else if (!menu_LocalizationMode && bLocalizationMode) { mpSystem->DeactivateLocalizationMode(); bLocalizationMode = false; @@ -125,20 +126,20 @@ void ViewerAR::Run() // Activate camera view d_image.Activate(); - glColor3f(1.0,1.0,1.0); + glColor3f(1.0, 1.0, 1.0); // Get last image and its computed pose from SLAM - GetImagePose(im,Tcw,status,vKeys,vMPs); + GetImagePose(im, Tcw, status, vKeys, vMPs); // Add text to image - PrintStatus(status,bLocalizationMode,im); + PrintStatus(status, bLocalizationMode, im); - if(menu_drawpoints) - DrawTrackedPoints(vKeys,vMPs,im); + if (menu_drawpoints) + DrawTrackedPoints(vKeys, vMPs, im); // Draw image - if(menu_drawim) - DrawImageTexture(imageTexture,im); + if (menu_drawim) + DrawImageTexture(imageTexture, im); glClear(GL_DEPTH_BUFFER_BIT); @@ -152,13 +153,13 @@ void ViewerAR::Run() LoadCameraPose(Tcw); // Draw virtual things - if(status==2) + if (status == 2) { - if(menu_clear) + if (menu_clear) { - if(!vpPlane.empty()) + if (!vpPlane.empty()) { - for(size_t i=0; iMapChanged()) + if (mpSystem->MapChanged()) { cout << "Map changed. All virtual elements are recomputed!" << endl; bRecompute = true; } } - for(size_t i=0; iRecompute(); } @@ -210,32 +211,29 @@ void ViewerAR::Run() pPlane->glTpw.Multiply(); // Draw cube - if(menu_drawcube) + if (menu_drawcube) { DrawCube(menu_cubesize); } // Draw grid plane - if(menu_drawgrid) + if (menu_drawgrid) { - DrawPlane(menu_ngrid,menu_sizegrid); + DrawPlane(menu_ngrid, menu_sizegrid); } glPopMatrix(); } } } - - } pangolin::FinishFrame(); - usleep(mT*1000); + usleep(mT * 1000); } - } -void ViewerAR::SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status, const vector &vKeys, const vector &vMPs) +void ViewerAR::SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status, const vector &vKeys, const vector &vMPs) { unique_lock lock(mMutexPoseImage); mImage = im.clone(); @@ -245,7 +243,7 @@ void ViewerAR::SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &st mvMPs = vMPs; } -void ViewerAR::GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, std::vector &vKeys, std::vector &vMPs) +void ViewerAR::GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, std::vector &vKeys, std::vector &vMPs) { unique_lock lock(mMutexPoseImage); im = mImage.clone(); @@ -257,29 +255,29 @@ void ViewerAR::GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, std::vector< void ViewerAR::LoadCameraPose(const cv::Mat &Tcw) { - if(!Tcw.empty()) + if (!Tcw.empty()) { pangolin::OpenGlMatrix M; - M.m[0] = Tcw.at(0,0); - M.m[1] = Tcw.at(1,0); - M.m[2] = Tcw.at(2,0); - M.m[3] = 0.0; + M.m[0] = Tcw.at(0, 0); + M.m[1] = Tcw.at(1, 0); + M.m[2] = Tcw.at(2, 0); + M.m[3] = 0.0; - M.m[4] = Tcw.at(0,1); - M.m[5] = Tcw.at(1,1); - M.m[6] = Tcw.at(2,1); - M.m[7] = 0.0; + M.m[4] = Tcw.at(0, 1); + M.m[5] = Tcw.at(1, 1); + M.m[6] = Tcw.at(2, 1); + M.m[7] = 0.0; - M.m[8] = Tcw.at(0,2); - M.m[9] = Tcw.at(1,2); - M.m[10] = Tcw.at(2,2); - M.m[11] = 0.0; + M.m[8] = Tcw.at(0, 2); + M.m[9] = Tcw.at(1, 2); + M.m[10] = Tcw.at(2, 2); + M.m[11] = 0.0; - M.m[12] = Tcw.at(0,3); - M.m[13] = Tcw.at(1,3); - M.m[14] = Tcw.at(2,3); - M.m[15] = 1.0; + M.m[12] = Tcw.at(0, 3); + M.m[13] = Tcw.at(1, 3); + M.m[14] = Tcw.at(2, 3); + M.m[15] = 1.0; M.Load(); } @@ -287,22 +285,46 @@ void ViewerAR::LoadCameraPose(const cv::Mat &Tcw) void ViewerAR::PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im) { - if(!bLocMode) + if (!bLocMode) { - switch(status) + switch (status) { - case 1: {AddTextToImage("SLAM NOT INITIALIZED",im,255,0,0); break;} - case 2: {AddTextToImage("SLAM ON",im,0,255,0); break;} - case 3: {AddTextToImage("SLAM LOST",im,255,0,0); break;} + case 1: + { + AddTextToImage("SLAM NOT INITIALIZED", im, 255, 0, 0); + break; + } + case 2: + { + AddTextToImage("SLAM ON", im, 0, 255, 0); + break; + } + case 3: + { + AddTextToImage("SLAM LOST", im, 255, 0, 0); + break; + } } } else { - switch(status) + switch (status) + { + case 1: { - case 1: {AddTextToImage("SLAM NOT INITIALIZED",im,255,0,0); break;} - case 2: {AddTextToImage("LOCALIZATION ON",im,0,255,0); break;} - case 3: {AddTextToImage("LOCALIZATION LOST",im,255,0,0); break;} + AddTextToImage("SLAM NOT INITIALIZED", im, 255, 0, 0); + break; + } + case 2: + { + AddTextToImage("LOCALIZATION ON", im, 0, 255, 0); + break; + } + case 3: + { + AddTextToImage("LOCALIZATION LOST", im, 255, 0, 0); + break; + } } } } @@ -311,34 +333,34 @@ void ViewerAR::AddTextToImage(const string &s, cv::Mat &im, const int r, const i { int l = 10; //imText.rowRange(im.rows-imText.rows,imText.rows) = cv::Mat::zeros(textSize.height+10,im.cols,im.type()); - cv::putText(im,s,cv::Point(l,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l-1,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l+1,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l-1,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l+1,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l-1,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - cv::putText(im,s,cv::Point(l+1,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); - - cv::putText(im,s,cv::Point(l,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(r,g,b),2,8); + cv::putText(im, s, cv::Point(l, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l - 1, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l + 1, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l - 1, im.rows - (l - 1)), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l, im.rows - (l - 1)), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l + 1, im.rows - (l - 1)), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l - 1, im.rows - (l + 1)), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l, im.rows - (l + 1)), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + cv::putText(im, s, cv::Point(l + 1, im.rows - (l + 1)), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8); + + cv::putText(im, s, cv::Point(l, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(r, g, b), 2, 8); } void ViewerAR::DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im) { - if(!im.empty()) + if (!im.empty()) { - imageTexture.Upload(im.data,GL_RGB,GL_UNSIGNED_BYTE); + imageTexture.Upload(im.data, GL_RGB, GL_UNSIGNED_BYTE); imageTexture.RenderToViewportFlipY(); } } -void ViewerAR::DrawCube(const float &size,const float x, const float y, const float z) +void ViewerAR::DrawCube(const float &size, const float x, const float y, const float z) { - pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x,-size-y,-z); + pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x, -size - y, -z); glPushMatrix(); M.Multiply(); - pangolin::glDrawColouredCube(-size,size); + pangolin::glDrawColouredCube(-size, size); glPopMatrix(); } @@ -346,63 +368,60 @@ void ViewerAR::DrawPlane(Plane *pPlane, int ndivs, float ndivsize) { glPushMatrix(); pPlane->glTpw.Multiply(); - DrawPlane(ndivs,ndivsize); + DrawPlane(ndivs, ndivsize); glPopMatrix(); } void ViewerAR::DrawPlane(int ndivs, float ndivsize) { // Plane parallel to x-z at origin with normal -y - const float minx = -ndivs*ndivsize; - const float minz = -ndivs*ndivsize; - const float maxx = ndivs*ndivsize; - const float maxz = ndivs*ndivsize; - + const float minx = -ndivs * ndivsize; + const float minz = -ndivs * ndivsize; + const float maxx = ndivs * ndivsize; + const float maxz = ndivs * ndivsize; glLineWidth(2); - glColor3f(0.7f,0.7f,1.0f); + glColor3f(0.7f, 0.7f, 1.0f); glBegin(GL_LINES); - for(int n = 0; n<=2*ndivs; n++) + for (int n = 0; n <= 2 * ndivs; n++) { - glVertex3f(minx+ndivsize*n,0,minz); - glVertex3f(minx+ndivsize*n,0,maxz); - glVertex3f(minx,0,minz+ndivsize*n); - glVertex3f(maxx,0,minz+ndivsize*n); + glVertex3f(minx + ndivsize * n, 0, minz); + glVertex3f(minx + ndivsize * n, 0, maxz); + glVertex3f(minx, 0, minz + ndivsize * n); + glVertex3f(maxx, 0, minz + ndivsize * n); } glEnd(); - } void ViewerAR::DrawTrackedPoints(const std::vector &vKeys, const std::vector &vMPs, cv::Mat &im) { const int N = vKeys.size(); - - for(int i=0; i &vMPs, const int iterations) +Plane *ViewerAR::DetectPlane(const cv::Mat Tcw, const std::vector &vMPs, const int iterations) { // Retrieve 3D points vector vPoints; vPoints.reserve(vMPs.size()); - vector vPointMP; + vector vPointMP; vPointMP.reserve(vMPs.size()); - for(size_t i=0; iObservations()>5) + if (pMP->Observations() > 5) { vPoints.push_back(pMP->GetWorldPos()); vPointMP.push_back(pMP); @@ -412,16 +431,15 @@ Plane* ViewerAR::DetectPlane(const cv::Mat Tcw, const std::vector &vM const int N = vPoints.size(); - if(N<50) + if (N < 50) return NULL; - // Indices for minimum set selection vector vAllIndices; vAllIndices.reserve(N); vector vAvailableIndices; - for(int i=0; i &vM vector bestvDist; //RANSAC - for(int n=0; n(3,0); - const float b = vt.at(3,1); - const float c = vt.at(3,2); - const float d = vt.at(3,3); + const float a = vt.at(3, 0); + const float b = vt.at(3, 1); + const float c = vt.at(3, 2); + const float d = vt.at(3, 3); - vector vDistances(N,0); + vector vDistances(N, 0); - const float f = 1.0f/sqrt(a*a+b*b+c*c+d*d); + const float f = 1.0f / sqrt(a * a + b * b + c * c + d * d); - for(int i=0; i(0)*a+vPoints[i].at(1)*b+vPoints[i].at(2)*c+d)*f; + vDistances[i] = fabs(vPoints[i].at(0) * a + vPoints[i].at(1) * b + vPoints[i].at(2) * c + d) * f; } vector vSorted = vDistances; - sort(vSorted.begin(),vSorted.end()); + sort(vSorted.begin(), vSorted.end()); - int nth = max((int)(0.2*N),20); + int nth = max((int)(0.2 * N), 20); const float medianDist = vSorted[nth]; - if(medianDist &vM } // Compute threshold inlier/outlier - const float th = 1.4*bestDist; - vector vbInliers(N,false); + const float th = 1.4 * bestDist; + vector vbInliers(N, false); int nInliers = 0; - for(int i=0; i vInlierMPs(nInliers,NULL); + vector vInlierMPs(nInliers, NULL); int nin = 0; - for(int i=0; i &vMPs, const cv::Mat &Tcw):mvMPs(vMPs),mTcw(Tcw.clone()) +Plane::Plane(const std::vector &vMPs, const cv::Mat &Tcw) : mvMPs(vMPs), mTcw(Tcw.clone()) { - rang = -3.14f/2+((float)rand()/RAND_MAX)*3.14f; + rang = -3.14f / 2 + ((float)rand() / RAND_MAX) * 3.14f; Recompute(); } @@ -518,125 +536,123 @@ void Plane::Recompute() const int N = mvMPs.size(); // Recompute plane with all points - cv::Mat A = cv::Mat(N,4,CV_32F); - A.col(3) = cv::Mat::ones(N,1,CV_32F); + cv::Mat A = cv::Mat(N, 4, CV_32F); + A.col(3) = cv::Mat::ones(N, 1, CV_32F); - o = cv::Mat::zeros(3,1,CV_32F); + o = cv::Mat::zeros(3, 1, CV_32F); int nPoints = 0; - for(int i=0; iisBad()) + MapPoint *pMP = mvMPs[i]; + if (!pMP->isBad()) { cv::Mat Xw = pMP->GetWorldPos(); - o+=Xw; - A.row(nPoints).colRange(0,3) = Xw.t(); + o += Xw; + A.row(nPoints).colRange(0, 3) = Xw.t(); nPoints++; } } A.resize(nPoints); - cv::Mat u,w,vt; - cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV); + cv::Mat u, w, vt; + cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV); - float a = vt.at(3,0); - float b = vt.at(3,1); - float c = vt.at(3,2); + float a = vt.at(3, 0); + float b = vt.at(3, 1); + float c = vt.at(3, 2); - o = o*(1.0f/nPoints); - const float f = 1.0f/sqrt(a*a+b*b+c*c); + o = o * (1.0f / nPoints); + const float f = 1.0f / sqrt(a * a + b * b + c * c); // Compute XC just the first time - if(XC.empty()) + if (XC.empty()) { - cv::Mat Oc = -mTcw.colRange(0,3).rowRange(0,3).t()*mTcw.rowRange(0,3).col(3); - XC = Oc-o; + cv::Mat Oc = -mTcw.colRange(0, 3).rowRange(0, 3).t() * mTcw.rowRange(0, 3).col(3); + XC = Oc - o; } - if((XC.at(0)*a+XC.at(1)*b+XC.at(2)*c)>0) + if ((XC.at(0) * a + XC.at(1) * b + XC.at(2) * c) > 0) { - a=-a; - b=-b; - c=-c; + a = -a; + b = -b; + c = -c; } - const float nx = a*f; - const float ny = b*f; - const float nz = c*f; + const float nx = a * f; + const float ny = b * f; + const float nz = c * f; - n = (cv::Mat_(3,1)<(3, 1) << nx, ny, nz); - cv::Mat up = (cv::Mat_(3,1) << 0.0f, 1.0f, 0.0f); + cv::Mat up = (cv::Mat_(3, 1) << 0.0f, 1.0f, 0.0f); cv::Mat v = up.cross(n); const float sa = cv::norm(v); const float ca = up.dot(n); - const float ang = atan2(sa,ca); - Tpw = cv::Mat::eye(4,4,CV_32F); - - - Tpw.rowRange(0,3).colRange(0,3) = ExpSO3(v*ang/sa)*ExpSO3(up*rang); - o.copyTo(Tpw.col(3).rowRange(0,3)); - - glTpw.m[0] = Tpw.at(0,0); - glTpw.m[1] = Tpw.at(1,0); - glTpw.m[2] = Tpw.at(2,0); - glTpw.m[3] = 0.0; - - glTpw.m[4] = Tpw.at(0,1); - glTpw.m[5] = Tpw.at(1,1); - glTpw.m[6] = Tpw.at(2,1); - glTpw.m[7] = 0.0; - - glTpw.m[8] = Tpw.at(0,2); - glTpw.m[9] = Tpw.at(1,2); - glTpw.m[10] = Tpw.at(2,2); - glTpw.m[11] = 0.0; - - glTpw.m[12] = Tpw.at(0,3); - glTpw.m[13] = Tpw.at(1,3); - glTpw.m[14] = Tpw.at(2,3); - glTpw.m[15] = 1.0; - + const float ang = atan2(sa, ca); + Tpw = cv::Mat::eye(4, 4, CV_32F); + + Tpw.rowRange(0, 3).colRange(0, 3) = ExpSO3(v * ang / sa) * ExpSO3(up * rang); + o.copyTo(Tpw.col(3).rowRange(0, 3)); + + glTpw.m[0] = Tpw.at(0, 0); + glTpw.m[1] = Tpw.at(1, 0); + glTpw.m[2] = Tpw.at(2, 0); + glTpw.m[3] = 0.0; + + glTpw.m[4] = Tpw.at(0, 1); + glTpw.m[5] = Tpw.at(1, 1); + glTpw.m[6] = Tpw.at(2, 1); + glTpw.m[7] = 0.0; + + glTpw.m[8] = Tpw.at(0, 2); + glTpw.m[9] = Tpw.at(1, 2); + glTpw.m[10] = Tpw.at(2, 2); + glTpw.m[11] = 0.0; + + glTpw.m[12] = Tpw.at(0, 3); + glTpw.m[13] = Tpw.at(1, 3); + glTpw.m[14] = Tpw.at(2, 3); + glTpw.m[15] = 1.0; } Plane::Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz) { - n = (cv::Mat_(3,1)<(3,1)<(3, 1) << nx, ny, nz); + o = (cv::Mat_(3, 1) << ox, oy, oz); - cv::Mat up = (cv::Mat_(3,1) << 0.0f, 1.0f, 0.0f); + cv::Mat up = (cv::Mat_(3, 1) << 0.0f, 1.0f, 0.0f); cv::Mat v = up.cross(n); const float s = cv::norm(v); const float c = up.dot(n); - const float a = atan2(s,c); - Tpw = cv::Mat::eye(4,4,CV_32F); - const float rang = -3.14f/2+((float)rand()/RAND_MAX)*3.14f; + const float a = atan2(s, c); + Tpw = cv::Mat::eye(4, 4, CV_32F); + const float rang = -3.14f / 2 + ((float)rand() / RAND_MAX) * 3.14f; cout << rang; - Tpw.rowRange(0,3).colRange(0,3) = ExpSO3(v*a/s)*ExpSO3(up*rang); - o.copyTo(Tpw.col(3).rowRange(0,3)); - - glTpw.m[0] = Tpw.at(0,0); - glTpw.m[1] = Tpw.at(1,0); - glTpw.m[2] = Tpw.at(2,0); - glTpw.m[3] = 0.0; - - glTpw.m[4] = Tpw.at(0,1); - glTpw.m[5] = Tpw.at(1,1); - glTpw.m[6] = Tpw.at(2,1); - glTpw.m[7] = 0.0; - - glTpw.m[8] = Tpw.at(0,2); - glTpw.m[9] = Tpw.at(1,2); - glTpw.m[10] = Tpw.at(2,2); - glTpw.m[11] = 0.0; - - glTpw.m[12] = Tpw.at(0,3); - glTpw.m[13] = Tpw.at(1,3); - glTpw.m[14] = Tpw.at(2,3); - glTpw.m[15] = 1.0; + Tpw.rowRange(0, 3).colRange(0, 3) = ExpSO3(v * a / s) * ExpSO3(up * rang); + o.copyTo(Tpw.col(3).rowRange(0, 3)); + + glTpw.m[0] = Tpw.at(0, 0); + glTpw.m[1] = Tpw.at(1, 0); + glTpw.m[2] = Tpw.at(2, 0); + glTpw.m[3] = 0.0; + + glTpw.m[4] = Tpw.at(0, 1); + glTpw.m[5] = Tpw.at(1, 1); + glTpw.m[6] = Tpw.at(2, 1); + glTpw.m[7] = 0.0; + + glTpw.m[8] = Tpw.at(0, 2); + glTpw.m[9] = Tpw.at(1, 2); + glTpw.m[10] = Tpw.at(2, 2); + glTpw.m[11] = 0.0; + + glTpw.m[12] = Tpw.at(0, 3); + glTpw.m[13] = Tpw.at(1, 3); + glTpw.m[14] = Tpw.at(2, 3); + glTpw.m[15] = 1.0; } -} +} // namespace ORB_SLAM2 diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc index 6bc09c50be..c650022848 100644 --- a/Examples/Stereo/stereo_euroc.cc +++ b/Examples/Stereo/stereo_euroc.cc @@ -18,16 +18,16 @@ * along with ORB-SLAM2. If not, see . */ +#include +#include +#include +#include +#include +#include "unistd.h" -#include -#include -#include -#include -#include +#include -#include - -#include +#include using namespace std; @@ -36,9 +36,10 @@ void LoadImages(const string &strPathLeft, const string &strPathRight, const str int main(int argc, char **argv) { - if(argc != 6) + if (argc != 6) { - cerr << endl << "Usage: ./stereo_euroc path_to_vocabulary path_to_settings path_to_left_folder path_to_right_folder path_to_times_file" << endl; + cerr << endl + << "Usage: ./stereo_euroc path_to_vocabulary path_to_settings path_to_left_folder path_to_right_folder path_to_times_file" << endl; return 1; } @@ -48,13 +49,13 @@ int main(int argc, char **argv) vector vTimeStamp; LoadImages(string(argv[3]), string(argv[4]), string(argv[5]), vstrImageLeft, vstrImageRight, vTimeStamp); - if(vstrImageLeft.empty() || vstrImageRight.empty()) + if (vstrImageLeft.empty() || vstrImageRight.empty()) { cerr << "ERROR: No images in provided path." << endl; return 1; } - if(vstrImageLeft.size()!=vstrImageRight.size()) + if (vstrImageLeft.size() != vstrImageRight.size()) { cerr << "ERROR: Different number of left and right images." << endl; return 1; @@ -62,7 +63,7 @@ int main(int argc, char **argv) // Read rectification parameters cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); - if(!fsSettings.isOpened()) + if (!fsSettings.isOpened()) { cerr << "ERROR: Wrong path to settings" << endl; return -1; @@ -86,59 +87,61 @@ int main(int argc, char **argv) int rows_r = fsSettings["RIGHT.height"]; int cols_r = fsSettings["RIGHT.width"]; - if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || - rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) + if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) { cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; return -1; } - cv::Mat M1l,M2l,M1r,M2r; - cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); - cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); - + cv::Mat M1l, M2l, M1r, M2r; + cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), cv::Size(cols_l, rows_l), CV_32F, M1l, M2l); + cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), cv::Size(cols_r, rows_r), CV_32F, M1r, M2r); const int nImages = vstrImageLeft.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::STEREO, true); // Vector for tracking time statistics vector vTimesTrack; vTimesTrack.resize(nImages); - cout << endl << "-------" << endl; + cout << endl + << "-------" << endl; cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; + cout << "Images in the sequence: " << nImages << endl + << endl; // Main loop cv::Mat imLeft, imRight, imLeftRect, imRightRect; - for(int ni=0; ni >(t2 - t1).count(); + double ttrack = std::chrono::duration_cast>(t2 - t1).count(); - vTimesTrack[ni]=ttrack; + vTimesTrack[ni] = ttrack; // Wait to load the next frame - double T=0; - if(ni0) - T = tframe-vTimeStamp[ni-1]; - - if(ttrack 0) + T = tframe - vTimeStamp[ni - 1]; + + if (ttrack < T) + usleep((T - ttrack) * 1e6); } // Stop all threads SLAM.Shutdown(); // Tracking time statistics - sort(vTimesTrack.begin(),vTimesTrack.end()); + sort(vTimesTrack.begin(), vTimesTrack.end()); float totaltime = 0; - for(int ni=0; ni> t; - vTimeStamps.push_back(t/1e9); - + vTimeStamps.push_back(t / 1e9); } } } diff --git a/Examples/Stereo/stereo_kitti.cc b/Examples/Stereo/stereo_kitti.cc index cb8bc4096e..1f9598435d 100644 --- a/Examples/Stereo/stereo_kitti.cc +++ b/Examples/Stereo/stereo_kitti.cc @@ -18,16 +18,16 @@ * along with ORB-SLAM2. If not, see . */ +#include +#include +#include +#include +#include +#include "unistd.h" -#include -#include -#include -#include -#include +#include -#include - -#include +#include using namespace std; @@ -36,9 +36,10 @@ void LoadImages(const string &strPathToSequence, vector &vstrImageLeft, int main(int argc, char **argv) { - if(argc != 4) + if (argc != 4) { - cerr << endl << "Usage: ./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl; + cerr << endl + << "Usage: ./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl; return 1; } @@ -51,28 +52,31 @@ int main(int argc, char **argv) const int nImages = vstrImageLeft.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::STEREO, true); // Vector for tracking time statistics vector vTimesTrack; vTimesTrack.resize(nImages); - cout << endl << "-------" << endl; + cout << endl + << "-------" << endl; cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; + cout << "Images in the sequence: " << nImages << endl + << endl; // Main loop cv::Mat imLeft, imRight; - for(int ni=0; ni >(t2 - t1).count(); + double ttrack = std::chrono::duration_cast>(t2 - t1).count(); - vTimesTrack[ni]=ttrack; + vTimesTrack[ni] = ttrack; // Wait to load the next frame - double T=0; - if(ni0) - T = tframe-vTimestamps[ni-1]; - - if(ttrack 0) + T = tframe - vTimestamps[ni - 1]; + + if (ttrack < T) + usleep((T - ttrack) * 1e6); } // Stop all threads SLAM.Shutdown(); // Tracking time statistics - sort(vTimesTrack.begin(),vTimesTrack.end()); + sort(vTimesTrack.begin(), vTimesTrack.end()); float totaltime = 0; - for(int ni=0; ni &vstrImageLeft, ifstream fTimes; string strPathTimeFile = strPathToSequence + "/times.txt"; fTimes.open(strPathTimeFile.c_str()); - while(!fTimes.eof()) + while (!fTimes.eof()) { string s; - getline(fTimes,s); - if(!s.empty()) + getline(fTimes, s); + if (!s.empty()) { stringstream ss; ss << s; @@ -154,7 +159,7 @@ void LoadImages(const string &strPathToSequence, vector &vstrImageLeft, vstrImageLeft.resize(nTimes); vstrImageRight.resize(nTimes); - for(int i=0; i +#include namespace ORB_SLAM2 { -LocalMapping::LocalMapping(Map *pMap, const float bMonocular): - mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap), - mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true) +LocalMapping::LocalMapping(Map *pMap, const float bMonocular) : mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap), + mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true) { } -void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser) +void LocalMapping::SetLoopCloser(LoopClosing *pLoopCloser) { mpLoopCloser = pLoopCloser; } void LocalMapping::SetTracker(Tracking *pTracker) { - mpTracker=pTracker; + mpTracker = pTracker; } void LocalMapping::Run() @@ -49,13 +49,13 @@ void LocalMapping::Run() mbFinished = false; - while(1) + while (1) { // Tracking will see that Local Mapping is busy SetAcceptKeyFrames(false); // Check if there are keyframes in the queue - if(CheckNewKeyFrames()) + if (CheckNewKeyFrames()) { // BoW conversion and insertion in Map ProcessNewKeyFrame(); @@ -66,7 +66,7 @@ void LocalMapping::Run() // Triangulate new MapPoints CreateNewMapPoints(); - if(!CheckNewKeyFrames()) + if (!CheckNewKeyFrames()) { // Find more matches in neighbor keyframes and fuse point duplications SearchInNeighbors(); @@ -74,11 +74,11 @@ void LocalMapping::Run() mbAbortBA = false; - if(!CheckNewKeyFrames() && !stopRequested()) + if (!CheckNewKeyFrames() && !stopRequested()) { // Local BA - if(mpMap->KeyFramesInMap()>2) - Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap); + if (mpMap->KeyFramesInMap() > 2) + Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap); // Check redundant local Keyframes KeyFrameCulling(); @@ -86,14 +86,14 @@ void LocalMapping::Run() mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame); } - else if(Stop()) + else if (Stop()) { // Safe area to stop - while(isStopped() && !CheckFinish()) + while (isStopped() && !CheckFinish()) { usleep(3000); } - if(CheckFinish()) + if (CheckFinish()) break; } @@ -102,7 +102,7 @@ void LocalMapping::Run() // Tracking will see that Local Mapping is busy SetAcceptKeyFrames(true); - if(CheckFinish()) + if (CheckFinish()) break; usleep(3000); @@ -115,14 +115,13 @@ void LocalMapping::InsertKeyFrame(KeyFrame *pKF) { unique_lock lock(mMutexNewKFs); mlNewKeyFrames.push_back(pKF); - mbAbortBA=true; + mbAbortBA = true; } - bool LocalMapping::CheckNewKeyFrames() { unique_lock lock(mMutexNewKFs); - return(!mlNewKeyFrames.empty()); + return (!mlNewKeyFrames.empty()); } void LocalMapping::ProcessNewKeyFrame() @@ -137,16 +136,16 @@ void LocalMapping::ProcessNewKeyFrame() mpCurrentKeyFrame->ComputeBoW(); // Associate MapPoints to the new keyframe and update normal and descriptor - const vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); + const vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); - for(size_t i=0; iisBad()) + if (!pMP->isBad()) { - if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)) + if (!pMP->IsInKeyFrame(mpCurrentKeyFrame)) { pMP->AddObservation(mpCurrentKeyFrame, i); pMP->UpdateNormalAndDepth(); @@ -158,7 +157,7 @@ void LocalMapping::ProcessNewKeyFrame() } } } - } + } // Update links in the Covisibility Graph mpCurrentKeyFrame->UpdateConnections(); @@ -170,34 +169,34 @@ void LocalMapping::ProcessNewKeyFrame() void LocalMapping::MapPointCulling() { // Check Recent Added MapPoints - list::iterator lit = mlpRecentAddedMapPoints.begin(); + list::iterator lit = mlpRecentAddedMapPoints.begin(); const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId; int nThObs; - if(mbMonocular) + if (mbMonocular) nThObs = 2; else nThObs = 3; const int cnThObs = nThObs; - while(lit!=mlpRecentAddedMapPoints.end()) + while (lit != mlpRecentAddedMapPoints.end()) { - MapPoint* pMP = *lit; - if(pMP->isBad()) + MapPoint *pMP = *lit; + if (pMP->isBad()) { lit = mlpRecentAddedMapPoints.erase(lit); } - else if(pMP->GetFoundRatio()<0.25f ) + else if (pMP->GetFoundRatio() < 0.25f) { pMP->SetBadFlag(); lit = mlpRecentAddedMapPoints.erase(lit); } - else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs) + else if (((int)nCurrentKFid - (int)pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs) { pMP->SetBadFlag(); lit = mlpRecentAddedMapPoints.erase(lit); } - else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3) + else if (((int)nCurrentKFid - (int)pMP->mnFirstKFid) >= 3) lit = mlpRecentAddedMapPoints.erase(lit); else lit++; @@ -208,17 +207,17 @@ void LocalMapping::CreateNewMapPoints() { // Retrieve neighbor keyframes in covisibility graph int nn = 10; - if(mbMonocular) - nn=20; - const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); + if (mbMonocular) + nn = 20; + const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); - ORBmatcher matcher(0.6,false); + 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)); + cv::Mat Tcw1(3, 4, CV_32F); + Rcw1.copyTo(Tcw1.colRange(0, 3)); tcw1.copyTo(Tcw1.col(3)); cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter(); @@ -229,49 +228,49 @@ void LocalMapping::CreateNewMapPoints() const float &invfx1 = mpCurrentKeyFrame->invfx; const float &invfy1 = mpCurrentKeyFrame->invfy; - const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor; + const float ratioFactor = 1.5f * mpCurrentKeyFrame->mfScaleFactor; - int nnew=0; + int nnew = 0; // Search matches with epipolar restriction and triangulate - for(size_t i=0; i0 && CheckNewKeyFrames()) + if (i > 0 && CheckNewKeyFrames()) return; - KeyFrame* pKF2 = vpNeighKFs[i]; + KeyFrame *pKF2 = vpNeighKFs[i]; // Check first that baseline is not too short cv::Mat Ow2 = pKF2->GetCameraCenter(); - cv::Mat vBaseline = Ow2-Ow1; + cv::Mat vBaseline = Ow2 - Ow1; const float baseline = cv::norm(vBaseline); - if(!mbMonocular) + if (!mbMonocular) { - if(baselinemb) - continue; + if (baseline < pKF2->mb) + continue; } else { const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2); - const float ratioBaselineDepth = baseline/medianDepthKF2; + const float ratioBaselineDepth = baseline / medianDepthKF2; - if(ratioBaselineDepth<0.01) + if (ratioBaselineDepth < 0.01) continue; } // Compute Fundamental Matrix - cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2); + cv::Mat F12 = ComputeF12(mpCurrentKeyFrame, pKF2); // Search matches that fullfil epipolar constraint - vector > vMatchedIndices; - matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false); + vector> vMatchedIndices; + matcher.SearchForTriangulation(mpCurrentKeyFrame, pKF2, F12, vMatchedIndices, false); 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)); + cv::Mat Tcw2(3, 4, CV_32F); + Rcw2.copyTo(Tcw2.colRange(0, 3)); tcw2.copyTo(Tcw2.col(3)); const float &fx2 = pKF2->fx; @@ -283,65 +282,64 @@ void LocalMapping::CreateNewMapPoints() // Triangulate each match const int nmatches = vMatchedIndices.size(); - for(int ikp=0; ikpmvKeysUn[idx1]; - const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1]; - bool bStereo1 = kp1_ur>=0; + 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; + bool bStereo2 = kp2_ur >= 0; // Check parallax between rays - cv::Mat xn1 = (cv::Mat_(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0); - cv::Mat xn2 = (cv::Mat_(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0); + cv::Mat xn1 = (cv::Mat_(3, 1) << (kp1.pt.x - cx1) * invfx1, (kp1.pt.y - cy1) * invfy1, 1.0); + cv::Mat xn2 = (cv::Mat_(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)); + 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 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])); + 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); + cosParallaxStereo = min(cosParallaxStereo1, cosParallaxStereo2); cv::Mat x3D; - if(cosParallaxRays0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)) + if (cosParallaxRays < cosParallaxStereo && cosParallaxRays > 0 && (bStereo1 || bStereo2 || cosParallaxRays < 0.9998)) { // Linear Triangulation Method - cv::Mat A(4,4,CV_32F); - A.row(0) = xn1.at(0)*Tcw1.row(2)-Tcw1.row(0); - A.row(1) = xn1.at(1)*Tcw1.row(2)-Tcw1.row(1); - A.row(2) = xn2.at(0)*Tcw2.row(2)-Tcw2.row(0); - A.row(3) = xn2.at(1)*Tcw2.row(2)-Tcw2.row(1); + cv::Mat A(4, 4, CV_32F); + A.row(0) = xn1.at(0) * Tcw1.row(2) - Tcw1.row(0); + A.row(1) = xn1.at(1) * Tcw1.row(2) - Tcw1.row(1); + A.row(2) = xn2.at(0) * Tcw2.row(2) - Tcw2.row(0); + A.row(3) = xn2.at(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); + 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(3)==0) + if (x3D.at(3) == 0) continue; // Euclidean coordinates - x3D = x3D.rowRange(0,3)/x3D.at(3); - + x3D = x3D.rowRange(0, 3) / x3D.at(3); } - else if(bStereo1 && cosParallaxStereo1UnprojectStereo(idx1); + x3D = mpCurrentKeyFrame->UnprojectStereo(idx1); } - else if(bStereo2 && cosParallaxStereo2UnprojectStereo(idx2); } @@ -351,93 +349,93 @@ void LocalMapping::CreateNewMapPoints() cv::Mat x3Dt = x3D.t(); //Check triangulation in front of cameras - float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at(2); - if(z1<=0) + float z1 = Rcw1.row(2).dot(x3Dt) + tcw1.at(2); + if (z1 <= 0) continue; - float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at(2); - if(z2<=0) + float z2 = Rcw2.row(2).dot(x3Dt) + tcw2.at(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(0); - const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at(1); - const float invz1 = 1.0/z1; + const float x1 = Rcw1.row(0).dot(x3Dt) + tcw1.at(0); + const float y1 = Rcw1.row(1).dot(x3Dt) + tcw1.at(1); + const float invz1 = 1.0 / z1; - if(!bStereo1) + if (!bStereo1) { - float u1 = fx1*x1*invz1+cx1; - float v1 = fy1*y1*invz1+cy1; + 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) + 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 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) + 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(0); - const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at(1); - const float invz2 = 1.0/z2; - if(!bStereo2) + const float x2 = Rcw2.row(0).dot(x3Dt) + tcw2.at(0); + const float y2 = Rcw2.row(1).dot(x3Dt) + tcw2.at(1); + const float invz2 = 1.0 / z2; + if (!bStereo2) { - float u2 = fx2*x2*invz2+cx2; - float v2 = fy2*y2*invz2+cy2; + 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) + 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 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) + if ((errX2 * errX2 + errY2 * errY2 + errX2_r * errX2_r) > 7.8 * sigmaSquare2) continue; } //Check scale consistency - cv::Mat normal1 = x3D-Ow1; + cv::Mat normal1 = x3D - Ow1; float dist1 = cv::norm(normal1); - cv::Mat normal2 = x3D-Ow2; + cv::Mat normal2 = x3D - Ow2; float dist2 = cv::norm(normal2); - if(dist1==0 || dist2==0) + if (dist1 == 0 || dist2 == 0) continue; - const float ratioDist = dist2/dist1; - const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave]; + const float ratioDist = dist2 / dist1; + const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave] / pKF2->mvScaleFactors[kp2.octave]; /*if(fabs(ratioDist-ratioOctave)>ratioFactor) continue;*/ - if(ratioDist*ratioFactorratioOctave*ratioFactor) + if (ratioDist * ratioFactor < ratioOctave || ratioDist > ratioOctave * ratioFactor) continue; // Triangulation is succesfull - MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap); + MapPoint *pMP = new MapPoint(x3D, mpCurrentKeyFrame, mpMap); - pMP->AddObservation(mpCurrentKeyFrame,idx1); - pMP->AddObservation(pKF2,idx2); + pMP->AddObservation(mpCurrentKeyFrame, idx1); + pMP->AddObservation(pKF2, idx2); - mpCurrentKeyFrame->AddMapPoint(pMP,idx1); - pKF2->AddMapPoint(pMP,idx2); + mpCurrentKeyFrame->AddMapPoint(pMP, idx1); + pKF2->AddMapPoint(pMP, idx2); pMP->ComputeDistinctiveDescriptors(); @@ -455,73 +453,71 @@ void LocalMapping::SearchInNeighbors() { // Retrieve neighbor keyframes int nn = 10; - if(mbMonocular) - nn=20; - const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); - vector vpTargetKFs; - for(vector::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++) + if (mbMonocular) + nn = 20; + const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); + vector vpTargetKFs; + for (vector::const_iterator vit = vpNeighKFs.begin(), vend = vpNeighKFs.end(); vit != vend; vit++) { - KeyFrame* pKFi = *vit; - if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId) + KeyFrame *pKFi = *vit; + if (pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId) continue; vpTargetKFs.push_back(pKFi); pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId; // Extend to some second neighbors - const vector vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5); - for(vector::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++) + const vector vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5); + for (vector::const_iterator vit2 = vpSecondNeighKFs.begin(), vend2 = vpSecondNeighKFs.end(); vit2 != vend2; vit2++) { - KeyFrame* pKFi2 = *vit2; - if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId) + KeyFrame *pKFi2 = *vit2; + if (pKFi2->isBad() || pKFi2->mnFuseTargetForKF == mpCurrentKeyFrame->mnId || pKFi2->mnId == mpCurrentKeyFrame->mnId) continue; vpTargetKFs.push_back(pKFi2); } } - // Search matches by projection from current KF in target KFs ORBmatcher matcher; - vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); - for(vector::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++) + vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); + for (vector::iterator vit = vpTargetKFs.begin(), vend = vpTargetKFs.end(); vit != vend; vit++) { - KeyFrame* pKFi = *vit; + KeyFrame *pKFi = *vit; - matcher.Fuse(pKFi,vpMapPointMatches); + matcher.Fuse(pKFi, vpMapPointMatches); } // Search matches by projection from target KFs in current KF - vector vpFuseCandidates; - vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size()); + vector vpFuseCandidates; + vpFuseCandidates.reserve(vpTargetKFs.size() * vpMapPointMatches.size()); - for(vector::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++) + for (vector::iterator vitKF = vpTargetKFs.begin(), vendKF = vpTargetKFs.end(); vitKF != vendKF; vitKF++) { - KeyFrame* pKFi = *vitKF; + KeyFrame *pKFi = *vitKF; - vector vpMapPointsKFi = pKFi->GetMapPointMatches(); + vector vpMapPointsKFi = pKFi->GetMapPointMatches(); - for(vector::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++) + for (vector::iterator vitMP = vpMapPointsKFi.begin(), vendMP = vpMapPointsKFi.end(); vitMP != vendMP; vitMP++) { - MapPoint* pMP = *vitMP; - if(!pMP) + MapPoint *pMP = *vitMP; + if (!pMP) continue; - if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId) + if (pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId) continue; pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId; vpFuseCandidates.push_back(pMP); } } - matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates); - + matcher.Fuse(mpCurrentKeyFrame, vpFuseCandidates); // Update points vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); - for(size_t i=0, iend=vpMapPointMatches.size(); iisBad()) + if (!pMP->isBad()) { pMP->ComputeDistinctiveDescriptors(); pMP->UpdateNormalAndDepth(); @@ -540,16 +536,15 @@ cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) cv::Mat R2w = pKF2->GetRotation(); cv::Mat t2w = pKF2->GetTranslation(); - cv::Mat R12 = R1w*R2w.t(); - cv::Mat t12 = -R1w*R2w.t()*t2w+t1w; + cv::Mat R12 = R1w * R2w.t(); + cv::Mat t12 = -R1w * R2w.t() * t2w + t1w; cv::Mat t12x = SkewSymmetricMatrix(t12); const cv::Mat &K1 = pKF1->mK; const cv::Mat &K2 = pKF2->mK; - - return K1.t().inv()*t12x*R12*K2.inv(); + return K1.t().inv() * t12x * R12 * K2.inv(); } void LocalMapping::RequestStop() @@ -563,7 +558,7 @@ void LocalMapping::RequestStop() bool LocalMapping::Stop() { unique_lock lock(mMutexStop); - if(mbStopRequested && !mbNotStop) + if (mbStopRequested && !mbNotStop) { mbStopped = true; cout << "Local Mapping STOP" << endl; @@ -589,11 +584,11 @@ void LocalMapping::Release() { unique_lock lock(mMutexStop); unique_lock lock2(mMutexFinish); - if(mbFinished) + if (mbFinished) return; mbStopped = false; mbStopRequested = false; - for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = mlNewKeyFrames.begin(), lend = mlNewKeyFrames.end(); lit != lend; lit++) delete *lit; mlNewKeyFrames.clear(); @@ -609,14 +604,14 @@ bool LocalMapping::AcceptKeyFrames() void LocalMapping::SetAcceptKeyFrames(bool flag) { unique_lock lock(mMutexAccept); - mbAcceptKeyFrames=flag; + mbAcceptKeyFrames = flag; } bool LocalMapping::SetNotStop(bool flag) { unique_lock lock(mMutexStop); - if(flag && mbStopped) + if (flag && mbStopped) return false; mbNotStop = flag; @@ -635,71 +630,71 @@ void LocalMapping::KeyFrameCulling() // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen // in at least other 3 keyframes (in the same or finer scale) // We only consider close stereo points - vector vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames(); + vector vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames(); - for(vector::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++) + for (vector::iterator vit = vpLocalKeyFrames.begin(), vend = vpLocalKeyFrames.end(); vit != vend; vit++) { - KeyFrame* pKF = *vit; - if(pKF->mnId==0) + KeyFrame *pKF = *vit; + if (pKF->mnId == 0) continue; - const vector vpMapPoints = pKF->GetMapPointMatches(); + const vector vpMapPoints = pKF->GetMapPointMatches(); int nObs = 3; - const int thObs=nObs; - int nRedundantObservations=0; - int nMPs=0; - for(size_t i=0, iend=vpMapPoints.size(); iisBad()) + if (!pMP->isBad()) { - if(!mbMonocular) + if (!mbMonocular) { - if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0) + if (pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0) continue; } nMPs++; - if(pMP->Observations()>thObs) + if (pMP->Observations() > thObs) { const int &scaleLevel = pKF->mvKeysUn[i].octave; - const map observations = pMP->GetObservations(); - int nObs=0; - for(map::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + const map observations = pMP->GetObservations(); + int nObs = 0; + for (map::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; - if(pKFi==pKF) + KeyFrame *pKFi = mit->first; + if (pKFi == pKF) continue; const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave; - if(scaleLeveli<=scaleLevel+1) + if (scaleLeveli <= scaleLevel + 1) { nObs++; - if(nObs>=thObs) + if (nObs >= thObs) break; } } - if(nObs>=thObs) + if (nObs >= thObs) { nRedundantObservations++; } } } } - } + } - if(nRedundantObservations>0.9*nMPs) + if (nRedundantObservations > 0.9 * nMPs) pKF->SetBadFlag(); } } cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v) { - return (cv::Mat_(3,3) << 0, -v.at(2), v.at(1), - v.at(2), 0,-v.at(0), - -v.at(1), v.at(0), 0); + return (cv::Mat_(3, 3) << 0, -v.at(2), v.at(1), + v.at(2), 0, -v.at(0), + -v.at(1), v.at(0), 0); } void LocalMapping::RequestReset() @@ -709,11 +704,11 @@ void LocalMapping::RequestReset() mbResetRequested = true; } - while(1) + while (1) { { unique_lock lock2(mMutexReset); - if(!mbResetRequested) + if (!mbResetRequested) break; } usleep(3000); @@ -723,11 +718,11 @@ void LocalMapping::RequestReset() void LocalMapping::ResetIfRequested() { unique_lock lock(mMutexReset); - if(mbResetRequested) + if (mbResetRequested) { mlNewKeyFrames.clear(); mlpRecentAddedMapPoints.clear(); - mbResetRequested=false; + mbResetRequested = false; } } @@ -746,7 +741,7 @@ bool LocalMapping::CheckFinish() void LocalMapping::SetFinish() { unique_lock lock(mMutexFinish); - mbFinished = true; + mbFinished = true; unique_lock lock2(mMutexStop); mbStopped = true; } @@ -757,4 +752,4 @@ bool LocalMapping::isFinished() return mbFinished; } -} //namespace ORB_SLAM +} // namespace ORB_SLAM2 diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 5e317dd420..2566b70344 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -28,57 +28,56 @@ #include "ORBmatcher.h" -#include -#include +#include +#include +#include "unistd.h" namespace ORB_SLAM2 { -LoopClosing::LoopClosing(Map *pMap, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale): - mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap), - mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true), - mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0) +LoopClosing::LoopClosing(Map *pMap, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale) : mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap), + mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true), + mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0) { mnCovisibilityConsistencyTh = 3; } void LoopClosing::SetTracker(Tracking *pTracker) { - mpTracker=pTracker; + mpTracker = pTracker; } void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper) { - mpLocalMapper=pLocalMapper; + mpLocalMapper = pLocalMapper; } - void LoopClosing::Run() { - mbFinished =false; + mbFinished = false; - while(1) + while (1) { // Check if there are keyframes in the queue - if(CheckNewKeyFrames()) + if (CheckNewKeyFrames()) { // Detect loop candidates and check covisibility consistency - if(DetectLoop()) + if (DetectLoop()) { - // Compute similarity transformation [sR|t] - // In the stereo/RGBD case s=1 - if(ComputeSim3()) - { - // Perform loop fusion and pose graph optimization - CorrectLoop(); - } + // Compute similarity transformation [sR|t] + // In the stereo/RGBD case s=1 + if (ComputeSim3()) + { + // Perform loop fusion and pose graph optimization + CorrectLoop(); + } } - } + } ResetIfRequested(); - if(CheckFinish()) + if (CheckFinish()) break; usleep(5000); @@ -90,14 +89,14 @@ void LoopClosing::Run() void LoopClosing::InsertKeyFrame(KeyFrame *pKF) { unique_lock lock(mMutexLoopQueue); - if(pKF->mnId!=0) + if (pKF->mnId != 0) mlpLoopKeyFrameQueue.push_back(pKF); } bool LoopClosing::CheckNewKeyFrames() { unique_lock lock(mMutexLoopQueue); - return(!mlpLoopKeyFrameQueue.empty()); + return (!mlpLoopKeyFrameQueue.empty()); } bool LoopClosing::DetectLoop() @@ -111,7 +110,7 @@ bool LoopClosing::DetectLoop() } //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection - if(mpCurrentKF->mnIdmnId < mLastLoopKFid + 10) { mpKeyFrameDB->add(mpCurrentKF); mpCurrentKF->SetErase(); @@ -121,27 +120,27 @@ bool LoopClosing::DetectLoop() // Compute reference BoW similarity score // This is the lowest score to a connected keyframe in the covisibility graph // We will impose loop candidates to have a higher similarity than this - const vector vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames(); + const vector vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames(); const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec; float minScore = 1; - for(size_t i=0; iisBad()) + KeyFrame *pKF = vpConnectedKeyFrames[i]; + if (pKF->isBad()) continue; const DBoW2::BowVector &BowVec = pKF->mBowVec; float score = mpORBVocabulary->score(CurrentBowVec, BowVec); - if(score vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore); + vector vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore); // If there are no loop candidates, just add new keyframe and return false - if(vpCandidateKFs.empty()) + if (vpCandidateKFs.empty()) { mpKeyFrameDB->add(mpCurrentKF); mvConsistentGroups.clear(); @@ -156,53 +155,53 @@ bool LoopClosing::DetectLoop() mvpEnoughConsistentCandidates.clear(); vector vCurrentConsistentGroups; - vector vbConsistentGroup(mvConsistentGroups.size(),false); - for(size_t i=0, iend=vpCandidateKFs.size(); i vbConsistentGroup(mvConsistentGroups.size(), false); + for (size_t i = 0, iend = vpCandidateKFs.size(); i < iend; i++) { - KeyFrame* pCandidateKF = vpCandidateKFs[i]; + KeyFrame *pCandidateKF = vpCandidateKFs[i]; - set spCandidateGroup = pCandidateKF->GetConnectedKeyFrames(); + set spCandidateGroup = pCandidateKF->GetConnectedKeyFrames(); spCandidateGroup.insert(pCandidateKF); bool bEnoughConsistent = false; bool bConsistentForSomeGroup = false; - for(size_t iG=0, iendG=mvConsistentGroups.size(); iG sPreviousGroup = mvConsistentGroups[iG].first; + set sPreviousGroup = mvConsistentGroups[iG].first; bool bConsistent = false; - for(set::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++) + for (set::iterator sit = spCandidateGroup.begin(), send = spCandidateGroup.end(); sit != send; sit++) { - if(sPreviousGroup.count(*sit)) + if (sPreviousGroup.count(*sit)) { - bConsistent=true; - bConsistentForSomeGroup=true; + bConsistent = true; + bConsistentForSomeGroup = true; break; } } - if(bConsistent) + if (bConsistent) { int nPreviousConsistency = mvConsistentGroups[iG].second; int nCurrentConsistency = nPreviousConsistency + 1; - if(!vbConsistentGroup[iG]) + if (!vbConsistentGroup[iG]) { - ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency); + ConsistentGroup cg = make_pair(spCandidateGroup, nCurrentConsistency); vCurrentConsistentGroups.push_back(cg); - vbConsistentGroup[iG]=true; //this avoid to include the same group more than once + vbConsistentGroup[iG] = true; //this avoid to include the same group more than once } - if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent) + if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent) { mvpEnoughConsistentCandidates.push_back(pCandidateKF); - bEnoughConsistent=true; //this avoid to insert the same candidate more than once + bEnoughConsistent = true; //this avoid to insert the same candidate more than once } } } // If the group is not consistent with any previous group insert with consistency counter set to zero - if(!bConsistentForSomeGroup) + if (!bConsistentForSomeGroup) { - ConsistentGroup cg = make_pair(spCandidateGroup,0); + ConsistentGroup cg = make_pair(spCandidateGroup, 0); vCurrentConsistentGroups.push_back(cg); } } @@ -210,11 +209,10 @@ bool LoopClosing::DetectLoop() // Update Covisibility Consistent Groups mvConsistentGroups = vCurrentConsistentGroups; - // Add Current Keyframe to database mpKeyFrameDB->add(mpCurrentKF); - if(mvpEnoughConsistentCandidates.empty()) + if (mvpEnoughConsistentCandidates.empty()) { mpCurrentKF->SetErase(); return false; @@ -236,43 +234,43 @@ bool LoopClosing::ComputeSim3() // We compute first ORB matches for each candidate // If enough matches are found, we setup a Sim3Solver - ORBmatcher matcher(0.75,true); + ORBmatcher matcher(0.75, true); - vector vpSim3Solvers; + vector vpSim3Solvers; vpSim3Solvers.resize(nInitialCandidates); - vector > vvpMapPointMatches; + vector> vvpMapPointMatches; vvpMapPointMatches.resize(nInitialCandidates); vector vbDiscarded; vbDiscarded.resize(nInitialCandidates); - int nCandidates=0; //candidates with enough matches + int nCandidates = 0; //candidates with enough matches - for(int i=0; iSetNotErase(); - if(pKF->isBad()) + if (pKF->isBad()) { vbDiscarded[i] = true; continue; } - int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]); + int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]); - if(nmatches<20) + if (nmatches < 20) { vbDiscarded[i] = true; continue; } else { - Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale); - pSolver->SetRansacParameters(0.99,20,300); + Sim3Solver *pSolver = new Sim3Solver(mpCurrentKF, pKF, vvpMapPointMatches[i], mbFixScale); + pSolver->SetRansacParameters(0.99, 20, 300); vpSim3Solvers[i] = pSolver; } @@ -283,55 +281,55 @@ bool LoopClosing::ComputeSim3() // Perform alternatively RANSAC iterations for each candidate // until one is succesful or all fail - while(nCandidates>0 && !bMatch) + while (nCandidates > 0 && !bMatch) { - for(int i=0; i vbInliers; int nInliers; bool bNoMore; - Sim3Solver* pSolver = vpSim3Solvers[i]; - cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers); + Sim3Solver *pSolver = vpSim3Solvers[i]; + cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers); // If Ransac reachs max. iterations discard keyframe - if(bNoMore) + if (bNoMore) { - vbDiscarded[i]=true; + vbDiscarded[i] = true; nCandidates--; } // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences - if(!Scm.empty()) + if (!Scm.empty()) { - vector vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast(NULL)); - for(size_t j=0, jend=vbInliers.size(); j vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast(NULL)); + for (size_t j = 0, jend = vbInliers.size(); j < jend; j++) { - if(vbInliers[j]) - vpMapPointMatches[j]=vvpMapPointMatches[i][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); + matcher.SearchBySim3(mpCurrentKF, pKF, vpMapPointMatches, s, R, t, 7.5); - g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s); + g2o::Sim3 gScm(Converter::toMatrix3d(R), Converter::toVector3d(t), s); const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale); // If optimization is succesful stop ransacs and continue - if(nInliers>=20) + if (nInliers >= 20) { bMatch = true; mpMatchedKF = pKF; - g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0); - mg2oScw = gScm*gSmw; + g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()), Converter::toVector3d(pKF->GetTranslation()), 1.0); + mg2oScw = gScm * gSmw; mScw = Converter::toCvMat(mg2oScw); mvpCurrentMatchedPoints = vpMapPointMatches; @@ -341,62 +339,61 @@ bool LoopClosing::ComputeSim3() } } - if(!bMatch) + if (!bMatch) { - for(int i=0; iSetErase(); + for (int i = 0; i < nInitialCandidates; i++) + mvpEnoughConsistentCandidates[i]->SetErase(); mpCurrentKF->SetErase(); return false; } // Retrieve MapPoints seen in Loop Keyframe and neighbors - vector vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames(); + vector vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames(); vpLoopConnectedKFs.push_back(mpMatchedKF); mvpLoopMapPoints.clear(); - for(vector::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++) + for (vector::iterator vit = vpLoopConnectedKFs.begin(); vit != vpLoopConnectedKFs.end(); vit++) { - KeyFrame* pKF = *vit; - vector vpMapPoints = pKF->GetMapPointMatches(); - for(size_t i=0, iend=vpMapPoints.size(); i vpMapPoints = pKF->GetMapPointMatches(); + for (size_t i = 0, iend = vpMapPoints.size(); i < iend; i++) { - MapPoint* pMP = vpMapPoints[i]; - if(pMP) + MapPoint *pMP = vpMapPoints[i]; + if (pMP) { - if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId) + if (!pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId) { mvpLoopMapPoints.push_back(pMP); - pMP->mnLoopPointForKF=mpCurrentKF->mnId; + pMP->mnLoopPointForKF = mpCurrentKF->mnId; } } } } // Find more matches projecting with the computed Sim3 - matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10); + matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints, 10); // If enough matches accept Loop int nTotalMatches = 0; - for(size_t i=0; i=40) + if (nTotalMatches >= 40) { - for(int i=0; iSetErase(); return true; } else { - for(int i=0; iSetErase(); mpCurrentKF->SetErase(); return false; } - } void LoopClosing::CorrectLoop() @@ -408,14 +405,14 @@ void LoopClosing::CorrectLoop() mpLocalMapper->RequestStop(); // If a Global Bundle Adjustment is running, abort it - if(isRunningGBA()) + if (isRunningGBA()) { unique_lock lock(mMutexGBA); mbStopGBA = true; mnFullBAIdx++; - if(mpThreadGBA) + if (mpThreadGBA) { mpThreadGBA->detach(); delete mpThreadGBA; @@ -423,7 +420,7 @@ void LoopClosing::CorrectLoop() } // Wait until Local Mapping has effectively stopped - while(!mpLocalMapper->isStopped()) + while (!mpLocalMapper->isStopped()) { usleep(1000); } @@ -436,62 +433,61 @@ void LoopClosing::CorrectLoop() mvpCurrentConnectedKFs.push_back(mpCurrentKF); KeyFrameAndPose CorrectedSim3, NonCorrectedSim3; - CorrectedSim3[mpCurrentKF]=mg2oScw; + CorrectedSim3[mpCurrentKF] = mg2oScw; cv::Mat Twc = mpCurrentKF->GetPoseInverse(); - { // Get Map Mutex unique_lock lock(mpMap->mMutexMapUpdate); - for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) + for (vector::iterator vit = mvpCurrentConnectedKFs.begin(), vend = mvpCurrentConnectedKFs.end(); vit != vend; vit++) { - KeyFrame* pKFi = *vit; + KeyFrame *pKFi = *vit; cv::Mat Tiw = pKFi->GetPose(); - if(pKFi!=mpCurrentKF) + if (pKFi != mpCurrentKF) { - cv::Mat Tic = Tiw*Twc; - cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3); - cv::Mat tic = Tic.rowRange(0,3).col(3); - g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0); - g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw; + cv::Mat Tic = Tiw * Twc; + cv::Mat Ric = Tic.rowRange(0, 3).colRange(0, 3); + cv::Mat tic = Tic.rowRange(0, 3).col(3); + g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric), Converter::toVector3d(tic), 1.0); + g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw; //Pose corrected with the Sim3 of the loop closure - CorrectedSim3[pKFi]=g2oCorrectedSiw; + CorrectedSim3[pKFi] = g2oCorrectedSiw; } - cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3); - cv::Mat tiw = Tiw.rowRange(0,3).col(3); - g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0); + cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3); + cv::Mat tiw = Tiw.rowRange(0, 3).col(3); + g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0); //Pose without correction - NonCorrectedSim3[pKFi]=g2oSiw; + NonCorrectedSim3[pKFi] = g2oSiw; } // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop - for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++) + for (KeyFrameAndPose::iterator mit = CorrectedSim3.begin(), mend = CorrectedSim3.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; + KeyFrame *pKFi = mit->first; g2o::Sim3 g2oCorrectedSiw = mit->second; g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse(); - g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi]; + g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi]; - vector vpMPsi = pKFi->GetMapPointMatches(); - for(size_t iMP=0, endMPi = vpMPsi.size(); iMP vpMPsi = pKFi->GetMapPointMatches(); + for (size_t iMP = 0, endMPi = vpMPsi.size(); iMP < endMPi; iMP++) { - MapPoint* pMPi = vpMPsi[iMP]; - if(!pMPi) + MapPoint *pMPi = vpMPsi[iMP]; + if (!pMPi) continue; - if(pMPi->isBad()) + if (pMPi->isBad()) continue; - if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) + if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId) continue; // Project with non-corrected pose and project back with corrected pose cv::Mat P3Dw = pMPi->GetWorldPos(); - Eigen::Matrix eigP3Dw = Converter::toVector3d(P3Dw); - Eigen::Matrix eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw)); + Eigen::Matrix eigP3Dw = Converter::toVector3d(P3Dw); + Eigen::Matrix eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw)); cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw); pMPi->SetWorldPos(cvCorrectedP3Dw); @@ -505,9 +501,9 @@ void LoopClosing::CorrectLoop() Eigen::Vector3d eigt = g2oCorrectedSiw.translation(); double s = g2oCorrectedSiw.scale(); - eigt *=(1./s); //[R t/s;0 1] + eigt *= (1. / s); //[R t/s;0 1] - cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt); + cv::Mat correctedTiw = Converter::toCvSE3(eigR, eigt); pKFi->SetPose(correctedTiw); @@ -517,23 +513,22 @@ void LoopClosing::CorrectLoop() // Start Loop Fusion // Update matched map points and replace if duplicated - for(size_t i=0; iGetMapPoint(i); - if(pCurMP) + MapPoint *pLoopMP = mvpCurrentMatchedPoints[i]; + MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i); + if (pCurMP) pCurMP->Replace(pLoopMP); else { - mpCurrentKF->AddMapPoint(pLoopMP,i); - pLoopMP->AddObservation(mpCurrentKF,i); + mpCurrentKF->AddMapPoint(pLoopMP, i); + pLoopMP->AddObservation(mpCurrentKF, i); pLoopMP->ComputeDistinctiveDescriptors(); } } } - } // Project MapPoints observed in the neighborhood of the loop keyframe @@ -541,23 +536,22 @@ void LoopClosing::CorrectLoop() // Fuse duplications. SearchAndFuse(CorrectedSim3); - // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop - map > LoopConnections; + map> LoopConnections; - for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) + for (vector::iterator vit = mvpCurrentConnectedKFs.begin(), vend = mvpCurrentConnectedKFs.end(); vit != vend; vit++) { - KeyFrame* pKFi = *vit; - vector vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames(); + KeyFrame *pKFi = *vit; + vector vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames(); // Update connections. Detect new links. pKFi->UpdateConnections(); - LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames(); - for(vector::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++) + LoopConnections[pKFi] = pKFi->GetConnectedKeyFrames(); + for (vector::iterator vit_prev = vpPreviousNeighbors.begin(), vend_prev = vpPreviousNeighbors.end(); vit_prev != vend_prev; vit_prev++) { LoopConnections[pKFi].erase(*vit_prev); } - for(vector::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++) + for (vector::iterator vit2 = mvpCurrentConnectedKFs.begin(), vend2 = mvpCurrentConnectedKFs.end(); vit2 != vend2; vit2++) { LoopConnections[pKFi].erase(*vit2); } @@ -576,35 +570,35 @@ void LoopClosing::CorrectLoop() mbRunningGBA = true; mbFinishedGBA = false; mbStopGBA = false; - mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId); + mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId); // Loop closed. Release Local Mapping. - mpLocalMapper->Release(); + mpLocalMapper->Release(); - mLastLoopKFid = mpCurrentKF->mnId; + mLastLoopKFid = mpCurrentKF->mnId; } void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap) { ORBmatcher matcher(0.8); - for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++) + for (KeyFrameAndPose::const_iterator mit = CorrectedPosesMap.begin(), mend = CorrectedPosesMap.end(); mit != mend; mit++) { - KeyFrame* pKF = mit->first; + KeyFrame *pKF = mit->first; g2o::Sim3 g2oScw = mit->second; cv::Mat cvScw = Converter::toCvMat(g2oScw); - vector vpReplacePoints(mvpLoopMapPoints.size(),static_cast(NULL)); - matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints); + vector vpReplacePoints(mvpLoopMapPoints.size(), static_cast(NULL)); + matcher.Fuse(pKF, cvScw, mvpLoopMapPoints, 4, vpReplacePoints); // Get Map Mutex unique_lock lock(mpMap->mMutexMapUpdate); const int nLP = mvpLoopMapPoints.size(); - for(int i=0; iReplace(mvpLoopMapPoints[i]); } @@ -612,7 +606,6 @@ void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap) } } - void LoopClosing::RequestReset() { { @@ -620,12 +613,12 @@ void LoopClosing::RequestReset() mbResetRequested = true; } - while(1) + while (1) { { - unique_lock lock2(mMutexReset); - if(!mbResetRequested) - break; + unique_lock lock2(mMutexReset); + if (!mbResetRequested) + break; } usleep(5000); } @@ -634,11 +627,11 @@ void LoopClosing::RequestReset() void LoopClosing::ResetIfRequested() { unique_lock lock(mMutexReset); - if(mbResetRequested) + if (mbResetRequested) { mlpLoopKeyFrameQueue.clear(); - mLastLoopKFid=0; - mbResetRequested=false; + mLastLoopKFid = 0; + mbResetRequested = false; } } @@ -646,8 +639,8 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) { cout << "Starting Global Bundle Adjustment" << endl; - int idx = mnFullBAIdx; - Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false); + int idx = mnFullBAIdx; + Optimizer::GlobalBundleAdjustemnt(mpMap, 10, &mbStopGBA, nLoopKF, false); // Update all MapPoints and KeyFrames // Local Mapping was active during BA, that means that there might be new keyframes @@ -655,17 +648,17 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) // We need to propagate the correction through the spanning tree { unique_lock lock(mMutexGBA); - if(idx!=mnFullBAIdx) + if (idx != mnFullBAIdx) return; - if(!mbStopGBA) + if (!mbStopGBA) { cout << "Global Bundle Adjustment finished" << endl; cout << "Updating map ..." << endl; mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped - while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished()) + while (!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished()) { usleep(1000); } @@ -674,22 +667,21 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) unique_lock lock(mpMap->mMutexMapUpdate); // Correct keyframes starting at map first keyframe - list lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end()); + list lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(), mpMap->mvpKeyFrameOrigins.end()); - while(!lpKFtoCheck.empty()) + while (!lpKFtoCheck.empty()) { - KeyFrame* pKF = lpKFtoCheck.front(); - const set sChilds = pKF->GetChilds(); + KeyFrame *pKF = lpKFtoCheck.front(); + const set sChilds = pKF->GetChilds(); cv::Mat Twc = pKF->GetPoseInverse(); - for(set::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++) + for (set::const_iterator sit = sChilds.begin(); sit != sChilds.end(); sit++) { - KeyFrame* pChild = *sit; - if(pChild->mnBAGlobalForKF!=nLoopKF) + KeyFrame *pChild = *sit; + if (pChild->mnBAGlobalForKF != nLoopKF) { - cv::Mat Tchildc = pChild->GetPose()*Twc; - pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;//*Tcorc*pKF->mTcwGBA; - pChild->mnBAGlobalForKF=nLoopKF; - + cv::Mat Tchildc = pChild->GetPose() * Twc; + pChild->mTcwGBA = Tchildc * pKF->mTcwGBA; //*Tcorc*pKF->mTcwGBA; + pChild->mnBAGlobalForKF = nLoopKF; } lpKFtoCheck.push_back(pChild); } @@ -700,16 +692,16 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) } // Correct MapPoints - const vector vpMPs = mpMap->GetAllMapPoints(); + const vector vpMPs = mpMap->GetAllMapPoints(); - for(size_t i=0; iisBad()) + if (pMP->isBad()) continue; - if(pMP->mnBAGlobalForKF==nLoopKF) + if (pMP->mnBAGlobalForKF == nLoopKF) { // If optimized by Global BA, just update pMP->SetWorldPos(pMP->mPosGBA); @@ -717,24 +709,24 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) else { // Update according to the correction of its reference keyframe - KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); + KeyFrame *pRefKF = pMP->GetReferenceKeyFrame(); - if(pRefKF->mnBAGlobalForKF!=nLoopKF) + if (pRefKF->mnBAGlobalForKF != nLoopKF) continue; // Map to non-corrected camera - cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3); - cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3); - cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw; + cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0, 3).colRange(0, 3); + cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0, 3).col(3); + cv::Mat Xc = Rcw * pMP->GetWorldPos() + tcw; // Backproject using corrected camera cv::Mat Twc = pRefKF->GetPoseInverse(); - cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3); - cv::Mat twc = Twc.rowRange(0,3).col(3); + cv::Mat Rwc = Twc.rowRange(0, 3).colRange(0, 3); + cv::Mat twc = Twc.rowRange(0, 3).col(3); - pMP->SetWorldPos(Rwc*Xc+twc); + pMP->SetWorldPos(Rwc * Xc + twc); } - } + } mpMap->InformNewBigChange(); @@ -772,5 +764,4 @@ bool LoopClosing::isFinished() return mbFinished; } - -} //namespace ORB_SLAM +} // namespace ORB_SLAM2 diff --git a/src/System.cc b/src/System.cc index 8df4157095..6da47bc19f 100644 --- a/src/System.cc +++ b/src/System.cc @@ -18,58 +18,59 @@ * along with ORB-SLAM2. If not, see . */ - - #include "System.h" #include "Converter.h" #include #include #include +#include "unistd.h" namespace ORB_SLAM2 { System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, - const bool bUseViewer):mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false),mbActivateLocalizationMode(false), - mbDeactivateLocalizationMode(false) + const bool bUseViewer) : mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false), mbActivateLocalizationMode(false), + mbDeactivateLocalizationMode(false) { // Output welcome message - cout << endl << - "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl << - "This program comes with ABSOLUTELY NO WARRANTY;" << endl << - "This is free software, and you are welcome to redistribute it" << endl << - "under certain conditions. See LICENSE.txt." << endl << endl; + cout << endl + << "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl + << "This program comes with ABSOLUTELY NO WARRANTY;" << endl + << "This is free software, and you are welcome to redistribute it" << endl + << "under certain conditions. See LICENSE.txt." << endl + << endl; cout << "Input sensor was set to: "; - if(mSensor==MONOCULAR) + if (mSensor == MONOCULAR) cout << "Monocular" << endl; - else if(mSensor==STEREO) + else if (mSensor == STEREO) cout << "Stereo" << endl; - else if(mSensor==RGBD) + else if (mSensor == RGBD) cout << "RGB-D" << endl; //Check settings file cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); - if(!fsSettings.isOpened()) + if (!fsSettings.isOpened()) { - cerr << "Failed to open settings file at: " << strSettingsFile << endl; - exit(-1); + cerr << "Failed to open settings file at: " << strSettingsFile << endl; + exit(-1); } - //Load ORB Vocabulary - cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; + cout << endl + << "Loading ORB Vocabulary. This could take a while..." << endl; mpVocabulary = new ORBVocabulary(); bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); - if(!bVocLoad) + if (!bVocLoad) { cerr << "Wrong path to vocabulary. " << endl; cerr << "Falied to open at: " << strVocFile << endl; exit(-1); } - cout << "Vocabulary loaded!" << endl << endl; + cout << "Vocabulary loaded!" << endl + << endl; //Create KeyFrame Database mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); @@ -87,17 +88,17 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor); //Initialize the Local Mapping thread and launch - mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); - mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper); + mpLocalMapper = new LocalMapping(mpMap, mSensor == MONOCULAR); + mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper); //Initialize the Loop Closing thread and launch - mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); + mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor != MONOCULAR); mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser); //Initialize the Viewer thread and launch - if(bUseViewer) + if (bUseViewer) { - mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile); + mpViewer = new Viewer(this, mpFrameDrawer, mpMapDrawer, mpTracker, strSettingsFile); mptViewer = new thread(&Viewer::Run, mpViewer); mpTracker->SetViewer(mpViewer); } @@ -115,21 +116,21 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp) { - if(mSensor!=STEREO) + if (mSensor != STEREO) { cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl; exit(-1); - } + } // Check mode change { unique_lock lock(mMutexMode); - if(mbActivateLocalizationMode) + if (mbActivateLocalizationMode) { mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped - while(!mpLocalMapper->isStopped()) + while (!mpLocalMapper->isStopped()) { usleep(1000); } @@ -137,7 +138,7 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const mpTracker->InformOnlyTracking(true); mbActivateLocalizationMode = false; } - if(mbDeactivateLocalizationMode) + if (mbDeactivateLocalizationMode) { mpTracker->InformOnlyTracking(false); mpLocalMapper->Release(); @@ -147,15 +148,15 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const // Check reset { - unique_lock lock(mMutexReset); - if(mbReset) - { - mpTracker->Reset(); - mbReset = false; - } + unique_lock lock(mMutexReset); + if (mbReset) + { + mpTracker->Reset(); + mbReset = false; + } } - cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp); + cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft, imRight, timestamp); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -166,21 +167,21 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) { - if(mSensor!=RGBD) + if (mSensor != RGBD) { cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl; exit(-1); - } + } // Check mode change { unique_lock lock(mMutexMode); - if(mbActivateLocalizationMode) + if (mbActivateLocalizationMode) { mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped - while(!mpLocalMapper->isStopped()) + while (!mpLocalMapper->isStopped()) { usleep(1000); } @@ -188,7 +189,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub mpTracker->InformOnlyTracking(true); mbActivateLocalizationMode = false; } - if(mbDeactivateLocalizationMode) + if (mbDeactivateLocalizationMode) { mpTracker->InformOnlyTracking(false); mpLocalMapper->Release(); @@ -198,15 +199,15 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub // Check reset { - unique_lock lock(mMutexReset); - if(mbReset) - { - mpTracker->Reset(); - mbReset = false; - } + unique_lock lock(mMutexReset); + if (mbReset) + { + mpTracker->Reset(); + mbReset = false; + } } - cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp); + cv::Mat Tcw = mpTracker->GrabImageRGBD(im, depthmap, timestamp); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -217,7 +218,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) { - if(mSensor!=MONOCULAR) + if (mSensor != MONOCULAR) { cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl; exit(-1); @@ -226,12 +227,12 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) // Check mode change { unique_lock lock(mMutexMode); - if(mbActivateLocalizationMode) + if (mbActivateLocalizationMode) { mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped - while(!mpLocalMapper->isStopped()) + while (!mpLocalMapper->isStopped()) { usleep(1000); } @@ -239,7 +240,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) mpTracker->InformOnlyTracking(true); mbActivateLocalizationMode = false; } - if(mbDeactivateLocalizationMode) + if (mbDeactivateLocalizationMode) { mpTracker->InformOnlyTracking(false); mpLocalMapper->Release(); @@ -249,15 +250,15 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) // Check reset { - unique_lock lock(mMutexReset); - if(mbReset) - { - mpTracker->Reset(); - mbReset = false; - } + unique_lock lock(mMutexReset); + if (mbReset) + { + mpTracker->Reset(); + mbReset = false; + } } - cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp); + cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -281,11 +282,11 @@ void System::DeactivateLocalizationMode() bool System::MapChanged() { - static int n=0; + static int n = 0; int curn = mpMap->GetLastBigChangeIdx(); - if(nRequestFinish(); mpLoopCloser->RequestFinish(); - if(mpViewer) + if (mpViewer) { mpViewer->RequestFinish(); - while(!mpViewer->isFinished()) + while (!mpViewer->isFinished()) usleep(5000); } // Wait until all thread have effectively stopped - while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA()) + while (!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA()) { usleep(5000); } - if(mpViewer) + if (mpViewer) pangolin::BindToContext("ORB-SLAM2: Map Viewer"); } void System::SaveTrajectoryTUM(const string &filename) { - cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; - if(mSensor==MONOCULAR) + cout << endl + << "Saving camera trajectory to " << filename << " ..." << endl; + if (mSensor == MONOCULAR) { cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl; return; } - vector vpKFs = mpMap->GetAllKeyFrames(); - sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + vector vpKFs = mpMap->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); // Transform all keyframes so that the first keyframe is at the origin. // After a loop closure the first keyframe might not be at the origin. @@ -345,47 +347,49 @@ void System::SaveTrajectoryTUM(const string &filename) // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag // which is true when tracking failed (lbL). - list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); list::iterator lbL = mpTracker->mlbLost.begin(); - for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), - lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) + for (list::iterator lit = mpTracker->mlRelativeFramePoses.begin(), + lend = mpTracker->mlRelativeFramePoses.end(); + lit != lend; lit++, lRit++, lT++, lbL++) { - if(*lbL) + if (*lbL) continue; - KeyFrame* pKF = *lRit; + KeyFrame *pKF = *lRit; - cv::Mat Trw = cv::Mat::eye(4,4,CV_32F); + cv::Mat Trw = cv::Mat::eye(4, 4, CV_32F); // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. - while(pKF->isBad()) + while (pKF->isBad()) { - Trw = Trw*pKF->mTcp; + Trw = Trw * pKF->mTcp; pKF = pKF->GetParent(); } - Trw = Trw*pKF->GetPose()*Two; + Trw = Trw * pKF->GetPose() * Two; - cv::Mat Tcw = (*lit)*Trw; - cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); - cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); + cv::Mat Tcw = (*lit) * Trw; + cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t(); + cv::Mat twc = -Rwc * Tcw.rowRange(0, 3).col(3); vector q = Converter::toQuaternion(Rwc); - f << setprecision(6) << *lT << " " << setprecision(9) << twc.at(0) << " " << twc.at(1) << " " << twc.at(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; + f << setprecision(6) << *lT << " " << setprecision(9) << twc.at(0) << " " << twc.at(1) << " " << twc.at(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; } f.close(); - cout << endl << "trajectory saved!" << endl; + cout << endl + << "trajectory saved!" << endl; } - void System::SaveKeyFrameTrajectoryTUM(const string &filename) { - cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl; + cout << endl + << "Saving keyframe trajectory to " << filename << " ..." << endl; - vector vpKFs = mpMap->GetAllKeyFrames(); - sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + vector vpKFs = mpMap->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); // Transform all keyframes so that the first keyframe is at the origin. // After a loop closure the first keyframe might not be at the origin. @@ -395,13 +399,13 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename) f.open(filename.c_str()); f << fixed; - for(size_t i=0; iSetPose(pKF->GetPose()*Two); + // pKF->SetPose(pKF->GetPose()*Two); - if(pKF->isBad()) + if (pKF->isBad()) continue; cv::Mat R = pKF->GetRotation().t(); @@ -409,24 +413,25 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename) cv::Mat t = pKF->GetCameraCenter(); f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at(0) << " " << t.at(1) << " " << t.at(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; - } f.close(); - cout << endl << "trajectory saved!" << endl; + cout << endl + << "trajectory saved!" << endl; } void System::SaveTrajectoryKITTI(const string &filename) { - cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; - if(mSensor==MONOCULAR) + cout << endl + << "Saving camera trajectory to " << filename << " ..." << endl; + if (mSensor == MONOCULAR) { cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl; return; } - vector vpKFs = mpMap->GetAllKeyFrames(); - sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + vector vpKFs = mpMap->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); // Transform all keyframes so that the first keyframe is at the origin. // After a loop closure the first keyframe might not be at the origin. @@ -442,33 +447,32 @@ void System::SaveTrajectoryKITTI(const string &filename) // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag // which is true when tracking failed (lbL). - list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); - for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++) + for (list::iterator lit = mpTracker->mlRelativeFramePoses.begin(), lend = mpTracker->mlRelativeFramePoses.end(); lit != lend; lit++, lRit++, lT++) { - ORB_SLAM2::KeyFrame* pKF = *lRit; + ORB_SLAM2::KeyFrame *pKF = *lRit; - cv::Mat Trw = cv::Mat::eye(4,4,CV_32F); + cv::Mat Trw = cv::Mat::eye(4, 4, CV_32F); - while(pKF->isBad()) + while (pKF->isBad()) { - // cout << "bad parent" << endl; - Trw = Trw*pKF->mTcp; + // cout << "bad parent" << endl; + Trw = Trw * pKF->mTcp; pKF = pKF->GetParent(); } - Trw = Trw*pKF->GetPose()*Two; + Trw = Trw * pKF->GetPose() * Two; - cv::Mat Tcw = (*lit)*Trw; - cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); - cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); + cv::Mat Tcw = (*lit) * Trw; + cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t(); + cv::Mat twc = -Rwc * Tcw.rowRange(0, 3).col(3); - f << setprecision(9) << Rwc.at(0,0) << " " << Rwc.at(0,1) << " " << Rwc.at(0,2) << " " << twc.at(0) << " " << - Rwc.at(1,0) << " " << Rwc.at(1,1) << " " << Rwc.at(1,2) << " " << twc.at(1) << " " << - Rwc.at(2,0) << " " << Rwc.at(2,1) << " " << Rwc.at(2,2) << " " << twc.at(2) << endl; + f << setprecision(9) << Rwc.at(0, 0) << " " << Rwc.at(0, 1) << " " << Rwc.at(0, 2) << " " << twc.at(0) << " " << Rwc.at(1, 0) << " " << Rwc.at(1, 1) << " " << Rwc.at(1, 2) << " " << twc.at(1) << " " << Rwc.at(2, 0) << " " << Rwc.at(2, 1) << " " << Rwc.at(2, 2) << " " << twc.at(2) << endl; } f.close(); - cout << endl << "trajectory saved!" << endl; + cout << endl + << "trajectory saved!" << endl; } int System::GetTrackingState() @@ -477,7 +481,7 @@ int System::GetTrackingState() return mTrackingState; } -vector System::GetTrackedMapPoints() +vector System::GetTrackedMapPoints() { unique_lock lock(mMutexState); return mTrackedMapPoints; @@ -489,4 +493,4 @@ vector System::GetTrackedKeyPointsUn() return mTrackedKeyPointsUn; } -} //namespace ORB_SLAM +} // namespace ORB_SLAM2 diff --git a/src/Tracking.cc b/src/Tracking.cc index 2273b2ce48..42cc0f2c23 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -18,35 +18,33 @@ * along with ORB-SLAM2. If not, see . */ - #include "Tracking.h" -#include -#include - -#include"ORBmatcher.h" -#include"FrameDrawer.h" -#include"Converter.h" -#include"Map.h" -#include"Initializer.h" +#include +#include -#include"Optimizer.h" -#include"PnPsolver.h" +#include "ORBmatcher.h" +#include "FrameDrawer.h" +#include "Converter.h" +#include "Map.h" +#include "Initializer.h" +#include "unistd.h" -#include +#include "Optimizer.h" +#include "PnPsolver.h" -#include +#include +#include using namespace std; namespace ORB_SLAM2 { -Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor): - mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc), - mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL), - mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0) +Tracking::Tracking(System *pSys, ORBVocabulary *pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase *pKFDB, const string &strSettingPath, const int sensor) : mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc), + mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL), + mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0) { // Load camera parameters from settings file @@ -56,20 +54,20 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, float cx = fSettings["Camera.cx"]; float cy = fSettings["Camera.cy"]; - cv::Mat K = cv::Mat::eye(3,3,CV_32F); - K.at(0,0) = fx; - K.at(1,1) = fy; - K.at(0,2) = cx; - K.at(1,2) = cy; + cv::Mat K = cv::Mat::eye(3, 3, CV_32F); + K.at(0, 0) = fx; + K.at(1, 1) = fy; + K.at(0, 2) = cx; + K.at(1, 2) = cy; K.copyTo(mK); - cv::Mat DistCoef(4,1,CV_32F); + cv::Mat DistCoef(4, 1, CV_32F); DistCoef.at(0) = fSettings["Camera.k1"]; DistCoef.at(1) = fSettings["Camera.k2"]; DistCoef.at(2) = fSettings["Camera.p1"]; DistCoef.at(3) = fSettings["Camera.p2"]; const float k3 = fSettings["Camera.k3"]; - if(k3!=0) + if (k3 != 0) { DistCoef.resize(5); DistCoef.at(4) = k3; @@ -79,31 +77,31 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, mbf = fSettings["Camera.bf"]; float fps = fSettings["Camera.fps"]; - if(fps==0) - fps=30; + if (fps == 0) + fps = 30; // Max/Min Frames to insert keyframes and to check relocalisation mMinFrames = 0; mMaxFrames = fps; - cout << endl << "Camera Parameters: " << endl; + cout << endl + << "Camera Parameters: " << endl; cout << "- fx: " << fx << endl; cout << "- fy: " << fy << endl; cout << "- cx: " << cx << endl; cout << "- cy: " << cy << endl; cout << "- k1: " << DistCoef.at(0) << endl; cout << "- k2: " << DistCoef.at(1) << endl; - if(DistCoef.rows==5) + if (DistCoef.rows == 5) cout << "- k3: " << DistCoef.at(4) << endl; cout << "- p1: " << DistCoef.at(2) << endl; cout << "- p2: " << DistCoef.at(3) << endl; cout << "- fps: " << fps << endl; - int nRGB = fSettings["Camera.RGB"]; mbRGB = nRGB; - if(mbRGB) + if (mbRGB) cout << "- color order: RGB (ignored if grayscale)" << endl; else cout << "- color order: BGR (ignored if grayscale)" << endl; @@ -116,148 +114,146 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, int fIniThFAST = fSettings["ORBextractor.iniThFAST"]; int fMinThFAST = fSettings["ORBextractor.minThFAST"]; - mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + mpORBextractorLeft = new ORBextractor(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); - if(sensor==System::STEREO) - mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + if (sensor == System::STEREO) + mpORBextractorRight = new ORBextractor(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); - if(sensor==System::MONOCULAR) - mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + if (sensor == System::MONOCULAR) + mpIniORBextractor = new ORBextractor(2 * nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); - cout << endl << "ORB Extractor Parameters: " << endl; + cout << endl + << "ORB Extractor Parameters: " << endl; cout << "- Number of Features: " << nFeatures << endl; cout << "- Scale Levels: " << nLevels << endl; cout << "- Scale Factor: " << fScaleFactor << endl; cout << "- Initial Fast Threshold: " << fIniThFAST << endl; cout << "- Minimum Fast Threshold: " << fMinThFAST << endl; - if(sensor==System::STEREO || sensor==System::RGBD) + if (sensor == System::STEREO || sensor == System::RGBD) { - mThDepth = mbf*(float)fSettings["ThDepth"]/fx; - cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl; + mThDepth = mbf * (float)fSettings["ThDepth"] / fx; + cout << endl + << "Depth Threshold (Close/Far Points): " << mThDepth << endl; } - if(sensor==System::RGBD) + if (sensor == System::RGBD) { mDepthMapFactor = fSettings["DepthMapFactor"]; - if(fabs(mDepthMapFactor)<1e-5) - mDepthMapFactor=1; + if (fabs(mDepthMapFactor) < 1e-5) + mDepthMapFactor = 1; else - mDepthMapFactor = 1.0f/mDepthMapFactor; + mDepthMapFactor = 1.0f / mDepthMapFactor; } - } void Tracking::SetLocalMapper(LocalMapping *pLocalMapper) { - mpLocalMapper=pLocalMapper; + mpLocalMapper = pLocalMapper; } void Tracking::SetLoopClosing(LoopClosing *pLoopClosing) { - mpLoopClosing=pLoopClosing; + mpLoopClosing = pLoopClosing; } void Tracking::SetViewer(Viewer *pViewer) { - mpViewer=pViewer; + mpViewer = pViewer; } - cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp) { mImGray = imRectLeft; cv::Mat imGrayRight = imRectRight; - if(mImGray.channels()==3) + if (mImGray.channels() == 3) { - if(mbRGB) + if (mbRGB) { - cvtColor(mImGray,mImGray,CV_RGB2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY); + cvtColor(mImGray, mImGray, CV_RGB2GRAY); + cvtColor(imGrayRight, imGrayRight, CV_RGB2GRAY); } else { - cvtColor(mImGray,mImGray,CV_BGR2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY); + cvtColor(mImGray, mImGray, CV_BGR2GRAY); + cvtColor(imGrayRight, imGrayRight, CV_BGR2GRAY); } } - else if(mImGray.channels()==4) + else if (mImGray.channels() == 4) { - if(mbRGB) + if (mbRGB) { - cvtColor(mImGray,mImGray,CV_RGBA2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY); + cvtColor(mImGray, mImGray, CV_RGBA2GRAY); + cvtColor(imGrayRight, imGrayRight, CV_RGBA2GRAY); } else { - cvtColor(mImGray,mImGray,CV_BGRA2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY); + cvtColor(mImGray, mImGray, CV_BGRA2GRAY); + cvtColor(imGrayRight, imGrayRight, CV_BGRA2GRAY); } } - mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth); Track(); return mCurrentFrame.mTcw.clone(); } - -cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp) +cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB, const cv::Mat &imD, const double ×tamp) { mImGray = imRGB; cv::Mat imDepth = imD; - if(mImGray.channels()==3) + if (mImGray.channels() == 3) { - if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGB2GRAY); + if (mbRGB) + cvtColor(mImGray, mImGray, CV_RGB2GRAY); else - cvtColor(mImGray,mImGray,CV_BGR2GRAY); + cvtColor(mImGray, mImGray, CV_BGR2GRAY); } - else if(mImGray.channels()==4) + else if (mImGray.channels() == 4) { - if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGBA2GRAY); + if (mbRGB) + cvtColor(mImGray, mImGray, CV_RGBA2GRAY); else - cvtColor(mImGray,mImGray,CV_BGRA2GRAY); + cvtColor(mImGray, mImGray, CV_BGRA2GRAY); } - if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F) - imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor); + if ((fabs(mDepthMapFactor - 1.0f) > 1e-5) || imDepth.type() != CV_32F) + imDepth.convertTo(imDepth, CV_32F, mDepthMapFactor); - mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + mCurrentFrame = Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth); Track(); return mCurrentFrame.mTcw.clone(); } - cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) { mImGray = im; - if(mImGray.channels()==3) + if (mImGray.channels() == 3) { - if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGB2GRAY); + if (mbRGB) + cvtColor(mImGray, mImGray, CV_RGB2GRAY); else - cvtColor(mImGray,mImGray,CV_BGR2GRAY); + cvtColor(mImGray, mImGray, CV_BGR2GRAY); } - else if(mImGray.channels()==4) + else if (mImGray.channels() == 4) { - if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGBA2GRAY); + if (mbRGB) + cvtColor(mImGray, mImGray, CV_RGBA2GRAY); else - cvtColor(mImGray,mImGray,CV_BGRA2GRAY); + cvtColor(mImGray, mImGray, CV_BGRA2GRAY); } - if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) - mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) + mCurrentFrame = Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth); else - mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + mCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth); Track(); @@ -266,26 +262,26 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) void Tracking::Track() { - if(mState==NO_IMAGES_YET) + if (mState == NO_IMAGES_YET) { mState = NOT_INITIALIZED; } - mLastProcessedState=mState; + mLastProcessedState = mState; // Get Map Mutex -> Map cannot be changed unique_lock lock(mpMap->mMutexMapUpdate); - if(mState==NOT_INITIALIZED) + if (mState == NOT_INITIALIZED) { - if(mSensor==System::STEREO || mSensor==System::RGBD) + if (mSensor == System::STEREO || mSensor == System::RGBD) StereoInitialization(); else MonocularInitialization(); mpFrameDrawer->Update(this); - if(mState!=OK) + if (mState != OK) return; } else @@ -294,24 +290,24 @@ void Tracking::Track() bool bOK; // Initial camera pose estimation using motion model or relocalization (if tracking is lost) - if(!mbOnlyTracking) + if (!mbOnlyTracking) { // Local Mapping is activated. This is the normal behaviour, unless // you explicitly activate the "only tracking" mode. - if(mState==OK) + if (mState == OK) { // Local Mapping might have changed some MapPoints tracked in last frame CheckReplacedInLastFrame(); - if(mVelocity.empty() || mCurrentFrame.mnId vpMPsMM; + vector vpMPsMM; vector vbOutMM; cv::Mat TcwMM; - if(!mVelocity.empty()) + if (!mVelocity.empty()) { bOKMM = TrackWithMotionModel(); vpMPsMM = mCurrentFrame.mvpMapPoints; @@ -365,24 +361,24 @@ void Tracking::Track() } bOKReloc = Relocalization(); - if(bOKMM && !bOKReloc) + if (bOKMM && !bOKReloc) { mCurrentFrame.SetPose(TcwMM); mCurrentFrame.mvpMapPoints = vpMPsMM; mCurrentFrame.mvbOutlier = vbOutMM; - if(mbVO) + if (mbVO) { - for(int i =0; iIncreaseFound(); } } } } - else if(bOKReloc) + else if (bOKReloc) { mbVO = false; } @@ -395,9 +391,9 @@ void Tracking::Track() mCurrentFrame.mpReferenceKF = mpReferenceKF; // If we have an initial estimation of the camera pose and matching. Track the local map. - if(!mbOnlyTracking) + if (!mbOnlyTracking) { - if(bOK) + if (bOK) bOK = TrackLocalMap(); } else @@ -405,28 +401,28 @@ void Tracking::Track() // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes // the camera we will use the local map again. - if(bOK && !mbVO) + if (bOK && !mbVO) bOK = TrackLocalMap(); } - if(bOK) + if (bOK) mState = OK; else - mState=LOST; + mState = LOST; // Update drawer mpFrameDrawer->Update(this); // If tracking were good, check if we insert a keyframe - if(bOK) + if (bOK) { // Update motion model - if(!mLastFrame.mTcw.empty()) + if (!mLastFrame.mTcw.empty()) { - cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F); - mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3)); - mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3)); - mVelocity = mCurrentFrame.mTcw*LastTwc; + cv::Mat LastTwc = cv::Mat::eye(4, 4, CV_32F); + mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0, 3).colRange(0, 3)); + mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0, 3).col(3)); + mVelocity = mCurrentFrame.mTcw * LastTwc; } else mVelocity = cv::Mat(); @@ -434,44 +430,44 @@ void Tracking::Track() mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); // Clean VO matches - for(int i=0; iObservations()<1) + MapPoint *pMP = mCurrentFrame.mvpMapPoints[i]; + if (pMP) + if (pMP->Observations() < 1) { mCurrentFrame.mvbOutlier[i] = false; - mCurrentFrame.mvpMapPoints[i]=static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); } } // Delete temporal MapPoints - for(list::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++) + for (list::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit != lend; lit++) { - MapPoint* pMP = *lit; + MapPoint *pMP = *lit; delete pMP; } mlpTemporalPoints.clear(); // Check if we need to insert a new keyframe - if(NeedNewKeyFrame()) + if (NeedNewKeyFrame()) CreateNewKeyFrame(); // We allow points with high innovation (considererd outliers by the Huber Function) // pass to the new keyframe, so that bundle adjustment will finally decide // if they are outliers or not. We don't want next frame to estimate its position // with those points so we discard them in the frame. - for(int i=0; i(NULL); + if (mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i]) + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); } } // Reset if the camera get lost soon after initialization - if(mState==LOST) + if (mState == LOST) { - if(mpMap->KeyFramesInMap()<=5) + if (mpMap->KeyFramesInMap() <= 5) { cout << "Track lost soon after initialisation, reseting..." << endl; mpSystem->Reset(); @@ -479,20 +475,20 @@ void Tracking::Track() } } - if(!mCurrentFrame.mpReferenceKF) + if (!mCurrentFrame.mpReferenceKF) mCurrentFrame.mpReferenceKF = mpReferenceKF; mLastFrame = Frame(mCurrentFrame); } // Store frame pose information to retrieve the complete camera trajectory afterwards. - if(!mCurrentFrame.mTcw.empty()) + if (!mCurrentFrame.mTcw.empty()) { - cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse(); + cv::Mat Tcr = mCurrentFrame.mTcw * mCurrentFrame.mpReferenceKF->GetPoseInverse(); mlRelativeFramePoses.push_back(Tcr); mlpReferences.push_back(mpReferenceKF); mlFrameTimes.push_back(mCurrentFrame.mTimeStamp); - mlbLost.push_back(mState==LOST); + mlbLost.push_back(mState == LOST); } else { @@ -500,40 +496,38 @@ void Tracking::Track() mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); mlpReferences.push_back(mlpReferences.back()); mlFrameTimes.push_back(mlFrameTimes.back()); - mlbLost.push_back(mState==LOST); + mlbLost.push_back(mState == LOST); } - } - void Tracking::StereoInitialization() { - if(mCurrentFrame.N>500) + if (mCurrentFrame.N > 500) { // Set Frame pose to the origin - mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); + mCurrentFrame.SetPose(cv::Mat::eye(4, 4, CV_32F)); // Create KeyFrame - KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + KeyFrame *pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB); // Insert KeyFrame in the map mpMap->AddKeyFrame(pKFini); // Create MapPoints and asscoiate to KeyFrame - for(int i=0; i0) + if (z > 0) { cv::Mat x3D = mCurrentFrame.UnprojectStereo(i); - MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap); - pNewMP->AddObservation(pKFini,i); - pKFini->AddMapPoint(pNewMP,i); + MapPoint *pNewMP = new MapPoint(x3D, pKFini, mpMap); + pNewMP->AddObservation(pKFini, i); + pKFini->AddMapPoint(pNewMP, i); pNewMP->ComputeDistinctiveDescriptors(); pNewMP->UpdateNormalAndDepth(); mpMap->AddMapPoint(pNewMP); - mCurrentFrame.mvpMapPoints[i]=pNewMP; + mCurrentFrame.mvpMapPoints[i] = pNewMP; } } @@ -542,11 +536,11 @@ void Tracking::StereoInitialization() mpLocalMapper->InsertKeyFrame(pKFini); mLastFrame = Frame(mCurrentFrame); - mnLastKeyFrameId=mCurrentFrame.mnId; + mnLastKeyFrameId = mCurrentFrame.mnId; mpLastKeyFrame = pKFini; mvpLocalKeyFrames.push_back(pKFini); - mvpLocalMapPoints=mpMap->GetAllMapPoints(); + mvpLocalMapPoints = mpMap->GetAllMapPoints(); mpReferenceKF = pKFini; mCurrentFrame.mpReferenceKF = pKFini; @@ -556,30 +550,30 @@ void Tracking::StereoInitialization() mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); - mState=OK; + mState = OK; } } void Tracking::MonocularInitialization() { - if(!mpInitializer) + if (!mpInitializer) { // Set Reference Frame - if(mCurrentFrame.mvKeys.size()>100) + if (mCurrentFrame.mvKeys.size() > 100) { mInitialFrame = Frame(mCurrentFrame); mLastFrame = Frame(mCurrentFrame); mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size()); - for(size_t i=0; i(NULL); - fill(mvIniMatches.begin(),mvIniMatches.end(),-1); + mpInitializer = static_cast(NULL); + fill(mvIniMatches.begin(), mvIniMatches.end(), -1); return; } // Find correspondences - ORBmatcher matcher(0.9,true); - int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100); + ORBmatcher matcher(0.9, true); + int nmatches = matcher.SearchForInitialization(mInitialFrame, mCurrentFrame, mvbPrevMatched, mvIniMatches, 100); // Check if there are enough correspondences - if(nmatches<100) + if (nmatches < 100) { delete mpInitializer; - mpInitializer = static_cast(NULL); + mpInitializer = static_cast(NULL); return; } - cv::Mat Rcw; // Current Camera Rotation - cv::Mat tcw; // Current Camera Translation + cv::Mat Rcw; // Current Camera Rotation + cv::Mat tcw; // Current Camera Translation vector vbTriangulated; // Triangulated Correspondences (mvIniMatches) - if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) + if (mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) { - for(size_t i=0, iend=mvIniMatches.size(); i=0 && !vbTriangulated[i]) + if (mvIniMatches[i] >= 0 && !vbTriangulated[i]) { - mvIniMatches[i]=-1; + mvIniMatches[i] = -1; nmatches--; } } // Set Frame Poses - mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); - cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F); - Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3)); - tcw.copyTo(Tcw.rowRange(0,3).col(3)); + mInitialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F)); + cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F); + Rcw.copyTo(Tcw.rowRange(0, 3).colRange(0, 3)); + tcw.copyTo(Tcw.rowRange(0, 3).col(3)); mCurrentFrame.SetPose(Tcw); CreateInitialMapMonocular(); @@ -637,9 +631,8 @@ void Tracking::MonocularInitialization() void Tracking::CreateInitialMapMonocular() { // Create KeyFrames - KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB); - KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); - + KeyFrame *pKFini = new KeyFrame(mInitialFrame, mpMap, mpKeyFrameDB); + KeyFrame *pKFcur = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB); pKFini->ComputeBoW(); pKFcur->ComputeBoW(); @@ -649,21 +642,21 @@ void Tracking::CreateInitialMapMonocular() mpMap->AddKeyFrame(pKFcur); // Create MapPoints and asscoiate to keyframes - for(size_t i=0; iAddMapPoint(pMP,i); - pKFcur->AddMapPoint(pMP,mvIniMatches[i]); + pKFini->AddMapPoint(pMP, i); + pKFcur->AddMapPoint(pMP, mvIniMatches[i]); - pMP->AddObservation(pKFini,i); - pMP->AddObservation(pKFcur,mvIniMatches[i]); + pMP->AddObservation(pKFini, i); + pMP->AddObservation(pKFcur, mvIniMatches[i]); pMP->ComputeDistinctiveDescriptors(); pMP->UpdateNormalAndDepth(); @@ -683,13 +676,13 @@ void Tracking::CreateInitialMapMonocular() // Bundle Adjustment cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl; - Optimizer::GlobalBundleAdjustemnt(mpMap,20); + Optimizer::GlobalBundleAdjustemnt(mpMap, 20); // Set median depth to 1 float medianDepth = pKFini->ComputeSceneMedianDepth(2); - float invMedianDepth = 1.0f/medianDepth; + float invMedianDepth = 1.0f / medianDepth; - if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100) + if (medianDepth < 0 || pKFcur->TrackedMapPoints(1) < 100) { cout << "Wrong initialization, reseting..." << endl; Reset(); @@ -698,17 +691,17 @@ void Tracking::CreateInitialMapMonocular() // Scale initial baseline cv::Mat Tc2w = pKFcur->GetPose(); - Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth; + Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth; pKFcur->SetPose(Tc2w); // Scale points - vector vpAllMapPoints = pKFini->GetMapPointMatches(); - for(size_t iMP=0; iMP vpAllMapPoints = pKFini->GetMapPointMatches(); + for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++) { - if(vpAllMapPoints[iMP]) + if (vpAllMapPoints[iMP]) { - MapPoint* pMP = vpAllMapPoints[iMP]; - pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth); + MapPoint *pMP = vpAllMapPoints[iMP]; + pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth); } } @@ -716,12 +709,12 @@ void Tracking::CreateInitialMapMonocular() mpLocalMapper->InsertKeyFrame(pKFcur); mCurrentFrame.SetPose(pKFcur->GetPose()); - mnLastKeyFrameId=mCurrentFrame.mnId; + mnLastKeyFrameId = mCurrentFrame.mnId; mpLastKeyFrame = pKFcur; mvpLocalKeyFrames.push_back(pKFcur); mvpLocalKeyFrames.push_back(pKFini); - mvpLocalMapPoints=mpMap->GetAllMapPoints(); + mvpLocalMapPoints = mpMap->GetAllMapPoints(); mpReferenceKF = pKFcur; mCurrentFrame.mpReferenceKF = pKFcur; @@ -733,19 +726,19 @@ void Tracking::CreateInitialMapMonocular() mpMap->mvpKeyFrameOrigins.push_back(pKFini); - mState=OK; + mState = OK; } void Tracking::CheckReplacedInLastFrame() { - for(int i =0; iGetReplaced(); - if(pRep) + MapPoint *pRep = pMP->GetReplaced(); + if (pRep) { mLastFrame.mvpMapPoints[i] = pRep; } @@ -753,7 +746,6 @@ void Tracking::CheckReplacedInLastFrame() } } - bool Tracking::TrackReferenceKeyFrame() { // Compute Bag of Words vector @@ -761,12 +753,12 @@ bool Tracking::TrackReferenceKeyFrame() // We perform first an ORB matching with the reference keyframe // If enough matches are found we setup a PnP solver - ORBmatcher matcher(0.7,true); - vector vpMapPointMatches; + ORBmatcher matcher(0.7, true); + vector vpMapPointMatches; - int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches); + int nmatches = matcher.SearchByBoW(mpReferenceKF, mCurrentFrame, vpMapPointMatches); - if(nmatches<15) + if (nmatches < 15) return false; mCurrentFrame.mvpMapPoints = vpMapPointMatches; @@ -776,80 +768,80 @@ bool Tracking::TrackReferenceKeyFrame() // Discard outliers int nmatchesMap = 0; - for(int i =0; i(NULL); - mCurrentFrame.mvbOutlier[i]=false; + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvbOutlier[i] = false; pMP->mbTrackInView = false; pMP->mnLastFrameSeen = mCurrentFrame.mnId; nmatches--; } - else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0) nmatchesMap++; } } - return nmatchesMap>=10; + return nmatchesMap >= 10; } void Tracking::UpdateLastFrame() { // Update pose according to reference keyframe - KeyFrame* pRef = mLastFrame.mpReferenceKF; + KeyFrame *pRef = mLastFrame.mpReferenceKF; cv::Mat Tlr = mlRelativeFramePoses.back(); - mLastFrame.SetPose(Tlr*pRef->GetPose()); + mLastFrame.SetPose(Tlr * pRef->GetPose()); - if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR || !mbOnlyTracking) + if (mnLastKeyFrameId == mLastFrame.mnId || mSensor == System::MONOCULAR || !mbOnlyTracking) return; // Create "visual odometry" MapPoints // We sort points according to their measured depth by the stereo/RGB-D sensor - vector > vDepthIdx; + vector> vDepthIdx; vDepthIdx.reserve(mLastFrame.N); - for(int i=0; i0) + if (z > 0) { - vDepthIdx.push_back(make_pair(z,i)); + vDepthIdx.push_back(make_pair(z, i)); } } - if(vDepthIdx.empty()) + if (vDepthIdx.empty()) return; - sort(vDepthIdx.begin(),vDepthIdx.end()); + sort(vDepthIdx.begin(), vDepthIdx.end()); // We insert all close points (depthObservations()<1) + else if (pMP->Observations() < 1) { bCreateNew = true; } - if(bCreateNew) + if (bCreateNew) { cv::Mat x3D = mLastFrame.UnprojectStereo(i); - MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i); + MapPoint *pNewMP = new MapPoint(x3D, mpMap, &mLastFrame, i); - mLastFrame.mvpMapPoints[i]=pNewMP; + mLastFrame.mvpMapPoints[i] = pNewMP; mlpTemporalPoints.push_back(pNewMP); nPoints++; @@ -859,39 +851,39 @@ void Tracking::UpdateLastFrame() nPoints++; } - if(vDepthIdx[j].first>mThDepth && nPoints>100) + if (vDepthIdx[j].first > mThDepth && nPoints > 100) break; } } bool Tracking::TrackWithMotionModel() { - ORBmatcher matcher(0.9,true); + ORBmatcher matcher(0.9, true); // Update last frame pose according to its reference keyframe // Create "visual odometry" points if in Localization Mode UpdateLastFrame(); - mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw); + mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw); - fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); + fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast(NULL)); // Project points seen in previous frame int th; - if(mSensor!=System::STEREO) - th=15; + if (mSensor != System::STEREO) + th = 15; else - th=7; - int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR); + th = 7; + int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, th, mSensor == System::MONOCULAR); // If few matches, uses a wider window search - if(nmatches<20) + if (nmatches < 20) { - fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); - nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); + fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast(NULL)); + nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th, mSensor == System::MONOCULAR); } - if(nmatches<20) + if (nmatches < 20) return false; // Optimize frame pose with all matches @@ -899,32 +891,32 @@ bool Tracking::TrackWithMotionModel() // Discard outliers int nmatchesMap = 0; - for(int i =0; i(NULL); - mCurrentFrame.mvbOutlier[i]=false; + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvbOutlier[i] = false; pMP->mbTrackInView = false; pMP->mnLastFrameSeen = mCurrentFrame.mnId; nmatches--; } - else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0) nmatchesMap++; } - } + } - if(mbOnlyTracking) + if (mbOnlyTracking) { - mbVO = nmatchesMap<10; - return nmatches>20; + mbVO = nmatchesMap < 10; + return nmatches > 20; } - return nmatchesMap>=10; + return nmatchesMap >= 10; } bool Tracking::TrackLocalMap() @@ -941,58 +933,56 @@ bool Tracking::TrackLocalMap() mnMatchesInliers = 0; // Update MapPoints Statistics - for(int i=0; iIncreaseFound(); - if(!mbOnlyTracking) + if (!mbOnlyTracking) { - if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0) mnMatchesInliers++; } else mnMatchesInliers++; } - else if(mSensor==System::STEREO) - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); - + else if (mSensor == System::STEREO) + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); } } // Decide if the tracking was succesful // More restrictive if there was a relocalization recently - if(mCurrentFrame.mnIdisStopped() || mpLocalMapper->stopRequested()) + if (mpLocalMapper->isStopped() || mpLocalMapper->stopRequested()) return false; const int nKFs = mpMap->KeyFramesInMap(); // Do not insert keyframes if not enough frames have passed from last relocalisation - if(mCurrentFrame.mnIdmMaxFrames) + if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs > mMaxFrames) return false; // Tracked MapPoints in the reference keyframe int nMinObs = 3; - if(nKFs<=2) - nMinObs=2; + if (nKFs <= 2) + nMinObs = 2; int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs); // Local Mapping accept keyframes? @@ -1000,14 +990,14 @@ bool Tracking::NeedNewKeyFrame() // Check how many "close" points are being tracked and how many could be potentially created. int nNonTrackedClose = 0; - int nTrackedClose= 0; - if(mSensor!=System::MONOCULAR) + int nTrackedClose = 0; + if (mSensor != System::MONOCULAR) { - for(int i =0; i0 && mCurrentFrame.mvDepth[i] 0 && mCurrentFrame.mvDepth[i] < mThDepth) { - if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]) + if (mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]) nTrackedClose++; else nNonTrackedClose++; @@ -1015,39 +1005,39 @@ bool Tracking::NeedNewKeyFrame() } } - bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70); + bool bNeedToInsertClose = (nTrackedClose < 100) && (nNonTrackedClose > 70); // Thresholds float thRefRatio = 0.75f; - if(nKFs<2) + if (nKFs < 2) thRefRatio = 0.4f; - if(mSensor==System::MONOCULAR) + if (mSensor == System::MONOCULAR) thRefRatio = 0.9f; // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion - const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames; + const bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId + mMaxFrames; // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle - const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle); + const bool c1b = (mCurrentFrame.mnId >= mnLastKeyFrameId + mMinFrames && bLocalMappingIdle); //Condition 1c: tracking is weak - const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers15); + const bool c2 = ((mnMatchesInliers < nRefMatches * thRefRatio || bNeedToInsertClose) && mnMatchesInliers > 15); - if((c1a||c1b||c1c)&&c2) + if ((c1a || c1b || c1c) && c2) { // If the mapping accepts keyframes, insert keyframe. // Otherwise send a signal to interrupt BA - if(bLocalMappingIdle) + if (bLocalMappingIdle) { return true; } else { mpLocalMapper->InterruptBA(); - if(mSensor!=System::MONOCULAR) + if (mSensor != System::MONOCULAR) { - if(mpLocalMapper->KeyframesInQueue()<3) + if (mpLocalMapper->KeyframesInQueue() < 3) return true; else return false; @@ -1062,63 +1052,63 @@ bool Tracking::NeedNewKeyFrame() void Tracking::CreateNewKeyFrame() { - if(!mpLocalMapper->SetNotStop(true)) + if (!mpLocalMapper->SetNotStop(true)) return; - KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB); mpReferenceKF = pKF; mCurrentFrame.mpReferenceKF = pKF; - if(mSensor!=System::MONOCULAR) + if (mSensor != System::MONOCULAR) { mCurrentFrame.UpdatePoseMatrices(); // We sort points by the measured depth by the stereo/RGBD sensor. // We create all those MapPoints whose depth < mThDepth. // If there are less than 100 close points we create the 100 closest. - vector > vDepthIdx; + vector> vDepthIdx; vDepthIdx.reserve(mCurrentFrame.N); - for(int i=0; i0) + if (z > 0) { - vDepthIdx.push_back(make_pair(z,i)); + vDepthIdx.push_back(make_pair(z, i)); } } - if(!vDepthIdx.empty()) + if (!vDepthIdx.empty()) { - sort(vDepthIdx.begin(),vDepthIdx.end()); + sort(vDepthIdx.begin(), vDepthIdx.end()); int nPoints = 0; - for(size_t j=0; jObservations()<1) + else if (pMP->Observations() < 1) { bCreateNew = true; - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); } - if(bCreateNew) + if (bCreateNew) { cv::Mat x3D = mCurrentFrame.UnprojectStereo(i); - MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap); - pNewMP->AddObservation(pKF,i); - pKF->AddMapPoint(pNewMP,i); + MapPoint *pNewMP = new MapPoint(x3D, pKF, mpMap); + pNewMP->AddObservation(pKF, i); + pKF->AddMapPoint(pNewMP, i); pNewMP->ComputeDistinctiveDescriptors(); pNewMP->UpdateNormalAndDepth(); mpMap->AddMapPoint(pNewMP); - mCurrentFrame.mvpMapPoints[i]=pNewMP; + mCurrentFrame.mvpMapPoints[i] = pNewMP; nPoints++; } else @@ -1126,7 +1116,7 @@ void Tracking::CreateNewKeyFrame() nPoints++; } - if(vDepthIdx[j].first>mThDepth && nPoints>100) + if (vDepthIdx[j].first > mThDepth && nPoints > 100) break; } } @@ -1143,14 +1133,14 @@ void Tracking::CreateNewKeyFrame() void Tracking::SearchLocalPoints() { // Do not search map points already matched - for(vector::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++) + for (vector::iterator vit = mCurrentFrame.mvpMapPoints.begin(), vend = mCurrentFrame.mvpMapPoints.end(); vit != vend; vit++) { - MapPoint* pMP = *vit; - if(pMP) + MapPoint *pMP = *vit; + if (pMP) { - if(pMP->isBad()) + if (pMP->isBad()) { - *vit = static_cast(NULL); + *vit = static_cast(NULL); } else { @@ -1161,34 +1151,34 @@ void Tracking::SearchLocalPoints() } } - int nToMatch=0; + int nToMatch = 0; // Project points in frame and check its visibility - for(vector::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++) + for (vector::iterator vit = mvpLocalMapPoints.begin(), vend = mvpLocalMapPoints.end(); vit != vend; vit++) { - MapPoint* pMP = *vit; - if(pMP->mnLastFrameSeen == mCurrentFrame.mnId) + MapPoint *pMP = *vit; + if (pMP->mnLastFrameSeen == mCurrentFrame.mnId) continue; - if(pMP->isBad()) + if (pMP->isBad()) continue; // Project (this fills MapPoint variables for matching) - if(mCurrentFrame.isInFrustum(pMP,0.5)) + if (mCurrentFrame.isInFrustum(pMP, 0.5)) { pMP->IncreaseVisible(); nToMatch++; } } - if(nToMatch>0) + if (nToMatch > 0) { ORBmatcher matcher(0.8); int th = 1; - if(mSensor==System::RGBD) - th=3; + if (mSensor == System::RGBD) + th = 3; // If the camera has been relocalised recently, perform a coarser search - if(mCurrentFrame.mnId::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) + for (vector::const_iterator itKF = mvpLocalKeyFrames.begin(), itEndKF = mvpLocalKeyFrames.end(); itKF != itEndKF; itKF++) { - KeyFrame* pKF = *itKF; - const vector vpMPs = pKF->GetMapPointMatches(); + KeyFrame *pKF = *itKF; + const vector vpMPs = pKF->GetMapPointMatches(); - for(vector::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++) + for (vector::const_iterator itMP = vpMPs.begin(), itEndMP = vpMPs.end(); itMP != itEndMP; itMP++) { - MapPoint* pMP = *itMP; - if(!pMP) + MapPoint *pMP = *itMP; + if (!pMP) continue; - if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId) + if (pMP->mnTrackReferenceForFrame == mCurrentFrame.mnId) continue; - if(!pMP->isBad()) + if (!pMP->isBad()) { mvpLocalMapPoints.push_back(pMP); - pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId; + pMP->mnTrackReferenceForFrame = mCurrentFrame.mnId; } } } } - void Tracking::UpdateLocalKeyFrames() { // Each map point vote for the keyframes in which it has been observed - map keyframeCounter; - for(int i=0; i keyframeCounter; + for (int i = 0; i < mCurrentFrame.N; i++) { - if(mCurrentFrame.mvpMapPoints[i]) + if (mCurrentFrame.mvpMapPoints[i]) { - MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; - if(!pMP->isBad()) + MapPoint *pMP = mCurrentFrame.mvpMapPoints[i]; + if (!pMP->isBad()) { - const map observations = pMP->GetObservations(); - for(map::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) + const map observations = pMP->GetObservations(); + for (map::const_iterator it = observations.begin(), itend = observations.end(); it != itend; it++) keyframeCounter[it->first]++; } else { - mCurrentFrame.mvpMapPoints[i]=NULL; + mCurrentFrame.mvpMapPoints[i] = NULL; } } } - if(keyframeCounter.empty()) + if (keyframeCounter.empty()) return; - int max=0; - KeyFrame* pKFmax= static_cast(NULL); + int max = 0; + KeyFrame *pKFmax = static_cast(NULL); mvpLocalKeyFrames.clear(); - mvpLocalKeyFrames.reserve(3*keyframeCounter.size()); + mvpLocalKeyFrames.reserve(3 * keyframeCounter.size()); // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points - for(map::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++) + for (map::const_iterator it = keyframeCounter.begin(), itEnd = keyframeCounter.end(); it != itEnd; it++) { - KeyFrame* pKF = it->first; + KeyFrame *pKF = it->first; - if(pKF->isBad()) + if (pKF->isBad()) continue; - if(it->second>max) + if (it->second > max) { - max=it->second; - pKFmax=pKF; + max = it->second; + pKFmax = pKF; } mvpLocalKeyFrames.push_back(it->first); pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId; } - // Include also some not-already-included keyframes that are neighbors to already-included keyframes - for(vector::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) + for (vector::const_iterator itKF = mvpLocalKeyFrames.begin(), itEndKF = mvpLocalKeyFrames.end(); itKF != itEndKF; itKF++) { // Limit the number of keyframes - if(mvpLocalKeyFrames.size()>80) + if (mvpLocalKeyFrames.size() > 80) break; - KeyFrame* pKF = *itKF; + KeyFrame *pKF = *itKF; - const vector vNeighs = pKF->GetBestCovisibilityKeyFrames(10); + const vector vNeighs = pKF->GetBestCovisibilityKeyFrames(10); - for(vector::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++) + for (vector::const_iterator itNeighKF = vNeighs.begin(), itEndNeighKF = vNeighs.end(); itNeighKF != itEndNeighKF; itNeighKF++) { - KeyFrame* pNeighKF = *itNeighKF; - if(!pNeighKF->isBad()) + KeyFrame *pNeighKF = *itNeighKF; + if (!pNeighKF->isBad()) { - if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) + if (pNeighKF->mnTrackReferenceForFrame != mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pNeighKF); - pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId; + pNeighKF->mnTrackReferenceForFrame = mCurrentFrame.mnId; break; } } } - const set spChilds = pKF->GetChilds(); - for(set::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++) + const set spChilds = pKF->GetChilds(); + for (set::const_iterator sit = spChilds.begin(), send = spChilds.end(); sit != send; sit++) { - KeyFrame* pChildKF = *sit; - if(!pChildKF->isBad()) + KeyFrame *pChildKF = *sit; + if (!pChildKF->isBad()) { - if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) + if (pChildKF->mnTrackReferenceForFrame != mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pChildKF); - pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId; + pChildKF->mnTrackReferenceForFrame = mCurrentFrame.mnId; break; } } } - KeyFrame* pParent = pKF->GetParent(); - if(pParent) + KeyFrame *pParent = pKF->GetParent(); + if (pParent) { - if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId) + if (pParent->mnTrackReferenceForFrame != mCurrentFrame.mnId) { mvpLocalKeyFrames.push_back(pParent); - pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId; + pParent->mnTrackReferenceForFrame = mCurrentFrame.mnId; break; } } - } - if(pKFmax) + if (pKFmax) { mpReferenceKF = pKFmax; mCurrentFrame.mpReferenceKF = mpReferenceKF; @@ -1345,45 +1332,45 @@ bool Tracking::Relocalization() // Relocalization is performed when tracking is lost // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation - vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame); + vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame); - if(vpCandidateKFs.empty()) + if (vpCandidateKFs.empty()) return false; const int nKFs = vpCandidateKFs.size(); // We perform first an ORB matching with each candidate // If enough matches are found we setup a PnP solver - ORBmatcher matcher(0.75,true); + ORBmatcher matcher(0.75, true); - vector vpPnPsolvers; + vector vpPnPsolvers; vpPnPsolvers.resize(nKFs); - vector > vvpMapPointMatches; + vector> vvpMapPointMatches; vvpMapPointMatches.resize(nKFs); vector vbDiscarded; vbDiscarded.resize(nKFs); - int nCandidates=0; + int nCandidates = 0; - for(int i=0; iisBad()) + KeyFrame *pKF = vpCandidateKFs[i]; + if (pKF->isBad()) vbDiscarded[i] = true; else { - int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]); - if(nmatches<15) + int nmatches = matcher.SearchByBoW(pKF, mCurrentFrame, vvpMapPointMatches[i]); + if (nmatches < 15) { vbDiscarded[i] = true; continue; } else { - PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]); - pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991); + PnPsolver *pSolver = new PnPsolver(mCurrentFrame, vvpMapPointMatches[i]); + pSolver->SetRansacParameters(0.99, 10, 300, 4, 0.5, 5.991); vpPnPsolvers[i] = pSolver; nCandidates++; } @@ -1393,13 +1380,13 @@ bool Tracking::Relocalization() // Alternatively perform some iterations of P4P RANSAC // Until we found a camera pose supported by enough inliers bool bMatch = false; - ORBmatcher matcher2(0.9,true); + ORBmatcher matcher2(0.9, true); - while(nCandidates>0 && !bMatch) + while (nCandidates > 0 && !bMatch) { - for(int i=0; iiterate(5,bNoMore,vbInliers,nInliers); + PnPsolver *pSolver = vpPnPsolvers[i]; + cv::Mat Tcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers); // If Ransac reachs max. iterations discard keyframe - if(bNoMore) + if (bNoMore) { - vbDiscarded[i]=true; + vbDiscarded[i] = true; nCandidates--; } // If a Camera Pose is computed, optimize - if(!Tcw.empty()) + if (!Tcw.empty()) { Tcw.copyTo(mCurrentFrame.mTcw); - set sFound; + set sFound; const int np = vbInliers.size(); - for(int j=0; j(NULL); + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); // If few inliers, search by projection in a coarse window and optimize again - if(nGood<50) + if (nGood < 50) { - int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100); + int nadditional = matcher2.SearchByProjection(mCurrentFrame, vpCandidateKFs[i], sFound, 10, 100); - if(nadditional+nGood>=50) + if (nadditional + nGood >= 50) { nGood = Optimizer::PoseOptimization(&mCurrentFrame); // If many inliers but still not enough, search by projection again in a narrower window // the camera has been already optimized with many points - if(nGood>30 && nGood<50) + if (nGood > 30 && nGood < 50) { sFound.clear(); - for(int ip =0; ip=50) + if (nGood + nadditional >= 50) { nGood = Optimizer::PoseOptimization(&mCurrentFrame); - for(int io =0; io=50) + if (nGood >= 50) { bMatch = true; break; @@ -1489,7 +1475,7 @@ bool Tracking::Relocalization() } } - if(!bMatch) + if (!bMatch) { return false; } @@ -1498,17 +1484,16 @@ bool Tracking::Relocalization() mnLastRelocFrameId = mCurrentFrame.mnId; return true; } - } void Tracking::Reset() { cout << "System Reseting" << endl; - if(mpViewer) + if (mpViewer) { mpViewer->RequestStop(); - while(!mpViewer->isStopped()) + while (!mpViewer->isStopped()) usleep(3000); } @@ -1534,10 +1519,10 @@ void Tracking::Reset() Frame::nNextId = 0; mState = NO_IMAGES_YET; - if(mpInitializer) + if (mpInitializer) { delete mpInitializer; - mpInitializer = static_cast(NULL); + mpInitializer = static_cast(NULL); } mlRelativeFramePoses.clear(); @@ -1545,7 +1530,7 @@ void Tracking::Reset() mlFrameTimes.clear(); mlbLost.clear(); - if(mpViewer) + if (mpViewer) mpViewer->Release(); } @@ -1557,20 +1542,20 @@ void Tracking::ChangeCalibration(const string &strSettingPath) float cx = fSettings["Camera.cx"]; float cy = fSettings["Camera.cy"]; - cv::Mat K = cv::Mat::eye(3,3,CV_32F); - K.at(0,0) = fx; - K.at(1,1) = fy; - K.at(0,2) = cx; - K.at(1,2) = cy; + cv::Mat K = cv::Mat::eye(3, 3, CV_32F); + K.at(0, 0) = fx; + K.at(1, 1) = fy; + K.at(0, 2) = cx; + K.at(1, 2) = cy; K.copyTo(mK); - cv::Mat DistCoef(4,1,CV_32F); + cv::Mat DistCoef(4, 1, CV_32F); DistCoef.at(0) = fSettings["Camera.k1"]; DistCoef.at(1) = fSettings["Camera.k2"]; DistCoef.at(2) = fSettings["Camera.p1"]; DistCoef.at(3) = fSettings["Camera.p2"]; const float k3 = fSettings["Camera.k3"]; - if(k3!=0) + if (k3 != 0) { DistCoef.resize(5); DistCoef.at(4) = k3; @@ -1587,6 +1572,4 @@ void Tracking::InformOnlyTracking(const bool &flag) mbOnlyTracking = flag; } - - -} //namespace ORB_SLAM +} // namespace ORB_SLAM2 diff --git a/src/Viewer.cc b/src/Viewer.cc index dec3204f53..1da47d1d1b 100644 --- a/src/Viewer.cc +++ b/src/Viewer.cc @@ -20,26 +20,26 @@ #include "Viewer.h" #include +#include "unistd.h" #include namespace ORB_SLAM2 { -Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath): - mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking), - mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false) +Viewer::Viewer(System *pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath) : mpSystem(pSystem), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpTracker(pTracking), + mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false) { cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); float fps = fSettings["Camera.fps"]; - if(fps<1) - fps=30; - mT = 1e3/fps; + if (fps < 1) + fps = 30; + mT = 1e3 / fps; mImageWidth = fSettings["Camera.width"]; mImageHeight = fSettings["Camera.height"]; - if(mImageWidth<1 || mImageHeight<1) + if (mImageWidth < 1 || mImageHeight < 1) { mImageWidth = 640; mImageHeight = 480; @@ -56,33 +56,32 @@ void Viewer::Run() mbFinished = false; mbStopped = false; - pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer",1024,768); + pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer", 1024, 768); // 3D Mouse handler requires depth testing to be enabled glEnable(GL_DEPTH_TEST); // Issue specific OpenGl we might need - glEnable (GL_BLEND); - glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175)); - pangolin::Var menuFollowCamera("menu.Follow Camera",true,true); - pangolin::Var menuShowPoints("menu.Show Points",true,true); - pangolin::Var menuShowKeyFrames("menu.Show KeyFrames",true,true); - pangolin::Var menuShowGraph("menu.Show Graph",true,true); - pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true); - pangolin::Var menuReset("menu.Reset",false,false); + pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175)); + pangolin::Var menuFollowCamera("menu.Follow Camera", true, true); + pangolin::Var menuShowPoints("menu.Show Points", true, true); + pangolin::Var menuShowKeyFrames("menu.Show KeyFrames", true, true); + pangolin::Var menuShowGraph("menu.Show Graph", true, true); + pangolin::Var menuLocalizationMode("menu.Localization Mode", false, true); + pangolin::Var menuReset("menu.Reset", false, false); // Define Camera Render Object (for view / scene browsing) pangolin::OpenGlRenderState s_cam( - pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000), - pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0) - ); + pangolin::ProjectionMatrix(1024, 768, mViewpointF, mViewpointF, 512, 389, 0.1, 1000), + pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0)); // Add named OpenGL viewport to window and provide 3D Handler - pangolin::View& d_cam = pangolin::CreateDisplay() - .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f) - .SetHandler(new pangolin::Handler3D(s_cam)); + pangolin::View &d_cam = pangolin::CreateDisplay() + .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f) + .SetHandler(new pangolin::Handler3D(s_cam)); pangolin::OpenGlMatrix Twc; Twc.SetIdentity(); @@ -92,59 +91,59 @@ void Viewer::Run() bool bFollow = true; bool bLocalizationMode = false; - while(1) + while (1) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc); - if(menuFollowCamera && bFollow) + if (menuFollowCamera && bFollow) { s_cam.Follow(Twc); } - else if(menuFollowCamera && !bFollow) + else if (menuFollowCamera && !bFollow) { - s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); + s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0)); s_cam.Follow(Twc); bFollow = true; } - else if(!menuFollowCamera && bFollow) + else if (!menuFollowCamera && bFollow) { bFollow = false; } - if(menuLocalizationMode && !bLocalizationMode) + if (menuLocalizationMode && !bLocalizationMode) { mpSystem->ActivateLocalizationMode(); bLocalizationMode = true; } - else if(!menuLocalizationMode && bLocalizationMode) + else if (!menuLocalizationMode && bLocalizationMode) { mpSystem->DeactivateLocalizationMode(); bLocalizationMode = false; } d_cam.Activate(s_cam); - glClearColor(1.0f,1.0f,1.0f,1.0f); + glClearColor(1.0f, 1.0f, 1.0f, 1.0f); mpMapDrawer->DrawCurrentCamera(Twc); - if(menuShowKeyFrames || menuShowGraph) - mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph); - if(menuShowPoints) + if (menuShowKeyFrames || menuShowGraph) + mpMapDrawer->DrawKeyFrames(menuShowKeyFrames, menuShowGraph); + if (menuShowPoints) mpMapDrawer->DrawMapPoints(); pangolin::FinishFrame(); cv::Mat im = mpFrameDrawer->DrawFrame(); - cv::imshow("ORB-SLAM2: Current Frame",im); + cv::imshow("ORB-SLAM2: Current Frame", im); cv::waitKey(mT); - if(menuReset) + if (menuReset) { menuShowGraph = true; menuShowKeyFrames = true; menuShowPoints = true; menuLocalizationMode = false; - if(bLocalizationMode) + if (bLocalizationMode) mpSystem->DeactivateLocalizationMode(); bLocalizationMode = false; bFollow = true; @@ -153,15 +152,15 @@ void Viewer::Run() menuReset = false; } - if(Stop()) + if (Stop()) { - while(isStopped()) + while (isStopped()) { usleep(3000); } } - if(CheckFinish()) + if (CheckFinish()) break; } @@ -195,7 +194,7 @@ bool Viewer::isFinished() void Viewer::RequestStop() { unique_lock lock(mMutexStop); - if(!mbStopped) + if (!mbStopped) mbStopRequested = true; } @@ -210,9 +209,9 @@ bool Viewer::Stop() unique_lock lock(mMutexStop); unique_lock lock2(mMutexFinish); - if(mbFinishRequested) + if (mbFinishRequested) return false; - else if(mbStopRequested) + else if (mbStopRequested) { mbStopped = true; mbStopRequested = false; @@ -220,7 +219,6 @@ bool Viewer::Stop() } return false; - } void Viewer::Release() @@ -229,4 +227,4 @@ void Viewer::Release() mbStopped = false; } -} +} // namespace ORB_SLAM2