就SE3Tracker.cpp中trackFrame
// ============ track frame ============
//将初始估计记录下来(记录了参考帧到当前帧的刚度变换),然后定义一个6自由度矩阵的误差判别计算对象ls,定义cell数量以及最终的残差
Sophus::SE3f referenceToFrame = frameToReference_initialEstimate.inverse().cast<float>();
LGS6 ls;
int numCalcResidualCalls[PYRAMID_LEVELS]; //cell数量 5层 金字塔
int numCalcWarpUpdateCalls[PYRAMID_LEVELS];
float last_residual = 0; //最终残差
for(int lvl=SE3TRACKING_MAX_LEVEL-1;lvl >= SE3TRACKING_MIN_LEVEL;lvl--) //金字塔的迭代从level-4到level-1
{
numCalcResidualCalls[lvl] = 0;
numCalcWarpUpdateCalls[lvl] = 0;
//有参考帧的逆深度信息和对应的像素点坐标 计算在参考帧坐标系下的3D点
reference->makePointCloud(lvl);
//对参考帧当前层构建点云，然后使用callOptimized函数调用calcResidualAndBuffers计算参考帧3D点变换到当前帧的残差(灰度)和(对应点像素)梯度,
/// 参数为后面括号里面的所有数据，这是调用的宏定义
callOptimized(calcResidualAndBuffers, (reference->posData[lvl], reference->colorAndVarData[lvl], SE3TRACKING_MIN_LEVEL == lvl ? reference->pointPosInXYGrid[lvl] : 0, reference->numData[lvl], frame, referenceToFrame, lvl, (plotTracking && lvl == SE3TRACKING_MIN_LEVEL)));
// buf_warped_size 记录了 匹配点数量 包括好的匹配点 和 不好的匹配点 数量
//判断这个warp的好坏(如果改变的太多，已经超过了这一层图片的1%)，那么我们认为差别太大，Tracking失败，返回一个空的SE3
if(buf_warped_size < MIN_GOODPERALL_PIXEL_ABSMIN * (width>>lvl)*(height>>lvl))
{
diverged = true;
trackingWasGood = false;
return SE3();
}
//是否使用了简单的仿射变换,
// 那么把通过calcResidualAndBuffers函数更新的affineEstimation_a_lastIt以及affineEstimation_b_lastIt，赋值给仿射变换系数
if(useAffineLightningEstimation)
{
affineEstimation_a = affineEstimation_a_lastIt;
affineEstimation_b = affineEstimation_b_lastIt;
}
//调用calcWeightsAndResidual用方差归一化残差，并记录调用次数
///这里直接使用calcResidualAndBuffers的结果.
float lastErr = callOptimized(calcWeightsAndResidual,(referenceToFrame));
numCalcResidualCalls[lvl]++;
float LM_lambda = settings.lambdaInitial[lvl]; //λ LM优化的 调整量 与高斯牛顿的区别之处 (A+λI)x=b
for(int iteration=0; iteration < settings.maxItsPerLvl[lvl]; iteration++)
{
//计算雅克比向量以及A和b
// 计算 加权平均误差 以及误差权重
///这一步跟上面的calcWeightsAndResidual没有关系.
callOptimized(calculateWarpUpdate,(ls));
numCalcWarpUpdateCalls[lvl]++;
iterationNumber = iteration;
int incTry=0;
while(true)
{
// solve LS system with current lambda
Vector6 b = -ls.b;
Matrix6x6 A = ls.A;
for(int i=0;i<6;i++) A(i,i) *= 1+LM_lambda; //(A+λI) LM 调整量
//计算收敛的delta更新SE3
//LDLT矩阵分解 dse3李代数更新量
Vector6 inc = A.ldlt().solve(b);
incTry++;
// apply increment. pretty sure this way round is correct, but hard to test.
// 对 变换矩阵 左乘 se3指数映射 进行 更新
Sophus::SE3f new_referenceToFrame = Sophus::SE3f::exp((inc)) * referenceToFrame;
//Sophus::SE3f new_referenceToFrame = referenceToFrame * Sophus::SE3f::exp((inc));
// re-evaluate residual
callOptimized(calcResidualAndBuffers, (reference->posData[lvl], reference->colorAndVarData[lvl], SE3TRACKING_MIN_LEVEL == lvl ? reference->pointPosInXYGrid[lvl] : 0, reference->numData[lvl], frame, new_referenceToFrame, lvl, (plotTracking && lvl == SE3TRACKING_MIN_LEVEL)));
if(buf_warped_size < MIN_GOODPERALL_PIXEL_ABSMIN* (width>>lvl)*(height>>lvl))
{
// 匹配点数量记录 包括好的匹配点 和 不好的匹配点 数量
// 匹配点数量少于阈值 优化失败
diverged = true;
trackingWasGood = false;
return SE3();
}
// 新变换下的误差
float error = callOptimized(calcWeightsAndResidual,(new_referenceToFrame));
numCalcResidualCalls[lvl]++;
// accept inc?
if(error < lastErr) //误差变小
{
// accept inc
// 更新 新求解的 变换矩阵
referenceToFrame = new_referenceToFrame;
if(useAffineLightningEstimation)
{
// 更新 仿射变换系数
affineEstimation_a = affineEstimation_a_lastIt;
affineEstimation_b = affineEstimation_b_lastIt;
}
// 打印调试信息
if(enablePrintDebugInfo && printTrackingIterationInfo)
{
// debug output
printf("(%d-%d): ACCEPTED increment of %f with lambda %.1f, residual: %f -> %f\n",
lvl,iteration, sqrt(inc.dot(inc)), LM_lambda, lastErr, error);
printf(" p=%.4f %.4f %.4f %.4f %.4f %.4f\n",
referenceToFrame.log()[0],referenceToFrame.log()[1],referenceToFrame.log()[2],
referenceToFrame.log()[3],referenceToFrame.log()[4],referenceToFrame.log()[5]);
}
// converged?
if(error / lastErr > settings.convergenceEps[lvl])
{
if(enablePrintDebugInfo && printTrackingIterationInfo)
{
printf("(%d-%d): FINISHED pyramid level (last residual reduction too small).\n",
lvl,iteration);
}
iteration = settings.maxItsPerLvl[lvl];
}
last_residual = lastErr = error;
if(LM_lambda <= 0.2)
LM_lambda = 0;
else
LM_lambda *= settings.lambdaSuccessFac;
break;
}
else // 误差反而没有减少
{
if(enablePrintDebugInfo && printTrackingIterationInfo)
{
printf("(%d-%d): REJECTED increment of %f with lambda %.1f, (residual: %f -> %f)\n",
lvl,iteration, sqrt(inc.dot(inc)), LM_lambda, lastErr, error);
}
if(!(inc.dot(inc) > settings.stepSizeMin[lvl]))
{
if(enablePrintDebugInfo && printTrackingIterationInfo)
{
printf("(%d-%d): FINISHED pyramid level (stepsize too small).\n",
lvl,iteration);
}
iteration = settings.maxItsPerLvl[lvl];
break;
}
// 调整 LM 参数
if(LM_lambda == 0)
LM_lambda = 0.2;
else
LM_lambda *= std::pow(settings.lambdaFailFac, incTry);
}
}
}
}着重理清
calcResidualAndBufferscalcWeightsAndResidualcalculateWarpUpdate在track中怎样实现的.
首先出现的
callOptimized(calcResidualAndBuffers, (reference->posData[lvl], reference->colorAndVarData[lvl], SE3TRACKING_MIN_LEVEL == lvl ? reference->pointPosInXYGrid[lvl] : 0, reference->numData[lvl], frame, referenceToFrame, lvl, (plotTracking && lvl == SE3TRACKING_MIN_LEVEL)));
调用了calcResidualAndBuffers函数,
float SE3Tracker::calcResidualAndBuffers(
const Eigen::Vector3f* refPoint, //参考帧 3d坐标 起始 地址指针
const Eigen::Vector2f* refColVar, // 参考帧 灰度和 逆深度方差 起始地址
int* idxBuf, //匹配点好坏标志图 指针
int refNum, // 参考帧 3d点数量
Frame* frame, //当前帧 指针
const Sophus::SE3f& referenceToFrame, //参考帧 变换到 当前帧下得 欧式变换矩阵 引用
int level, //金字塔层级
bool plotResidual //匹配误差图标志
)该函数主要做了如下几件事：
- 如果参考帧坐标系下的3D点pi能投影到当前帧，则把变换到当前帧坐标系下得到的3D点p′i，记录在变量
buf_warped_x、buf_warped_y、buf_warped_z -
*(buf_warped_x+idx) = Wxp(0);// 参考帧 3d点 通过R,t变换矩阵 变换到 当前图像坐标系下的坐标值 X *(buf_warped_y+idx) = Wxp(1); *(buf_warped_z+idx) = Wxp(2); - 计算p′i在当前帧的投影点位置的梯度
buf_warped_dx、buf_warped_dx，以及残差buf_warped_residual -
//给梯度变量赋值时乘上了焦距 *(buf_warped_dx+idx) = fx_l * resInterp[0];// 当前帧匹配点亚像素 梯度 *(buf_warped_dy+idx) = fy_l * resInterp[1];// 匹配点亚像素 梯度 *(buf_warped_residual+idx) = residual;// 对应 匹配点 像素误差 - 记录参考帧下点pi的逆深度
buf_d以及（逆深度的）方差buf_idepthVar -
*(buf_d+idx) = 1.0f / (*refPoint)[2]; // 参考帧 Z轴 倒数 = 参考帧逆深度 *(buf_idepthVar+idx) = (*refColVar)[1]; // 参考帧逆深度方差然后通过
float lastErr = callOptimized(calcWeightsAndResidual,(referenceToFrame));调用calcWeightsAndResidual函数,该函数的功能是计算归一化方差的光度误差系数，也就是计算公式(14)，并且乘以了Huber-weight，最终把这个系数存在数组
buf_weight_p中。 -
-
{ float tx = referenceToFrame.translation()[0]; float ty = referenceToFrame.translation()[1]; float tz = referenceToFrame.translation()[2]; float sumRes = 0; // 每个像素点都有一个光度匹配误差，而每个点匹配的可信度根据其偏导数可以求得， // 加权每一个误差 然后求所有点 光度匹配误差的均值 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 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); // 误差权重 方差倒数 float weighted_rp = fabs(rp*sqrtf(w_p)); //fabs 求绝对值 |r|加权的误差 // 为了减少外点（outliers）对算法的影响，论文中使用了迭代变权重最小二乘 // 其实在实现的时候，这里给的权重就是Huber-weight。 float wh = fabs(weighted_rp < (settings.huber_d/2) ? 1 : (settings.huber_d/2) / weighted_rp); //加权误差的和 sumRes += wh * w_p * rp*rp;// 加权误差和 *(buf_weight_p+i) = wh * w_p; // 乘上 Huber-weight 权重 的最终误差 } return sumRes / buf_warped_size; //返回加权误差均值 }首先可以看到px,py等用的是calcResidualAndBuffers函数记录的数据.
-
-
可能是考虑参考帧到当前帧的位姿变换比较小，所以作者只考虑了位移t而忽略旋转R。这样使得式子(14)中的偏导的形式简单了很多.所以就可以写成
-
-
其中第一个偏导是在参考帧的3D点p的位置求的，
dx和dy分别是点p在图像Ij投影位置处两个方向的梯度；而第二个偏导是在点p在参考帧中的逆深度Di(p)处求的，所有最终我们得到式子(∗)中的结果. -
对应到上面代码中的
-
float g0 = (tx * pz - tz * px) / (pz*pz*d); float g1 = (ty * pz - tz * py) / (pz*pz*d);
其中gx,gy为上面的dx,dy包含了焦距.float drpdd = gx * g0 + gy * g1;
同时通过这个函数得到了lastErr=sumRes / buf_warped_size 加权误差均值*(buf_warped_dx+idx) = fx_l * resInterp[0]; *(buf_warped_dy+idx) = fy_l * resInterp[1]; - 接下来通过callOptimized(calculateWarpUpdate,(ls));调用 calculateWarpUpdate
-
{ //首先现将ls参数初始化成默认值 ls.initialize(width*height); for(int i=0;i<buf_warped_size;i++) { float px = *(buf_warped_x+i); float py = *(buf_warped_y+i); float pz = *(buf_warped_z+i); float r = *(buf_warped_residual+i); float gx = *(buf_warped_dx+i); float gy = *(buf_warped_dy+i); // step 3 + step 5 comp 6d error vector float z = 1.0f / pz; float z_sqr = 1.0f / (pz*pz); //这块的雅克比跟书上的一致 Vector6 v; 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; // step 6: integrate into A and b: //最后更新到ls参数中，得到最小二乘法方程 ls.update(v, r, *(buf_weight_p+i)); } // solve ls ls.finish(); //result = ls.A.ldlt().solve(ls.b); }首先px,py等用的是calcResidualAndBuffers函数记录的数据,没有用到calcWeightsAndResidual这个函数所提到的归一化方差的光度误差系数,我就以为calcWeightsAndResidual函数只是用来计算lastErr用来后面的模型误差的对比,其实不然.它是用到优化更新里面的,注意到
ls.update(v, r, *(buf_weight_p+i))这里的buf_weight_p是由calcWeightsAndResidual函数记录的.
-
加权LM算法的部分,首先根据误差大小，计算每个误差的权重，其一用于计算误差的加权和(也就是后来用到lastErr)，其二用作更新时的误差权重矩阵。 J转置*J * dse3 = -J转置*w*error 这里的w是误差权重矩阵，error是总的误差加权和
-
就其二来说,
float w_p = 1.0f / ((cameraPixelNoise2) + s * drpdd * drpdd); // 误差权重 方差倒数代码对应的公式
-
float weighted_rp = fabs(rp*sqrtf(w_p)); // |r|加权的误差// 其实在实现的时候，这里给的权重就是Huber-weight。 float wh = fabs(weighted_rp < (settings.huber_d/2) ? 1 : (settings.huber_d/2) / weighted_rp);
参考: