Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

why calculate res_toZeroF in fixLinearizationF like this? #243

Open
biggiantpigeon opened this issue Feb 22, 2022 · 0 comments
Open

why calculate res_toZeroF in fixLinearizationF like this? #243

biggiantpigeon opened this issue Feb 22, 2022 · 0 comments

Comments

@biggiantpigeon
Copy link

biggiantpigeon commented Feb 22, 2022

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?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant