LSD-SLAM(2)关于tracking线程的目标函数的推导

在LSD-SLAM的tracking部分，作者采用的是基于se(3)的图像调整策略。优化目标是带权重的光度误差，其中误差的权重随着像素亮度方差和深度方差的增大而减小。

论文公式如下所示：

上式中，p为像素坐标[p_x,p_y,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);// dfloat rp = *(buf_warped_residual+i); // r_pfloat gx = *(buf_warped_dx+i);// \delta_x Ifloat gy = *(buf_warped_dy+i);  // \delta_y Ifloat 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_pfloat drpdd = gx * g0 + gy * g1;// ommitting the minusfloat w_p = 1.0f / ((cameraPixelNoise2) + s * drpdd * drpdd);        //这里的权重也就是论文公式(14)部分，即目标函数的分母

推导过程如下：

1、权重分为两个部分：图像的亮度方差带来的权重σ_I，和深度方差带来的权重。其中σ_I为已知常量，Vi(p)也可以通过先验得到，也算是一个已知量。因此，唯一需要计算的就是光度误差关于逆深度的导数

令，则

其中，便是I_j上像素的梯度；u的表达式在论文中做了阐述（这里作者貌似是忽略了内参矩阵K的存在）：

我们重新整理之后（这里忽略了旋转量），得到

这里要注意的是，p_x和p_y是指像素坐标而不是3D坐标。

接下来就是矩阵的求导（这里只需要求前两项）

又因为空间中某一点在相机坐标系的坐标可以表示为，并满足

那么进一步简化求导公式如下

上式中的也就是将源相机参考系下的坐标变换到了目标参考系下（忽略旋转），我们重新表达为,所以最终表达式如下

最后汇总

最终就变成了代码中的样子。

关于雅克比矩阵的推导可以参考高翔博士的视觉SLAM14讲中第8章，里面对光度误差模型关于位姿的李代数求解给出了详细的推导。

更多关于LSD-SLAM源码的部分可以参考我的github，地址：https://github.com/Wangxuefeng92/LSDaddexplain

EMAIL:[e-mail]