update
electech6
parent
feedb9bdd8
commit
d10c678558
|
|
@ -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]++;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
#ifndef CONFIG_H
|
||||
#define CONFIG_H
|
||||
|
||||
//#define REGISTER_TIMES
|
||||
|
||||
#endif // CONFIG_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
|
||||
|
|
|
|||
|
|
@ -280,6 +280,6 @@ cv::Mat NormalizeRotation(const cv::Mat &R);
|
|||
|
||||
}
|
||||
|
||||
} //namespace ORB_SLAM2
|
||||
} //namespace ORB_SLAM3
|
||||
|
||||
#endif // IMUTYPES_H
|
||||
|
|
|
|||
|
|
@ -43,9 +43,11 @@
|
|||
namespace ORB_SLAM3
|
||||
{
|
||||
|
||||
// 打印中间信息
|
||||
class Verbose
|
||||
{
|
||||
public:
|
||||
// 显示信息量程度
|
||||
enum eLevel
|
||||
{
|
||||
VERBOSITY_QUIET=0,
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
16
src/Atlas.cc
16
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->SetCurrentMap();
|
||||
mspMaps.insert(mpCurrentMap);
|
||||
mpCurrentMap = new Map(mnLastInitKFidMap); //新建地图
|
||||
mpCurrentMap->SetCurrentMap(); //设置为活跃地图
|
||||
mspMaps.insert(mpCurrentMap); //插入地图集
|
||||
}
|
||||
|
||||
void Atlas::ChangeMap(Map* pMap)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
130
src/G2oTypes.cc
130
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 c=0;c<9;c++)
|
||||
Info(r,c)=cvInfo.at<float>(r,c);
|
||||
Info = (Info+Info.transpose())/2;
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
|
||||
Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
|
||||
for(int i=0;i<9;i++)
|
||||
if(eigs[i]<1e-12)
|
||||
eigs[i]=0;
|
||||
Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
|
||||
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(); // 矩阵特征值
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @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 Rbw1 = Rwb1.transpose();
|
||||
const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;
|
||||
const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb; // Ri
|
||||
const Eigen::Matrix3d Rbw1 = Rwb1.transpose(); // Ri.t()
|
||||
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::Vector3d er = LogSO3(eR);
|
||||
const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);
|
||||
const Eigen::Matrix3d eR = dR.transpose() * Rbw1 * Rwb2; // r△Rij
|
||||
const Eigen::Vector3d er = LogSO3(eR); // r△φij
|
||||
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 Rbw1 = Rwb1.transpose();
|
||||
const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;
|
||||
const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg;
|
||||
Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3,2);
|
||||
Gm(0,1) = -IMU::GRAVITY_VALUE;
|
||||
Gm(1,0) = IMU::GRAVITY_VALUE;
|
||||
const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb; // Ri
|
||||
const Eigen::Matrix3d Rbw1 = Rwb1.transpose(); // Ri.t()
|
||||
const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb; // Rj
|
||||
const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg; // Rwg
|
||||
Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3, 2);
|
||||
Gm(0, 1) = -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::Vector3d er = LogSO3(eR);
|
||||
const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);
|
||||
const Eigen::Matrix3d eR = dR.transpose() * Rbw1 * Rwb2; // r△Rij
|
||||
const Eigen::Vector3d er = LogSO3(eR); // r△φij
|
||||
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);
|
||||
_jacobianOplus[0].block<3,3>(3,3) = Rwb.transpose()*VP->estimate().Rwb;
|
||||
// LOG(Rbw*R*EXP(φ)) = LOG(EXP(LOG(Rbw*R) + Jr(-1)*φ)) = LOG(Rbw*R) + Jr(-1)*φ
|
||||
_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;
|
||||
|
|
|
|||
203
src/ImuTypes.cc
203
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);
|
||||
dV = cv::Mat::zeros(3,1,CV_32F);
|
||||
dP = cv::Mat::zeros(3,1,CV_32F);
|
||||
JRg = cv::Mat::zeros(3,3,CV_32F);
|
||||
JVg = cv::Mat::zeros(3,3,CV_32F);
|
||||
JVa = cv::Mat::zeros(3,3,CV_32F);
|
||||
JPg = cv::Mat::zeros(3,3,CV_32F);
|
||||
JPa = cv::Mat::zeros(3,3,CV_32F);
|
||||
C = cv::Mat::zeros(15,15,CV_32F);
|
||||
Info=cv::Mat();
|
||||
db = cv::Mat::zeros(6,1,CV_32F);
|
||||
b=b_;
|
||||
bu=b_;
|
||||
avgA = cv::Mat::zeros(3,1,CV_32F);
|
||||
avgW = cv::Mat::zeros(3,1,CV_32F);
|
||||
dT=0.0f;
|
||||
mvMeasurements.clear();
|
||||
dR = cv::Mat::eye(3, 3, CV_32F);
|
||||
dV = cv::Mat::zeros(3, 1, CV_32F);
|
||||
dP = cv::Mat::zeros(3, 1, CV_32F);
|
||||
JRg = cv::Mat::zeros(3, 3, CV_32F);
|
||||
JVg = cv::Mat::zeros(3, 3, CV_32F);
|
||||
JVa = cv::Mat::zeros(3, 3, CV_32F);
|
||||
JPg = cv::Mat::zeros(3, 3, CV_32F);
|
||||
JPa = cv::Mat::zeros(3, 3, CV_32F);
|
||||
C = cv::Mat::zeros(15, 15, CV_32F);
|
||||
Info = cv::Mat();
|
||||
db = cv::Mat::zeros(6, 1, CV_32F);
|
||||
b = b_;
|
||||
bu = b_; // 更新后的偏置
|
||||
avgA = cv::Mat::zeros(3, 1, CV_32F); // 平均加速度
|
||||
avgW = cv::Mat::zeros(3, 1, CV_32F); // 平均角速度
|
||||
dT = 0.0f;
|
||||
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;
|
||||
dV = dV + dR*acc*dt;
|
||||
dP = dP + dV*dt + 0.5f*dR*acc*dt*dt; // 对应viorb论文的公式（2）的第三个，位移积分
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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++;
|
||||
|
|
|
|||
|
|
@ -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,9 +273,9 @@ void LocalMapping::Run()
|
|||
}
|
||||
|
||||
// scale refinement
|
||||
// Step 9.3 尺度优化
|
||||
// Step 9.3 在关键帧小于100时，会在满足一定时间间隔后多次进行尺度、重力方向优化
|
||||
if (((mpAtlas->KeyFramesInMap())<=100) &&
|
||||
((mTinit>25.0f && mTinit<25.5f)||
|
||||
((mTinit>25.0f && mTinit<25.5f)||
|
||||
(mTinit>35.0f && mTinit<35.5f)||
|
||||
(mTinit>45.0f && mTinit<45.5f)||
|
||||
(mTinit>55.0f && mTinit<55.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
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
1021
src/MLPnPsolver.cpp
1021
src/MLPnPsolver.cpp
File diff suppressed because it is too large
Load Diff
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
700
src/Optimizer.cc
700
src/Optimizer.cc
File diff suppressed because it is too large
Load Diff
|
|
@ -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 ×tamp, 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 ×tamp, 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;
|
||||
|
|
|
|||
635
src/Tracking.cc
635
src/Tracking.cc
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue