L S D 算 法

基数排序中的LSD方法和MSD方法
Lsd_slam的主函数位于main_live_odometry.cpp中，其中主要的SLAM 功能由slamNode.Loop();执行。

Slam.Loop()主要为一个读取新图像并处理的死循环，由newImageCallback处理图像。

newImageCallback执行slam的随机初始化和图像跟踪。

1 随机初始化
随机初始化函数为SlamSystem::randomInit
1.1 currentKeyFrame设置为当前图像的Frame
1.1.1 Frame类及其创建（构造函数）
Frame类中主要用来存储数据的类是Data类，Data类中保存了id、内参金字塔、图像金字塔、梯度图像金字塔、像素点深度金字塔、像素点逆深度方差金字塔等信息。

当用构造函数创建一个新的Frame类时，首先给data一个新的id，然后初始化Data类中的内参金字塔数据，然后对其他数据进行对应的初始化，再然后对图像金字塔赋值，不过只赋值金字塔的第0层。

Frame创建完后赋给currentKeyFrame。

1.2 DepthMap初始化
主要函数为DepthMap::initializeRandomly(Frame* new_frame)
首先复制了new_frame中的一些指针：互斥锁、new_frame、图像数据、和一个标志位activeKeyFrameIsReactivated。

计算图像梯度并且根据图像梯度给currentDepthMap赋初始值，并且将传进来的new_frame的深度设置为currentDepthMap中的值(Frame::setDepth)。

Frame::setDepth主要是对data.idepth[0]，和data.idepthVar进行了内参分配和赋值。

再把当前帧添加到keyFrameGraph中。

2 图像跟踪
首先根据新的图像创建一个Frame类trackingNewFrame，跟踪的主要算法位于
SE3 SE3Tracker::trackFrame(
TrackingReference* reference,
Frame* frame,
const SE3 frameToReference_initialEstimate)
reference为最近的一个KeyFrame，frame是当前帧，frameToReference_initialEstimate是初始值。

整个跟踪过程从最高层的图像金字塔（也就是图像尺寸最小的那一层）开始计算，一直往下迭代到第一层。

论文中采用了最小化归一化方差的光度误差：
(1)Ep(ξji)=∑p∈ΩDi∥rp2(p,ξji)σrp(p,ξji)2∥δ
E_p(xi_{ji})=sum_{textbf pin Omega_{D_i}}lVertfrac{r_p^2(textbf p,xi_{ji})}{sigma_{r_p(textbf p,xi_{ji})}^2}rVert_{delta}
Ep?(ξji?)=p∈ΩDi?∑?∥σrp?(p,ξji?)2?rp2?(p,ξji?)?∥δ?(
1)
(2)rp(p,ξji)=Ii(p)?Ij(ω(p,Di(p),ξji))
r_p(textbf p,xi_{ji})=I_i(textbf p)-I_j(omega(textbf p,D_i(textbf p),xi_{ji}))
rp?(p,ξji?)=Ii?(p)?Ij?(ω(p,Di?(p),ξji?))(2)
(3)σrp(p,ξji)2=2σI2+(?rp(p,ξji)?Di(p))2Vi(p)
sigma_{r_p(textbf p,xi_{ji})}^2=2sigma_I^2+(frac{partial r_p(textbf p,xi_{ji})}{partial D_i(textbf p)})^2V_i(textbf p) σrp?(p,ξji?)2?=2σI2?+(?Di?(p)?rp?(p,ξji?)?)2Vi?(p)(3)
reference-makePointCloud(lvl);
2.2 计算残差和梯度
float SE3Tracker::calcResidualAndBuffers(
const Eigen::Vector3f* refPoint,
const Eigen::Vector2f* refColVar,
int* idxBuf,
int refNum,
Frame* frame,
const Sophus::SE3f referenceToFrame,
int level,
bool plotResidual)
每个残差都会有一个权重，按以下代码计算：
float weight = fabsf(residual) 5.0f ? 1 : 5.0f - fabsf(residual); 计算梯度时把插值出来的梯度乘以当前层焦距。

该函数最终返回没有乘以权重的平均残差。

2.3 计算归一化残差
float SE3Tracker::calcWeightsAndResidual(
const Sophus::SE3f referenceToFrame)
(4)(x′y′z′)=(px-dpy-d1-d)+(txtytz)
begin{pmatrix}
end{pmatrix}=
begin{pmatrix}
textbf p_x-d
textbf p_y-d
end{pmatrix}+
begin{pmatrix}
end{pmatrix}
x′y′z′=px-dpy?-d1-d?+?tx?ty?tz?(4)
整理一下得到归一化坐标：
(5)ω(p,Di(p),ξ)=(px-d+tx1-d+tzpy-d+ty1-d+tz1)
omega(textbf p, D_i(textbf p),xi)=
begin{pmatrix}
frac{textbf p_x-d+t_x}{1-d+t_z}
frac{textbf p_y-d+t_y}{1-d+t_z}
end{pmatrix}
ω(p,Di?(p),ξ)=?1-d+tz?px?-d+tx?1-d+tz?py?-d+ty?1?(5)
然后就可以把(3)中的偏导表示出来：
(6)?rp(p,ξji)?Di(p)=?(Ii(p)?Ij(ω(p,Di(p),ξji)))?Di(p)=?Ij
(a)?a∣a=p?ω(d)?d∣d=Di(p)=?(dxfxdyfy)?(tx(1-d+tz)?tz(px-d+tx) (1-d+tz)2dty(1-d+tz)?tz(py-d+ty)(1-d+tz)2d)=?(dxfxtxz′?tzx′z
′2d+dyfytyz′?tzy′z′2d)
frac{partial r_p(textbf p,xi_{ji})}{partial D_i(textbf p)}= frac{partial(I_i(textbf p)-I_j(omega(textbf p,D_i(textbf p),xi_{ji})))}{partial D_i(textbf p)}
=-frac{partial I_j(textbf a)}{partialtextbf a}|_{textbf a = textbf p}cdotfrac{partialomega(d)}{partial d}|_{d=D_i(textbf p)} =-begin{pmatrix}
d_xf_x amp; d_yf_y
end{pmatrix}cdotbegin{pmatrix}
frac{t_x(1-d+t_z)-t_z(textbf p_x-d+t_x)}{(1-d+t_z)^2d}
frac{t_y(1-d+t_z)-t_z(textbf p_y-d+t_y)}{(1-d+t_z)^2d}
end{pmatrix}
=-(d_xf_xfrac{t_xz#x27;-t_zx#x27;}{z#x27;^2d}+d_yf_yfrac{t_y z#x27;-t_zy#x27;}{z#x27;^2d})
Di(p)rp(p,ξji?)?=?Di?(p)?(Ii?(p)?Ij?(ω(p,Di?(p),ξji?) ))?=?a?Ij?(a)?∣a=p?d?ω(d)?∣d=Di?(p)?=?(dx?fx?dy?fy?)?((1-d+t
z?)2dtx?(1-d+tz?)?tz?(px?-d+tx?)?(1-d+tz?)2dty?(1-d+tz?)?tz?(py
-d+ty?)?)=?(dx?fx?z′2dtx?z′?tz?x′?+dy?fy?z′2dty?z′?tz?y′
)(6)
上述公式跟代码是一致的，其中公式(6)第三行左边部分的偏导对应如下代码：
-- 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
其中gx和gy是乘上了焦距的梯度，与公式(6)一致，但是忽略了负号。

3 深度图DepthMap
mapping线程在SlamSystem的构造函数中被创建：
thread_mapping = boost::thread(SlamSystem::mappingThreadLoop, this);
mappingThreadLoop的主体是一个循环，用来处理尚未被建图的图像帧。

void SlamSystem::mappingThreadLoop()
printf("Started mapping thread!");
while(keepRunning)
if (!doMappingIteration())
boost::unique_lockboost::mutex
lock(unmappedTrackedFramesMutex);
unmappedTrackedFramesSignal.timed_wait(lock,boost::posix_ time::milliseconds(200)); -- slight chance of deadlock otherwise lock.unlock();
newFrameMappedMutex.lock();
newFrameMappedSignal.notify_all();
newFrameMappedMutex.unlock();
printf("Exited mapping thread ");
其中主要函数是doMappingIteration()，它主要用于创建关键帧或更新关键帧。

3.1 更新关键帧updateKeyframe()
更新关键帧的代码位于
void DepthMap::updateKeyframe(std::deque std::shared_ptrFrame referenceFrames)
referenceFrames中保存的是已经通过trackFrame()计算出位姿的图像帧的队列，队列头部是最老帧，尾部是最新帧。

对于referenceFrames中的每一帧，计算它到当前关键帧的相对变换，然后通过
frame-prepareForStereoWith(activeKeyFrame, refToKf, K, 0);
void Frame::prepareForStereoWith(Frame* other, Sim3 thisToOther, const Eigen::Matrix3f K, const int level)
Sim3 otherToThis = thisToOther.inverse();
--otherToThis = data.worldToCam * other-data.camToWorld;
K_otherToThis_R = K * otherToThis.rotationMatrix().castfloat() * otherToThis.scale();
otherToThis_t = otherToThis.translation().castfloat();
K_otherToThis_t = K * otherToThis_t;
thisToOther_t = thisToOther.translation().castfloat();
K_thisToOther_t = K * thisToOther_t;
thisToOther_R = thisToOther.rotationMatrix().castfloat() * thisToOther.scale();
otherToThis_R_row0 = thisToOther_R.col(0);
otherToThis_R_row1 = thisToOther_R.col(1);
otherToThis_R_row2 = thisToOther_R.col(2);
distSquared = otherToThis.translation().dot(otherToThis.translation());
referenceID = other-id();
referenceLevel = level;
来计算一些用于深度更新的中间结果。

得到中间结果后执行observeDepth()进行深度更新。

observeDepth()中起实际作用的是第一行代码：
threadReducer.reduce(boost::bind(DepthMap::observeDepthRow, this, _1, _2, _3), 3, height-3, 10);
threadReducer是一个IndexThreadReduce对象，它在初始化的时候就
会开启4个(Android下2个)workerLoop线程，并且每个线程都在等待todo_signal的信号。

reduce函数把threadReducer内的一个函数对象绑定了DepthMap::observeDepthRow，并且给函数参数赋值，初始化相关标志位，最后todo_signal发送通知信号。

这里相当于是把observeDepthRow()给并行加速了，observeDepthRow()要遍历整幅图像，IndexThreadReduce把遍历整幅图像的任务分给了若干个线程，以达到并行加速的目的。

3.2 observeDepthRow()
observeDepthRow遍历当前关键帧的深度图，除去无效点和黑名单上的点，对于剩下的有效点，如果该点没有深度假设，则创建深度observeDepthCreate()，如果已经有深度，则进行更新observeDepthUpdate()。

3.2.1 observeDepthCreate
(1) 计算极线
float epx, epy;
bool isGood = makeAndCheckEPL(x, y, refFrame, epx, epy, stats);
if(!isGood) return false;
ek=Kor=(fx0cx0fycy001)(oxoyoz)=(fxox+cxozfyoy+cyozoz)
textbf e_k = textbf{Ko}_r = begin{pmatrix}
f_x amp; 0 amp; c_x
0 amp; f_y amp; c_y
0 amp; 0 amp; 1
end{pmatrix}
begin{pmatrix}
end{pmatrix}=
begin{pmatrix}
f_xo_x+c_xo_z
f_yo_y+c_yo_z
end{pmatrix}
ek?=Kor?=?fx?00?0fy?0?cx?cy?1?ox?oy?oz?=?fx?ox?+cx?oz?fy?oy? +cy?oz?oz?
l=u?ek=(xy)?1oz(fxox+cxozfyoy+cyoz)
textbf l = textbf u - textbf e_k = begin{pmatrix}
end{pmatrix}-frac{1}{o_z}begin{pmatrix}
f_xo_x+c_xo_z
f_yo_y+c_yo_z
end{pmatrix}
l=u?ek?=(xy?)?oz?1?(fx?ox?+cx?oz?fy?oy?+cy?oz?)
上述公式跟代码里差了一个尺度1-oz1-o_z1-oz?。

-- ======= make epl ========
-- calculate the plane spanned by the two camera centers and the point (x,y,1)
-- intersect it with the keyframe's image plane (at depth=1) float epx = - fx * ref-thisToOther_t[0] +
ref-thisToOther_t[2]*(x - cx);
float epy = - fy * ref-thisToOther_t[1] + ref-thisToOther_t[2]*(y - cy);
得到极线向量后，计算梯度向量在极线方向上的分量值，在实际代码中计算的是这个分量值的平方。

float eplGradSquared = gx * epx + gy * epy;
eplGradSquared = eplGradSquared*eplGradSquared - eplLengthSquared;
-- square and norm with epl-length
if(eplGradSquared MIN_EPL_GRAD_SQUARED)
if(enablePrintDebugInfo)
stats-num_observe_skipped_small_epl_grad++;
return false;
然后计算极线与梯度向量所成的角度，根据论文Semi-Dense Visual Odometry for a Monocular Camera，当极线向量方向与梯度方向接近时，由几何误差导致的对逆深度的估计误差较小，当极线方向与梯度方向较大时，几何误差对逆深度的影响较大，其影响如图所示。

图中蓝色的?lepsilon_l?l?代表几何误差，红色的?λepsilon_{lambda}?λ?表示由几何误差带来的视差误差，虚线表示正确的匹配点必然所在的直线，实线表示极线方向。

-- ===== check epl-grad angle ======
if(eplGradSquared - (gx*gx+gy*gy) MIN_EPL_ANGLE_SQUARED)
if(enablePrintDebugInfo)
stats-num_observe_skipped_small_epl_angle++;
return false;
如果上述条件都符合，返回归一化极线向量。

(2) 计算深度
得到极线向量后，就要进行极线搜索了，对应的算法位于
-- find pixel in image (do stereo along epipolar line).
-- mat: NEW image
-- KinvP: point in OLD image (Kinv * (u_old, v_old, 1)), projected -- trafo: x_old = trafo * x_new; (from new to old image)
-- realVal: descriptor in OLD image.
-- returns: result_idepth : point depth in new camera's coordinate system
-- returns: result_u-v : point's coordinates in new camera's coordinate system
-- returns: idepth_var: (approximated) measurement variance of inverse depth of result_point_NEW
-- returns error if sucessful; -1 if out of bounds, -2 if not found.
inline float DepthMap::doLineStereo(
const float u, const float v, const float epxn, const float epyn,
const float min_idepth, const float prior_idepth, float max_idepth,
const Frame* const referenceFrame, const float* referenceFrameImage,
float result_idepth, float result_var, float result_eplLength,
RunningStats* stats)
pˉk=K?1puvpinf=1dkRr←kpˉk+tr←k
bartextbf p_k = textbf K^{-1}textbf p_{uv}
textbf p_{inf} = frac{1}{d_k}textbf R_{rleftarrow k}bartextbf p_k + textbf t_{rleftarrow k}
pˉ?k?=K?1puv?pinf?=dk?1?Rr←k?pˉ?k?+tr←k?
p1r=1drK?1(pr?0.5nlr1)p2r=1drK?1(pr?0.5nlr1)
textbf p_1^r = frac{1}{d_r}textbf K^{-1}begin{pmatrix}
textbf p_r - 0.5ntextbf l_r
end{pmatrix}
textbf p_2^r = frac{1}{d_r}textbf K^{-1}begin{pmatrix}
textbf p_r - 0.5ntextbf l_r
end{pmatrix}
p1r?=dr?1?K?1(pr?0.5nlr?1?)p2r?=dr?1?K?1(pr?0.5nlr?1?)
变换到当前关键帧坐标系下为：
1dkK?1(p11)=Rk←r1drK?1(pr?0.5nlr)+tk←t1dkK?1(p21)=Rk←r1dr
K?1(pr?0.5nlr)+tk←t
frac{1}{d_k}textbf K^{-1}begin{pmatrix}
textbf p_1
end{pmatrix}=textbf R_{kleftarrow r}frac{1}{d_r}textbf K^{-1}begin{pmatrix}
textbf p_r - 0.5ntextbf l_r
end{pmatrix} + textbf t_{kleftarrow t}
frac{1}{d_k}textbf K^{-1}begin{pmatrix}
textbf p_2
end{pmatrix}=textbf R_{kleftarrow r}frac{1}{d_r}textbf K^{-1}begin{pmatrix}
textbf p_r - 0.5ntextbf l_r
end{pmatrix} + textbf t_{kleftarrow t}
dk?1?K?1(p1?1?)=Rk←r?dr?1?K?1(pr?0.5nlr?)+tk←t?dk?1?K?1(p2 1)=Rk←r?dr?1?K?1(pr?0.5nlr?)+tk←t?
两式相减有：
(p2?p10)=dkdrKRk←rK?1(nlr0)
begin{pmatrix}
textbf p_2 - textbf p_1
end{pmatrix}=frac{d_k}{d_r}textbf Ktextbf R_{kleftarrow r}textbf K^{-1}begin{pmatrix}
ntextbf l_r
end{pmatrix}
(p2?p1?0?)=dr?dk?KRk←r?K?1(nlr?0?)
在当前关键帧上匹配的极线长度为：
∣p2?p1∣=∣dkdr∣?∣KRk←rK?1∣?∣nlr∣≈ndkdr
lvert textbf p_2 - textbf p_1rvert=lvertfrac{d_k}{d_r}rvertcdotlverttextbf Ktextbf R_{kleftarrow r}textbf K^{-1}rvertcdotlvert ntextbf l_rrvertapprox nfrac{d_k}{d_r}
∣p2?p1?∣=∣dr?dk?∣?∣KRk←r?K?1∣?∣nlr?∣≈n dr?dk?
由公式中可以看出，当两帧间旋转比较小的时候，两帧上的极线段成比例，比例系数为dkdrfrac{d_k}{d_r}dr?dk?。

pclose=Rr←kpˉk+1dmaxtr←k
textbf p_{close} = textbf R_{rleftarrow k}bartextbf p_k + frac{1}{d_{max}}textbf t_{rleftarrow k}
pclose?=Rr←k?pˉ?k?+dmax?1?tr←k?
把dmaxd_{max}dmax?换成dmind_{min}dmin?就得到了pfartextbf p_{far}pfar?。

然后判断待搜索的极线段长度是否过长，超出阈值则调整pclosep_{close}pclose?，使总长度为MAX_EPL_LENGTH_CROP(30)个像素。

然后计算搜索步长，设极线上的搜索区间的pclosetextbf p_{close}pclose?到pfartextbf p_{far}pfar?的长度为ΔlDelta lΔl，x、y方向的增量为Δx,ΔyDelta x, Delta yΔx,Δy，则搜索步长为
(Δx-Δl,Δy-Δl)(Delta x-Delta l, Delta y-Delta l)(Δx-Δl,Δy-Δl)。

然后判断pclosetextbf p_{close}pclose?和pfartextbf p_{far}pfar?是否在图像内和极线段长度是否过短。

如果极线段过短，则设置为MIN_EPL_LENGTH_CROP，如果近点在图像外，就把近点沿极线方向移到图像内，然后再次判断极线段是否符合要求。

接下来就是遍历极线段进行灰度匹配：
float cpx = pFar[0];
float cpy = pFar[1];
float val_cp_m2 = getInterpolatedElement(referenceFrameImage,cpx-2.0f*incx,
cpy-2.0f*incy, width);
float val_cp_m1 = getInterpolatedElement(referenceFrameImage,cpx-incx, cpy-incy, width);
float val_cp = getInterpolatedElement(referenceFrameImage,cpx, cpy, width);
float val_cp_p1 = getInterpolatedElement(referenceFrameImage,cpx+incx, cpy+incy, width);
float val_cp_p2;
经过遍历得到误差最小的匹配位置后，判断最小误差是不是大于阈值，
如果误差过大，返回-3，否则返回-2。

if(best_match_err 4.0f*(float)MAX_ERROR_STEREO)
if(enablePrintDebugInfo)
stats-num_stereo_invalid_bigErr++;
return -3;
-- check if clear enough winner
if(abs(loopCBest - loopCSecond) 1.0f MIN_DISTANCE_ERROR_STEREO * best_match_err second_best_match_err)
if(enablePrintDebugInfo)
stats-num_stereo_invalid_unclear_winner++;
return -2;
计算亚像素精度位置
记上一步遍历得到的误差最小匹配位置为p1textbf p_1p1?，前一个位置为p0textbf p_0p0?，后一个位置为p2textbf p_2p2?，计算误差关于这3个位置的梯度
g0+,g1?,g1+,g2?g_{0^+},g_{1^-},g_{1^+},g_{2^-}g0+?,g1?,g1+?,g2?。

然后判断g0+,g1?g_{0^+},g_{1^-}g0+?,g1?和g1+,g2?g_{1^+},g_{2^-}g1+?,g2?哪一对异号，取异号的那一对计算梯度为0的位置，然后利用泰勒展开重新计算匹配误差。

然后判断新误差是否满足要求，这里还计算了当前关键帧极线段上的梯度平方和gradAlongLine，gradAlongLine越大，对误差的容忍就越大。

计算当前关键帧下的深度
得到匹配点prtextbf p_rpr?后，就可以计算逆深度和αalphaα(逆深度和视差的比值)，推导过程如下：
pk=(xkyk1),pr=(xryr1)=K?1(urvr1)
textbf p_k = begin{pmatrix}
end{pmatrix},
textbf p_r = begin{pmatrix}
end{pmatrix} = textbf K^{-1}begin{pmatrix}
end{pmatrix}
pk?=?xk?yk?1?,pr?=?xr?yr?1?=K?1?ur?vr?1?
根据两帧间已知的位姿变换，有
pr=Rpkd?1+t=(r0r1r2)pkd?1+(txtytz)=(r0pkd?1+txr1pkd?1+tyr2pk d?1+tz)
textbf p_r = textbf Rtextbf p_kd^{-1} + textbf t = begin{pmatrix} textbf r_0
textbf r_1
textbf r_2
end{pmatrix}textbf p_kd^{-1} + begin{pmatrix}
textbf t_x
textbf t_y
textbf t_z
end{pmatrix} = begin{pmatrix}
textbf r_0textbf p_kd^{-1} + t_x
textbf r_1textbf p_kd^{-1} + t_y
textbf r_2textbf p_kd^{-1} + t_z
end{pmatrix}
pr?=Rpk?d?1+t=?r0?r1?r2?pk?d?1+?tx?ty?tz?=?r0?pk?d?1+tx?r1?p k?d?1+ty?r2?pk?d?1+tz?
为了求解其中的ddd，只需要一个方程就可以了，为了减少误差，选择极线段在u,v方向上分量较长的方向来求解，以下列举u方向上求解ddd。

xr=r0pkd?1+txr2pkd?1+tz
x_r = frac{textbf r_0textbf p_kd^{-1} + t_x}{textbf r_2textbf p_kd^{-1} + t_z}
xr?=r2?pk?d?1+tz?r0?pk?d?1+tx?
d=r0pk?r2pkxrtzxr?tx
d = frac{textbf r_0textbf p_k - textbf r_2textbf p_kx_r}{t_zx_r- t_x}
d=tz?xr?tx?r0?pk?r2?pk?xr?
有了ddd的表达式，αalphaα也可以推导出来：
α=?d?λ=?d?x?x?u?u?λ=r2pkxr?r0pktz(tx?tzx)2fxinv?u?λ
alpha = frac{partial d}{partiallambda}=frac{partial d}{partial x}frac{partial x}{partial u}frac{partial u}{partiallambda} = frac{textbf r_2textbf p_kx_r - textbf r_0textbf p_kt_z}{(t_x - t_zx)^2}f_x^{inv}frac{partial u}{partiallambda}
α=?λ?d?=?x?d?u?x?λ?u?=(tx?tz?x)2r2?pk?xr?r0?pk?tz?fxinv?
λ?u?
计算逆深度方差
逆深度方差为：
σd,obs2=α2(σλ(ξ,π)2+σλ(I)2)
sigma_{d,obs}^2 = alpha^2(sigma_{lambda_(xi,pi)}^2 + sigma_{lambda(textbf I)}^2)
σd,obs2?=α2(σλ(?ξ,π)2?+σλ(I)2?)
其中σλ(ξ,π)sigma_{lambda_(xi,pi)}σλ(?ξ,π)?表示极线搜索的几何误差：
σλ(ξ,π)=?g,g?σl2?g,l?2,σλ(I)2=2σi2gp2
sigma_{lambda_(xi,pi)} = frac{langletextbf g,textbf granglesigma_l^2}{langletextbf g, textbf lrangle^2}, sigma_{lambda(textbf I)}^2 = frac{2sigma_i^2}{g_p^2} σλ(?ξ,π)?=?g,l?2?g,g?σl2?,σλ(I)2?=gp2?2σi2?
而且σd,obs2sigma_{d,obs}^2σd,obs2?有上界，
σd,obs2≤α2(min?{σλ(ξ,π)2}+min?{σλ(I)2})
sigma_{d,obs}^2 le alpha^2(min{sigma_{lambda_(xi,pi)}^2} + min{sigma_{lambda(textbf I)}^2})
σd,obs2?≤α2(min{σλ(?ξ,π)2?}+min{σλ(I)2?})
这跟代码中是一致的：
-- ================= calc var (in NEW image) ====================
-- calculate error from photometric noise
float photoDispError = 4.0f * cameraPixelNoise2 - (gradAlongLine + DIVISION_EPS);
float trackingErrorFac = 0.25f*(1.0f+referenceFrame-initialTrackedResidual);
-- calculate error from geometric noise (wrong camera pose - calibration)
Eigen::Vector2f gradsInterp = getInterpolatedElement42(activeKeyFrame-gradients(0), u, v, width);
float geoDispError = (gradsInterp[0]*epxn + gradsInterp[1]*epyn) + DIVISION_EPS;
geoDispError = trackingErrorFac*trackingErrorFac*(gradsInterp[0]*gradsInterp[0] + gradsInterp[1]*gradsInterp[1]) - (geoDispError*geoDispError);
--geoDispError *= (0.5 + 0.5 *result_idepth) * (0.5 + 0.5 *result_idepth);
-- final error consists of a small constant part (discretization error),
-- geometric and photometric error.
result_var = alpha*alpha*((didSubpixel ? 0.05f : 0.5f)*sampleDist*sampleDist + geoDispError + photoDispError); --
其中trackingErrorFac就是公式中的σlsigma_lσl?，在代码中还加了一点离散误差sampleDist。

3.2.2 observeDepthUpdate
这一步即进行深度更新，进行深度更新与创建深度有些不同的地方。

极线搜索范围的选择
-- which exact point to track, and where from.
float sv = sqrt(target-idepth_var_smoothed);
float min_idepth = target-idepth_smoothed - sv*STEREO_EPL_VAR_FAC;
float max_idepth = target-idepth_smoothed + sv*STEREO_EPL_VAR_FAC;
if(min_idepth 0) min_idepth = 0;
if(max_idepth 1-MIN_DEPTH) max_idepth = 1-MIN_DEPTH;
极线搜索范围由min_idepth和max_idepth决定，而这两个值由当前的先验信息决定。

逆深度融合
在执行doLineStereo后会得到新的逆深度以及方差，融合的过程是一个标准的EKF更新，也可以看成是两个高斯分布相乘。

代码中还对对先验方差增加了一点不确定性，其中SUCC_VAR_INC_FAC = 1.01f。

-- do textbook ekf update:
-- increase var by a little (prediction-uncertainty)
float id_var = target-idepth_var*SUCC_VAR_INC_FAC;
-- update var with observation
float w = result_var - (result_var + id_var);
float new_idepth = (1-w)*result_idepth + w*target-idepth;
target-idepth = UNZERO(new_idepth);
3.3 regularizeDepthMapFillHoles
这个函数的功能是对深度图中没有被列入黑名单但是又没有进行立体匹配的像素点创建深度假设，为了提高效率，采用了多线程并行计算的方式。

该函数首先是计算具有有效逆深度像素的积分图，即执行buildRegIntegralBuffer()，该函数也是利用多线程的方式进行并行计算。

得到的积分图存在validityIntegralBuffer中，然后执行
threadReducer.reduce(boost::bind(DepthMap::regularizeDepthMa pFillHolesRow, this, _1, _2, _3), 3, height-2, 10);
来填补深度。

如果某个点没有被列入黑名单且周围5x5领域内有足够的已经具有逆深度的有效点，就把领域内的有效点的逆深度和方差进行加权平均作为自己的深度假设。

3.4 regularizeDepthMap
这个函数主要是对当前关键帧的深度图用5x5的窗口进行平滑，也是采用多线程并行计算的方式。

如果窗口内有效的逆深度太少，就把当前点置为无效并且加入黑名单。

平滑的过程可以看成一个标准高斯概率密度相乘的算法。

这里有一个标志位removeOcclusions，默认为false，如果设置为true，如果领域内其他点逆深度与中心点逆深度相差过大，就把它阻塞，如果阻塞的点大于不阻塞的点，就把中心点设置为无效。

在构造新的关键帧时，会调用2次regularizeDepthMap，并且第一次设置removeOcclusions = true
4 SlamSystem::changeKeyframe
在进行图像跟踪的时候如果发现当前图像距离当前关键帧距离大于一定值，就把createNewKeyFrame置为true，从而允许创建新的关键帧。

if (createNewKeyFrame)
finishCurrentKeyframe();
changeKeyframe(false, true, 1.0f);
lambda^*＝min_{lambda}(i_{ref}-I_p(lambda_0)-g_pDeltalambda)^2 tag{14} MATLAB的.p文件是.m文件的加密形式(为了防止算法的暴露),在调用的时候优先级大于.m文件,也是根据文件名来调用.
double start = double(getTickCount());
论文回顾之一一种新的直线段检测算法---LSD：a Line Segment Detector
在上述算法中，还有两个要点我们没有解释。

一是line support region具体是怎么得到了，二是怎样进行误差控制的。

bdm-match(left_lbd, right_lbd, matches);
如果有哪些地方表述的不够得体和清晰，有存在的任何问题，欢迎评论和指正，谢谢各路大佬。

为了求解其中的ddd，只需要一个方程就可以了，为了减少误差，选择极线段在u,v方向上分量较长的方向来求解，以下列举u方向上求解ddd。

boldsymbol{P_1=R^{-1}(P_2-t)=R'P_2+t'=}dboldsymbol{R'K^{-1}p _2+t'}
从图中我们可以发现LSD算法正如其名字一样，从低位开始保证其有序(红框内部分)；当然，只要熟练掌握键索引计数法，LSD的编写将会很简单；具体代码如下：。