在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); //