HasStateBlocks::getState behaves in a non predictible way
VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
(find it here)
sometimes gives a VectorComposite
with empty vectors while getStateVector
works fine.
An example (with a landmark state) of what we experienced in the objectslam imu demo:
WOLF_INFO("lmk id: ", lmk->id())
VectorComposite state_lmk = lmk->getState();
WOLF_INFO(lmk->getStructure())
WOLF_INFO("size state: ", lmk->getStateVector().size())
WOLF_INFO("P size: ", state_lmk['P'].size())
WOLF_INFO("O size: ", state_lmk['O'].size())
WOLF_INFO("state: ", lmk->getStateVector().transpose())
WOLF_INFO("P: ", state_lmk['P'].transpose())
WOLF_INFO("O: ", state_lmk['O'].transpose())
sometimes produced this normal output:
[info][16:34:56] lmk id: 9
[info][16:34:56] PO
[info][16:34:56] size state: 7
[info][16:34:56] P size: 3
[info][16:34:56] O size: 4
[info][16:34:56] state: -0.368204 0.555138 0.0321504 0.321517 0.105367 0.272674 0.900652
[info][16:34:56] P: -0.368204 0.555138 0.0321504
[info][16:34:56] O: 0.321517 0.105367 0.272674 0.900652
and then this abnormal one
[info][16:34:56] lmk id: 10
[info][16:34:56] PO
[info][16:34:56] size state: 7
[info][16:34:56] P size: 0
[info][16:34:56] O size: 0
[info][16:34:56] state: -0.204143 0.641925 0.0194907 0.286745 -0.0919677 0.833397 0.463432
[info][16:34:56] P:
[info][16:34:56] O:
As for landmark 10, it's not the first time it is checked.
I think this might be a bug in getState(): it directly uses state_block_map_
instead of using the ::getStateBlock() method.
Edited by Mederic Fourmy