update

Examples/Monocular-Inertial/mono_inertial_tum_vi.cc

@@ -64,9 +64,9 @@ int main(int argc, char **argv)
     vector< vector<double> > vTimestampsCam;        //图像时间戳
     vector< vector<cv::Point3f> > vAcc, vGyro;      //加速度计，陀螺仪
     vector< vector<double> > vTimestampsImu;        //IMU时间戳
-    vector<int> nImages;                            
+    vector<int> nImages;                            //图像序列    
     vector<int> nImu;
-    vector<int> first_imu(num_seq,0);
+    vector<int> first_imu(num_seq,0);               //记录和第一帧图像时间戳最接近的imu时间戳索引
 
     vstrImageFilenames.resize(num_seq);
     vTimestampsCam.resize(num_seq);
@@ -168,13 +168,16 @@ int main(int argc, char **argv)
             {
                 // cout << "t_cam " << tframe << endl;
                 // Step 6 把上一图像帧和当前图像帧之间的imu信息存储在vImuMeas里
-                // 注意第一个图像帧没有对应的imu数据 //?是否存在一帧,因为之前是从最接近图像第一帧的imu算起,可能无效
+                // 注意第一个图像帧没有对应的imu数据 
+                // seq: 数据集序列索引；first_imu[seq]：当前数据集中和当前帧图像最接近的imu时间戳索引；ni：图像的索引
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
+                    // 存储IMU的加速度计信息vAcc、陀螺仪信息vGyro、时间戳信息vTimestampsImu
                     vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
                     // cout << "t_imu = " << fixed << vImuMeas.back().t << endl;
+                    // 更新
                     first_imu[seq]++;
                 }
             }

Examples/euroc_eval_examples.sh

@@ -0,0 +1,36 @@
+#!/bin/bash
+pathDatasetEuroc='/Datasets/EuRoC' #Example, it is necesary to change it by the dataset path
+
+# Single Session Example (Pure visual)
+echo "Launching MH01 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo
+echo "------------------------------------"
+echo "Evaluation of MH01 trajectory with Stereo sensor"
+python ../evaluation/evaluate_ate_scale.py ../evaluation/Ground_truth/EuRoC_left_cam/MH01_GT.txt f_dataset-MH01_stereo.txt --plot MH01_stereo.pdf
+
+
+
+# MultiSession Example (Pure visual)
+echo "Launching Machine Hall with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo
+echo "------------------------------------"
+echo "Evaluation of MAchine Hall trajectory with Stereo sensor"
+python ../evaluation/evaluate_ate_scale.py ../evaluation/Ground_truth/EuRoC_left_cam/MH_GT.txt f_dataset-MH01_to_MH05_stereo.txt --plot MH01_to_MH05_stereo.pdf
+
+
+# Single Session Example (Visual-Inertial)
+echo "Launching V102 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi
+echo "------------------------------------"
+echo "Evaluation of V102 trajectory with Monocular-Inertial sensor"
+python ../evaluation/evaluate_ate_scale.py "$pathDatasetEuroc"/V102/mav0/state_groundtruth_estimate0/data.csv f_dataset-V102_monoi.txt --plot V102_monoi.pdf
+
+
+# MultiSession Monocular Examples
+
+echo "Launching Vicon Room 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi
+echo "------------------------------------"
+echo "Evaluation of Vicon Room 2 trajectory with Stereo sensor"
+python ../evaluation/evaluate_ate_scale.py ../evaluation/Ground_truth/EuRoC_imu/V2_GT.txt f_dataset-V201_to_V203_monoi.txt --plot V201_to_V203_monoi.pdf
+

Examples/euroc_examples.sh

@@ -0,0 +1,182 @@
+#!/bin/bash
+pathDatasetEuroc='/Datasets/EuRoC' #Example, it is necesary to change it by the dataset path
+
+#------------------------------------
+# Monocular Examples
+echo "Launching MH01 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt dataset-MH01_mono
+
+echo "Launching MH02 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt dataset-MH02_mono
+
+echo "Launching MH03 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt dataset-MH03_mono
+
+echo "Launching MH04 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt dataset-MH04_mono
+
+echo "Launching MH05 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH05_mono
+
+echo "Launching V101 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt dataset-V101_mono
+
+echo "Launching V102 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt dataset-V102_mono
+
+echo "Launching V103 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V103_mono
+
+echo "Launching V201 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt dataset-V201_mono
+
+echo "Launching V202 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt dataset-V202_mono
+
+echo "Launching V203 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V203_mono
+
+# MultiSession Monocular Examples
+echo "Launching Machine Hall with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_mono
+
+echo "Launching Vicon Room 1 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_mono
+
+echo "Launching Vicon Room 2 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_mono
+
+#------------------------------------
+# Stereo Examples
+echo "Launching MH01 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo
+
+echo "Launching MH02 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereo
+
+echo "Launching MH03 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereo
+
+echo "Launching MH04 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereo
+
+echo "Launching MH05 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereo
+
+echo "Launching V101 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt dataset-V101_stereo
+
+echo "Launching V102 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt dataset-V102_stereo
+
+echo "Launching V103 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V103_stereo
+
+echo "Launching V201 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt dataset-V201_stereo
+
+echo "Launching V202 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt dataset-V202_stereo
+
+echo "Launching V203 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V203_stereo
+
+# MultiSession Stereo Examples
+echo "Launching Machine Hall with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo
+
+echo "Launching Vicon Room 1 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereo
+
+echo "Launching Vicon Room 2 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereo
+
+#------------------------------------
+# Monocular-Inertial Examples
+echo "Launching MH01 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_monoi
+
+echo "Launching MH02 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_monoi
+
+echo "Launching MH03 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_monoi
+
+echo "Launching MH04 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_monoi
+
+echo "Launching MH05 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_monoi
+
+echo "Launching V101 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_monoi
+
+echo "Launching V102 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi
+
+echo "Launching V103 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_monoi
+
+echo "Launching V201 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_monoi
+
+echo "Launching V202 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_monoi
+
+echo "Launching V203 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_monoi
+
+# MultiSession Monocular Examples
+echo "Launching Machine Hall with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_monoi
+
+echo "Launching Vicon Room 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_monoi
+
+echo "Launching Vicon Room 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi
+
+#------------------------------------
+# Stereo-Inertial Examples
+echo "Launching MH01 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereoi
+
+echo "Launching MH02 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereoi
+
+echo "Launching MH03 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereoi
+
+echo "Launching MH04 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereoi
+
+echo "Launching MH05 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereoi
+
+echo "Launching V101 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_stereoi
+
+echo "Launching V102 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_stereoi
+
+echo "Launching V103 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_stereoi
+
+echo "Launching V201 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_stereoi
+
+echo "Launching V202 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_stereoi
+
+echo "Launching V203 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_stereoi
+
+# MultiSession Stereo-Inertial Examples
+echo "Launching Machine Hall with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereoi
+
+echo "Launching Vicon Room 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereoi
+
+echo "Launching Vicon Room 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereoi

Examples/tum_vi_eval_examples.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+pathDatasetTUM_VI='/Datasets/TUM_VI' #Example, it is necesary to change it by the dataset path
+
+
+# Single Session Example
+
+echo "Launching Magistrale 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data ./Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt ./Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_stereoi
+echo "------------------------------------"
+echo "Evaluation of Magistrale 1 trajectory with Stereo-Inertial sensor"
+python ../evaluation/evaluate_ate_scale.py "$pathDatasetTUM_VI"/magistrale1_512_16/mav0/mocap0/data.csv f_dataset-magistrale1_512_stereoi.txt --plot magistrale1_512_stereoi.pdf

Examples/tum_vi_examples.sh

@@ -0,0 +1,240 @@
+#!/bin/bash
+pathDatasetTUM_VI='/Datasets/TUM_VI' #Example, it is necesary to change it by the dataset path
+
+#------------------------------------
+# Monocular Examples
+echo "Launching Room 1 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room1_512.txt dataset-room1_512_mono
+
+echo "Launching Room 2 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room2_512.txt dataset-room2_512_mono
+
+echo "Launching Room 3 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room3_512.txt dataset-room3_512_mono
+
+echo "Launching Room 4 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room4_512.txt dataset-room4_512_mono
+
+echo "Launching Room 5 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room5_512.txt dataset-room5_512_mono
+
+echo "Launching Room 6 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room6_512.txt dataset-room6_512_mono
+
+
+#------------------------------------
+# Stereo Examples
+echo "Launching Room 1 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room1_512.txt dataset-room1_512_stereo
+
+echo "Launching Room 2 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room2_512.txt dataset-room2_512_stereo
+
+echo "Launching Room 3 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room3_512.txt dataset-room3_512_stereo
+
+echo "Launching Room 4 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room4_512.txt dataset-room4_512_stereo
+
+echo "Launching Room 5 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room5_512.txt dataset-room5_512_stereo
+
+echo "Launching Room 6 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room6_512.txt dataset-room6_512_stereo
+
+
+#------------------------------------
+# Monocular-Inertial Examples
+echo "Launching Corridor 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor1_512.txt dataset-corridor1_512_monoi
+
+echo "Launching Corridor 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor2_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor2_512.txt dataset-corridor2_512_monoi
+
+echo "Launching Corridor 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor3_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor3_512.txt dataset-corridor3_512_monoi
+
+echo "Launching Corridor 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor4_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor4_512.txt dataset-corridor4_512_monoi
+
+echo "Launching Corridor 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor5_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor5_512.txt dataset-corridor5_512_monoi
+
+
+echo "Launching Magistrale 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_monoi
+
+echo "Launching Magistrale 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale2_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale2_512.txt dataset-magistrale2_512
+
+echo "Launching Magistrale 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale3_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale3_512.txt dataset-magistrale3_512_monoi
+
+echo "Launching Magistrale 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale4_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale4_512.txt dataset-magistrale4_512_monoi
+
+echo "Launching Magistrale 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale5_512.txt dataset-magistrale5_512_monoi
+
+echo "Launching Magistrale 6 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale6_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale6_512.txt dataset-magistrale6_512_monoi
+
+
+echo "Launching Outdoor 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors1_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors1_512.txt dataset-outdoors1_512_monoi
+
+echo "Launching Outdoor 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors2_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors2_512.txt dataset-outdoors2_512_monoi
+
+echo "Launching Outdoor 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors3_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors3_512.txt dataset-outdoors3_512_monoi
+
+echo "Launching Outdoor 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors4_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors4_512.txt dataset-outdoors4_512_monoi
+
+echo "Launching Outdoor 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors5_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors5_512.txt dataset-outdoors5_512_monoi
+
+echo "Launching Outdoor 6 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors6_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors6_512.txt dataset-outdoors6_512_monoi
+
+echo "Launching Outdoor 7 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors7_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors7_512.txt dataset-outdoors7_512_monoi
+
+echo "Launching Outdoor 8 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors8_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors8_512.txt dataset-outdoors8_512_monoi
+
+
+echo "Launching Room 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room1_512.txt Monocular-Inertial/TUM_IMU/dataset-room1_512.txt dataset-room1_512_monoi
+
+echo "Launching Room 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room2_512.txt Monocular-Inertial/TUM_IMU/dataset-room2_512.txt dataset-room2_512_monoi
+
+echo "Launching Room 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room3_512.txt Monocular-Inertial/TUM_IMU/dataset-room3_512.txt dataset-room3_512_monoi
+
+echo "Launching Room 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room4_512.txt Monocular-Inertial/TUM_IMU/dataset-room4_512.txt dataset-room4_512_monoi
+
+echo "Launching Room 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room5_512.txt Monocular-Inertial/TUM_IMU/dataset-room5_512.txt dataset-room5_512_monoi
+
+echo "Launching Room 6 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room6_512.txt Monocular-Inertial/TUM_IMU/dataset-room6_512.txt dataset-room6_512_monoi
+
+
+echo "Launching Slides 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Monocular-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-slides1_512_monoi
+
+echo "Launching Slides 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-slides2_512.txt Monocular-Inertial/TUM_IMU/dataset-slides2_512.txt dataset-slides2_512_monoi
+
+echo "Launching Slides 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-slides3_512.txt Monocular-Inertial/TUM_IMU/dataset-slides3_512.txt dataset-slides3_512_monoi
+
+
+# MultiSession Monocular Examples
+echo "Launching Room 1, Magistrale 1, Magistrale 5 and Slides 1 in the same session with Stereo-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-room1_mag1_mag5_slides1_monoi
+
+echo "Launching all Rooms (1-6) in the same session with Stereo-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Stereo-Inertial/TUM_IMU/dataset-room2_512.txt "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Stereo-Inertial/TUM_IMU/dataset-room3_512.txt "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Stereo-Inertial/TUM_IMU/dataset-room4_512.txt "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Stereo-Inertial/TUM_IMU/dataset-room5_512.txt "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-rooms123456_monoi
+
+#------------------------------------
+# Stereo-Inertial Examples
+echo "Launching Corridor 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor1_512.txt dataset-corridor1_512_stereoi
+
+echo "Launching Corridor 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor2_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor2_512.txt dataset-corridor2_512_stereoi
+
+echo "Launching Corridor 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor3_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor3_512.txt dataset-corridor3_512_stereoi
+
+echo "Launching Corridor 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor4_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor4_512.txt dataset-corridor4_512_stereoi
+
+echo "Launching Corridor 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor5_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor5_512.txt dataset-corridor5_512_stereoi
+
+
+echo "Launching Magistrale 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_stereoi
+
+echo "Launching Magistrale 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale2_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale2_512.txt dataset-magistrale2_512_stereoi
+
+echo "Launching Magistrale 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale3_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale3_512.txt dataset-magistrale3_512_stereoi
+
+echo "Launching Magistrale 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale4_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale4_512.txt dataset-magistrale4_512_stereoi
+
+echo "Launching Magistrale 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt dataset-magistrale5_512_stereoi
+
+echo "Launching Magistrale 6 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale6_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale6_512.txt dataset-magistrale6_512_stereoi
+
+
+echo "Launching Outdoor 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors1_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors1_512.txt outdoors1_512_stereoi
+
+echo "Launching Outdoor 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors2_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors2_512.txt outdoors2_512_stereoi
+
+echo "Launching Outdoor 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors3_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors3_512.txt outdoors3_512
+
+echo "Launching Outdoor 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors4_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors4_512.txt outdoors4_512
+
+echo "Launching Outdoor 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors5_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors5_512.txt outdoors5_512_stereoi
+
+echo "Launching Outdoor 6 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors6_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors6_512.txt outdoors6_512_stereoi
+
+echo "Launching Outdoor 7 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors7_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors7_512.txt outdoors7_512_stereoi
+
+echo "Launching Outdoor 8 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors8_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors8_512.txt outdoors8_512_stereoi
+
+
+echo "Launching Room 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt dataset-room1_512_stereoi
+
+echo "Launching Room 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Stereo-Inertial/TUM_IMU/dataset-room2_512.txt dataset-room2_512_stereoi
+
+echo "Launching Room 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Stereo-Inertial/TUM_IMU/dataset-room3_512.txt dataset-room3_512_stereoi
+
+echo "Launching Room 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Stereo-Inertial/TUM_IMU/dataset-room4_512.txt dataset-room4_512_stereoi
+
+echo "Launching Room 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Stereo-Inertial/TUM_IMU/dataset-room5_512.txt dataset-room5_512_stereoi
+
+echo "Launching Room 6 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-room6_512_stereoi
+
+
+echo "Launching Slides 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-slides1_512_stereoi
+
+echo "Launching Slides 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides2_512.txt Stereo-Inertial/TUM_IMU/dataset-slides2_512.txt dataset-slides2_512_stereoi
+
+echo "Launching Slides 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides3_512.txt Stereo-Inertial/TUM_IMU/dataset-slides3_512.txt dataset-slides3_512_stereoi
+
+
+# MultiSession Stereo-Inertial Examples
+echo "Launching Room 1, Magistrale 1, Magistrale 5 and Slides 1 in the same session with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-room1_mag1_mag5_slides1_stereoi
+
+echo "Launching all Rooms (1-6) in the same session with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Stereo-Inertial/TUM_IMU/dataset-room2_512.txt "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Stereo-Inertial/TUM_IMU/dataset-room3_512.txt "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Stereo-Inertial/TUM_IMU/dataset-room4_512.txt "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Stereo-Inertial/TUM_IMU/dataset-room5_512.txt "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-rooms123456_stereoi

include/Atlas.h

@@ -50,7 +50,7 @@ public:
     Atlas();
     Atlas(int initKFid); // When its initialization the first map is created
     ~Atlas();
-
+    // 创建新地图
     void CreateNewMap();
     void ChangeMap(Map* pMap);
 
@@ -107,7 +107,7 @@ public:
 
 protected:
 
-    std::set<Map*> mspMaps;
+    std::set<Map*> mspMaps;     //存放所有的地图
     std::set<Map*> mspBadMaps;
     Map* mpCurrentMap;
 

include/Config.h

@@ -0,0 +1,6 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+
+//#define REGISTER_TIMES
+
+#endif // CONFIG_H

include/G2oTypes.h

@@ -336,6 +336,13 @@ public:
     }
 };
 
+/** 
+ * @brief 视觉重投影的边
+ * 这里或许会有疑问，OptimizableTypes.h 里面不是定义了视觉重投影的边么？
+ * 原因是这样，那里面定义的边也用，不过只是在纯视觉时，没有imu情况下，因为用已经定义好的节点就好
+ * 但是加入imu时，优化要有残差的边与重投影的边同时存在，且两个边可能连接同一个位姿节点，所以需要重新弄一个包含imu位姿的节点
+ * 因此，边也需要重新写，并且在imu优化时使用这个边
+ */
 class EdgeMono : public g2o::BaseBinaryEdge<2,Eigen::Vector2d,g2o::VertexSBAPointXYZ,VertexPose>
 {
 public:
@@ -489,6 +496,7 @@ public:
     const int cam_idx;
 };
 
+// TO COMMENT
 class EdgeInertial : public g2o::BaseMultiEdge<9,Vector9d>
 {
 public:
@@ -858,6 +866,6 @@ public:
     Eigen::Vector3d dtij;
 };
 
-} //namespace ORB_SLAM2
+} //namespace ORB_SLAM3
 
 #endif // G2OTYPES_H

include/ImuTypes.h

@@ -280,6 +280,6 @@ cv::Mat NormalizeRotation(const cv::Mat &R);
 
 }
 
-} //namespace ORB_SLAM2
+} //namespace ORB_SLAM3
 
 #endif // IMUTYPES_H

include/System.h

@@ -43,9 +43,11 @@
 namespace ORB_SLAM3
 {
 
+// 打印中间信息
 class Verbose
 {
 public:
+    // 显示信息量程度
     enum eLevel
     {
         VERBOSITY_QUIET=0,

include/Tracking.h

@@ -109,9 +109,9 @@ public:
         NO_IMAGES_YET=0,                //当前无图像
         NOT_INITIALIZED=1,              //有图像但是没有完成初始化
         OK=2,                           //正常跟踪状态
-        RECENTLY_LOST=3,                //IMU模式：当前地图中的KF>10,且丢失时间<5秒。纯视觉模式没有该状态
+        RECENTLY_LOST=3,                //IMU模式：当前地图中的KF>10,且丢失时间<5秒。纯视觉模式：没有该状态
         LOST=4,                         //IMU模式：当前帧跟丢超过5s。纯视觉模式：重定位失败
-        OK_KLT=5
+        OK_KLT=5                        //未使用
     };
 
     eTrackingState mState;

src/Atlas.cc

@@ -55,15 +55,23 @@ Atlas::~Atlas()
     }
 }
 
+/**
+ * @brief 创建新地图，如果当前活跃地图有效，先存储当前地图为不活跃地图，然后新建地图；否则，可以直接新建地图。
+ * 
+ */
 void Atlas::CreateNewMap()
 {
+    // 锁住地图集
     unique_lock<mutex> lock(mMutexAtlas);
     cout << "Creation of new map with id: " << Map::nNextId << endl;
+
+    // 如果当前活跃地图有效，先存储当前地图为不活跃地图后退出
     if(mpCurrentMap){
         cout << "Exits current map " << endl;
+        // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1
         if(!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
             mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1; //The init KF is the next of current maximum
-
+        // 将当前地图储存起来，其实就是把mIsInUse标记为false
         mpCurrentMap->SetStoredMap();
         cout << "Saved map with ID: " << mpCurrentMap->GetId() << endl;
 
@@ -72,9 +80,9 @@ void Atlas::CreateNewMap()
     }
     cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl;
 
-    mpCurrentMap = new Map(mnLastInitKFidMap);
+    mpCurrentMap = new Map(mnLastInitKFidMap);  //新建地图
-    mpCurrentMap->SetCurrentMap();
+    mpCurrentMap->SetCurrentMap();              //设置为活跃地图
-    mspMaps.insert(mpCurrentMap);
+    mspMaps.insert(mpCurrentMap);               //插入地图集
 }
 
 void Atlas::ChangeMap(Map* pMap)

src/Frame.cc

@@ -364,8 +364,8 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
 
     mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
 #endif
-	//求出特征点的个数
 	
+    //提取特征点的个数
     N = mvKeys.size();
 	//如果没有能够成功提取出特征点，那么就直接返回了
     if(mvKeys.empty())
@@ -1369,9 +1369,11 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
 
     mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
 #endif
-
+    // 左图中提取的特征点数目
     Nleft = mvKeys.size();
+    // 右图中提取的特征点数目
     Nright = mvKeysRight.size();
+    // 特征点总数
     N = Nleft + Nright;
 
     if(N == 0)

src/G2oTypes.cc

@@ -487,35 +487,46 @@ VertexAccBias::VertexAccBias(Frame *pF)
     setEstimate(ba);
 }
 
-
+/** 
-
+ * @brief 局部地图中imu的局部地图优化（此时已经初始化完毕不需要再优化重力方向与尺度）
+ * @param pInt 预积分相关内容
+ */
 EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(Converter::toMatrix3d(pInt->JRg)),
     JVg(Converter::toMatrix3d(pInt->JVg)), JPg(Converter::toMatrix3d(pInt->JPg)), JVa(Converter::toMatrix3d(pInt->JVa)),
     JPa(Converter::toMatrix3d(pInt->JPa)), mpInt(pInt), dt(pInt->dT)
 {
+    // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的
     // This edge links 6 vertices
+    // 6元边
     resize(6);
+    // 1. 定义重力
     g << 0, 0, -IMU::GRAVITY_VALUE;
-    cv::Mat cvInfo = pInt->C.rowRange(0,9).colRange(0,9).inv(cv::DECOMP_SVD);
+    // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵
+    cv::Mat cvInfo = pInt->C.rowRange(0, 9).colRange(0, 9).inv(cv::DECOMP_SVD);
+    // 转成eigen Matrix9d
     Matrix9d Info;
-    for(int r=0;r<9;r++)
+    for (int r = 0; r < 9; r++)
-        for(int c=0;c<9;c++)
+        for (int c = 0; c < 9; c++)
-            Info(r,c)=cvInfo.at<float>(r,c);
+            Info(r, c) = cvInfo.at<float>(r, c);
-    Info = (Info+Info.transpose())/2;
+    // 3. 强制让其成为对角矩阵
-    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
+    Info = (Info + Info.transpose()) / 2;
-     Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
+    // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）
-     for(int i=0;i<9;i++)
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 9, 9>> es(Info);
-         if(eigs[i]<1e-12)
+    Eigen::Matrix<double, 9, 1> eigs = es.eigenvalues(); // 矩阵特征值
-             eigs[i]=0;
+    for (int i = 0; i < 9; i++)
-    Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
+        if (eigs[i] < 1e-12)
+            eigs[i] = 0;
+    // asDiagonal 生成对角矩阵
+    Info = es.eigenvectors() * eigs.asDiagonal() * es.eigenvectors().transpose();
     setInformation(Info);
 }
 
-
+/** 
-
+ * @brief 计算误差
-
+ */
 void EdgeInertial::computeError()
 {
+    // 计算残差
     // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
     const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
     const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
@@ -536,6 +547,7 @@ void EdgeInertial::computeError()
     _error << er, ev, ep;
 }
 
+// 计算雅克比矩阵
 void EdgeInertial::linearizeOplus()
 {
     const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
@@ -549,42 +561,52 @@ void EdgeInertial::linearizeOplus()
     Eigen::Vector3d dbg;
     dbg << db.bwx, db.bwy, db.bwz;
 
-    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;
+    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb; // Ri
-    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();
+    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();    // Ri.t()
-    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;
+    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb; // Rj
 
     const Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b1));
-    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;
+    const Eigen::Matrix3d eR = dR.transpose() * Rbw1 * Rwb2;   // r△Rij
-    const Eigen::Vector3d er = LogSO3(eR);
+    const Eigen::Vector3d er = LogSO3(eR);                     // r△φij
-    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);
+    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er); // Jr^-1(log(△Rij))
 
+    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度
     // Jacobians wrt Pose 1
+    // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导
     _jacobianOplus[0].setZero();
      // rotation
+    // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导
     _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // OK
+    // (3,0)起点的3*3块表示速度残差对pose1的旋转求导
     _jacobianOplus[0].block<3,3>(3,0) = Skew(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK
+    // (6,0)起点的3*3块表示位置残差对pose1的旋转求导
     _jacobianOplus[0].block<3,3>(6,0) = Skew(Rbw1*(VP2->estimate().twb - VP1->estimate().twb
                                                    - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK
     // translation
+    // (6,3)起点的3*3块表示位置残差对pose1的位置求导
     _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // OK
 
     // Jacobians wrt Velocity 1
+    // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导
     _jacobianOplus[1].setZero();
     _jacobianOplus[1].block<3,3>(3,0) = -Rbw1; // OK
     _jacobianOplus[1].block<3,3>(6,0) = -Rbw1*dt; // OK
 
     // Jacobians wrt Gyro 1
+    // _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导
     _jacobianOplus[2].setZero();
     _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg; // OK
     _jacobianOplus[2].block<3,3>(3,0) = -JVg; // OK
     _jacobianOplus[2].block<3,3>(6,0) = -JPg; // OK
 
     // Jacobians wrt Accelerometer 1
+    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导
     _jacobianOplus[3].setZero();
     _jacobianOplus[3].block<3,3>(3,0) = -JVa; // OK
     _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK
 
     // Jacobians wrt Pose 2
+    // _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
     _jacobianOplus[4].setZero();
     // rotation
     _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK
@@ -592,33 +614,43 @@ void EdgeInertial::linearizeOplus()
     _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK
 
     // Jacobians wrt Velocity 2
+    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
     _jacobianOplus[5].setZero();
     _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
 }
 
+// localmapping中imu初始化所用的边，除了正常的几个优化变量外还优化了重力方向与尺度
 EdgeInertialGS::EdgeInertialGS(IMU::Preintegrated *pInt):JRg(Converter::toMatrix3d(pInt->JRg)),
     JVg(Converter::toMatrix3d(pInt->JVg)), JPg(Converter::toMatrix3d(pInt->JPg)), JVa(Converter::toMatrix3d(pInt->JVa)),
     JPa(Converter::toMatrix3d(pInt->JPa)), mpInt(pInt), dt(pInt->dT)
 {
+    // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的
     // This edge links 8 vertices
+    // 8元边
     resize(8);
+    // 1. 定义重力
     gI << 0, 0, -IMU::GRAVITY_VALUE;
-    cv::Mat cvInfo = pInt->C.rowRange(0,9).colRange(0,9).inv(cv::DECOMP_SVD);
+    // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵
+    cv::Mat cvInfo = pInt->C.rowRange(0, 9).colRange(0, 9).inv(cv::DECOMP_SVD);
+    // 转成eigen Matrix9d
     Matrix9d Info;
     for(int r=0;r<9;r++)
         for(int c=0;c<9;c++)
             Info(r,c)=cvInfo.at<float>(r,c);
+    // 3. 强制让其成为对角矩阵
     Info = (Info+Info.transpose())/2;
+    // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）
     Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
-     Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
+    Eigen::Matrix<double, 9, 1> eigs = es.eigenvalues(); // 矩阵特征值
      for(int i=0;i<9;i++)
          if(eigs[i]<1e-12)
              eigs[i]=0;
+    // asDiagonal 生成对角矩阵
     Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
     setInformation(Info);
 }
 
-
+// 计算误差
 
 void EdgeInertialGS::computeError()
 {
@@ -638,6 +670,8 @@ void EdgeInertialGS::computeError()
     const Eigen::Vector3d dV = Converter::toVector3d(mpInt->GetDeltaVelocity(b));
     const Eigen::Vector3d dP = Converter::toVector3d(mpInt->GetDeltaPosition(b));
 
+    // 计算残差。广义上讲都是真实值 = 残差 + imu，旋转为imu*残差=真实值
+    // dR.transpose() 为imu预积分的值，VP1->estimate().Rwb.transpose() * VP2->estimate().Rwb 为相机的Rwc在乘上相机与imu的标定外参矩阵
     const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
     const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(s*(VV2->estimate() - VV1->estimate()) - g*dt) - dV;
     const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(s*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt) - g*dt*dt/2) - dP;
@@ -645,6 +679,7 @@ void EdgeInertialGS::computeError()
     _error << er, ev, ep;
 }
 
+// 计算雅克比矩阵
 void EdgeInertialGS::linearizeOplus()
 {
     const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
@@ -655,53 +690,66 @@ void EdgeInertialGS::linearizeOplus()
     const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
     const VertexGDir* VGDir = static_cast<const VertexGDir*>(_vertices[6]);
     const VertexScale* VS = static_cast<const VertexScale*>(_vertices[7]);
+    // 1. 获取偏置的该变量，因为要对这个东西求导
     const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]);
     const IMU::Bias db = mpInt->GetDeltaBias(b);
 
+    // 陀螺仪的偏置改变量
     Eigen::Vector3d dbg;
     dbg << db.bwx, db.bwy, db.bwz;
 
-    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;
+    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;  // Ri
-    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();
+    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();     // Ri.t()
-    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;
+    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;  // Rj
-    const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg;
+    const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg; // Rwg
-    Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3,2);
+    Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3, 2);
-    Gm(0,1) = -IMU::GRAVITY_VALUE;
+    Gm(0, 1) = -IMU::GRAVITY_VALUE;
-    Gm(1,0) = IMU::GRAVITY_VALUE;
+    Gm(1, 0) = IMU::GRAVITY_VALUE;
     const double s = VS->estimate();
     const Eigen::MatrixXd dGdTheta = Rwg*Gm;
+    // 预积分得来的dR
     const Eigen::Matrix3d dR = Converter::toMatrix3d(mpInt->GetDeltaRotation(b));
-    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;
+    const Eigen::Matrix3d eR = dR.transpose() * Rbw1 * Rwb2;   // r△Rij
-    const Eigen::Vector3d er = LogSO3(eR);
+    const Eigen::Vector3d er = LogSO3(eR);                     // r△φij
-    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);
+    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er); // Jr^-1(log(△Rij))
 
+    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度
     // Jacobians wrt Pose 1
+    // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导
     _jacobianOplus[0].setZero();
      // rotation
+    // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导
     _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1;
+    // (3,0)起点的3*3块表示速度残差对pose1的旋转求导
     _jacobianOplus[0].block<3,3>(3,0) = Skew(Rbw1*(s*(VV2->estimate() - VV1->estimate()) - g*dt));
+    // (6,0)起点的3*3块表示位置残差对pose1的旋转求导
     _jacobianOplus[0].block<3,3>(6,0) = Skew(Rbw1*(s*(VP2->estimate().twb - VP1->estimate().twb
                                                    - VV1->estimate()*dt) - 0.5*g*dt*dt));
     // translation
+    // (6,3)起点的3*3块表示位置残差对pose1的位置求导
     _jacobianOplus[0].block<3,3>(6,3) = -s*Eigen::Matrix3d::Identity();
 
     // Jacobians wrt Velocity 1
+    // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导
     _jacobianOplus[1].setZero();
     _jacobianOplus[1].block<3,3>(3,0) = -s*Rbw1;
     _jacobianOplus[1].block<3,3>(6,0) = -s*Rbw1*dt;
 
     // Jacobians wrt Gyro bias
+    // _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导
     _jacobianOplus[2].setZero();
     _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg;
     _jacobianOplus[2].block<3,3>(3,0) = -JVg;
     _jacobianOplus[2].block<3,3>(6,0) = -JPg;
 
     // Jacobians wrt Accelerometer bias
+    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导
     _jacobianOplus[3].setZero();
     _jacobianOplus[3].block<3,3>(3,0) = -JVa;
     _jacobianOplus[3].block<3,3>(6,0) = -JPa;
 
     // Jacobians wrt Pose 2
+    // _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
     _jacobianOplus[4].setZero();
     // rotation
     _jacobianOplus[4].block<3,3>(0,0) = invJr;
@@ -709,15 +757,18 @@ void EdgeInertialGS::linearizeOplus()
     _jacobianOplus[4].block<3,3>(6,3) = s*Rbw1*Rwb2;
 
     // Jacobians wrt Velocity 2
+    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
     _jacobianOplus[5].setZero();
     _jacobianOplus[5].block<3,3>(3,0) = s*Rbw1;
 
     // Jacobians wrt Gravity direction
+    // _jacobianOplus[3] 9*2矩阵 总体来说就是三个残差分别对重力方向求导
     _jacobianOplus[6].setZero();
     _jacobianOplus[6].block<3,2>(3,0) = -Rbw1*dGdTheta*dt;
     _jacobianOplus[6].block<3,2>(6,0) = -0.5*Rbw1*dGdTheta*dt*dt;
 
     // Jacobians wrt scale factor
+    // _jacobianOplus[3] 9*1矩阵 总体来说就是三个残差分别对尺度求导
     _jacobianOplus[7].setZero();
     _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate());
     _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt);
@@ -754,9 +805,13 @@ void EdgePriorPoseImu::linearizeOplus()
 {
     const VertexPose* VP = static_cast<const VertexPose*>(_vertices[0]);
     const Eigen::Vector3d er = LogSO3(Rwb.transpose()*VP->estimate().Rwb);
+    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是3旋转3平移3速度6偏置）× 每个节点待优化值的维度
+    // 源码可读性太差了。。。里面会自动分配矩阵大小，计算改变量时按照对应位置来
     _jacobianOplus[0].setZero();
-    _jacobianOplus[0].block<3,3>(0,0) = InverseRightJacobianSO3(er);
+    // LOG(Rbw*R*EXP(φ)) = LOG(EXP(LOG(Rbw*R) + Jr(-1)*φ)) = LOG(Rbw*R) + Jr(-1)*φ
-    _jacobianOplus[0].block<3,3>(3,3) = Rwb.transpose()*VP->estimate().Rwb;
+    _jacobianOplus[0].block<3, 3>(0, 0) = InverseRightJacobianSO3(er); // Jr(-1)
+    // Rbw*(t + R*δt - twb) = Rbw*(t - twb) + Rbw*R*δt
+    _jacobianOplus[0].block<3, 3>(3, 3) = Rwb.transpose() * VP->estimate().Rwb; // Rbw*R
     _jacobianOplus[1].setZero();
     _jacobianOplus[1].block<3,3>(6,0) = Eigen::Matrix3d::Identity();
     _jacobianOplus[2].setZero();
@@ -859,6 +914,7 @@ Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z)
     }
 }
 
+// 反对称矩阵
 Eigen::Matrix3d Skew(const Eigen::Vector3d &w)
 {
     Eigen::Matrix3d W;

src/ImuTypes.cc

@@ -27,6 +27,11 @@ namespace IMU
 
 const float eps = 1e-4;
 
+/** 
+ * @brief 强制让R变成一个正交矩阵
+ * @param R 待优化的旋转矩阵
+ * @return 优化后的矩阵
+ */
 cv::Mat NormalizeRotation(const cv::Mat &R)
 {
     cv::Mat U,w,Vt;
@@ -34,6 +39,11 @@ cv::Mat NormalizeRotation(const cv::Mat &R)
     return U*Vt;
 }
 
+/** 
+ * @brief 计算反对称矩阵
+ * @param v 3维向量
+ * @return 反对称矩阵
+ */
 cv::Mat Skew(const cv::Mat &v)
 {
     const float x = v.at<float>(0);
@@ -44,6 +54,11 @@ cv::Mat Skew(const cv::Mat &v)
             -y,  x, 0);
 }
 
+/** 
+ * @brief 计算SO3
+ * @param xyz 李代数
+ * @return SO3
+ */
 cv::Mat ExpSO3(const float &x, const float &y, const float &z)
 {
     cv::Mat I = cv::Mat::eye(3,3,CV_32F);
@@ -58,6 +73,11 @@ cv::Mat ExpSO3(const float &x, const float &y, const float &z)
         return (I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2);
 }
 
+/** 
+ * @brief 计算SO3
+ * @param xyz 李代数
+ * @return SO3
+ */
 Eigen::Matrix<double,3,3> ExpSO3(const double &x, const double &y, const double &z)
 {
     Eigen::Matrix<double,3,3> I = Eigen::MatrixXd::Identity(3,3);
@@ -80,11 +100,21 @@ Eigen::Matrix<double,3,3> ExpSO3(const double &x, const double &y, const double
         return (I + W*sin(d)/d + W*W*(1.0-cos(d))/d2);
 }
 
+/** 
+ * @brief 计算SO3
+ * @param v 李代数
+ * @return SO3
+ */
 cv::Mat ExpSO3(const cv::Mat &v)
 {
     return ExpSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
 }
 
+/** 
+ * @brief 计算so3
+ * @param R SO3
+ * @return so3
+ */
 cv::Mat LogSO3(const cv::Mat &R)
 {
     const float tr = R.at<float>(0,0)+R.at<float>(1,1)+R.at<float>(2,2);
@@ -102,6 +132,11 @@ cv::Mat LogSO3(const cv::Mat &R)
         return theta*w/s;
 }
 
+/** 
+ * @brief 计算右雅可比
+ * @param xyz 李代数
+ * @return Jr
+ */
 cv::Mat RightJacobianSO3(const float &x, const float &y, const float &z)
 {
     cv::Mat I = cv::Mat::eye(3,3,CV_32F);
@@ -120,11 +155,21 @@ cv::Mat RightJacobianSO3(const float &x, const float &y, const float &z)
     }
 }
 
+/** 
+ * @brief 计算右雅可比
+ * @param v so3
+ * @return Jr
+ */
 cv::Mat RightJacobianSO3(const cv::Mat &v)
 {
     return RightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
 }
 
+/** 
+ * @brief 计算右雅可比的逆
+ * @param xyz so3
+ * @return Jr^-1
+ */
 cv::Mat InverseRightJacobianSO3(const float &x, const float &y, const float &z)
 {
     cv::Mat I = cv::Mat::eye(3,3,CV_32F);
@@ -143,6 +188,11 @@ cv::Mat InverseRightJacobianSO3(const float &x, const float &y, const float &z)
     }
 }
 
+/** 
+ * @brief 计算右雅可比的逆
+ * @param v so3
+ * @return Jr^-1
+ */
 cv::Mat InverseRightJacobianSO3(const cv::Mat &v)
 {
     return InverseRightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
@@ -190,6 +240,11 @@ IntegratedRotation::IntegratedRotation(const cv::Point3f &angVel, const Bias &im
     }
 }
 
+/** 
+ * @brief 预积分类构造函数，根据输入的偏置初始化预积分参数
+ * @param b_ 偏置
+ * @param calib imu标定参数的类
+ */
 Preintegrated::Preintegrated(const Bias &b_, const Calib &calib)
 {
     Nga = calib.Cov.clone();
@@ -206,6 +261,10 @@ Preintegrated::Preintegrated(Preintegrated* pImuPre): dT(pImuPre->dT), C(pImuPre
 
 }
 
+/** 
+ * @brief 复制上一帧的预积分
+ * @param pImuPre 上一帧的预积分
+ */
 void Preintegrated::CopyFrom(Preintegrated* pImuPre)
 {
     std::cout << "Preintegrated: start clone" << std::endl;
@@ -219,6 +278,7 @@ void Preintegrated::CopyFrom(Preintegrated* pImuPre)
     dR = pImuPre->dR.clone();
     dV = pImuPre->dV.clone();
     dP = pImuPre->dP.clone();
+    // 旋转关于陀螺仪偏置变化的雅克比，以此类推
     JRg = pImuPre->JRg.clone();
     JVg = pImuPre->JVg.clone();
     JVa = pImuPre->JVa.clone();
@@ -234,28 +294,34 @@ void Preintegrated::CopyFrom(Preintegrated* pImuPre)
     std::cout << "Preintegrated: end clone" << std::endl;
 }
 
-
+/** 
+ * @brief 初始化预积分
+ * @param b_ 偏置
+ */
 void Preintegrated::Initialize(const Bias &b_)
 {
-    dR = cv::Mat::eye(3,3,CV_32F);
+    dR = cv::Mat::eye(3, 3, CV_32F);
-    dV = cv::Mat::zeros(3,1,CV_32F);
+    dV = cv::Mat::zeros(3, 1, CV_32F);
-    dP = cv::Mat::zeros(3,1,CV_32F);
+    dP = cv::Mat::zeros(3, 1, CV_32F);
-    JRg = cv::Mat::zeros(3,3,CV_32F);
+    JRg = cv::Mat::zeros(3, 3, CV_32F);
-    JVg = cv::Mat::zeros(3,3,CV_32F);
+    JVg = cv::Mat::zeros(3, 3, CV_32F);
-    JVa = cv::Mat::zeros(3,3,CV_32F);
+    JVa = cv::Mat::zeros(3, 3, CV_32F);
-    JPg = cv::Mat::zeros(3,3,CV_32F);
+    JPg = cv::Mat::zeros(3, 3, CV_32F);
-    JPa = cv::Mat::zeros(3,3,CV_32F);
+    JPa = cv::Mat::zeros(3, 3, CV_32F);
-    C = cv::Mat::zeros(15,15,CV_32F);
+    C = cv::Mat::zeros(15, 15, CV_32F);
-    Info=cv::Mat();
+    Info = cv::Mat();
-    db = cv::Mat::zeros(6,1,CV_32F);
+    db = cv::Mat::zeros(6, 1, CV_32F);
-    b=b_;
+    b = b_;
-    bu=b_;
+    bu = b_;  // 更新后的偏置
-    avgA = cv::Mat::zeros(3,1,CV_32F);
+    avgA = cv::Mat::zeros(3, 1, CV_32F);  // 平均加速度
-    avgW = cv::Mat::zeros(3,1,CV_32F);
+    avgW = cv::Mat::zeros(3, 1, CV_32F);  // 平均角速度
-    dT=0.0f;
+    dT = 0.0f;
-    mvMeasurements.clear();
+    mvMeasurements.clear();  // 存放imu数据及dt
 }
 
+/** 
+ * @brief 根据新的偏置重新积分mvMeasurements里的数据 Optimizer::InertialOptimization调用
+ */ 
 void Preintegrated::Reintegrate()
 {
     std::unique_lock<std::mutex> lock(mMutex);
@@ -299,8 +365,8 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con
     
     // Update delta position dP and velocity dV (rely on no-updated delta rotation)
     // 根据没有更新的dR来更新dP与dV  eq.(38)
-    dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
+    dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;	// 对应viorb论文的公式（2）的第三个，位移积分
-    dV = dV + dR*acc*dt;
+    dV = dV + dR*acc*dt;					// 对应viorb论文的公式（2）的第二个，速度积分
 
     // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
     // 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新
@@ -335,9 +401,9 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con
     // 小量delta初始为0，更新后通常也为0，故省略了小量的更新
     // Update covariance
     // Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响
-    C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();
+    C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t(); 	// B矩阵为9*6矩阵 Nga 6*6对角矩阵，3个陀螺仪噪声的平方，3个加速度计噪声的平方
     // 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵
-    C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;
+    C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;	// NgaWalk 6*6 随机游走对角矩阵
 
     // Update rotation jacobian wrt bias correction
     // 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t
@@ -350,6 +416,10 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con
     dT += dt;
 }
 
+/** 
+ * @brief 融合两个预积分，发生在删除关键帧的时候，3帧变2帧，需要把两段预积分融合
+ * @param pPrev 前面的预积分
+ */
 void Preintegrated::MergePrevious(Preintegrated* pPrev)
 {
     if (pPrev==this)
@@ -376,6 +446,10 @@ void Preintegrated::MergePrevious(Preintegrated* pPrev)
 
 }
 
+/** 
+ * @brief 更新偏置
+ * @param bu_ 偏置
+ */
 void Preintegrated::SetNewBias(const Bias &bu_)
 {
     std::unique_lock<std::mutex> lock(mMutex);
@@ -389,12 +463,22 @@ void Preintegrated::SetNewBias(const Bias &bu_)
     db.at<float>(5) = bu_.baz-b.baz;
 }
 
+/** 
+ * @brief 获得当前偏置与输入偏置的改变量
+ * @param b_ 偏置
+ * @return 改变量
+ */
 IMU::Bias Preintegrated::GetDeltaBias(const Bias &b_)
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return IMU::Bias(b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz,b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz);
 }
 
+/** 
+ * @brief 根据新的偏置计算新的dR
+ * @param b_ 新的偏置
+ * @return dR
+ */
 cv::Mat Preintegrated::GetDeltaRotation(const Bias &b_)
 {
     std::unique_lock<std::mutex> lock(mMutex);
@@ -405,6 +489,11 @@ cv::Mat Preintegrated::GetDeltaRotation(const Bias &b_)
     return NormalizeRotation(dR*ExpSO3(JRg*dbg));
 }
 
+/** 
+ * @brief 根据新的偏置计算新的dV
+ * @param b_ 新的偏置
+ * @return dV
+ */
 cv::Mat Preintegrated::GetDeltaVelocity(const Bias &b_)
 {
     std::unique_lock<std::mutex> lock(mMutex);
@@ -414,6 +503,11 @@ cv::Mat Preintegrated::GetDeltaVelocity(const Bias &b_)
     return dV + JVg*dbg + JVa*dba;
 }
 
+/** 
+ * @brief 根据新的偏置计算新的dP
+ * @param b_ 新的偏置
+ * @return dP
+ */
 cv::Mat Preintegrated::GetDeltaPosition(const Bias &b_)
 {
     std::unique_lock<std::mutex> lock(mMutex);
@@ -423,61 +517,101 @@ cv::Mat Preintegrated::GetDeltaPosition(const Bias &b_)
     return dP + JPg*dbg + JPa*dba;
 }
 
+/** 
+ * @brief 返回经过db(δba, δbg)更新后的dR,与上面是一个意思
+ * @return dR
+ */
 cv::Mat Preintegrated::GetUpdatedDeltaRotation()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return NormalizeRotation(dR*ExpSO3(JRg*db.rowRange(0,3)));
 }
 
+/** 
+ * @brief 返回经过db(δba, δbg)更新后的dV,与上面是一个意思
+ * @return dV
+ */
 cv::Mat Preintegrated::GetUpdatedDeltaVelocity()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return dV + JVg*db.rowRange(0,3) + JVa*db.rowRange(3,6);
 }
 
+/** 
+ * @brief 返回经过db(δba, δbg)更新后的dP,与上面是一个意思
+ * @return dP
+ */
 cv::Mat Preintegrated::GetUpdatedDeltaPosition()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return dP + JPg*db.rowRange(0,3) + JPa*db.rowRange(3,6);
 }
 
+/** 
+ * @brief 获取dR
+ * @return dR
+ */
 cv::Mat Preintegrated::GetOriginalDeltaRotation()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return dR.clone();
 }
 
+/** 
+ * @brief 获取dV
+ * @return dV
+ */
 cv::Mat Preintegrated::GetOriginalDeltaVelocity()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return dV.clone();
 }
 
+/** 
+ * @brief 获取dP
+ * @return dP
+ */
 cv::Mat Preintegrated::GetOriginalDeltaPosition()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return dP.clone();
 }
 
+/** 
+ * @brief 获取b,更新前的偏置
+ * @return b
+ */
 Bias Preintegrated::GetOriginalBias()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return b;
 }
 
+/** 
+ * @brief 获取bu,更新后的偏置
+ * @return bu
+ */
 Bias Preintegrated::GetUpdatedBias()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return bu;
 }
 
+/** 
+ * @brief 获取db,更新前后的偏置差
+ * @return db
+ */
 cv::Mat Preintegrated::GetDeltaBias()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     return db.clone();
 }
 
-Eigen::Matrix<double,15,15> Preintegrated::GetInformationMatrix()
+/** 
+ * @brief 获取信息矩阵，没有用到，也就是C矩阵,其中9~15每个元素取倒数，
+ * @return 信息矩阵
+ */
+Eigen::Matrix<double, 15, 15> Preintegrated::GetInformationMatrix()
 {
     std::unique_lock<std::mutex> lock(mMutex);
     if(Info.empty())
@@ -495,6 +629,10 @@ Eigen::Matrix<double,15,15> Preintegrated::GetInformationMatrix()
     return EI;
 }
 
+/** 
+ * @brief 赋值新的偏置
+ * @param b 偏置
+ */
 void Bias::CopyFrom(Bias &b)
 {
     bax = b.bax;
@@ -529,21 +667,34 @@ std::ostream& operator<< (std::ostream &out, const Bias &b)
     return out;
 }
 
+/** 
+ * @brief 设置参数
+ * @param Tbc_ 位姿变换
+ * @param ng 噪声
+ * @param na 噪声
+ * @param ngw 随机游走
+ * @param naw 随机游走
+ */
 void Calib::Set(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw)
 {
     Tbc = Tbc_.clone();
     Tcb = cv::Mat::eye(4,4,CV_32F);
     Tcb.rowRange(0,3).colRange(0,3) = Tbc.rowRange(0,3).colRange(0,3).t();
     Tcb.rowRange(0,3).col(3) = -Tbc.rowRange(0,3).colRange(0,3).t()*Tbc.rowRange(0,3).col(3);
+	
+	// 噪声协方差
     Cov = cv::Mat::eye(6,6,CV_32F);
     const float ng2 = ng*ng;
     const float na2 = na*na;
+	
     Cov.at<float>(0,0) = ng2;
     Cov.at<float>(1,1) = ng2;
     Cov.at<float>(2,2) = ng2;
     Cov.at<float>(3,3) = na2;
     Cov.at<float>(4,4) = na2;
     Cov.at<float>(5,5) = na2;
+	
+	// 随机游走协方差
     CovWalk = cv::Mat::eye(6,6,CV_32F);
     const float ngw2 = ngw*ngw;
     const float naw2 = naw*naw;
@@ -555,6 +706,10 @@ void Calib::Set(const cv::Mat &Tbc_, const float &ng, const float &na, const flo
     CovWalk.at<float>(5,5) = naw2;
 }
 
+/** 
+ * @brief imu标定参数的构造函数
+ * @param calib imu标定参数
+ */
 Calib::Calib(const Calib &calib)
 {
     Tbc = calib.Tbc.clone();
@@ -565,4 +720,4 @@ Calib::Calib(const Calib &calib)
 
 } //namespace IMU
 
-} //namespace ORB_SLAM2
+} //namespace ORB_SLAM3

src/KeyFrameDatabase.cc

@@ -649,22 +649,33 @@ bool compFirst(const pair<float, KeyFrame*> & a, const pair<float, KeyFrame*> &
 }
 
 
+/**
+ * @brief 找到N个融合候选N个回环候选
+ * 
+ * @param[in] pKF 当前关键帧(我们要寻找这个关键帧的回环候选帧和融合候选帧)
+ * @param[out] vpLoopCand 记录找到的回环候选帧
+ * @param[out] vpMergeCand 记录找到的融合候选帧
+ * @param[in] nNumCandidates 期望的候选数目,即回环和候选分别应该有多少个
+ */
 void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &vpLoopCand, vector<KeyFrame*> &vpMergeCand, int nNumCandidates)
 {
+    // Step 1统计与当前关键帧有相同单词的关键帧
     list<KeyFrame*> lKFsSharingWords;
     //set<KeyFrame*> spInsertedKFsSharing;
+    // 当前关键帧的共视关键帧(避免将当前关键帧的共视关键帧加入回环检测)
     set<KeyFrame*> spConnectedKF;
 
     // Search all keyframes that share a word with current frame
     {
         unique_lock<mutex> lock(mMutex);
-
+        // 拿到当前关键帧的共视关键帧
         spConnectedKF = pKF->GetConnectedKeyFrames();
 
+        // 遍历当前关键帧bow向量的每个单词
         for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
-        {
+        {   // 拿到当前单词的逆向索引(所有有当前单词的关键帧)
             list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];
-
+            // 遍历每个有该单词的关键帧
             for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
             {
                 KeyFrame* pKFi=*lit;
@@ -672,16 +683,21 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
                 {
                     continue;
                 }*/
+                // 如果此关键帧没有被当前关键帧访问过(防止重复添加)
                 if(pKFi->mnPlaceRecognitionQuery!=pKF->mnId)
                 {   
+                    // 初始化公共单词数为0
                     pKFi->mnPlaceRecognitionWords=0;
+                    // 如果该关键帧不是当前关键帧的共视关键帧
                     if(!spConnectedKF.count(pKFi))
                     {
-
+                        // 标记改关键帧被当前关键帧访问到
                         pKFi->mnPlaceRecognitionQuery=pKF->mnId;
+                        // 把当前关键帧添加到有公共单词的关键帧列表中
                         lKFsSharingWords.push_back(pKFi);
                     }
                 }
+                // 递增该关键帧与当前关键帧的公共单词数
                 pKFi->mnPlaceRecognitionWords++;
                 /*if(spInsertedKFsSharing.find(pKFi) == spInsertedKFsSharing.end())
                 {
@@ -692,59 +708,77 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
             }
         }
     }
+    // 如果没有有公共单词的关键帧,直接返回
     if(lKFsSharingWords.empty())
         return;
 
     // Only compare against those keyframes that share enough words
+    // Step 2 统计所有候选帧中与当前关键帧的公共单词数最多的单词数,并筛选
     int maxCommonWords=0;
     for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
     {
         if((*lit)->mnPlaceRecognitionWords>maxCommonWords)
             maxCommonWords=(*lit)->mnPlaceRecognitionWords;
     }
-
+    // 取0.8倍为阀值
     int minCommonWords = maxCommonWords*0.8f;
-
+    // 这里的pair是 <相似度,候选帧的指针> : 记录所有大于minCommonWords的候选帧与当前关键帧的相似度
     list<pair<float,KeyFrame*> > lScoreAndMatch;
-
+    // 只是个统计变量,貌似没有用到
     int nscores=0;
 
     // Compute similarity score.
+    // 对所有大于minCommonWords的候选帧计算相似度
+
+
+    // 遍历所有有公共单词的候选帧
     for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
     {
         KeyFrame* pKFi = *lit;
-
+        // 如果当前帧的公共单词数大于minCommonWords
         if(pKFi->mnPlaceRecognitionWords>minCommonWords)
         {
             nscores++;
+            // 计算相似度
             float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
+            // 记录该候选帧与当前帧的相似度
             pKFi->mPlaceRecognitionScore=si;
+            // 记录到容器里, 每个元素是<相似度,候选帧的指针>
             lScoreAndMatch.push_back(make_pair(si,pKFi));
         }
     }
-
+    // 如果为空,直接返回,表示没有符合上述条件的关键帧
     if(lScoreAndMatch.empty())
         return;
-
+    // Step 3 : 用小组得分排序得到top3总分里最高分的关键帧,作为候选帧
+    // 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
     list<pair<float,KeyFrame*> > lAccScoreAndMatch;
     float bestAccScore = 0;
 
     // Lets now accumulate score by covisibility
+    // 变量所有被lScoreAndMatch记录的pair <相似度,候选关键帧>
     for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
     {
+        // 候选关键帧
         KeyFrame* pKFi = it->second;
+        // 与候选关键帧共视关系最好的10个关键帧
         vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
-
+        // 初始化最大相似度为该候选关键帧自己的相似度
         float bestScore = it->first;
+        // 初始化小组累计得分为改候选关键帧自己的相似度
         float accScore = bestScore;
+        // 初始化组内相似度最高的帧为该候选关键帧本身
         KeyFrame* pBestKF = pKFi;
+        // 遍历与当前关键帧共视关系最好的10帧
         for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
         {
             KeyFrame* pKF2 = *vit;
+            // 如果改关键帧没有被当前关键帧访问过(这里标记的是有没有公共单词)则跳过
             if(pKF2->mnPlaceRecognitionQuery!=pKF->mnId)
                 continue;
-
+            // 累加小组总分
             accScore+=pKF2->mPlaceRecognitionScore;
+            // 如果大与组内最高分,则重新记录
             if(pKF2->mPlaceRecognitionScore>bestScore)
             {
                 pBestKF=pKF2;
@@ -752,39 +786,51 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
             }
 
         }
+        // 统计以组为单位的累计相似度和组内相似度最高的关键帧, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
         lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+        // 统计最高得分, 这个bestAccSocre没有用到
         if(accScore>bestAccScore)
             bestAccScore=accScore;
     }
 
     //cout << "Amount of candidates: " << lAccScoreAndMatch.size() << endl;
-
+    // 按相似度从大到小排序
     lAccScoreAndMatch.sort(compFirst);
-
+    // 最后返回的变量, 记录回环的候选帧
     vpLoopCand.reserve(nNumCandidates);
+    // 最后返回的变量, 记录融合候选帧
     vpMergeCand.reserve(nNumCandidates);
+    // 避免重复添加
     set<KeyFrame*> spAlreadyAddedKF;
     //cout << "Candidates in score order " << endl;
     //for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
     int i = 0;
     list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin();
+    // 遍历lAccScoreAndMatch中所有的pair, 每个pair为<小组总相似度,组内相似度最高的关键帧指针>
     while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates))
     {
         //cout << "Accum score: " << it->first << endl;
+        // 拿到候选关键帧的指针
         KeyFrame* pKFi = it->second;
         if(pKFi->isBad())
             continue;
 
+        // 如果没有被重复添加
         if(!spAlreadyAddedKF.count(pKFi))
         {   
+            // 如果候选帧与当前关键帧在同一个地图里,且候选者数量还不足够
             if(pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates)
             {
+                // 添加到回环候选帧里
                 vpLoopCand.push_back(pKFi);
             }
+            // 如果候选者与当前关键帧不再同一个地图里, 且候选者数量还不足够, 且候选者所在地图不是bad
             else if(pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad())
             {   
+                // 添加到融合候选帧里
                 vpMergeCand.push_back(pKFi);
             }
+            // 防止重复添加
             spAlreadyAddedKF.insert(pKFi);
         }
         i++;

src/LocalMapping.cc

@@ -120,12 +120,14 @@ void LocalMapping::Run()
             // Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
             CreateNewMapPoints();
 
-            mbAbortBA = false;
+            mbAbortBA = false; // 注意orbslam2中放在了函数SearchInNeighbors（用到了mbAbortBA）后面，应该放这里更合适
 			// 已经处理完队列中的最后的一个关键帧
             if(!CheckNewKeyFrames())
             {
                 // Find more matches in neighbor keyframes and fuse point duplications
                 //  Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
+                // 先完成相邻关键帧与当前关键帧的地图点的融合（在相邻关键帧中查找当前关键帧的地图点），
+                // 再完成当前关键帧与相邻关键帧的地图点的融合（在当前关键帧中查找当前相邻关键帧的地图点）
                 SearchInNeighbors();
             }
 
@@ -144,19 +146,19 @@ void LocalMapping::Run()
 			// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
             if(!CheckNewKeyFrames() && !stopRequested())
             {
-                // 地图中关键帧数目大于2个
+                // 当前地图中关键帧数目大于2个
                 if(mpAtlas->KeyFramesInMap()>2)
                 {
                     // Step 6.1 处于IMU模式并且当前关键帧所在的地图已经完成IMU初始化
                     if(mbInertial && mpCurrentKeyFrame->GetMap()->isImuInitialized())
                     {
-                        // 计算上一帧到当前帧相机光心的距离 + 上上帧到上一帧相机光心的距离
+                        // 计算上一关键帧到当前关键帧相机光心的距离 + 上上关键帧到上一关键帧相机光心的距离
                         float dist = cv::norm(mpCurrentKeyFrame->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->GetCameraCenter()) +
                                 cv::norm(mpCurrentKeyFrame->mPrevKF->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->mPrevKF->GetCameraCenter());
                         // 如果距离大于5厘米，记录当前KF和上一KF时间戳的差，累加到mTinit
                         if(dist>0.05)
                             mTinit += mpCurrentKeyFrame->mTimeStamp - mpCurrentKeyFrame->mPrevKF->mTimeStamp;
-                        // 当前关键帧所在的地图已经完成IMU BA2   
+                        // 当前关键帧所在的地图尚未完成IMU BA2（IMU第三阶段初始化）   
                         if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2())
                         {
                             // 如果累计时间差小于10s 并且 距离小于2厘米，认为运动幅度太小，不足以初始化IMU，将mbBadImu设置为true
@@ -166,9 +168,10 @@ void LocalMapping::Run()
                                 unique_lock<mutex> lock(mMutexReset);
                                 mbResetRequestedActiveMap = true;
                                 mpMapToReset = mpCurrentKeyFrame->GetMap();
-                                mbBadImu = true;
+                                mbBadImu = true;    //在跟踪线程里会重置当前活跃地图
                             }
                         }
+                        // 判断成功跟踪匹配的点数是否足够多
                         // 条件---------1.1、跟踪成功的内点数目大于75-----1.2、并且是单目--或--2.1、跟踪成功的内点数目大于100-----2.2、并且不是单目             
                         bool bLarge = ((mpTracker->GetMatchesInliers()>75)&&mbMonocular)||((mpTracker->GetMatchesInliers()>100)&&!mbMonocular);
                         Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA, bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());
@@ -206,9 +209,11 @@ void LocalMapping::Run()
 #endif
 
                 // Initialize IMU here
-                // Step 7 当前关键帧所在地图的IMU初始化
+                // Step 7 当前关键帧所在地图未完成IMU初始化（第一阶段）
                 if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)
                 {
+                    // 在函数InitializeIMU里设置IMU成功初始化标志 SetImuInitialized
+                    // IMU第一阶段初始化
                     if (mbMonocular)
                         InitializeIMU(1e2, 1e10, true);
                     else
@@ -217,6 +222,7 @@ void LocalMapping::Run()
 
 
                 // Check redundant local Keyframes
+                // 跟踪中关键帧插入条件比较松，交给LocalMapping线程的关键帧会比较密，这里再删除冗余
                 // Step 8 检测并剔除当前帧相邻的关键帧中冗余的关键帧
                 // 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到
                 KeyFrameCulling();
@@ -227,16 +233,16 @@ void LocalMapping::Run()
                 timeKFCulling_ms = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndKFCulling - time_EndLBA).count();
                 vdKFCullingSync_ms.push_back(timeKFCulling_ms);
 #endif
-				// Step 9 如果累计时间差小于100s 并且 是IMU模式，进行VIBA
+				// Step 9 如果距离初始化成功累计时间差小于100s 并且 是IMU模式，进行VIBA
                 if ((mTinit<100.0f) && mbInertial)
                 {
-                    // Step 9.1 根据条件判断是否进行VIBA1
+                    // Step 9.1 根据条件判断是否进行VIBA1（IMU第二阶段初始化）
                     // 条件：1、当前关键帧所在的地图还未完成IMU初始化---并且--------2、正常跟踪状态----------
                     if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called
                     {
                         // 当前关键帧所在的地图还未完成VIBA 1
                         if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){
-                            // 如果累计时间差大于5s，开始VIBA 1
+                            // 如果累计时间差大于5s，开始VIBA1（IMU第二阶段初始化）
                             if (mTinit>5.0f)
                             {
                                 cout << "start VIBA 1" << endl;
@@ -250,10 +256,10 @@ void LocalMapping::Run()
                             }
                         }
                         //else if (mbNotBA2){
-                        // Step 9.2 根据条件判断是否进行VIBA2
+                        // Step 9.2 根据条件判断是否进行VIBA2（IMU第三阶段初始化）
                         // 当前关键帧所在的地图还未完成VIBA 2
                         else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){
-                            // 如果累计时间差大于15s，开始VIBA 2
+                            // 如果累计时间差大于15s，开始VIBA2（IMU第三阶段初始化）
                             if (mTinit>15.0f){ // 15.0f
                                 cout << "start VIBA 2" << endl;
                                 mpCurrentKeyFrame->GetMap()->SetIniertialBA2();
@@ -267,7 +273,7 @@ void LocalMapping::Run()
                         }
 
                         // scale refinement
-                        // Step 9.3 尺度优化
+                        // Step 9.3 在关键帧小于100时，会在满足一定时间间隔后多次进行尺度、重力方向优化
                         if (((mpAtlas->KeyFramesInMap())<=100) &&
                                 ((mTinit>25.0f && mTinit<25.5f)||       
                                 (mTinit>35.0f && mTinit<35.5f)||
@@ -409,6 +415,9 @@ void LocalMapping::ProcessNewKeyFrame()
     mpAtlas->AddKeyFrame(mpCurrentKeyFrame);
 }
 
+/**
+ * @brief 处理新的关键帧，使队列为空，注意这里只是处理了关键帧，并没有生成MP
+ */
 void LocalMapping::EmptyQueue()
 {
     while(CheckNewKeyFrames())
@@ -487,10 +496,12 @@ void LocalMapping::CreateNewMapPoints()
 	// Step 1：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
     vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
 
+    // imu模式下在附近添加更多的帧进来
     if (mbInertial)
     {
         KeyFrame* pKF = mpCurrentKeyFrame;
         int count=0;
+        // 在总数不够且上一关键帧存在，且添加的帧没有超过总数时
         while((vpNeighKFs.size()<=nn)&&(pKF->mPrevKF)&&(count++<nn))
         {
             vector<KeyFrame*>::iterator it = std::find(vpNeighKFs.begin(), vpNeighKFs.end(), pKF->mPrevKF);
@@ -561,7 +572,7 @@ void LocalMapping::CreateNewMapPoints()
             const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
             // baseline与景深的比例
             const float ratioBaselineDepth = baseline/medianDepthKF2;
-            // 如果比例特别小，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点
+            // 如果特别远(比例特别小)，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点
             if(ratioBaselineDepth<0.01)
                 continue;
         }
@@ -573,10 +584,11 @@ void LocalMapping::CreateNewMapPoints()
         // Search matches that fullfil epipolar constraint
         // Step 5：通过BoW对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对
         vector<pair<size_t,size_t> > vMatchedIndices;
+        // imu相关，非imu时为false
         bool bCoarse = mbInertial &&
                 ((!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && mpCurrentKeyFrame->GetMap()->GetIniertialBA1())||
                  mpTracker->mState==Tracking::RECENTLY_LOST);
-
+        // 通过极线约束的方式找到匹配点（且该点还没有成为MP，注意非单目已经生成的MP这里直接跳过不做匹配，所以最后并不会覆盖掉特征点对应的MP）
         matcher.SearchForTriangulation_(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false,bCoarse);
 
         auto Rcw2 = pKF2->GetRotation_();
@@ -611,8 +623,10 @@ void LocalMapping::CreateNewMapPoints()
             const cv::KeyPoint &kp1 = (mpCurrentKeyFrame -> NLeft == -1) ? mpCurrentKeyFrame->mvKeysUn[idx1]
                                                                          : (idx1 < mpCurrentKeyFrame -> NLeft) ? mpCurrentKeyFrame -> mvKeys[idx1]
                                                                                                                : mpCurrentKeyFrame -> mvKeysRight[idx1 - mpCurrentKeyFrame -> NLeft];
+            // mvuRight中存放着极限校准后双目特征点在右目对应的像素横坐标，如果不是基线校准的双目或者没有找到匹配点，其值将为-1（或者rgbd）
             const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
             bool bStereo1 = (!mpCurrentKeyFrame->mpCamera2 && kp1_ur>=0);
+            // 查看点idx1是否为右目的点
             const bool bRight1 = (mpCurrentKeyFrame -> NLeft == -1 || idx1 < mpCurrentKeyFrame -> NLeft) ? false
                                                                                : true;
             // 当前匹配在邻接关键帧中的特征点
@@ -620,11 +634,13 @@ void LocalMapping::CreateNewMapPoints()
                                                             : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2]
                                                                                      : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft];
             // mvuRight中存放着双目的深度值，如果不是双目，其值将为-1
+            // mvuRight中存放着极限校准后双目特征点在右目对应的像素横坐标，如果不是基线校准的双目或者没有找到匹配点，其值将为-1（或者rgbd）
             const float kp2_ur = pKF2->mvuRight[idx2];
             bool bStereo2 = (!pKF2->mpCamera2 && kp2_ur>=0);
+            // 查看点idx2是否为右目的点
             const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false
                                                                                : true;
-
+            // 当目前为左右目时，确定两个点所在相机之间的位姿关系
             if(mpCurrentKeyFrame->mpCamera2 && pKF2->mpCamera2){
                 if(bRight1 && bRight2){
                     Rcw1 = mpCurrentKeyFrame->GetRightRotation_();
@@ -724,10 +740,20 @@ void LocalMapping::CreateNewMapPoints()
 			// Step 6.4：三角化恢复3D点
             cv::Matx31f x3D;
             bool bEstimated = false;
+            // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视线角正常
+            // cosParallaxRays<cosParallaxStereo表明前后帧视线角比双目视线角大，所以用前后帧三角化而来，反之使用双目的，如果没有双目则跳过
+            // 视差角度小时用三角法恢复3D点，视差角大时（离相机近）用双目恢复3D点（双目以及深度有效）
             if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 ||
                (cosParallaxRays<0.9998 && mbInertial) || (cosParallaxRays<0.9998 && !mbInertial)))
             {
                 // Linear Triangulation Method
+                // 见Initializer.cpp的Triangulate函数,A矩阵构建的方式类似，不同的是乘的反对称矩阵那个是像素坐标构成的，而这个是归一化坐标构成的
+                // Pc = Tcw*Pw， 此处Tcw行数默认为3，因为不会计算第4行所以没有去掉，
+                // 左右两面乘Pc的反对称矩阵 [Pc]x * Tcw *Pw = 0 构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度
+                //  0 -1 y    Tcw.row(0)     -Tcw.row(1) + y*Tcw.row(2)
+                //  1 0 -x *  Tcw.row(1)  =   Tcw.row(0) - x*Tcw.row(2)
+                // -y x  0    Tcw.row(2)    x*Tcw.row(1) - y*Tcw.row(0)
+                // 发现上述矩阵线性相关，所以取前两维，两个点构成了4行的矩阵，就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式
                 cv::Matx14f A_r0 = xn1(0) * Tcw1.row(2) - Tcw1.row(0);
                 cv::Matx14f A_r1 = xn1(1) * Tcw1.row(2) - Tcw1.row(1);
                 cv::Matx14f A_r2 = xn2(0) * Tcw2.row(2) - Tcw2.row(0);
@@ -1044,7 +1070,12 @@ void LocalMapping::SearchInNeighbors()
     mpCurrentKeyFrame->UpdateConnections();
 }
 
-// 根据两关键帧的姿态计算两个关键帧之间的基本矩阵
+/**
+ * @brief 根据两关键帧的姿态计算两个关键帧之间的基本矩阵
+ * @param  pKF1 关键帧1
+ * @param  pKF2 关键帧2
+ * @return      基本矩阵
+ */
 cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
 {
     // 先构造两帧之间的R12,t12
@@ -1139,7 +1170,9 @@ void LocalMapping::Release()
     cout << "Local Mapping RELEASE" << endl;
 }
 
-// 查看当前是否允许接受关键帧
+/**
+ * @brief 查看是否接收关键帧，也就是当前线程是否在处理数据，当然tracking线程也不会全看这个值，他会根据队列阻塞情况
+ */
 bool LocalMapping::AcceptKeyFrames()
 {
     unique_lock<mutex> lock(mMutexAccept);
@@ -1153,7 +1186,9 @@ void LocalMapping::SetAcceptKeyFrames(bool flag)
     mbAcceptKeyFrames=flag;
 }
 
-// 设置 mbnotStop标志的状态
+/**
+ * @brief 如果不让它暂停，即使发出了暂停信号也不暂停
+ */
 bool LocalMapping::SetNotStop(bool flag)
 {
     unique_lock<mutex> lock(mMutexStop);
@@ -1166,7 +1201,9 @@ bool LocalMapping::SetNotStop(bool flag)
     return true;
 }
 
-// 终止BA
+/**
+ * @brief 放弃这次BA
+ */
 void LocalMapping::InterruptBA()
 {
     mbAbortBA = true;
@@ -1192,15 +1229,17 @@ void LocalMapping::KeyFrameCulling()
     // scaleLeveli：pKFi的金字塔尺度
     // scaleLevel：pKF的金字塔尺度
     const int Nd = 21; // MODIFICATION_STEREO_IMU 20 This should be the same than that one from LIBA
-    mpCurrentKeyFrame->UpdateBestCovisibles();
+    mpCurrentKeyFrame->UpdateBestCovisibles();		// 更新共视关系
+    // 1. 根据Covisibility Graph提取当前帧的共视关键帧
     vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
 
     float redundant_th;
+    // 非IMU时
     if(!mbInertial)
         redundant_th = 0.9;
-    else if (mbMonocular)
+    else if (mbMonocular)	// imu 且单目时
         redundant_th = 0.9;
-    else
+    else	// 其他imu时
         redundant_th = 0.5;
 
     const bool bInitImu = mpAtlas->isImuInitialized();
@@ -1212,6 +1251,7 @@ void LocalMapping::KeyFrameCulling()
     {
         int count = 0;
         KeyFrame* aux_KF = mpCurrentKeyFrame;
+        // 找到第前21个关键帧的关键帧id
         while(count<Nd && aux_KF->mPrevKF)
         {
             aux_KF = aux_KF->mPrevKF;
@@ -1306,18 +1346,22 @@ void LocalMapping::KeyFrameCulling()
 		// Step 4：该关键帧90%以上的有效地图点被判断为冗余的，则删除该关键帧
         if(nRedundantObservations>redundant_th*nMPs)
         {
+            // imu模式下需要更改前后关键帧的连续性，且预积分要叠加起来
             if (mbInertial)
             {
+                // 关键帧少于Nd个，跳过不删
                 if (mpAtlas->KeyFramesInMap()<=Nd)
                     continue;
-
+                // 关键帧与当前关键帧id差一个，跳过不删
                 if(pKF->mnId>(mpCurrentKeyFrame->mnId-2))
                     continue;
-
+                // 关键帧具有前后关键帧
                 if(pKF->mPrevKF && pKF->mNextKF)
                 {
                     const float t = pKF->mNextKF->mTimeStamp-pKF->mPrevKF->mTimeStamp;
-
+                    // 下面两个括号里的内容一模一样
+                    // imu初始化了，且距当前帧的ID超过21，且前后两个关键帧时间间隔小于3s
+                    // 或者时间间隔小于0.5s
                     if((bInitImu && (pKF->mnId<last_ID) && t<3.) || (t<0.5))
                     {
                         pKF->mNextKF->mpImuPreintegrated->MergePrevious(pKF->mpImuPreintegrated);
@@ -1327,6 +1371,7 @@ void LocalMapping::KeyFrameCulling()
                         pKF->mPrevKF = NULL;
                         pKF->SetBadFlag();
                     }
+                    // 没经过imu初始化的第三阶段，且关键帧与其前一个关键帧的距离小于0.02m，且前后两个关键帧时间间隔小于3s
                     else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && (cv::norm(pKF->GetImuPosition()-pKF->mPrevKF->GetImuPosition())<0.02) && (t<3))
                     {
                         pKF->mNextKF->mpImuPreintegrated->MergePrevious(pKF->mpImuPreintegrated);
@@ -1338,11 +1383,13 @@ void LocalMapping::KeyFrameCulling()
                     }
                 }
             }
+            // 非imu就没那么多事儿了，直接干掉
             else
             {
                 pKF->SetBadFlag();
             }
         }
+        // 遍历共视关键帧个数超过一定，就不弄了
         if((count > 20 && mbAbortBA) || count>100) // MODIFICATION originally 20 for mbabortBA check just 10 keyframes
         {
             break;
@@ -1350,7 +1397,11 @@ void LocalMapping::KeyFrameCulling()
     }
 }
 
-// 计算三维向量v的反对称矩阵
+/**
+ * @brief 返回反对称矩阵
+ * @param v 三维向量
+ * @return v的反对称矩阵
+ */
 cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
 {
     return (cv::Mat_<float>(3,3) <<             0, -v.at<float>(2), v.at<float>(1),
@@ -1388,6 +1439,9 @@ void LocalMapping::RequestReset()
     cout << "LM: Map reset, Done!!!" << endl;
 }
 
+/**
+ * @brief 接收重置当前地图的信号
+ */
 void LocalMapping::RequestResetActiveMap(Map* pMap)
 {
     {
@@ -1465,14 +1519,18 @@ void LocalMapping::RequestFinish()
     mbFinishRequested = true;
 }
 
-// 检查是否已经有外部线程请求终止当前线程
+/**
+ * @brief 查看完成信号，跳出while循环
+ */
 bool LocalMapping::CheckFinish()
 {
     unique_lock<mutex> lock(mMutexFinish);
     return mbFinishRequested;
 }
 
-// 设置当前线程已经真正地结束了
+/**
+ * @brief 设置当前线程已经真正地结束了
+ */
 void LocalMapping::SetFinish()
 {
     unique_lock<mutex> lock(mMutexFinish);
@@ -1481,20 +1539,30 @@ void LocalMapping::SetFinish()
     mbStopped = true;
 }
 
-// 当前线程的run函数是否已经终止
+/**
+ * @brief 当前线程的run函数是否已经终止
+ */
 bool LocalMapping::isFinished()
 {
     unique_lock<mutex> lock(mMutexFinish);
     return mbFinished;
 }
 
+/** 
+ * @brief imu初始化
+ * @param priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用
+ * @param priorA 加速度计偏置的信息矩阵系数
+ * @param bFIBA 是否做BA优化
+ */
 void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
 {
+    // 1. 将所有关键帧放入列表及向量里，且查看是否满足初始化条件
     if (mbResetRequested)
         return;
 
     float minTime;
     int nMinKF;
+    // 从时间及帧数上限制初始化，不满足下面的不进行初始化
     if (mbMonocular)
     {
         minTime = 2.0;
@@ -1506,11 +1574,12 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
         nMinKF = 10;
     }
 
-
+    // 当前地图大于10帧才进行初始化
     if(mpAtlas->KeyFramesInMap()<nMinKF)
         return;
 
     // Retrieve all keyframe in temporal order
+    // 按照顺序存放目前地图里的关键帧，顺序按照前后顺序来，包括当前关键帧
     list<KeyFrame*> lpKF;
     KeyFrame* pKF = mpCurrentKeyFrame;
     while(pKF->mPrevKF)
@@ -1519,8 +1588,10 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
         pKF = pKF->mPrevKF;
     }
     lpKF.push_front(pKF);
+    // 以相同内容再构建一个vector
     vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
 
+    // TODO 跟上面重复？
     if(vpKF.size()<nMinKF)
         return;
 
@@ -1530,17 +1601,19 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
 
     bInitializing = true;
 
+    // 先处理新关键帧，防止堆积且保证数据量充足
     while(CheckNewKeyFrames())
     {
         ProcessNewKeyFrame();
         vpKF.push_back(mpCurrentKeyFrame);
         lpKF.push_back(mpCurrentKeyFrame);
     }
-
+    // 2. 正式IMU初始化
     const int N = vpKF.size();
     IMU::Bias b(0,0,0,0,0,0);
 
     // Compute and KF velocities mRwg estimation
+    // 在IMU没有初始化情况下
     if (!mpCurrentKeyFrame->GetMap()->isImuInitialized())
     {
         cv::Mat cvRwg;
@@ -1551,20 +1624,27 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
                 continue;
             if (!(*itKF)->mPrevKF)
                 continue;
-
+            // Rwb（imu坐标转到初始化前世界坐标系下的坐标）*更新偏置后的速度，可以理解为在世界坐标系下的速度矢量
             dirG -= (*itKF)->mPrevKF->GetImuRotation()*(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
+            // 求取实际的速度，位移/时间
             cv::Mat _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT;
             (*itKF)->SetVelocity(_vel);
             (*itKF)->mPrevKF->SetVelocity(_vel);
         }
-
+        // 归一化
         dirG = dirG/cv::norm(dirG);
+        // 原本的重力方向
         cv::Mat gI = (cv::Mat_<float>(3,1) << 0.0f, 0.0f, -1.0f);
+        // 求速度方向与重力方向的角轴
         cv::Mat v = gI.cross(dirG);
+        // 求角轴长度
         const float nv = cv::norm(v);
+        // 求转角大小
         const float cosg = gI.dot(dirG);
         const float ang = acos(cosg);
-        cv::Mat vzg = v*ang/nv;
+        // 先计算旋转向量，在除去角轴大小
+        cv::Mat vzg = v * ang / nv;
+        // 获得重力方向到当前速度方向的旋转向量
         cvRwg = IMU::ExpSO3(vzg);
         mRwg = Converter::toMatrix3d(cvRwg);
         mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs;
@@ -1578,9 +1658,11 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
 
     mScale=1.0;
 
+    // 暂时没发现在别的地方出现过
     mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp;
 
     std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+    // 计算残差及偏置差，优化尺度重力方向及Ri Rj Vi Vj Pi Pj
     Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
@@ -1588,7 +1670,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     cout << "bg after inertial-only optimization: " << mbg << endl;
     cout << "ba after inertial-only optimization: " << mba << endl;*/
 
-
+    // 尺度太小的话初始化认为失败
     if (mScale<1e-1)
     {
         cout << "scale too small" << endl;
@@ -1596,20 +1678,26 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
         return;
     }
 
-
+    // 到此时为止，前面做的东西没有改变map
 
     // Before this line we are not changing the map
 
     unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
     std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+    // 尺度变化超过设定值，或者非单目时（无论带不带imu，但这个函数只在带imu时才执行，所以这个可以理解为双目imu）
     if ((fabs(mScale-1.f)>0.00001)||!mbMonocular)
     {
+        // 恢复重力方向与尺度信息
         mpAtlas->GetCurrentMap()->ApplyScaledRotation(Converter::toCvMat(mRwg).t(),mScale,true);
+        // 更新普通帧的位姿，主要是当前帧与上一帧
         mpTracker->UpdateFrameIMU(mScale,vpKF[0]->GetImuBias(),mpCurrentKeyFrame);
     }
     std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
 
     // Check if initialization OK
+    // 即使初始化成功后面还会执行这个函数重新初始化
+    // 在之前没有初始化成功情况下（此时刚刚初始化成功）对每一帧都标记，后面的kf全部都在tracking里面标记为true
+    // 也就是初始化之前的那些关键帧即使有imu信息也不算
     if (!mpAtlas->isImuInitialized())
         for(int i=0;i<N;i++)
         {
@@ -1622,8 +1710,10 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     cout << "bg: " << mpCurrentKeyFrame->GetGyroBias() << endl;*/
 
     std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
+    // 代码里都为true
     if (bFIBA)
     {
+        // 承接上一步纯imu优化，按照之前的结果更新了尺度信息及适应重力方向，所以要结合地图进行一次视觉加imu的全局优化，这次带了MP等信息
         if (priorA!=0.f)
             Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, 0, NULL, true, priorG, priorA);
         else
@@ -1637,6 +1727,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     if (!mpAtlas->isImuInitialized())
     {
         cout << "IMU in Map " << mpAtlas->GetCurrentMap()->GetId() << " is initialized" << endl;
+        // ! 重要！标记初始化成功
         mpAtlas->SetImuInitialized();
         mpTracker->t0IMU = mpTracker->mCurrentFrame.mTimeStamp;
         mpCurrentKeyFrame->bImu = true;
@@ -1671,6 +1762,9 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     return;
 }
 
+/**
+ * @brief 通过BA优化进行尺度更新，关键帧小于100，在这里的时间段内时多次进行尺度更新
+ */
 void LocalMapping::ScaleRefinement()
 {
     // Minimum number of keyframes to compute a solution
@@ -1680,6 +1774,7 @@ void LocalMapping::ScaleRefinement()
         return;
 
     // Retrieve all keyframes in temporal order
+    // 1. 检索所有的关键帧（当前地图）
     list<KeyFrame*> lpKF;
     KeyFrame* pKF = mpCurrentKeyFrame;
     while(pKF->mPrevKF)
@@ -1689,7 +1784,7 @@ void LocalMapping::ScaleRefinement()
     }
     lpKF.push_front(pKF);
     vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
-
+    // 加入新添加的帧
     while(CheckNewKeyFrames())
     {
         ProcessNewKeyFrame();
@@ -1698,11 +1793,12 @@ void LocalMapping::ScaleRefinement()
     }
 
     const int N = vpKF.size();
-
+    // 2. 更新旋转与尺度
     mRwg = Eigen::Matrix3d::Identity();
     mScale=1.0;
 
     std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+    // 优化重力方向与尺度
     Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale);
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
@@ -1714,8 +1810,10 @@ void LocalMapping::ScaleRefinement()
     }
 
     // Before this line we are not changing the map
+    // 3. 开始更新地图
     unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
     std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+    // 3.1 如果尺度更新较多，或是在双目imu情况下更新地图
     if ((fabs(mScale-1.f)>0.00001)||!mbMonocular)
     {
         mpAtlas->GetCurrentMap()->ApplyScaledRotation(Converter::toCvMat(mRwg).t(),mScale,true);
@@ -1723,6 +1821,7 @@ void LocalMapping::ScaleRefinement()
     }
     std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
 
+    // 3.2 优化的这段时间新进来的kf全部清空不要
     for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
     {
         (*lit)->SetBadFlag();
@@ -1738,14 +1837,17 @@ void LocalMapping::ScaleRefinement()
     return;
 }
 
-
+/**
-
+ * @brief 返回是否正在做IMU的初始化，在tracking里面使用，如果为true，暂不添加关键帧
+ */
 bool LocalMapping::IsInitializing()
 {
     return bInitializing;
 }
 
-
+/**
+ * @brief 获取当前关键帧的时间戳，System::GetTimeFromIMUInit()中调用
+ */
 double LocalMapping::GetCurrKFTime()
 {
 
@@ -1757,9 +1859,12 @@ double LocalMapping::GetCurrKFTime()
         return 0.0;
 }
 
-KeyFrame* LocalMapping::GetCurrKF()
+/**
+ * @brief 获取当前关键帧
+ */
+KeyFrame *LocalMapping::GetCurrKF()
 {
     return mpCurrentKeyFrame;
 }
 
-} //namespace ORB_SLAM
+} // namespace ORB_SLAM3

src/MapPoint.cc

@@ -27,7 +27,9 @@ namespace ORB_SLAM3
 long unsigned int MapPoint::nNextId=0;
 mutex MapPoint::mGlobalMutex;
 
-MapPoint::MapPoint():
+/** 
+ * @brief 构造函数
+ */
     mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0),
     mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
     mnCorrectedReference(0), mnBAGlobalForKF(0), mnVisible(1), mnFound(1), mbBad(false),
@@ -36,6 +38,9 @@ MapPoint::MapPoint():
     mpReplaced = static_cast<MapPoint*>(NULL);
 }
 
+/** 
+ * @brief 构造函数
+ */
 MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
     mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
     mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
@@ -56,6 +61,9 @@ MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
     mnId=nNextId++;
 }
 
+/** 
+ * @brief 构造函数
+ */
 MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap):
     mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
     mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
@@ -77,6 +85,9 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
     mnId=nNextId++;
 }
 
+/** 
+ * @brief 构造函数
+ */
 MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF):
     mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0),
     mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0),
@@ -120,6 +131,10 @@ MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF
     mnId=nNextId++;
 }
 
+/**
+ * @brief 设定该mp的世界坐标
+ * @param Pos         坐标值
+ */
 void MapPoint::SetWorldPos(const cv::Mat &Pos)
 {
     unique_lock<mutex> lock2(mGlobalMutex);
@@ -128,12 +143,18 @@ void MapPoint::SetWorldPos(const cv::Mat &Pos)
     mWorldPosx = cv::Matx31f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2));
 }
 
+/**
+ * @brief 返回该mp的世界坐标
+ */
 cv::Mat MapPoint::GetWorldPos()
 {
     unique_lock<mutex> lock(mMutexPos);
     return mWorldPos.clone();
 }
 
+/**
+ * @brief 获取平均观测方向
+ */
 cv::Mat MapPoint::GetNormal()
 {
     unique_lock<mutex> lock(mMutexPos);
@@ -242,7 +263,10 @@ std::map<KeyFrame*, std::tuple<int,int>>  MapPoint::GetObservations()
     return mObservations;
 }
 
-// 被观测到的相机数目，单目+1，双目或RGB-D则+2
+/**
+ * @brief 返回被观测次数，双目一帧算两次，左右目各算各的
+ * @return nObs
+ */
 int MapPoint::Observations()
 {
     unique_lock<mutex> lock(mMutexFeatures);
@@ -280,6 +304,10 @@ void MapPoint::SetBadFlag()
     mpMap->EraseMapPoint(this);
 }
 
+/**
+ * @brief 判断该点是否已经被替换，因为替换并没有考虑普通帧的替换，不利于下一帧的跟踪，所以要坐下标记
+ * @return 替换的新的点
+ */
 MapPoint* MapPoint::GetReplaced()
 {
     unique_lock<mutex> lock1(mMutexFeatures);
@@ -329,9 +357,10 @@ void MapPoint::Replace(MapPoint* pMP)
 
         tuple<int,int> indexes = mit -> second;
         int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
-
+        // 2.1 判断新点是否已经在pKF里面
         if(!pMP->IsInKeyFrame(pKF))
         {
+            // 如果不在，替换特征点与mp的匹配关系
             if(leftIndex != -1){
                 pKF->ReplaceMapPointMatch(leftIndex, pMP);
                 pMP->AddObservation(pKF,leftIndex);
@@ -341,6 +370,8 @@ void MapPoint::Replace(MapPoint* pMP)
                 pMP->AddObservation(pKF,rightIndex);
             }
         }
+        // 如果新的MP在之前MP对应的关键帧里面，就撞车了。
+        // 本来目的想新旧MP融为一个，这样以来一个点有可能对应两个特征点，这样是决不允许的，所以删除旧的，不动新的
         else
         {
             if(leftIndex != -1){
@@ -361,6 +392,9 @@ void MapPoint::Replace(MapPoint* pMP)
     mpMap->EraseMapPoint(this);
 }
 
+/**
+ * @brief 判断这个点是否设置为bad了
+ */
 bool MapPoint::isBad()
 {
     unique_lock<mutex> lock1(mMutexFeatures,std::defer_lock);
@@ -370,19 +404,37 @@ bool MapPoint::isBad()
     return mbBad;
 }
 
+/**
+ * @brief Increase Visible
+ *
+ * Visible表示：
+ * 1. 该MapPoint在某些帧的视野范围内，通过Frame::isInFrustum()函数判断
+ * 2. 该MapPoint被这些帧观测到，但并不一定能和这些帧的特征点匹配上
+ *    例如：有一个MapPoint（记为M），在某一帧F的视野范围内，
+ *    但并不表明该点M可以和F这一帧的某个特征点能匹配上
+ */
 void MapPoint::IncreaseVisible(int n)
 {
     unique_lock<mutex> lock(mMutexFeatures);
     mnVisible+=n;
 }
 
+/**
+ * @brief Increase Found
+ *
+ * 能找到该点的帧数+n，n默认为1
+ * @see Tracking::TrackLocalMap()
+ */
 void MapPoint::IncreaseFound(int n)
 {
     unique_lock<mutex> lock(mMutexFeatures);
     mnFound+=n;
 }
 
-// 计算被找到的比例
+/**
+ * @brief 返回被找到/被看到
+ * @return 被找到/被看到
+ */
 float MapPoint::GetFoundRatio()
 {
     unique_lock<mutex> lock(mMutexFeatures);
@@ -478,18 +530,25 @@ void MapPoint::ComputeDistinctiveDescriptors()
 
     {
         unique_lock<mutex> lock(mMutexFeatures);
+        // 最好的描述子，该描述子相对于其他描述子有最小的距离中值
+        // 简化来讲，中值代表了这个描述子到其它描述子的平均距离
+        // 最好的描述子就是和其它描述子的平均距离最小
         mDescriptor = vDescriptors[BestIdx].clone();
     }
 }
 
-//获取当前地图点的描述子
+/**
+ * @brief 返回描述子
+ */
 cv::Mat MapPoint::GetDescriptor()
 {
     unique_lock<mutex> lock(mMutexFeatures);
     return mDescriptor.clone();
 }
 
-//获取当前地图点在某个关键帧的观测中，对应的特征点的ID
+/**
+ * @brief 返回这个点在关键帧中对应的特征点id
+ */
 tuple<int,int> MapPoint::GetIndexInKeyFrame(KeyFrame *pKF)
 {
     unique_lock<mutex> lock(mMutexFeatures);
@@ -499,6 +558,9 @@ tuple<int,int> MapPoint::GetIndexInKeyFrame(KeyFrame *pKF)
         return tuple<int,int>(-1,-1);
 }
 
+/**
+ * @brief return (mObservations.count(pKF));
+ */
 bool MapPoint::IsInKeyFrame(KeyFrame *pKF)
 {
     unique_lock<mutex> lock(mMutexFeatures);
@@ -589,6 +651,10 @@ void MapPoint::UpdateNormalAndDepth()
     }
 }
 
+/**
+ * @brief 设置平均观测方向
+ * @param normal         观测方向
+ */
 void MapPoint::SetNormalVector(cv::Mat& normal)
 {
     unique_lock<mutex> lock3(mMutexPos);
@@ -596,12 +662,18 @@ void MapPoint::SetNormalVector(cv::Mat& normal)
     mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
 }
 
+/**
+ * @brief 返回最近距离
+ */
 float MapPoint::GetMinDistanceInvariance()
 {
     unique_lock<mutex> lock(mMutexPos);
     return 0.8f*mfMinDistance;
 }
 
+/**
+ * @brief 返回最远距离
+ */
 float MapPoint::GetMaxDistanceInvariance()
 {
     unique_lock<mutex> lock(mMutexPos);

src/ORBextractor.cc

@@ -1640,7 +1640,7 @@ static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Ma
 					// 特征点本身直接乘缩放倍数就可以了
                     keypoint->pt *= scale;
                 }
-
+                // ?TODO vLappingArea 
                 if(keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]){
                     _keypoints.at(stereoIndex) = (*keypoint);
                     desc.row(i).copyTo(descriptors.row(stereoIndex));

src/OptimizableTypes.cpp

@@ -45,7 +45,9 @@ namespace ORB_SLAM3 {
         return os.good();
     }
 
-
+/** 
+ * @brief 求解二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXi
+ */
     void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
         g2o::VertexSE3Expmap * vi = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
         Eigen::Vector3d xyz_trans = vi->estimate().map(Xw);
@@ -88,7 +90,11 @@ namespace ORB_SLAM3 {
         return os.good();
     }
 
+/** 
+ * @brief 求解右目的二维像素坐标关于左目位姿的雅克比矩阵 _jacobianOplusXi
+ */
     void EdgeSE3ProjectXYZOnlyPoseToBody::linearizeOplus() {
+    // 获得三维点在右相机坐标系下的坐标
         g2o::VertexSE3Expmap * vi = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
         g2o::SE3Quat T_lw(vi->estimate());
         Eigen::Vector3d X_l = T_lw.map(Xw);
@@ -103,6 +109,12 @@ namespace ORB_SLAM3 {
                 -z_w , 0.f, x_w, 0.f, 1.f, 0.f,
                 y_w ,  -x_w , 0.f, 0.f, 0.f, 1.f;
 
+    /*
+        注意这里是对李代数求导，ρlw != tlw 所以不能使用Pl = Rlw*Pw + tlw
+        Pl = EXP(ξlw)*Pw    Pr = Rrl * EXP(ξlw) * Pw + trl  
+        让Pr 对 ξlw 求雅克比
+        相当于Rrl*(Pl 对 ξlw的雅克比)
+     */
         _jacobianOplusXi = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
     }
 
@@ -135,7 +147,9 @@ namespace ORB_SLAM3 {
         return os.good();
     }
 
-
+/** 
+ * @brief 求解二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXj  二维像素坐标关于三维点世界坐标的雅克比矩阵 _jacobianOplusXi  
+ */
     void EdgeSE3ProjectXYZ::linearizeOplus() {
         g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
         g2o::SE3Quat T(vj->estimate());
@@ -148,7 +162,7 @@ namespace ORB_SLAM3 {
         double z = xyz_trans[2];
 
         auto projectJac = -pCamera->projectJac(xyz_trans);
-
+    // Pc = Rcw*Pw + tcw  先求Pw改变对Pc的影响，所以直接为Rcw，前面再乘Pc对像素的影响
         _jacobianOplusXi =  projectJac * T.rotation().toRotationMatrix();
 
         Eigen::Matrix<double,3,6> SE3deriv;
@@ -188,7 +202,9 @@ namespace ORB_SLAM3 {
         return os.good();
     }
 
-
+/** 
+ * @brief 求解右目二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXj  右目二维像素坐标关于三维点世界坐标的雅克比矩阵 _jacobianOplusXi  
+ */
     void EdgeSE3ProjectXYZToBody::linearizeOplus() {
         g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
         g2o::SE3Quat T_lw(vj->estimate());
@@ -208,7 +224,12 @@ namespace ORB_SLAM3 {
         SE3deriv << 0.f, z,   -y, 1.f, 0.f, 0.f,
                 -z , 0.f, x, 0.f, 1.f, 0.f,
                 y ,  -x , 0.f, 0.f, 0.f, 1.f;
-
+    /*
+        注意这里是对李代数求导，ρlw != tlw 所以不能使用Pl = Rlw*Pw + tlw
+        Pl = EXP(ξlw)*Pw    Pr = Rrl * EXP(ξlw) * Pw + trl  
+        让Pr 对 ξlw 求雅克比
+        相当于Rrl*(Pl 对 ξlw的雅克比)
+     */
         _jacobianOplusXj = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
     }
 

src/System.cc

@@ -114,7 +114,7 @@ System::System(const string &strVocFile,                //词袋文件所在路
     mpAtlas = new Atlas(0);
 
     if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR)
-        // ? 如果是有imu的传感器类型，将mbIsInertial设置为imu属性,以后的跟踪和预积分将和这个标志有关
+        // 如果是有imu的传感器类型，设置mbIsInertial = true;以后的跟踪和预积分将和这个标志有关
         mpAtlas->SetInertialSensor();
 
     // Step 6 依次创建跟踪、局部建图、闭环、显示线程
@@ -175,6 +175,7 @@ System::System(const string &strVocFile,                //词袋文件所在路
     mpLoopCloser->SetLocalMapper(mpLocalMapper);
 
     // Fix verbosity
+    // 打印输出中间的信息，设置为安静模式
     Verbose::SetTh(Verbose::VERBOSITY_QUIET);
 
 }
@@ -354,7 +355,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
             mbReset = false;
             mbResetActiveMap = false;
         }
-        //如果检测到重置活动地图,讲重置地图设置
+        //如果检测到重置活动地图的标志为true,将重置地图
         else if(mbResetActiveMap)
         {
             cout << "SYSTEM-> Reseting active map in monocular case" << endl;
@@ -362,14 +363,14 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
             mbResetActiveMap = false;
         }
     }
-    // 如果是单目VIO模式，把IMU数据存储到mlQueueImuData
+    // 如果是单目VIO模式，把IMU数据存储到队列mlQueueImuData
     if (mSensor == System::IMU_MONOCULAR)
         for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
             mpTracker->GrabImuData(vImuMeas[i_imu]);
 
     // 计算相机位姿
     cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp,filename);
-
+    // 更新跟踪状态和参数
     unique_lock<mutex> lock2(mMutexState);
     mTrackingState = mpTracker->mState;
     mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;