視覺VIO--5.LM和Dogleg的對比與改進

題目1

a. 選用更優的LM策略，使得VINS-Mono在MH-05數據集上收斂速度更快或者精度更高
b. 實現dog-leg算法替換LM算法，並測試替換後的VINS-Mono在MH-05上算法精度

LM算法流程圖

初始化LM參數
normal equation求解
更新阻尼因子

解體思路：滑動優化過程中，特別是滑動窗中關鍵幀數量穩定時，margin掉一幀和新增一幀，優化問題的結構不會產生很大變化，因此上次優化結果可以利用起來用於下一次優化。

具體做法：

• 代碼中記錄優化中最後一次good step的lambda值和chi值
• 新的優化問題，若初始chi值比記錄的上次優化結果chi值小，說明上一次結果也是當前問題的初始線性-- 化點）是很好的初值，此時取上一次優化結果的lambda值的一半作爲當前優化問題的初始lambda值，使其更接近高斯牛頓法。
• 若新優化問題chi值更大，說明初始值策略差，取記錄的lambda值的兩倍作爲新問題初始lambda值，使其更新接近最速下降法。

這個方法通過利用歷史優化結果來初始化當前問題，在一開始就找到更好的收斂方向，減少向量優值迭代次數，既有更快的收斂速度，另外在迭代次數一定時，往往能優化更深，更接近迭代最優值，精度更高。

Dogleg算法計算了高斯牛頓法結果hgn和梯度下降法結果hsd，然後在置信區域內以一定策略獲取hgn和hsd，迭代過程中雖然需要計算更多的中間量，但是其在bad step時不需像LM再次計算Hession矩陣。

在MH_05)difficult數據集可以達到LM相似精度，並加速了1-2倍。通過調整dogleg迭代次數，可以達到更精確的結果。

第二題

• 1. 採用任何一種或多種加速方式（如多線程，sse指令集等）對信息矩陣的拼接函數加速，並給出詳細的實驗對比報告。

最簡單的並行計算–OpenMP的使用
在CMakeLists.txt中配置OpenMP

find_package(OpenMP REQUIRED)
if(OPENMP_FOUND)
message("OPENMP FOUND")
set(CMAKE_C_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()

增加omp.h頭文件

在void Problem::MakeHessian()中使用多線程

#ifdef USE_OPENMP
#pragma omp parallel for num_threads(4)
#endif
	for(size_t i = 0; i < verticies.size(); i++){
		auto v_i = verticies[i];
		if(v_i->IsFixed()) continue;
		auto jacobian_i = jacobians[i];
		ulong index_i = v_i->OrderingId();
		ulong dim_i=v_i->LocalDimension();
		// 魯棒核函數會修改殘差和信息矩陣，如果沒有設置robust cost function，就會返回原來的
		double drho;
		Matrix robustInfo(edge.second->Infomation().rows(),edge.second->Information().cols());
		edge.second->RobustInfo(drho,robustInfo);
	}

速度可以提升20%左右，精度保持不變

使用其他複雜並行方法可能存在的問題：由於多線程競爭同一資源，可能存在讀取衝突，會導致構建的Hessian矩陣丟失部分信息，導致計算的優化方向不準，導致迭代次數增加，精度下降。
在Marginalize中也可以進行並行優化加速
線程數量不同加速效果不一樣，並不是越多越好

Problem::MakeHessian()
遍歷所有邊，滑動窗口中的edges邊大約有1500左右
原始代碼：

    for (auto &edge: edges_) {
        edge.second->ComputeResidual();
        edge.second->ComputeJacobians();

        // TODO:: robust cost
        auto jacobians = edge.second->Jacobians();
        auto verticies = edge.second->Verticies();
        assert(jacobians.size() == verticies.size());
        for (size_t i = 0; i < verticies.size(); ++i) {
            auto v_i = verticies[i];
            if (v_i->IsFixed()) continue;    // Hessian 裏不需要添加它的信息，也就是它的雅克比爲 0

            auto jacobian_i = jacobians[i];
            ulong index_i = v_i->OrderingId();
            ulong dim_i = v_i->LocalDimension();

            // 魯棒核函數會修改殘差和信息矩陣，如果沒有設置 robust cost function，就會返回原來的
            double drho;
            MatXX robustInfo(edge.second->Information().rows(),edge.second->Information().cols());
            edge.second->RobustInfo(drho,robustInfo);

            MatXX JtW = jacobian_i.transpose() * robustInfo;
            for (size_t j = i; j < verticies.size(); ++j) {
                auto v_j = verticies[j];

                if (v_j->IsFixed()) continue;

                auto jacobian_j = jacobians[j];
                ulong index_j = v_j->OrderingId();
                ulong dim_j = v_j->LocalDimension();

                assert(v_j->OrderingId() != -1);
                MatXX hessian = JtW * jacobian_j;

                // 所有的信息矩陣疊加起來
                H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
                if (j != i) {
                    // 對稱的下三角
                    H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();

                }
            }
            b.segment(index_i, dim_i).noalias() -= drho * jacobian_i.transpose()* edge.second->Information() * edge.second->Residual();
        }

    }