Things about the Dynamic Initialization

[carolo44]

I've played with the dynamic initializer a little bit and got quite confused with the MLE process. What i don't understand is the derivation of the theta_error's jacobian with respect to the theta. This is in the Factor_GenericPrior.h's Evaluate function. Is there any material that leads to this derivation?

Any there is also a suggestion. The current method is kind of packing everything into a very large constrained Least-Square problem. And this is quite slow. I've tried to split the estimateInitial process into 2 steps: 1. solve the sfm problem(doesn't have to be done in a pure vision way, may aid it with the preintegration's rotation term) 2. then align the IMU preintegration result with the sfm result. This is basically how the VINS-MONO does the dynamic initialization. I think the ideas behind are the same but it can be done faster.

[goldbattle]

The prior is based on the orientation error between two orientations. For example, error_theta = -log(R1*R2^T) is an error between two in the tangent space. The derivative in respect to the two orientations can be derived, this identity is used: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf

As for your suggestion / question about dynamic initialization. Yes, using SFM as an initial guess is possible, but the actual expensive part of the initialization is the MLE which is performed after initialization the states to recover an optimized state and its covariance. Performing SFM is actually an additional computational cost. I have originally tried using SFM similar to VINS-Mono / ORB-SLAM w/ MapReuse papers (as you have suggested), recover the initial velocity, and then do a MLE, and it wasn't much different and cost more time. Additionally, the current implementation of Dong-Si's work enforces gravity magnitude in the linear system, which should give better performance. You should checkout the documentation and tech report which provides related works and some thoughts.

• https://docs.openvins.com/namespaceov__init.html
• https://pgeneva.com/downloads/reports/tr_init.pdf

[carolo44]

Lots of thanks for your reply! For the prior orientation's jacobian stuff.. i still can't derive the jacobian equation as the code has described... I take the R_lin as (I3x3 - skew_x(theta))R_i, then R_i.transpose() * R_lin is just I3x3 - skew(R_i.transpose()), then the jacobian is simple... Then i wrote a simple test program to verify both derivation and they both worked.

[goldbattle]

I-skew() is only valid for small angles. This shouldn't be used normally unless this holds.