Problem::getState/getTimeStamp considering not initialized processors

The implementation of Problem::getState/getTimeStamp() first implement the case processor_is_motion_list.empty() (taking last KF state or zeroState) and otherwise, fill the state/timeStamp iterating over the processors. But they could be not initialized yet so returning empty state or 0 timestamp (see related issue #333).

The implementation should be in the opposite order: first iterate over processors and afterwards deal with incomplete or missing state/timestamp.

Also, in case of no KF and a prior has been provided by the user, it is better if getState() returns this state than zero.

added To Do (release) label

changed the description

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

mentioned in merge request !380 (merged)

Should #345 be addressed first? @joanvallve ?

Either #345 or #333 should be addressed before, just for the case of getTimeStamp()

We should agree in some things regarding the expected behavior of these Problem::getXX() functions.

1. For time stamps, it returns the most recent one if available, otherwise TimeStamp::Invalid()
2. For states, it returns the blocks relevant to the time stamp only. Blocks that are not found to agree with the time stamp are not returned.

Currently, 2. is not true and blocks that are not found are returned with the zero value of the correct size -- or identity for manifold blocks.

I'll focus on getState, it is the only one requiring discussion.

There are 2 different getState now, one providing TimeStamp and the other without it (meaning the current state).

getState(TimeStamp, Structure)

Some (most) processors use it to initialize a frame state. Because of this, the old implementation of getState always provided a "valid" state (i.e. complete). In my opinion, that should stay like this. From the point of view of the processors, this function means:

"Problem, give me the best initialization of this structure at this timestamp".

As a final consideration, zero is the final alternative only in case of missing ( or not initialized) is_motion processors, no KFs and no prior.state defined by the user.

getState(Structure) is quite more simple and less critical (it is used by ROS node to publish current state), I would discuss about the other case and apply the same criterion in this one.

mentioned in issue #333

This is my first proposal

VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
{
    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);

    VectorComposite state;

    for (const auto& prc : processor_is_motion_list_)
    {
        const auto& prc_state = prc->getState(_ts);

        // transfer processor vector blocks to problem state
        for (const auto& pair_key_vec : prc_state)
        {
            const auto& key = pair_key_vec.first;

            if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
                state.insert(pair_key_vec);
        }
    }

    // check for empty blocks and fill them with the last KF or with zeros in the worst case
    VectorComposite state_last;
    const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
    if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);

    for (const auto& ckey : structure)
    {
        const auto& key = string(1,ckey);
        if (state.count(key) == 0)
        {
            auto state_last_it = state_last.find(key);

            if (state_last_it != state_last.end())
                state.emplace(key, state_last_it->second);

            else
                state.emplace(key, stateZero(key).at(key));
        }
    }

    return state;
}

added Doing label and removed To Do (release) label

I think this is ready to merge, waiting for #343 (closed) is not necessary I think.

@joanvallve can you have a look and validate (or not) this MR?

I would add that we may want to consider the prior information if provided by the user. I think it is only one line after checking KF:

VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
{
    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);

    VectorComposite state;

    for (const auto& prc : processor_is_motion_list_)
    {
        const auto& prc_state = prc->getState(_ts);

        // transfer processor vector blocks to problem state
        for (const auto& pair_key_vec : prc_state)
        {
            const auto& key = pair_key_vec.first;

            if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
                state.insert(pair_key_vec);
        }
    }

    // check for empty blocks and fill them with the last KF or with zeros in the worst case
    VectorComposite state_last;
    const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
    if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);
    else if (prior_options and prior_options_->mode != "nothing") state_last = prior_options_->state;

    for (const auto& ckey : structure)
    {
        const auto& key = string(1,ckey);
        if (state.count(key) == 0)
        {
            auto state_last_it = state_last.find(key);

            if (state_last_it != state_last.end())
                state.emplace(key, state_last_it->second);

            else
                state.emplace(key, stateZero(key).at(key));
        }
    }

    return state;
}

I assume a similar implementation of getState(Structure) right?

Yes, both follow exactly the same logic.

And oh yes, I forgot to add the prior info.

Done. @joanvallve when you give your OK I'll merge.

Great, let's go!

closed via merge request !380 (merged)

mentioned in commit 49306a54