openvslam 最佳化誤差問題隨機一致性核函式資訊矩陣（高斯牛頓）

MKT-porter發表於2024-08-08

SLAM隨機函式矩陣

最佳化問題

我們的目標就是找到一組a , b , λ a,b,\lambdaa,b,λ的解，使得式（1）整體值最小，也就是各個點到曲線的距離在y 方向的和最小。

魯棒核函式

假設現在散點中一個很離譜的錯誤點

由於右上角那個離譜的點，導致最佳化時將整個函式被拉偏了（可以對比圖3）。

那麼怎麼解決這種問題呢？g2o中提供了魯棒核函式來抑制某些誤差特別大的點，拉偏整個最佳化結果。

魯棒核函式不是g2o獨有的，這是非線性最佳化方法中的一種常用手段！

	//構造一個Huber魯棒核函式
        g2o::RobustKernelHuber* robust_kernel_huber = new g2o::RobustKernelHuber;
        robust_kernel_huber->setDelta(0.3);//設定delta的大小。注意這個要根據實際的應用場景去嘗試，然後選擇合適的大小
        e->setRobustKernel(robust_kernel_huber);//向邊中新增魯棒核函式

　　加入魯棒核函式之後，結果明顯好轉。

資訊矩陣

現在來考慮另一種情況，比方說在一次最佳化中，對於某一次測量，我們有十足的把握，它非常的準確，所以最佳化時我們希望對於這次測量給予更高的權重。

如上圖，假設我們認為左上角那個異常點是一個比較正確的點（只是假設），我們希望擬合的曲線儘量往這個點偏移。那麼我們就這可以設定這次測量邊的權重更大。

程式碼如下：

e->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 10);

　　因為測量值的維度為1，所以資訊矩陣也為1。如果我們把每一條邊的資訊矩陣都設定為一樣，那麼在最佳化時將認為所有邊的最佳化權重是一樣的，將不會對某一條邊執行過多的最佳化！

卡方分佈(Chi-squared)外點(outlier)剔除

https://zhuanlan.zhihu.com/p/58556978

ORB-SLAM2取

在openvslam

    // 自由度n=2
    constexpr float chi_sq_2D = 5.99146;
    const float sqrt_chi_sq_2D = std::sqrt(chi_sq_2D);
    // 自由度n=3
    constexpr float chi_sq_3D = 7.81473;
    const float sqrt_chi_sq_3D = std::sqrt(chi_sq_3D); //std::sqrt(7.81473)

    根據單目和雙目選擇對應閾值
    const auto sqrt_chi_sq = (keyfrm->camera_->setup_type_ == camera::setup_type_t::Monocular)
                                         ? sqrt_chi_sq_2D
                                         : sqrt_chi_sq_3D;
    

    // loss function  核函式設定
    if (use_huber_loss) {
        auto huber_kernel = new ::g2o::RobustKernelHuber();
        huber_kernel->setDelta(sqrt_chi_sq);
        edge_->setRobustKernel(huber_kernel);
    }

重投影誤差為