Fix typos

Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc

@@ -219,9 +219,8 @@ void ImageGrabber::SyncWithImu()
 
       if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff)
       {
-        std::cout << "big time difference" << std::endl;
+        // std::cout << "big time difference" << std::endl;
         continue;
-        // std::abort();
       }
       if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec())
           continue;

README.md

@@ -174,10 +174,10 @@ For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_
   ```
 
 ### Running Stereo-Inertial Node
-For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
+For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
 
   ```
-  rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]	
+  rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]	
   ```
   
 ### Running RGB_D Node
@@ -187,7 +187,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist
   rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
   ```
 
-**Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab:
+**Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab for a Stereo-Inertial configuration:
   ```
   roscore
   ```
@@ -197,7 +197,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist
   ```
   
   ```
-  rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu
+  rosbag play --pause V1_02_medium.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu
   ```
   
 Once ORB-SLAM3 has loaded the vocabulary, press space in the rosbag tab.

src/Frame.cc

@@ -118,7 +118,6 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
 
 
     N = mvKeys.size();
-    std::cout << "N = " << N << std::endl;
     if(mvKeys.empty())
         return;
 
@@ -336,7 +335,6 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
         invfy = 1.0f/fy;
 
         mbInitialComputations=false;
-        std::cout << mnMaxX << ", " << mnMinX << ", " << mnMaxY << ", " << mnMinY << "\n";
     }
 
 

src/Optimizer.cc

@@ -1678,11 +1678,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vector<Ke
             vpNonEnoughOptKFs.push_back(pKFi);
         }
     }
-    //Verbose::PrintMess("LM-LBA: Num fixed cameras " + to_string(num_fixedKF), Verbose::VERBOSITY_DEBUG);
-    //Verbose::PrintMess("LM-LBA: Num Points " + to_string(lLocalMapPoints.size()), Verbose::VERBOSITY_DEBUG);
-    //Verbose::PrintMess("LM-LBA: Num optimized cameras " + to_string(lLocalKeyFrames.size()), Verbose::VERBOSITY_DEBUG);
-    //Verbose::PrintMess("----------", Verbose::VERBOSITY_DEBUG);
-
 
     //Points
     for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
@@ -1733,29 +1728,12 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
             if(pMP)
                 if(!pMP->isBad() && pMP->GetMap() == pCurrentMap)
                 {
-                    /*if(sNumObsMP.find(pMP) == sNumObsMP.end())
+
+                    if(pMP->mnBALocalForKF!=pKF->mnId)
                     {
-                        sNumObsMP.insert(pMP);
-                        int index_mp = pMP->GetIndexInKeyFrame(pKFi);
-                        if(pKFi->mvuRight[index_mp]>=0)
-                        {
-                            // Stereo, it has at least 2 observations by pKFi
-                            if(pMP->mnBALocalForKF!=pKF->mnId)
-                            {
-                                lLocalMapPoints.push_back(pMP);
-                                pMP->mnBALocalForKF=pKF->mnId;
-                            }
-                        }
+                        lLocalMapPoints.push_back(pMP);
+                        pMP->mnBALocalForKF=pKF->mnId;
                     }
-                    else
-                    {*/
-                        if(pMP->mnBALocalForKF!=pKF->mnId)
-                        {
-                            lLocalMapPoints.push_back(pMP);
-                            pMP->mnBALocalForKF=pKF->mnId;
-                        }
-                    //}
-
                 }
         }
     }
@@ -1872,10 +1850,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
         if(pKFi->mnId>maxKFid)
             maxKFid=pKFi->mnId;
     }
-    //Verbose::PrintMess("LM-LBA: Fixed KFs added", Verbose::VERBOSITY_DEBUG);
-
-    //Verbose::PrintMess("LM-LBA: opt/fixed KFs: " + to_string(lLocalKeyFrames.size()) + "/" + to_string(lFixedCameras.size()), Verbose::VERBOSITY_DEBUG);
-    //Verbose::PrintMess("LM-LBA: local MPs: " + to_string(lLocalMapPoints.size()), Verbose::VERBOSITY_DEBUG);
 
     // Set MapPoint vertices
     const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
@@ -2182,21 +2156,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
         bRedrawError = true;
         string folder_name = "test_LBA";
         string name = "_PreLM_LBA";
-        //pMap->printReprojectionError(lLocalKeyFrames, pKF, name, folder_name);
         name = "_PreLM_LBA_Fixed";
-        //pMap->printReprojectionError(lFixedCameras, pKF, name, folder_name);
-        /*for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
-        {
-            g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
-            MapPoint* pMP = vpMapPointEdgeMono[i];
-
-            if(pMP->isBad())
-                continue;
-
-            Verbose::PrintMess("ERROR CHI2: " + to_string(e->chi2()) + "; DEPTH POSITIVE: " + to_string(e->isDepthPositive()), Verbose::VERBOSITY_NORMAL);
-        }*/
 
-        //return;
     }
 
 
@@ -2315,11 +2276,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
             Verbose::PrintMess("LM-LBA: Bad observations in mono " + to_string(numBadMonoMP) +  " and stereo " + to_string(numBadStereoMP), Verbose::VERBOSITY_DEBUG);
         }
     }
-    //Verbose::PrintMess("LM-LBA: Num fixed cameras " + to_string(num_fixedKF), Verbose::VERBOSITY_DEBUG);
-    //Verbose::PrintMess("LM-LBA: Num Points " + to_string(lLocalMapPoints.size()), Verbose::VERBOSITY_DEBUG);
-    //Verbose::PrintMess("LM-LBA: Num optimized cameras " + to_string(lLocalKeyFrames.size()), Verbose::VERBOSITY_DEBUG);
-    //Verbose::PrintMess("----------", Verbose::VERBOSITY_DEBUG);
-
 
     //Points
     for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
@@ -3932,8 +3888,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
 int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2,
                             const bool bFixScale, Eigen::Matrix<double,7,7> &mAcumHessian, const bool bAllPoints)
 {
-    // bool bShowImages = false;
-
     g2o::SparseOptimizer optimizer;
     g2o::BlockSolverX::LinearSolverType * linearSolver;
 
@@ -3944,12 +3898,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
     optimizer.setAlgorithm(solver);
 
-    // Calibration
-    /*const cv::Mat &K1 = pKF1->mK;
-    const cv::Mat &K2 = pKF2->mK;*/
-
-    //const cv::Mat &DistCoeff2 = pKF2->mDistCoef;
-
     // Camera poses
     const cv::Mat R1w = pKF1->GetRotation();
     const cv::Mat t1w = pKF1->GetTranslation();
@@ -3979,16 +3927,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
     vpEdges21.reserve(2*N);
     vbIsInKF2.reserve(2*N);
 
-    /*float cx1 = K1.at<float>(0,2);
-    float cy1 = K1.at<float>(1,2);
-    float fx1 = K1.at<float>(0,0);
-    float fy1 = K1.at<float>(1,1);
-
-    float cx2 = K2.at<float>(0,2);
-    float cy2 = K2.at<float>(1,2);
-    float fx2 = K2.at<float>(0,0);
-    float fy2 = K2.at<float>(1,1);*/
-
     const float deltaHuber = sqrt(th2);
 
     int nCorrespondences = 0;
@@ -3997,11 +3935,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
     int nOutKF2 = 0;
     int nMatchWithoutMP = 0;
 
-    /*cv::Mat img1 = cv::imread(pKF1->mNameFile, CV_LOAD_IMAGE_UNCHANGED);
-    cv::cvtColor(img1, img1, CV_GRAY2BGR);
-    cv::Mat img2 = cv::imread(pKF2->mNameFile, CV_LOAD_IMAGE_UNCHANGED);
-    cv::cvtColor(img2, img2, CV_GRAY2BGR);*/
-
     vector<int> vIdsOnlyInKF2;
 
     for(int i=0; i<N; i++)
@@ -4066,11 +3999,7 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
 
                 vIdsOnlyInKF2.push_back(id2);
             }
-
-            // cv::circle(img1, pKF1->mvKeys[i].pt, 1, cv::Scalar(0, 0, 255));
-
             continue;
-
         }
 
         if(i2<0 && !bAllPoints)
@@ -4093,42 +4022,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
         obs1 << kpUn1.pt.x, kpUn1.pt.y;
 
         ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ();
-        /*bool inKF1;
-        if(pMP1)
-        {
-            const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
-            obs1 << kpUn1.pt.x, kpUn1.pt.y;
-
-            inKF1 = true;
-        }
-        else
-        {
-            float invz = 1/P3D1c.at<float>(2);
-            float x = P3D1c.at<float>(0)*invz;
-            float y = P3D1c.at<float>(1)*invz;
-
-            obs1 << x, y;
-            kpUn1 = cv::KeyPoint(cv::Point2f(x, y), pMP1->mnTrackScaleLevel);
-
-            inKF1 = false;
-        }*/
-
-        /*if(bShowImages) //TODO test to project the matched points in the image
-        {
-            cv::circle(img1, pKF1->mvKeys[i].pt, 1, cv::Scalar(0, 255, 0));
-
-            Eigen::Matrix<double,3,1> eigP3D2c = Converter::toVector3d(P3D2c);
-            Eigen::Matrix<double,3,1> eigP3D1c = g2oS12.map(eigP3D2c);
-            cv::Mat cvP3D1c = Converter::toCvMat(eigP3D1c);
-
-            float invz = 1/cvP3D1c.at<float>(2);
-            float x = fx1 * cvP3D1c.at<float>(0)*invz + cx1;
-            float y = fy1 * cvP3D1c.at<float>(1)*invz + cy1;
-
-            cv::Point2f ptProjPoint(x, y);
-            cv::line(img1, pKF1->mvKeys[i].pt, ptProjPoint, cv::Scalar(255, 0, 0), 1);
-        }*/
-
 
         e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
         e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
@@ -4152,11 +4045,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
             inKF2 = true;
 
             nInKF2++;
-
-            /*if(bShowImages)
-            {
-                cv::circle(img2, pKF2->mvKeys[i2].pt, 1, cv::Scalar(0, 255, 0));
-            }*/
         }
         else
         {
@@ -4164,52 +4052,11 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
             float x = P3D2c.at<float>(0)*invz;
             float y = P3D2c.at<float>(1)*invz;
 
-            /*cv::Mat mat(1,2,CV_32F);
-            mat.at<float>(0,0) = x;
-            mat.at<float>(0,1) = y;
-            mat=mat.reshape(2);
-            cv::undistortPoints(mat,mat,K2,DistCoeff2,cv::Mat(),K2);
-            mat=mat.reshape(1);
-
-            x = mat.at<float>(0,0);
-            y = mat.at<float>(0,1);*/
-
             obs2 << x, y;
             kpUn2 = cv::KeyPoint(cv::Point2f(x, y), pMP2->mnTrackScaleLevel);
 
             inKF2 = false;
             nOutKF2++;
-
-            /*if(bShowImages)
-            {
-                cv::circle(img2, kpUn2.pt, 1, cv::Scalar(0, 0, 255));
-            }*/
-
-            //TODO print projection, because all of them become in outliers
-
-
-
-            // Project in image and check it is not outside
-            //float u = pKF2->fx * x + pKF2->cx;
-            //float v = pKF2->fy * y + pKF2->cy;
-            //obs2 << u, v;
-            //kpUn2 = cv::KeyPoint(cv::Point2f(u, v), pMP2->mnTrackScaleLevel);
-        }
-
-        {
-            Eigen::Matrix<double,3,1> eigP3D1c = Converter::toVector3d(P3D1c);
-            Eigen::Matrix<double,3,1> eigP3D2c = g2oS12.inverse().map(eigP3D1c);
-            cv::Mat cvP3D2c = Converter::toCvMat(eigP3D2c);
-
-            /*float invz = 1/cvP3D2c.at<float>(2);
-            float x = fx2 * cvP3D2c.at<float>(0)*invz + cx2;
-            float y = fy2 * cvP3D2c.at<float>(1)*invz + cy2;
-
-            if(bShowImages)
-            {
-                cv::Point2f ptProjPoint(x, y);
-                cv::line(img2, kpUn2.pt, ptProjPoint, cv::Scalar(255, 0, 0), 1);
-            }*/
         }
 
         ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = new ORB_SLAM3::EdgeInverseSim3ProjectXYZ();
@@ -4269,13 +4116,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
         e12->setRobustKernel(0);
         e21->setRobustKernel(0);
     }
-    /*if(bShowImages)
-    {
-        string pathImg1 = "./test_OptSim3/KF_" + to_string(pKF1->mnId) + "_Main.jpg";
-        cv::imwrite(pathImg1, img1);
-        string pathImg2 = "./test_OptSim3/KF_" + to_string(pKF1->mnId) + "_Matched.jpg";
-        cv::imwrite(pathImg2, img2);
-    }*/
 
     Verbose::PrintMess("Sim3: First Opt -> Correspondences: " + to_string(nCorrespondences) + "; nBad: " + to_string(nBad) + "; nBadOutKF2: " + to_string(nBadOutKF2), Verbose::VERBOSITY_DEBUG);
 
@@ -4321,7 +4161,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
     //Verbose::PrintMess("Sim3: Initial seed " + g2oS12, Verbose::VERBOSITY_DEBUG);
     g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
     g2oS12= vSim3_recov->estimate();
-
     //Verbose::PrintMess("Sim3: Optimized solution " + g2oS12, Verbose::VERBOSITY_DEBUG);
 
     return nIn;

src/Tracking.cc

@@ -672,10 +672,6 @@ void Tracking::PreintegrateIMU()
     mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
     mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;
 
-    if(!mpLastKeyFrame)
-    {
-        cout << "last KF is empty!" << endl;
-    }
     mCurrentFrame.setIntegrated();
 
     Verbose::PrintMess("Preintegration is finished!! ", Verbose::VERBOSITY_DEBUG);
@@ -1376,7 +1372,6 @@ void Tracking::StereoInitialization()
 
             if (cv::norm(mCurrentFrame.mpImuPreintegratedFrame->avgA-mLastFrame.mpImuPreintegratedFrame->avgA)<0.5)
             {
-                cout << cv::norm(mCurrentFrame.mpImuPreintegratedFrame->avgA) << endl;
                 cout << "not enough acceleration" << endl;
                 return;
             }