dso
dso copied to clipboard
why calculate res_toZeroF in fixLinearizationF like this?
Hi, thanks for the excellent work, there is some problem for me to understand the calculation for bM when marginalizing.
According to the paper, bM should be b' in the equation
and according to the code in marginalizePointsF(), I guess b'=J*res_toZeroF, where J is the jacobian calculated with the new states just after an optimize,and the residual is res_toZeroF, calculated bere in fixLinearizationF():
for(int i=0;i<patternNum;i+=4)
{
// PATTERN: rtz = resF - [JI*Jp Ja]*delta.
__m128 rtz = _mm_load_ps(((float*)&J->resF)+i);
//! res - J * delta_x
rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x));
rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y));
rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a));
rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b));
_mm_store_ps(((float*)&res_toZeroF)+i, rtz);
// if(res_toZeroF[i] > J->resF[i])
// printf("true");
}
I think the code means that the residual roll back to the point that this optimize starts, with resF the residual at point this optimize ends. Why like this? Why roll back the residual? In my understanding, the x0 in the equation should be zero at the end of one optimization(or at the beginning of the next), and b' should just be b=J*resF, the res caused by the next optimization is added in solveSystemF():
bM_top = (bM+ HM * getStitchedDeltaF());
Can someone be nice enough to explain this to me?