search

rpng / open_vins

An open source platform for visual-inertial navigation research.
https://docs.openvins.com
GNU General Public License v3.0
2.17k stars 636 forks source link

DynamicInitializer State Ordering of Gravity and Velocity #296

Closed jiaozhadexiaolang closed 1 year ago

jiaozhadexiaolang commented 1 year ago 
  p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first) + 6, 0, 3, 1);
jiaozhadexiaolang commented 1 year ago

size_feature * A_index_features.at(feat.first) + 6 should A_index_features.at(feat.first)

why? +6

jiaozhadexiaolang commented 1 year ago

v and g in the end of x_hat

goldbattle commented 1 year ago

The state should be the features, then velocity, then gravity https://github.com/rpng/open_vins/blob/c3259430f0574c2911ff8e6ce888f6a1fcff5928/ov_init/src/dynamic/DynamicInitializer.cpp#L224-L231 https://github.com/rpng/open_vins/blob/c3259430f0574c2911ff8e6ce888f6a1fcff5928/ov_init/src/dynamic/DynamicInitializer.cpp#L368-L369

qsisi commented 1 year ago

Sorry to bother you again, but in my understanding, the x_hat is ordered as feat、vel、grav:

https://github.com/rpng/open_vins/blob/58afb2daf47498911100edc7452ccd7e42a2db50/ov_init/src/dynamic/DynamicInitializer.cpp#L477-L478

but when accessing the features:

https://github.com/rpng/open_vins/blob/58afb2daf47498911100edc7452ccd7e42a2db50/ov_init/src/dynamic/DynamicInitializer.cpp#L538

there is a "+6" at indexing, does it wrong?