//投影点关于各个参数的导数。其是组成雅克比矩阵的元素
//有2个看一处看不懂的地方。除了相机内参和位姿用了FEJ
//对逆深度感觉也是用FEJ了呢.对光度参数和图像梯度没用FEJ能看出来
//高翔的博客是说位姿和光度用了FEJ 而宫益群的前面写的是对位姿和光度
//用了FEJ ，后面优化部分又写对相机内参和位姿用了FEJ 。前后矛盾，但是
//后面的说法和代码比较吻合。奇怪奇怪
// 注意固定线性化点。也就是计算雅克比时求导变量的值要固定，
// 而不是用每次迭代更新以后的值去求雅克比，这就是FEJ。
// ========> 这里只有位姿和是相机参数固定了 所以它们只对他们
// 做了FEJ ----合理了------
// 逆深度看似用了固定的位姿去求，但是逆深度的值是在
// 随着优化时刻变化的 所以其没被固定 也就不存在FEJ了
double PointFrameResidual::linearize(CalibHessian* HCalib)
{
state_NewEnergyWithOutlier = -1;
if(state_state == ResState::OOB)
{ state_NewState = ResState::OOB; return state_energy; }
FrameFramePrecalc* precalc = &(host->targetPrecalc[target->idx]);
float energyLeft=0;
const Eigen::Vector3f* dIl = target->dI;
//FEJ真正的计算位置
//const float* const Il = target->I;
//PRE_KRKiTll PRE_KtTll是随着优化不断变化的位姿量
const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll;
const Vec3f &PRE_KtTll = precalc->PRE_KtTll;
//PRE_RTll_0 PRE_tTll_0是固定量在计算之前固定 不随着优化变化而变化
// FEJ的位置
const Mat33f &PRE_RTll_0x = precalc->PRE_RTll_0;
const Vec3f &PRE_tTll_0 = precalc->PRE_tTll_0;
const float * const color = point->color;
const float * const weights = point->weights;
// 这个仿射的系数也是随着优化在变化
Vec2f affLL = precalc->PRE_aff_mode;
float b0 = precalc->PRE_b0_mode;
Vec6f d_xi_x, d_xi_y;
Vec4f d_C_x, d_C_y;
float d_d_x, d_d_y;
{
float drescale, u, v, new_idepth;
float Ku, Kv;
Vec3f KliP;
//点越界直接返回
if(!projectPoint(point->u, point->v, point->idepth_zero_scaled, 0, 0, HCalib,
PRE_RTll_0x, PRE_tTll_0, drescale, u, v, Ku, Kv, KliP, new_idepth))
{ state_NewState = ResState::OOB; return state_energy; }
centerProjectedTo = Vec3f(Ku, Kv, new_idepth);
// diff d_idepth
//target帧点坐标对逆深度求偏导 这块确实不理解 逆深度也相当于FEJ了啊 不懂了
d_d_x = drescale * (PRE_tTll_0[0] - PRE_tTll_0[2] * u) * SCALE_IDEPTH * HCalib->fxl();
d_d_y = drescale * (PRE_tTll_0[1] - PRE_tTll_0[2] * v) * SCALE_IDEPTH * HCalib->fyl();
// diff calib
//target帧点坐标对相机参数求偏导
d_C_x[2] = drescale*(PRE_RTll_0x(2, 0) * u - PRE_RTll_0x(0, 0));
d_C_x[3] = HCalib->fxl() * drescale * (PRE_RTll_0x(2, 1) * u-PRE_RTll_0x(0, 1)) * HCalib->fyli();
d_C_x[0] = KliP[0] * d_C_x[2];
d_C_x[1] = KliP[1] * d_C_x[3];
d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0x(2, 0) * v - PRE_RTll_0x(1, 0)) * HCalib->fxli();
d_C_y[3] = drescale * (PRE_RTll_0x(2, 1) * v - PRE_RTll_0x(1, 1));
d_C_y[0] = KliP[0] * d_C_y[2];
d_C_y[1] = KliP[1] * d_C_y[3];
d_C_x[0] = (d_C_x[0] + u) * SCALE_F;
d_C_x[1] *= SCALE_F;
d_C_x[2] = (d_C_x[2] + 1) * SCALE_C;
d_C_x[3] *= SCALE_C;
d_C_y[0] *= SCALE_F;
d_C_y[1] = (d_C_y[1] + v) * SCALE_F;
d_C_y[2] *= SCALE_C;
d_C_y[3] = (d_C_y[3] + 1) * SCALE_C;
// target帧点坐标对位姿增量求偏导 DSO追踪与优化公式56
// 对应论文公式13 dso详解博客公式
// 几何雅克比只用最原始的PRE_RTll_0以及PRE_tTll_0来计算偏导 属于固定了
// PRE_RTll_0与 PRE_tTll_0
d_xi_x[0] = new_idepth * HCalib->fxl();
d_xi_x[1] = 0;
d_xi_x[2] = -new_idepth * u * HCalib->fxl();
d_xi_x[3] = -u * v * HCalib->fxl();
d_xi_x[4] = (1 + u * u) * HCalib->fxl();
d_xi_x[5] = -v * HCalib->fxl();
d_xi_y[0] = 0;
d_xi_y[1] = new_idepth * HCalib->fyl();
d_xi_y[2] = -new_idepth * v * HCalib->fyl();
d_xi_y[3] = -(1 + v * v) * HCalib->fyl();
d_xi_y[4] = u * v * HCalib->fyl();
d_xi_y[5] = u * HCalib->fyl();
}
{
//位姿参数雅克比矩阵 =====>坐标对位姿求偏导
J->Jpdxi[0] = d_xi_x;
J->Jpdxi[1] = d_xi_y;
//相机参数雅克比矩阵 =====>坐标对相机参数求偏导
J->Jpdc[0] = d_C_x;
J->Jpdc[1] = d_C_y;
//逆深度雅克比矩阵 =====>坐标对逆深度求偏导
J->Jpdd[0] = d_d_x;
J->Jpdd[1] = d_d_y;
}
float JIdxJIdx_00 = 0, JIdxJIdx_11 = 0, JIdxJIdx_10 = 0;
float JabJIdx_00 = 0, JabJIdx_01 = 0, JabJIdx_10 = 0, JabJIdx_11 = 0;
float JabJab_00 = 0, JabJab_01 = 0, JabJab_11 = 0;
float wJI2_sum = 0;
//图像坐标的偏导数 以及光度仿射变换的偏导，是对8个点求
//是像素坐标关于梯度增量 光度增量等的偏导
for(int idx = 0; idx < patternNum; idx++)
{
float Ku, Kv;
if(!projectPoint(point->u+patternP[idx][0], point->v+patternP[idx][1], point->idepth_scaled, PRE_KRKiTll, PRE_KtTll, Ku, Kv))
{ state_NewState = ResState::OOB; return state_energy; }
projectedTo[idx][0] = Ku;
projectedTo[idx][1] = Kv;
Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0]));
float residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]);
//点坐标对光度变化增量求偏导
float drdA = (color[idx] - b0);
if(!std::isfinite((float)hitColor[0]))
{ state_NewState = ResState::OOB; return state_energy; }
float w = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + hitColor.tail<2>().squaredNorm()));
w = 0.5f * (w + weights[idx]);
float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual);
//hub范数
energyLeft += w * w * hw * residual * residual * (2 - hw);
{
if(hw < 1) hw = sqrtf(hw);
hw = hw * w;
hitColor[1]*=hw;
hitColor[2]*=hw;
J->resF[idx] = residual * hw;
//残差对图像坐标求偏导
J->JIdx[0][idx] = hitColor[1];
J->JIdx[1][idx] = hitColor[2];
//残差对光度求偏导
J->JabF[0][idx] = drdA * hw;
J->JabF[1][idx] = hw;
JIdxJIdx_00+=hitColor[1] * hitColor[1];
JIdxJIdx_11+=hitColor[2] * hitColor[2];
JIdxJIdx_10+=hitColor[1] * hitColor[2];
//转置计算体现在这 +表示的Σ
JabJIdx_00+= drdA * hw * hitColor[1];
JabJIdx_01+= drdA * hw * hitColor[2];
JabJIdx_10+= hw * hitColor[1];
JabJIdx_11+= hw * hitColor[2];
JabJab_00+= drdA * hw * drdA * hw;
JabJab_01+= drdA * hw * hw;
JabJab_11+= hw * hw;
wJI2_sum += hw * hw * (hitColor[1] * hitColor[1] + hitColor[2] * hitColor[2]);
if(setting_affineOptModeA < 0) J->JabF[0][idx] = 0;
if(setting_affineOptModeB < 0) J->JabF[1][idx] = 0;
}
}
//(残差 /像素坐标)转置*(残差 /像素坐标)
J->JIdx2(0,0) = JIdxJIdx_00;
J->JIdx2(0,1) = JIdxJIdx_10;
J->JIdx2(1,0) = JIdxJIdx_10;
J->JIdx2(1,1) = JIdxJIdx_11;
//(残差 /光度参数)转置*(残差 /像素坐标)
J->JabJIdx(0,0) = JabJIdx_00;
J->JabJIdx(0,1) = JabJIdx_01;
J->JabJIdx(1,0) = JabJIdx_10;
J->JabJIdx(1,1) = JabJIdx_11;
//(残差 /光度参数)转置*(残差 /光度参数)
J->Jab2(0,0) = JabJab_00;
J->Jab2(0,1) = JabJab_01;
J->Jab2(1,0) = JabJab_01;
J->Jab2(1,1) = JabJab_11;
state_NewEnergyWithOutlier = energyLeft;
if(energyLeft > std::max<float>(host->frameEnergyTH, target->frameEnergyTH) || wJI2_sum < 2)
{
energyLeft = std::max<float>(host->frameEnergyTH, target->frameEnergyTH);
//异常点
state_NewState = ResState::OUTLIER;
}
else
{
//正常点
state_NewState = ResState::IN;
}
state_NewEnergy = energyLeft;
//返回值是残差值
return energyLeft;
}
dso的FEJ部分代码
最新推荐文章于 2022-10-28 23:19:53 发布
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
