Factory for state blocks

The factory will be able to create state blocks on the fly given:

• a key: string to identify the state block
• a Eigen::VectorXd with the state
• a bool with fixed/unfixed

Strings will be identified as follows:

• "O", a quaternion if Vector::size() == 4;
• "O", an angle if Vector::size() == 1;
• "O3", "Q", "Quaternion", "StateQuaternion": a state quaternion. The size must be 4.
• "O2", "A", "Angle", "StateAngle": a state angle. The size must be 1.
• "H3", "Homogeneous3D", a 3D homogeneous vector. The size must be 4.
• "P","V","I","W",... and many other letters: a regular SB. The size is that given by the provided Eigen::Vector.
• "Base", "Vector": a regular SB . The size is that given by the provided Eigen::Vector.

That is, any string other than "O, O3, Q, Quaternion, StateQuaternion, O2, A, Angle, StateAngle, H3 or Homogeneous3D" will produce a regular state block of the size indicated by the vector, and without local parametrization.

See that for orientations, the size is given as part of the string.

created merge request !363 (merged) to address this issue

mentioned in merge request !363 (merged)

changed the description

@joanvallve would you have a look at this? I mean, the specification, and the implementation.

I implemented a minimal version of the proposal above:

There are these pre-defined creators, which are already registered when loading WOLF:

• "StateQuaternion", "StateAngle" and "StateHomogeneous3d" return the proper class.
• "O", returns a quaternion if Vector::size() == 4;
• "O", returns an angle if Vector::size() == 1;
• "O" throws a std::length_error if the vector size is not 1 or 4.
• "H", is TO BE IMPLEMENTED, but will return an homogeneous 3D vector. Not clear what to do for homogeneous 2D vectors, that's why I did not implement it yet.

Then, if calling the Factory with any other string, instead of throwing an error (as other factories do), the return is a valid state block with the provided vector as state:

• "StateBlock" returns a regular StateBlock. The size is that given by the provided Eigen::Vector.
• "P","V","I","W",... and many other letters: return a regular SB. The size is that given by the provided Eigen::Vector.
• "Base", "Vector": a regular SB . The size is that given by the provided Eigen::Vector.

These second group of strings is not defined anywhere. It's just that, by default, regular StateBlocks are created.

ALl gtests pass.

Working examples, all tested in gtest_factory_state_block.cpp:

    // REGULAR SB
    auto sbp = FactoryStateBlock::get().create("P", Eigen::Vector3d(1,2,3), false);
    auto sbv = FactoryStateBlock::get().create("StateBlock", Eigen::Vector2d(4,5), false);

    // DERIVED SB
    auto sbq = FactoryStateBlock::get().create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
    auto sba = FactoryStateBlock::get().create("StateAngle", Eigen::Vector1d(1), false);
    auto sbh = FactoryStateBlock::get().create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);

    // ORIENTATION TYPE CONDITIONED TO VECTOR SIZE
    auto sbq = FactoryStateBlock::get().create("O", Eigen::Vector4d(1,2,3,4), false);
    auto sba = FactoryStateBlock::get().create("O", Eigen::Vector1d(1), false);

    // IF WRONG SIZE PASSED TO "O" THROW ERROR
    auto sba = FactoryStateBlock::get().create("O", Eigen::Vector2d(1,2), false); // THROWS ERROR

Looks great! I didn't understand the issue about homogeneous 2D.

closed via merge request !363 (merged)

mentioned in commit 51ef6297

reopened

Sorry, I have some doubts. Are this letters going to be the key in hasStateBlocks? What about a sensor or a frame containing more than one state block refering to orientation information?

Uh ok

I dont know. These letters, yes, were those of the StateBlockComposites.

If we restore back to just the class names, the factory is easier, but less convenient.

We have to re-think it over maybe.

However, you can always define new letters for roll/pitch/yaw...

What if we reserve some letters for known things (the current prosopal), and also a factory case providing different classname and key?

Do you mean this?

WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d);

which expands to

namespace{ 
const bool __attribute__((used)) StateHomogeneous3dRegisteredWithH =                   \
     FactoryStateBlock::get().registerCreator("H", StateHomogeneous3d::create); 
}

You could do

WOLF_REGISTER_STATEBLOCK_WITH_KEY(Roll , StateAngle);
WOLF_REGISTER_STATEBLOCK_WITH_KEY(Pitch, StateAngle);
WOLF_REGISTER_STATEBLOCK_WITH_KEY(Yaw  , StateAngle);

is this what you mean?

Or, if only one letter want to be used (notice it's lowercase):

WOLF_REGISTER_STATEBLOCK_WITH_KEY(r , StateAngle);
WOLF_REGISTER_STATEBLOCK_WITH_KEY(p , StateAngle);
WOLF_REGISTER_STATEBLOCK_WITH_KEY(y , StateAngle);

Or you rather mean to implement (where?) a kind of dictionnary before calling the factory:

StateBlock Key --> StateBlock class
--------------     ----------------
B              --> StateBlock
r              --> StateAngle
p              --> StateAngle
P              --> StateBlock
Q              --> StateQuaternion
etc...

I don't know.. there are 2 different things:

1. Can we have 2 state blocks of the same type in the same HasStateBlocks object? I guess no since they would have the same key, right? Can a Quaternion have a different key than "O"?
2. Do we have to register a letter with an specific type of state block if we want a StateAngle/Quaternion/Homogeneous..?

1. Yes, you can have several SB of the same type with different keys inside StateBlockComposite, which is the main data in HasStateBlocks. You do:

Vector1d roll, pitch, yaw; // need Eigen::Vector for the factory below to work

// construct the SBs
const auto& SBr = FactoryStateBlock::get().create("StateBlockAngle", roll);
const auto& SBp = FactoryStateBlock::get().create("StateBlockAngle", pitch);
const auto& SBy = FactoryStateBlock::get().create("StateBlockAngle", yaw);

// add it to the stateBlockComposite
Frame/Sensor/Landmark/Capture->addStateBlock("r", SBr); // roll
Frame/Sensor/Landmark/Capture->addStateBlock("p", SBp); // pitch
Frame/Sensor/Landmark/Capture->addStateBlock("y", SBy); // yaw