diff --git a/docs/en/benchmark.md b/docs/en/benchmark.md index 35fedba..66229ab 100644 --- a/docs/en/benchmark.md +++ b/docs/en/benchmark.md @@ -2,15 +2,15 @@ We run our algorithm on EuRoC dataset on Ubuntu18.04 and macOS 10.14. And make comparisons with the state-of-the-art VIO systems. We analyze the accuracy of the algorithm by comparing the root mean squared error (RMSE) of the absolute trajectory error (ATE). ATE is given by the simple difference between the estimated trajectory and ground truth after it has been aligned so that it has a minimal error, and RMSE is the standard deviation of the residuals (prediction errors). The lower the RMSE, the better a given trajectory is able to fit its ground truth. We use "evo" tool to evaluate and compare the trajectory output of odometry and SLAM algorithms, and for more information please refer to [euroc evaluation](./tutorials/euroc_evaluation.md). We have achieved obvious better results on EuRoc and ADVIO datasets, which proves the effectiveness of our system. -**SF-VIO** completely disables the dynamic object removal strategy in XRSLAM, which can be enabled in the configuration *parsac_flag*. When the flag activated, it is referred to as **RD-VIO**. As shown in the following tables, the best results for visual-inertial algorithms are bolded. Comparing with other systems, SF-VIO showed significant improvements on many sequences on EuRoC dataset. Thanks to the additional stabilization effect, the significant drifts are canceled when using the subframe strategy in our system. +**XRSLAM** showed significant improvements on many sequences on EuRoC dataset. **XRSLAM(w. RD)** adopts a dynamic object removal strategy, which can be enabled in the configuration parameter `parsac_flag`. As shown in the following tables, the best results for visual-inertial algorithms are bolded. Comparing with other systems, Thanks to the additional stabilization effect, the significant drifts are canceled when using the subframe strategy in our system. -As a challenging dataset in real-world settings, ADVIO offers 23 diverse scenarios, encompassing indoor and outdoor environments, varying lighting conditions, and dynamic elements such as pedestrians and vehicles. Compared to SF-VIO without dynamic object removal strategies, RD-VIO showed significantly better RMSEs on ADVIO dataset. +As a challenging dataset in real-world settings, ADVIO offers 23 diverse scenarios, encompassing indoor and outdoor environments, varying lighting conditions, and dynamic elements such as pedestrians and vehicles. Aided with the dynamic object removal scheme, XRSLAM(w. RD) showed significantly better RMSEs on ADVIO dataset. **Tracking Accuracy (RMSE in meters) on the EuRoC Dataset.** | Algorithm | MH-01 | MH-02 | MH-03 | MH-04 | MH-05 | V1-01 | V1-02 | V1-03 | V2-01 | V2-02 | V2-03 | AVG | |-------------|-------|-------|-------|-------|-------|-------|-------|-------|-------|-------|-------|-------| -| **SF-VIO** | 0.109 | 0.147 | **0.131** | 0.189 | 0.240 | **0.056** | 0.101 | 0.134 | 0.066 | 0.089 | **0.122** | **0.125** | -| **RD-VIO** | **0.109** | 0.115 | 0.141 | 0.247 | 0.267 | 0.060 | 0.091 | 0.168 | 0.058 | 0.100 | 0.147 | 0.136 | +| **XRSLAM** | 0.109 | 0.147 | **0.131** | 0.189 | 0.240 | **0.056** | 0.101 | 0.134 | 0.066 | 0.089 | **0.122** | **0.125** | +| **XRSLAM(w. RD)** | **0.109** | 0.115 | 0.141 | 0.247 | 0.267 | 0.060 | 0.091 | 0.168 | 0.058 | 0.100 | 0.147 | 0.136 | | **LARVIO** | 0.132 | 0.137 | 0.168 | 0.237 | 0.314 | 0.083 | **0.064** | 0.086 | 0.148 | 0.077 | 0.168 | 0.147 | | **Open-VINS**| 0.111| 0.287 | 0.181 | **0.182** | 0.365 | 0.059 | 0.084 | **0.075** | 0.086 | **0.074** | 0.145 | 0.150 | | **VI-DSO** | 0.125 | **0.072** | 0.285 | 0.343 | **0.202** | 0.197 | 0.135 | 4.073 | 0.242 | 0.202 | 0.212 | 0.553 | @@ -23,7 +23,7 @@ As a challenging dataset in real-world settings, ADVIO offers 23 diverse scenari
**Accuracy on the ADVIO Dataset** -| Sequence | **SF-VIO** | **RD-VIO** | **VINS-Fusion(VIO)** | **LARVIO** | +| Sequence | **XRSLAM** | **XRSLAM(w. RD)** | **VINS-Fusion(VIO)** | **LARVIO** | | :------: | :--------: | :--------: | :------------------: | :--------: | | 01 | 2.177 | **1.788** | 2.339 | 5.049 | | 02 | **1.679** | 1.695 | 1.914 | 4.242 | diff --git a/xrslam/src/xrslam/utility/imu_parsac.h b/xrslam/src/xrslam/utility/imu_parsac.h index 0cab401..c214a46 100644 --- a/xrslam/src/xrslam/utility/imu_parsac.h +++ b/xrslam/src/xrslam/utility/imu_parsac.h @@ -38,12 +38,6 @@ struct IMU_Parsac { LotBox lotbox(size); lotbox.seed(seed); - if (m_verbose) { - std::cout << "============== IMU PARSAC ================" - << std::endl; - std::cout << "-- Matches Size: " << pts1.size() << std::endl; - } - double K = log(std::max(1 - confidence, 1.0e-5)); inlier_count = 0; @@ -55,39 +49,22 @@ struct IMU_Parsac { } SetBins(20, 20); - if (m_verbose) - std::cout << "-- SetBins()" << std::endl; CreateBucket(); - if (m_verbose) - std::cout << "-- CreateBucket()" << std::endl; BucketData(pts2); - if (m_verbose) - std::cout << "-- BucketData()" << std::endl; ConvertConfidencesBinToValidBin(binConfidences, m_validBinConfidencesPrior); - if (m_verbose) - std::cout << "-- ConvertConfidencesBinToValidBin()" << std::endl; ThresholdAndNormalizeConfidences(m_validBinConfidencesPrior); - if (m_verbose) - std::cout << "-- ThresholdAndNormalizeConfidences()" << std::endl; AccumulateConfidences(m_validBinConfidencesPrior, m_validBinConfidencesAccumulatedPrior); - if (m_verbose) - std::cout << "-- AccumulateConfidences()" << std::endl; Sampler sampler(m_validBinConfidencesAccumulatedPrior); - if (m_verbose) - std::cout << "-- sampler()" << std::endl; if (!ComputePriorDistribution(pts1, pts2)) { - if (m_verbose) - std::cout << "-- ComputePriorDistribution()" << std::endl; - // std::cout << "IMU Process" << std::endl; inlier_mask = std::vector(size, 1); return matrix<4>::Identity(); } @@ -96,8 +73,7 @@ struct IMU_Parsac { size_t iter_max = max_iteration; float scoreMax = -FLT_MAX, score; for (size_t iter = 0; iter < iter_max; ++iter) { - // if(m_verbose) std::cout << "iters: " << iter << " " << iter_max - // << std::endl; + std::tuple...> tsample; lotbox.refill_all(); @@ -147,12 +123,9 @@ struct IMU_Parsac { std::vector> validBinOutliers; ConvertInliersListToValidBin(current_inlier_mask, validBinInliers); - // if(m_verbose) std::cout << " -- - // ConvertInliersListToValidBin()" << std::endl; + score = ComputeScore(validBinInliers, m_validBinConfidences); - // if(m_verbose) std::cout << " -- ComputeScore()" << - // std::endl; if (score > scoreMax || score == scoreMax && (overlap_inlier_count > inlier_count)) { @@ -160,7 +133,6 @@ struct IMU_Parsac { model = current_model; inlier_count = overlap_inlier_count; validBinInliersBest = validBinInliers; - // inlier_mask.swap(overlap_inlier_mask); inlier_mask.swap(current_inlier_mask); double inlier_ratio = inlier_count / (double)size; double N = K / log(1 - pow(inlier_ratio, 5)); @@ -168,9 +140,7 @@ struct IMU_Parsac { iter_max = (size_t)ceil(N); } } - // if(m_verbose) std::cout << " -- finish update" << std::endl; } - // if(m_verbose) std::cout << " -- finish iterate" << std::endl; } if (inlier_count < ModelDoF) { @@ -178,14 +148,9 @@ struct IMU_Parsac { return matrix<4>::Identity(); } - if (m_verbose) - std::cout << "-- Before ComputeScore()" << std::endl; ComputeScore(validBinInliersBest, m_validBinConfidences); - if (m_verbose) - std::cout << "-- ComputeScore()" << std::endl; + ConvertConfidencesValidBinToBin(m_validBinConfidences, binConfidences); - if (m_verbose) - std::cout << "-- ConvertConfidencesValidBinToBin()" << std::endl; return model; } @@ -253,7 +218,6 @@ struct IMU_Parsac { size_t m_nValidBins; float m_BinHeight; float m_BinWidth; - bool m_verbose = false; double m_dynamic_probability = 0; double m_norm_scale = 1.0; @@ -303,10 +267,6 @@ struct IMU_Parsac { } void SetBins(const int nBinsX, const int nBinsY) { - // Some bug occurred! the m_nBinsX is set to be the nBinsX at unknown - // time！ if(m_nBinsX == nBinsX && m_nBinsY == nBinsY) - // return; - m_nBinsX = nBinsX; m_nBinsY = nBinsY; m_nBins = m_nBinsX * m_nBinsY; diff --git a/xrslam/src/xrslam/utility/parsac.h b/xrslam/src/xrslam/utility/parsac.h index dea296f..e21f091 100644 --- a/xrslam/src/xrslam/utility/parsac.h +++ b/xrslam/src/xrslam/utility/parsac.h @@ -84,11 +84,6 @@ struct Parsac { LotBox lotbox(size); lotbox.seed(seed); - if (m_verbose) { - std::cout << "============== PARSAC ================" << std::endl; - std::cout << "-- Matches Size: " << pts1.size() << std::endl; - } - double K = log(std::max(1 - confidence, 1.0e-5)); inlier_count = 0; @@ -100,40 +95,26 @@ struct Parsac { } SetBins(20, 20); - if (m_verbose) - std::cout << "-- SetBins()" << std::endl; CreateBucket(); - if (m_verbose) - std::cout << "-- CreateBucket()" << std::endl; BucketData(pts2); - if (m_verbose) - std::cout << "-- BucketData()" << std::endl; ConvertConfidencesBinToValidBin(binConfidences, m_validBinConfidencesPrior); - if (m_verbose) - std::cout << "-- ConvertConfidencesBinToValidBin()" << std::endl; ThresholdAndNormalizeConfidences(m_validBinConfidencesPrior); - if (m_verbose) - std::cout << "-- ThresholdAndNormalizeConfidences()" << std::endl; AccumulateConfidences(m_validBinConfidencesPrior, m_validBinConfidencesAccumulatedPrior); - if (m_verbose) - std::cout << "-- AccumulateConfidences()" << std::endl; Sampler sampler(m_validBinConfidencesAccumulatedPrior); - if (m_verbose) - std::cout << "-- sampler()" << std::endl; std::vector> validBinInliersBest; size_t iter_max = max_iteration; float scoreMax = 0, score; for (size_t iter = 0; iter < iter_max; ++iter) { - // if(m_verbose) std::cout << "iters: " << iter << std::endl; + std::tuple...> tsample; lotbox.refill_all(); @@ -166,11 +147,9 @@ struct Parsac { std::vector> validBinInliers; ConvertInliersListToValidBin(current_inlier_mask, validBinInliers); - // if(m_verbose) std::cout << " -- - // ConvertInliersListToValidBin()" << std::endl; + score = ComputeScore(validBinInliers, m_validBinConfidences); - // if(m_verbose) std::cout << " -- ComputeScore()" << - // std::endl; + if (score > scoreMax || score == scoreMax && (current_inlier_count > inlier_count)) { @@ -185,19 +164,12 @@ struct Parsac { iter_max = (size_t)ceil(N); } } - // if(m_verbose) std::cout << " -- finish update" << std::endl; } - // if(m_verbose) std::cout << " -- finish iterate" << std::endl; } - if (m_verbose) - std::cout << "-- Before ComputeScore()" << std::endl; + ComputeScore(validBinInliersBest, m_validBinConfidences); - if (m_verbose) - std::cout << "-- ComputeScore()" << std::endl; ConvertConfidencesValidBinToBin(m_validBinConfidences, binConfidences); - if (m_verbose) - std::cout << "-- ConvertConfidencesValidBinToBin()" << std::endl; return model; } @@ -219,7 +191,6 @@ struct Parsac { size_t m_nValidBins; float m_BinHeight; float m_BinWidth; - bool m_verbose = false; double m_norm_scale = 1.0; @@ -266,9 +237,7 @@ struct Parsac { } void SetBins(const int nBinsX, const int nBinsY) { - // Some bug occurred! the m_nBinsX is set to be the nBinsX at unknown - // time！ if(m_nBinsX == nBinsX && m_nBinsY == nBinsY) - // return; + m_nBinsX = nBinsX; m_nBinsY = nBinsY; m_nBins = m_nBinsX * m_nBinsY;