JakobEngel / dso

Direct Sparse Odometry
GNU General Public License v3.0
2.27k stars 906 forks source link

why calculate res_toZeroF in fixLinearizationF like this? #243

Open biggiantpigeon opened 2 years ago

biggiantpigeon commented 2 years ago

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 image 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?