在LSD-SLAM的tracking部分，作者采用的是基于se(3)的圖像調整政策。優化目标是帶權重的光度誤差，其中誤差的權重随着像素亮度方差和深度方差的增大而減小。
論文公式如下所示：
上式中，p為像素坐标[px,py,1]；ξ為前後兩幀的位姿變換的李代數表示；r是光度誤差；V是深度方差估計；D是逆深度；σI是圖像的亮度方差，是常量。
光度誤差模型本身不難了解，但是在對照論文看源碼的時候發現權重的求取比較難以了解，是以自己推導了一遍。
首先來看代碼
float px = *(buf_warped_x+i); // x'
float py = *(buf_warped_y+i); // y'
float pz = *(buf_warped_z+i); // z'
float d = *(buf_d+i); // d
float rp = *(buf_warped_residual+i); // r_p
float gx = *(buf_warped_dx+i); // \delta_x I
float gy = *(buf_warped_dy+i); // \delta_y I
float s = settings.var_weight * *(buf_idepthVar+i); // \sigma_d^2
// calc dw/dd (first 2 components):
//這裡做了近似，忽略了旋轉成分
float g0 = (tx * pz - tz * px) / (pz*pz*d);
float g1 = (ty * pz - tz * py) / (pz*pz*d);
// calc w_p
float drpdd = gx * g0 + gy * g1; // ommitting the minus
float w_p = 1.0f / ((cameraPixelNoise2) + s * drpdd * drpdd);
//這裡的權重也就是論文公式(14)部分，即目标函數的分母
推導過程如下：
1、權重分為兩個部分：圖像的亮度方差帶來的權重σI，和深度方差帶來的權重
。其中σI為已知常量，Vi(p)也可以通過先驗得到，也算是一個已知量。是以，唯一需要計算的就是光度誤差關于逆深度的導數
令
，則
其中
，便是Ij上像素的梯度；u的表達式在論文中做了闡述（這裡作者貌似是忽略了内參矩陣K的存在）：
我們重新整理之後（這裡忽略了旋轉量），得到
這裡要注意的是，px和py是指像素坐标而不是3D坐标。
接下來就是矩陣的求導（這裡隻需要求前兩項）
又因為空間中某一點在相機坐标系的坐标可以表示為
，并滿足
那麼進一步簡化求導公式如下
上式中的
也就是将源相機參考系下的坐标變換到了目标參考系下（忽略旋轉），我們重新表達為
,是以最終表達式如下
最後彙總
最終就變成了代碼中的樣子。
關于雅克比矩陣的推導可以參考高翔博士的視覺SLAM14講中第8章，裡面對光度誤差模型關于位姿的李代數求解給出了詳細的推導。
更多關于LSD-SLAM源碼的部分可以參考我的github，位址：https://github.com/Wangxuefeng92/LSDaddexplain
EMAIL:[email protected]