LSD-SLAM笔记之一致性约束
来源:互联网 发布:java char 编辑:程序博客网 时间:2024/05/20 01:08
建图一致性约束也就是做闭环检测和全局优化。由于论文中采用的是直接法,虽然代码中有fabmap
检测闭环的部分,但是其默认检测闭环的方式是帧与帧之间做双向跟踪。
这部分的代码入口在函数SlamSystem::constraintSearchThreadLoop,代码主要分了两种情况:
- 新关键帧队列为空,则在所有关键帧中随机选取测试闭环
- 新关键帧队列不为空,则取最早的新关键帧测试闭环
在测试闭环时调用的函数为SlamSystem::findConstraintsForNewKeyFrames。该函数主要是根据视差、关键帧连接关系,找出并且在删选处候选帧,然后对每个候选帧和测试的关键帧之间进行双向sim3跟踪,如果求解出的两个李代数满足马氏距离在一定范围内,则认为是闭环成功,并且在位姿图中添加边的约束。而图优化的部分在另外一个线程SlamSystem::optimizationThreadLoop,基本就是使用g2o,这部分就略过了。
关于如何选择候选帧,主要步骤如下:
- 视角和距离判别
- SE3跟踪检测
- Sim3跟踪检测
其实第一个步骤是根据每个关键帧求得的位姿取判断的,其实这是一个因果倒置的方式。在代码中把候选帧分为近处的候选帧和远处的候选帧,对于第二个步骤,只是检测处与当前闭环检测关键帧相近的候选帧。第三步则是核心,把删选处的每一个候选帧与检测的关键帧做双向的Sim3跟踪,如果都成功,并且两个
接下来主要看一下Sim(3)求解以及双向跟踪检测(reciprocal tracking check)。
1. Sim3求解
1.1 原理分析
1.1.1 Sim3图像对齐
首先和
然后我们看代价函数,
这里的光度残差和方差的定义和SE3跟踪是一样的:
以及深度残差和方差定义如下:
这里的
注意:论文中提及,把深度图的梯度近似为
0 ,因此对Dj(u) 求的偏导都可以忽略。
根据高斯牛顿算法,对
对
这就是最终求的
1.1.2 归一化方差
我们需要计算每个位置处的归一化方差。对于已知的向量
根据上式我们可以求得式子
代码在实现上和SE3跟踪一样,忽略旋转使用了近似:
从而有
对其求偏导可得
求得雅可比后,就可以求得对应的方差。
疑问1:这里和之前求
(4) 一样,都是近似可以忽略旋转的情况下计算的。在求得雅可比之后,变量尽量都用p′=(x′,y′,z′)T 的数据来表示,而不是用参考帧上的数据来表示呢?就以(9) 为例,完全可以化作1(1/d+tz)2d2 的形式。这两者有什么优劣差别?
1.1.3 残差雅可比
在Sim3中比SE3多了一个逆深度的雅克比。首先我们看一下光度残差的雅克比,这里和SE3部分唯一不同的地方在连式法则展开后最后一项变换模型有所差异,一个是对
我们可以看到最后一项是0,也就是说忽略最后一个0,对
然后我们看对逆深度
我们先看第二个雅可比
由于深度图的梯度近似为
这里的雅可比的第一项是逆深度对3D点求偏导,第二项是
1.1.4 ESM
由于图像是非凸的,因此对图像对齐问题,我们需要一个很好的初始化位置。但是对于查找闭环约束的时候,这个条件不容易满足,因此论文中提出了两种增加收敛半径的方法:使用高效二阶最小化方法(Efficient Second Order Minimization,ESM);从粗到细(Coarse-to-Fine)方法,也就是图像金字塔的方式。第二种方法是比较常见的,对于第一种方法之前没有碰到过ESM方法,这里将对ESM方法做个简明的介绍。
我们设代价函数为
这里的
把
对于最小化残差作为代价函数的情况,有
\mathbf J_{\text{esm}} = \frac{1}{2}\left(\frac{\partial I(\mathbf u)}{\partial\mathbf u}\bigg|_{\mathbf u = \omega(\mathbf p,\mathbf X)} + \frac{\partial I^*(\mathbf u)}{\partial\mathbf u}\bigg|_{\mathbf u = \omega(\mathbf 0,\mathbf X)}\right) \frac{\partial\omega(\mathbf x)}{\partial\mathbf x}\bigg|_{\mathbf x = \mathbf 0} \label{eq:esmjac}
\end{equation}
我们可以看出,ESM的雅可比是混合了两幅图像的梯度。
代码中其他步骤基本都和SE3跟踪相同。对于ESM的详细介绍见论文Real-time image-based tracking of planes using efficient second-order minimization。
1.2 代码剖析
Sim3有几个主要的函数,基本和SE3跟踪是一样的。接下里看一下这些代码。
1.2.1 Sim3Tracker::trackFrameSim3
这是Sim3跟踪的主体函数,跟踪图像帧reference
到frame
求相应的Sim3变换。
Sim3 Sim3Tracker::trackFrameSim3( TrackingReference* reference, Frame* frame, const Sim3& frameToReference_initialEstimate, int startLevel, int finalLevel)
由于主要的框架和SE3都是一样的,我们只看一下具体实现的那几个函数。
1.2.2 Sim3Tracker::calcSim3Buffers
我们看下代码,首先得到一些全局不变量,这里的rotMat
对应初始Sim3中的rotMatUnscaled
为transVec
是
// get static values int w = frame->width(level); int h = frame->height(level); Eigen::Matrix3f KLvl = frame->K(level); float fx_l = KLvl(0,0); float fy_l = KLvl(1,1); float cx_l = KLvl(0,2); float cy_l = KLvl(1,2); Eigen::Matrix3f rotMat = referenceToFrame.rxso3().matrix().cast<float>(); Eigen::Matrix3f rotMatUnscaled = referenceToFrame.rotationMatrix().cast<float>(); Eigen::Vector3f transVec = referenceToFrame.translation().cast<float>();
接下来计算了图像平面沿着光轴方向的旋转。我们可以把两帧的相对旋转分为主轴的旋转以及沿着主轴方向的旋转。
// Calculate rotation around optical axis for rotating source frame gradients Eigen::Vector3f forwardVector(0, 0, -1); Eigen::Vector3f rotatedForwardVector = rotMatUnscaled * forwardVector; Eigen::Quaternionf shortestBackRotation; shortestBackRotation.setFromTwoVectors(rotatedForwardVector, forwardVector); Eigen::Matrix3f rollMat = shortestBackRotation.toRotationMatrix() * rotMatUnscaled; float xRoll0 = rollMat(0, 0); float xRoll1 = rollMat(0, 1); float yRoll0 = rollMat(1, 0); float yRoll1 = rollMat(1, 1);
这里的setFromTwoVectors
函数,计算得到从rotatedForwardVector
到forwardVector
的旋转,然后乘以rotMatUnscaled
后就是图像平面沿着主轴方向forwardVector
的旋转。这里主要是在ESM求梯度时使用的。
在遍历前获取参考帧的3D点、灰度、梯度、图像帧的逆深度、逆深度方差和灰度、梯度的指针。
const Eigen::Vector3f* refPoint_max = reference->posData[level] + reference->numData[level]; const Eigen::Vector3f* refPoint = reference->posData[level]; const Eigen::Vector2f* refColVar = reference->colorAndVarData[level]; const Eigen::Vector2f* refGrad = reference->gradData[level]; const float* frame_idepth = frame->idepth(level); const float* frame_idepthVar = frame->idepthVar(level); const Eigen::Vector4f* frame_intensityAndGradients = frame->gradients(level);
然后遍历每个参考帧的3D点,把点通过初始的Sim3变换到图像帧的坐标系下,并且查看对应投影点是不是在图像平面范围内。
for(;refPoint<refPoint_max; refPoint++, refGrad++, refColVar++) { Eigen::Vector3f Wxp = rotMat * (*refPoint) + transVec; float u_new = (Wxp[0]/Wxp[2])*fx_l + cx_l; float v_new = (Wxp[1]/Wxp[2])*fy_l + cy_l; // step 1a: coordinates have to be in image: // (inverse test to exclude NANs) if(!(u_new > 1 && v_new > 1 && u_new < w-2 && v_new < h-2)) continue;
把数据保存到缓存变量中。如果使用了ESM,则把参考帧上的梯度方向旋转到当前帧的方向。并且这里的的梯度就是按照公式
*(buf_warped_x+idx) = Wxp(0); *(buf_warped_y+idx) = Wxp(1); *(buf_warped_z+idx) = Wxp(2); Eigen::Vector3f resInterp = getInterpolatedElement43(frame_intensityAndGradients, u_new, v_new, w); // save values#if USE_ESM_TRACKING == 1 // get rotated gradient of point float rotatedGradX = xRoll0 * (*refGrad)[0] + xRoll1 * (*refGrad)[1]; float rotatedGradY = yRoll0 * (*refGrad)[0] + yRoll1 * (*refGrad)[1]; *(buf_warped_dx+idx) = fx_l * 0.5f * (resInterp[0] + rotatedGradX); *(buf_warped_dy+idx) = fy_l * 0.5f * (resInterp[1] + rotatedGradY);#else *(buf_warped_dx+idx) = fx_l * resInterp[0]; *(buf_warped_dy+idx) = fy_l * resInterp[1];#endif
疑问2:在ESM求雅克比的时候,对参考帧像素位置求的雅克比得到的梯度为什么需要旋转到图像帧的方向上来?
这里和SE3一样,在计算残差的时候也是使用了迷之仿射变换,先不管这个问题。
float c1 = affineEstimation_a * (*refColVar)[0] + affineEstimation_b; float c2 = resInterp[2]; float residual_p = c1 - c2; float weight = fabsf(residual_p) < 2.0f ? 1 : 2.0f / fabsf(residual_p); sxx += c1*c1*weight; syy += c2*c2*weight; sx += c1*weight; sy += c2*weight; sw += weight; *(buf_warped_residual+idx) = residual_p; *(buf_idepthVar+idx) = (*refColVar)[1];
接下来的代码是Sim3特有的部分,计算逆深度的残差,如果当前图像帧对应位置的逆深度方差不为零(对应位置若为-1则表示没有深度),则在缓存变量中赋值。
// new (only for Sim3): int idx_rounded = (int)(u_new+0.5f) + w*(int)(v_new+0.5f); float var_frameDepth = frame_idepthVar[idx_rounded]; float ref_idepth = 1.0f / Wxp[2]; *(buf_d+idx) = 1.0f / (*refPoint)[2]; if(var_frameDepth > 0) { float residual_d = ref_idepth - frame_idepth[idx_rounded]; *(buf_residual_d+idx) = residual_d; *(buf_warped_idepthVar+idx) = var_frameDepth; } else { *(buf_residual_d+idx) = -1; *(buf_warped_idepthVar+idx) = -1; }
然后这个函数主要的功能都完成了。与SE3Tracker
中的calcResidualAndBuffers相对应,差别在于计算梯度的时候按照ESM求的,并且多了两个变量:逆深度残差buf_residual_d
和当前图像帧对应逆深度的方差buf_warped_idepthVar
。
1.2.3 Sim3Tracker::calcSim3WeightsAndResidual
这个函数主要计算对应的归一化方差以及最终的残差。这里和SE3Tracker
中的calcWeightsAndResidual对应,都有旋转很小的假设,认为只有平移。
float tx = referenceToFrame.translation()[0]; float ty = referenceToFrame.translation()[1]; float tz = referenceToFrame.translation()[2];
接下来遍历每个点,得到buff中对应的数据。
for(int i=0;i<buf_warped_size;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 rd = *(buf_residual_d+i); // r_d 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 float sv = settings.var_weight * *(buf_warped_idepthVar+i); // \sigma_d^2'
接下来计算归一化方差,这里比SE3多了g2
。通过公式w_d
计算如下
// calc dw/dd (first 2 components): float g0 = (tx * pz - tz * px) / (pz*pz*d); float g1 = (ty * pz - tz * py) / (pz*pz*d); float g2 = (pz - tz) / (pz*pz*d); // calc w_p float drpdd = gx * g0 + gy * g1; // ommitting the minus float w_p = 1.0f / (cameraPixelNoise2 + s * drpdd * drpdd); float w_d = 1.0f / (sv + g2*g2*s);
然后计算对应的Huber权重
float weighted_rd = fabs(rd*sqrtf(w_d)); float weighted_rp = fabs(rp*sqrtf(w_p)); float weighted_abs_res = sv > 0 ? weighted_rd+weighted_rp : weighted_rp; float wh = fabs(weighted_abs_res < settings.huber_d ? 1 : settings.huber_d / weighted_abs_res);
最后把保存光度误差的权重和逆深度误差的权重。
*(buf_weight_p+i) = wh * w_p; if(sv > 0) *(buf_weight_d+i) = wh * w_d; else *(buf_weight_d+i) = 0;
1.2.4 Sim3Tracker::calcSim3LGS
这个函数求高斯牛顿迭代的雅克比了。函数传进来的是一个7个参数的最小二乘的类。这个函数与的SE3Tracker
的函数calculateWarpUpdate对应,比SE3多了一个参数,就是尺度。在函数开始的时候,初始化了两个最小二乘的类,一个是4个参数的,一个是6个参数的。6个参数的对应于光度残差的雅克比,4参数对应逆深度残差的雅克比,这里都不是7,因为都先不保存非零项。
void Sim3Tracker::calcSim3LGS(NormalEquationsLeastSquares7 &ls7){ NormalEquationsLeastSquares4 ls4; NormalEquationsLeastSquares ls6; ls6.initialize(width*height); ls4.initialize(width*height);
然后看循环,遍历每个点
for(int i=0;i<buf_warped_size;i++) { float px = *(buf_warped_x+i); // x' float py = *(buf_warped_y+i); // y' float pz = *(buf_warped_z+i); // z' float wp = *(buf_weight_p+i); // wr/wp float wd = *(buf_weight_d+i); // wr/wd float rp = *(buf_warped_residual+i); // r_p float rd = *(buf_residual_d+i); // r_d float gx = *(buf_warped_dx+i); // \delta_x I float gy = *(buf_warped_dy+i); // \delta_y I float z = 1.0f / pz; float z_sqr = 1.0f / (pz*pz);
先计算光度残差的雅克比,这里和公式
Vector6 v; Vector4 v4; v[0] = z*gx + 0; v[1] = 0 + z*gy; v[2] = (-px * z_sqr) * gx + (-py * z_sqr) * gy; v[3] = (-px * py * z_sqr) * gx + (-(1.0 + py * py * z_sqr)) * gy; v[4] = (1.0 + px * px * z_sqr) * gx + (px * py * z_sqr) * gy; v[5] = (-py * z) * gx + (px * z) * gy;
然后是逆深度残差的雅克比,回顾一下公式
下面这部分代码和公式中的第2、3、4、6位置对应(从0开始)。
v4[0] = z_sqr; v4[1] = z_sqr * py; v4[2] = -z_sqr * px; v4[3] = z;
然后分别求对应的
这里是先求
l6
,这里的雅克比差了一个负号: ls6.update(v, rp, wp); // Jac = - v
对应调用了NormalEquationsLeastSquares4::update:
inline void NormalEquationsLeastSquares::update(const Vector6& J, const float& res, const float& weight){ A_opt.rankUpdate(J, weight); b -= J * (res * weight); error += res * res * weight; num_constraints += 1;}
很明显,这里求的时候
然后是求l4
:
ls4.update(v4, rd, wd); // Jac = v4
对应了NormalEquationsLeastSquares4::update:
inline void NormalEquationsLeastSquares4::update(const Vector4& J, const float& res, const float& weight){ // TODO: SSE optimization A.noalias() += J * J.transpose() * weight; b.noalias() -= J * (res * weight); error += res * res * weight; num_constraints += 1;}
函数的最后计算7参数的最小二乘,这里调用的finish
是没有除以约束个数的(SE3中直接除以约束个数了),所以是NoDivide
:
ls4.finishNoDivide(); ls6.finishNoDivide(); ls7.initializeFrom(ls6, ls4);
我们查看NormalEquationsLeastSquares7::initializeFrom:
void NormalEquationsLeastSquares7::initializeFrom(const NormalEquationsLeastSquares& ls6, const NormalEquationsLeastSquares4& ls4){ // set zero A.setZero(); b.setZero(); // add ls6 A.topLeftCorner<6,6>() = ls6.A; b.head<6>() = ls6.b; // add ls4 int remap[4] = {2,3,4,6}; for(int i=0;i<4;i++) { b[remap[i]] += ls4.b[i]; for(int j=0;j<4;j++) A(remap[i], remap[j]) += ls4.A(i,j); } num_constraints = ls6.num_constraints + ls4.num_constraints;}
这里按照公式l6
部分,然后把逆深度残差按照在矩阵块的位置对应拷贝。
最终回到Sim3Tracker::trackFrameSim3的位置就是最终求解增量的部分:
// solve LS system with current lambda Vector7 b = - ls7.b / ls7.num_constraints; Matrix7x7 A = ls7.A / ls7.num_constraints; for(int i=0;i<7;i++) A(i,i) *= 1+LM_lambda; Vector7 inc = A.ldlt().solve(b);
这里和SE3一样,最终把
2. 双向跟踪检测
论文中提及,为了避免误检测,采用双向跟踪检测的方式,对每一个候选帧都计算其与检测关键帧彼此跟踪的
e\left(\mathbf\xi_{j_k i}, \mathbf\xi_{i j_k}\right) := \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)^T \left(\mathbf\Sigma_{j_k i} + \text{Adj}_{j_k i}\mathbf\Sigma_{i j_k}\text{Adj}_{j_k i}^T\right)^{-1} \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)\label{eq:mdis}
\end{equation}
其实双向跟踪在一开始选取临近的候选帧的时候做SE3跟踪也用到了,但是只是对其中的
公式
有
因此有方差
上面只是给出了大概的推导,但是在tangent space的方差传播没有看过具体的材料,也不是很懂。
3. 总结
到这篇总结结束,LSD-SLAM主要的算法和代码都已经总结完毕了。在本篇总结中,主要是Sim3跟踪,论文中使用双向跟踪的方式,来确保两帧图像是匹配的。这样的策略以前在光流跟踪中看到过,但是用在图像匹配上还是第一次,但是理论上这都是一致的,只是变换的模型有差异而已。
另外这里有几个疑问:
- 不论是在SE3跟踪还是Sim3跟踪,在求归一化方差的时候,都使用了旋转微小的假设。在求雅可比的时候,坐标等变量的值是使用近似模型(小旋转假设前提下的)的坐标还是使用完整变换模型下的值呢?会有什么影响?
- 在ESM求雅克比的时候,对参考帧像素位置求的雅克比得到的梯度为什么需要旋转到图像帧的方向上来?
- 李代数tangent space上误差传播的计算不是很明确。
参考:
- Semi-Dense Visual Odometry for a Monocular Camera
- LSD-SLAM: Large-Scale Direct Monocular SLAM
- Efficient Compositional Approaches for Real-Time Robust Direct Visual Odometry from RGB-D Data
- Real-time image-based tracking of planes using efficient second-order minimization
- https://en.wikipedia.org/wiki/Mahalanobis_distance ↩
- LSD-SLAM笔记之一致性约束
- LSD-SLAM笔记之SE3Tracking
- LSD-SLAM笔记之DepthMap
- LSD-SLAM使用教程
- LSD-SLAM使用教程
- LSD-SLAM使用方法
- LSD-SLAM 安装运行
- LSD-SLAM jade 安装 catkin_make
- Ubuntu16.04 install lsd-slam
- Ubuntu14.04安装LSD-SLAM
- Ubuntu16.04安装LSD-SLAM
- lsd-slam源码解读第二篇:DataStructures
- lsd-slam源码解读第四篇:tracking
- lsd-slam源码解读第五篇:DepthEstimation
- Ubuntu 14.04+ROS Indigo+LSD-SLAM
- 一致性约束
- 基数排序之LSD
- 基数排序之LSD
- POJ-3723 Conscription
- JS技术(3)---节点增删
- js实现贪吃蛇小游戏
- CSS盒模式总结
- 一个智障的求逆序对的问题
- LSD-SLAM笔记之一致性约束
- svn在linux下启动命令
- 解决Maven新建webapp项目index.jsp报错
- CCF 2017 03 04修地铁(dijkstra变形)
- 欢迎使用CSDN-markdown编辑器
- HDU 5538 House Building——思路题
- Calico 的网络结构是什么?- 每天5分钟玩转 Docker 容器技术(68)
- FCIS算法的MXNet实现
- Oracle 学习(一)---数据库基本操作