Implement processor bootstrapping

This bootstrap is required for IMU processor, but some code needs to be inserted in core for it to work.

Please read the bootstrap details on the IMU plugin issue 29: mobile_robotics/wolf_projects/wolf_lib/plugins/imu#29 (closed)

For what concerns the core plugin, here are the necessary changes. They concern ProcessorMotion, Problem, all nodes having state blocks, and StateBlock:

1. ProcessorMotion:
   • Add a member bool ProcessorMotion::bootstrapping to control the following:
     • factors added are declared inactive --> probably in Derived::emplaceFactors()
     • bootstrap() is called
   • Add virtual void ProcessorMotion::bootstrap() method (virtual or pure virtual, TBD)
     • wait for the conditions to apply the bootstrap
     • compute the necessary elements
     • transform all the nodes in Problem with the new bootsrapped solution
     • re-activate all factors that were declared inactive
     • clear the bootstrapping flag
2. Problem:
   • Add void Problem::transformAll(VectorComposite("PO")) to transform all wolf tree after bootstrapping
3. Nodes (maybe in HasStateBlocks):
   • Add void AllNodes::transform(VectorComposite("PO")) to transform all node's state blocks
4. StateBlock:
   • Add void StateBlock::transform(VectorComposite("PO")) = 0 to transform the state block
   • Add bool StateBlock::transformable to control if transformation should be applied
   • Derive all state blocks to specific types.
   • Implement transform in all derived state blocks

added Agreement required Code organization Doing External API update enhancement labels

assigned to @jsola

changed the description

mentioned in issue mobile_robotics/wolf_projects/wolf_lib/plugins/imu#29 (closed)

changed the description

changed the description

marked the checklist item Add a member bool ProcessorMotion::bootstrapping to control the following: as completed

marked the checklist item bootstrap() is called as completed

marked the checklist item Add virtual void ProcessorMotion::bootstrap() method (virtual or pure virtual, TBD) as completed

changed the description

marked the checklist item factors added are declared inactive --> probably in Derived::emplaceFactors() as completed

marked the checklist item Add void Problem::transformAll(VectorComposite("PO")) to transform all wolf tree after bootstrapping as completed

marked the checklist item Add void AllNodes::transform(VectorComposite("PO")) to transform all node's state blocks as completed

marked the checklist item Add bool StateBlock::transformable to control if transformation should be applied as completed

@joanvallve regarding the point: "Derive all state blocks to specific types", we can condensate these "specific types" to some family, e.g.

1. StateNonTransformable: states that are not transformed, no matter what
2. StateVector: vectors that transform with a pure rotation
3. StatePoint: vectors that transform with rotation and translation
4. StateQuaternion, Angle, Homogeneous: specific types, they exist already
5. StateBlaBla: any new state block that may appear in the future

Do you see any inconvenient in doing this?

Note: in any case, the property StateBlock::transformable and method StateBlock::isTransformable() exist to block the capacity of transforming the object. So types 1,2,3,4 can be declared non-transformable too.

Possibly, after defining the types above, we can typedef them to more specific types:

typedef StateVector StateVelocity;
typedef StateVector StateAcceleration;
typedef StateVector StateAngularVelocity;
typedef StateVector StateAngularMomentum;
typedef StatePoint StatePosition;
typedef StatePoint StateCoM;                       // this one is really stupid, but just to illustrate
typedef StateNonTransformable StateIntrinsic;
typedef StateNonTransformable StateBias;
typedef StateNonTransformable StateGains;
typedef StateNonTransformable StateDiffDriveModel; // this one is really stupid, but just to illustrate

Not sure if typedef is enough, I think we need to register them to the factory.

Also, I would vote for specifying StatePoint2d and StatePoint3d (and the same for the rest).

code example (before reading your comments):

class StatePoint : public StateBlock
{
    StatePoint(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
        : StateBlock(_state, _fixed, nullptr)
    {
        setTransformable(_transformable);
    }
    void transform(const VectorComposite& _transformation) override
    {
        switch (getSize())
        {
            case 2:
                setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
            case 3:
                setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
        }
    }
};

typedef StatePoint StatePosition;

The thing is: can we pass with only the classes above, eventually separating 2d and 3d? So types Point2d, Vector2d, Point3d, Vector3d, NonTransformable?

Or do we need special types for velocity, acceleration, angular momentum, etc?

We would then remove the typedefs, yes I agree they would not work with factories

So, e.g.

class StatePoint3d : public StateBlock
{
    StatePoint(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
        : StateBlock(_state, _fixed, nullptr)
    {
        setTransformable(_transformable);
    }
    void transform(const VectorComposite& _transformation) override
    {
        setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
    }
};

class StatePoint2d : public StateBlock
{
    StatePoint(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
        : StateBlock(_state, _fixed, nullptr)
    {
        setTransformable(_transformable);
    }
    void transform(const VectorComposite& _transformation) override
    {
        setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
    }
};

About 2d and 3d, I think that having fully defined states is a good idea (otherwise, the required size has to be passed along with the class in the changes I'm implementing in the sensors MR). But this does not mean that we can implement everything in some

About velocity and etc. we can define the transformations in base classes.. (actually, to register a class in the factory, we don't need a class, just register the class with the corresponding string, no?)

2d, 3d --> OK, doing

Registration --> you need string, and you need the creator. I do not understand your second commment, actually.

The case of NonTransformable I find it not really useful since it doesn't have the size.. then it has to be inherited anyway...

This was discussed yesterday: a position is transformable, but a position extrinsic is not. Then, you use a flag to differentiate this. And you put it in the StateBlock because Frames are not derived and therefore they do not know who is transformable and who is not.

Usually, states in Frames are transformable. But who knows.

And states in Sensors, Landmarks and Captures are no issue since the owners are derived and know who is transformable and who is not

But I thought we agreed on specifying this in a flag in the constructor.

in the constructor of StateBlock? Yes, sure. I was just lazy

but will do it, sure

Because the sensor P and the frame P are the same stateblock type StatePoint3d, but it can be transformable or not

ok

(I think the threads in this conversation are a bit messed up)

(I started a thread below with your original question)

About registering, we could do this, right?

FactoryStateBlock::registerCreator(StateVelocity2d, StateVector2d::create);
FactoryStateBlock::registerCreator(StateAcceleration2d, StateVector2d::create);

Yes this would work

But: why would you want to register something called StateVelocity2d? Is it not practical to just use StateVector2d? jsut asking

I honestly don't know.. to be more verbose?

OK in any case, this can be done at any time. I just wanted to save coding a lot of classes! Registering is a different issue.

So let's think about pros and cons for registering very specific names or just generic names.

@joanvallve said: The case of NonTransformable I find it not really useful since it doesn't have the size.. then it has to be inherited anyway...

NonTransformable inherits from StateBlock. Why do you need to inherit it again? It's just a plain vector with no geometric meaning, so non transformable. for example, biases, or camera intrinsic params

The name StateNonTransformable is not great. Maybe StateParams is better

I'm messing up.. this is not related with this issue, but with the experience of the MR I'm working on, I would say that it is a good idea to have state block classes that completely define the state, so having also the size defined.

ah. Then what about this:

StateParams1
StateParams2
StateParams3
StateParams4
StateParams5
StateParams6
StateParams7

yes, or a templated StateParams<size> and just registering or inheriting with proper sizes.

I mean, otherwise, every time a developer comes with a new meaning for a state, it has to code a new class for that state. We should avoid this by providing standard types that do the job

template<size_t size>
class StateParams : public StateBlock
{
};

ah hahaha we came up with the same template idea

OK will go for the template solution by now

How about this:

inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
        fixed_(_fixed),
        state_size_(_size),
        state_(Eigen::VectorXd::Zero(_size)),
        local_param_ptr_(_local_param_ptr),
        state_updated_(false),
        fix_updated_(false),
        local_param_updated_(false),
        transformable_(_transformable)
{
    //    std::cout << "constructed           +sb" << std::endl;
}

template <size_t size>
class StateParams : public StateBlock
{
    StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false)
    {
    }
    void transform(const VectorComposite& _transformation) override
    {
        // non transformable! --> do nothing
    }
};

class StatePoint2d : public StateBlock
{
    StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
        : StateBlock(_state, _fixed, nullptr, _transformable)
    {
    }
    void transform(const VectorComposite& _transformation) override
    {
        setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
    }
};

class StatePoint3d : public StateBlock
{
    StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
        : StateBlock(_state, _fixed, nullptr, _transformable)
    {
    }
    void transform(const VectorComposite& _transformation) override
    {
        setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
    }
};

Should we check the transformable_ attribute inside or outside transform()?

StateDerived::transform(v){
  if isTransformable()
    setState( bla bla );
}

versus

if (state_derived->isTransformable())
  state_derived->transform(v);

I would say inside.. but because it is safer. Anyway, transformStateblocks() could be implemented in HasStateBlocks, there is no real difference...

transform() will work similarly to perturb() which is viral from Problem all the way down to StateBlock

changed the description

created branch 461-implement-processor-bootstrapping to address this issue

mentioned in merge request !455 (merged)

marked the checklist item Derive all state blocks to specific types. as completed

marked the checklist item Implement transform in all derived state blocks as completed

closed with merge request !455 (merged)

mentioned in commit 7c234a14

mentioned in issue #465 (closed)