dso icon indicating copy to clipboard operation
dso copied to clipboard

why calculate res_toZeroF in fixLinearizationF like this?

Open biggiantpigeon opened this issue 3 years ago • 0 comments

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?

biggiantpigeon avatar Feb 22 '22 02:02 biggiantpigeon