SVO學習日記 8 2021 2 1

#ifndef svo_pose_optimizer_h_

#define svo_pose_optimizer_h_
#include
namespace svo
}#endif

#include

#include
#include
#include
#include
#include
#include
namespace svo 
if(errors.
empty()
)return
;//根據總的errors計算中位數絕對誤差,誤差的尺度
//確定誤差整體是比較大還是比較小
vk::robust_cost::madscaleestimator scale_estimator;
estimated_scale = scale_estimator.
compute
(errors)
;//返回估計的標準差
num_obs = errors.
size()
; chi2_vec_init.
reserve
(num_obs)
;//初始卡方誤差
chi2_vec_final.
reserve
(num_obs)
;//最終卡方誤差
double scale = estimated_scale;
//把估計的標準差賦值給scale
//迭代優化位姿
for(size_t iter =
0;iter < n_iter;iter++
)//定義最小二乘的引數
b.setzero()
; a.
setzero()
;double
new_chi2
(0.0);
//計算殘差
for(
auto it = frame -
> fts_.
begin()
;it != frame -
> fts_.
end();
++it)
//配置最小二乘左邊項a的部分
j *= sqrt_inv_cov;
//權重是根據層數來確定的,越高雜訊越大,權重越小
double weight = weight_function.
value
(e.norm()
/ scale)
; a.
noalias()
+= j.
transpose()
* j * weight;
b.noalias()
-= j.
transpose()
* e * weight;
new_chi2 +
= e.
squarednorm()
* weight;
}//求解方法,dlt
const vector6d dt
(a.ldlt()
.solve
(b))
;//檢查誤差大小if(
(iter >
0&& new_chi2 > chi2)||(
bool
) std::
isnan((
double
)dt[0]
))//更新影象的位姿
se3 t_new = se3::
exp(dt)
* frame -
> t_f_w_;
//更新後的值
t_old = frame -
> t_f_w_;
//更新前的值
frame -
> t_f_w_ = t_new;
//然後把更新後的值賦值給更新前的值,以完成更新
chi2 = new_chi2;
//傳遞賦值
if(verbose)
std::cout <<
"it "
<< iter
<<
"\t success \t new_chi2 = "
<< new_chi2
<<
"\t norm(dt) = "
<< vk::
norm_max
(dt)
<< std::endl;
//收斂終止判定
if(vk::
norm_max
(dt)
<= eps)
break;}
//計算影象位姿的協方差,偽逆
const
double pixel_variance =
1.0;
//之前的j裡沒有fx,因此這裡要在a上乘以fx的平方,因為a = j^t * j
frame -
> cov_ = pixel_variance *
(a * std::
pow(frame -
> cam_ -
>
errormultiplier2()
,2))
.inverse()
;//去除較大重投影誤差的測量
//同理閾值要除以fx
double reproj_thresh_scaled = reproj_thresh / frame -
> cam_ -
>
errormultiplier2()
; size_t n_deleted_refs =0;
//計算優化後的誤差,如果大於閾值,就把點刪除
for(features::iterator it = frame -
> fts_.
begin()
;it != frame -
> fts_.
end();
++it)
}//計算優化前後的誤差的中位數
error_init =
0.0;
error_final =
0.0;
//所有測量值if(
!chi2_vec_init.
empty()
) error_init =
sqrt
(vk::
getmedian
(chi2_vec_init)
)*frame -
> cam_ -
>
errormultiplier2()
;//去除超過閾值的測量的中位數if(
!chi2_vec_final.
empty()
) error_final =
sqrt
(vk::
getmedian
(chi2_vec_final)
)*frame -
>cam_ -
>
errormultiplier2()
; 
estimated_scale *
= frame -
> cam_ -
>
errormultiplier2()
;if(verbose)
std::cout <<
"n deleted obs = "
<< n_deleted_refs
<<
"\t scale = "
<< estimated_scale
<<
"\t error init = "
<< error_init
<<
"\t error end = "
<< error_final << std::endl;
//從觀測中減去刪除的點
num_obs -
= n_deleted_refs;}}
}

SVO學習日記 2 2021 1 16

include include include include include include include include include include include include namespace svo 析構函式，主要作用是刪除幀上的特徵點 frame frame 初始化幀 void...

SVO學習日記 13 2021 2 22

include include include include include include include include include include include include include include include include include include includ...

SVO學習日記 14 2021 2 23

做了兩件事 1.通過影象對齊,計算乙個粗糙的位姿 2.建立乙個當前幀的區域性地圖,根據地圖對比位姿進一步優化 二.影象對齊 1.目的 迭代計算幀間位姿 解釋 用於影象對齊的地圖點是上一幀能看到的地圖點,我們知道影象幀間變換比較小,有理由相信上一幀所看到的場景大部分相同 過程 1 找到前一幀看到的地圖...