We may want to change the current child nodes list of the wolf nodes to shape better to our requirements.
Now, for all wolf nodes, a std::list contains all the child nodes. For searching and ordering purposes it is not very useful.
Specifically, we need:
Trajectory: The list of frames is sorted by timestamp. This requires a number of functions to maintain this order. We could use an std::map using the timestamp as the key. It would require a simple machinery just in case of changing the timestamp of a frame.
Rest of wolf nodes: removal/check existence of nodes is much faster in std::set than in std::list
Designs
Child items
...
Show closed items
Linked items
0
Link issues together to show that they're related.
Learn more.
If the set of frames must be ordered then I think that a list is just fine. We could get rid of the "maintenance" functions by providing an appropriate method of comparison and letting the standard library do its work.
Admittedly a hash map could be faster but you can't do, efficiently, some order-related operations, like looking for the closest timestamp for a given time. Maybe this operations are not needed and so an ordered list is not needed.
Regarding the rest of wolf nodes, I agree. If they are only tested for existence then a std::set ought to be better.
I also propose to change the name of the current methods FrameBase::getCaptureList() to FrameBase::getCaptures() and the analogous for all wolf nodes. Then, in each case, we'll see if it is a set, a map or a list.
Well for me a map is just a dictionary, so the keys are not necessarily ordered. It looks like std::map orders the keys, so for me std::map = map + ordered list.
In any case, I don't have an opinion on what should be used for wolf. My point was that using a map implied not having the keys ordered and so maybe there was some lost functionality but it looks like I was wrong.
Exploring a little with @jcasals we see that, a std::set can be used also to keep the Frames ordered by their TimeStamp just defining the FrameBase::operator() or defining it outside and providing it to the set template std::set<FrameBasePTr,FrameComp> with
I found in wolf an instance where a key-frame and a non key-frame have the same timestamp. When ProcessorMotion sets its origin, it creates a key-frame and also a non-estimated frame with the same timestamp.
There were just a couple more examples of this behaviour, that appeared when making the trajectory a map instead of a list. There are different ways to solve this issues, but what I am interested in is in knowing if this behaviour is intended or not in order to adapt the design of the new trajectory data structure accordingly.
The question, thus, is:
Should a key-frame and a non key-frame be allowed to share the same exact timestamp?
Focusing on the ProcessorMotion::setOrigin example, a solution proposed by @joanvallve was to perturb slightly the timestamp of the non-estimated frame. Another solution could be to define a richer key for the map. For instance a pair <TimeStamp, Bool> where the bool indicates if it is a key-frame or not. Obviously this would require a bit more care when updating the trajectory. Thoughts? @joanvallve@jsola
As we have it now, yes, usually at the time of creating a KF, another F is created at the same timestamp. When more data is received, this F will advance its time stamp. But for the instant it is created and until new data arrives, both timestamps coincide.
It is therefore possible, having more processors acting like this, that more than one F share the same timestamp with one KF.
One option is to use multimap instead of map. If this solves the issue, then it's easy.
Another option is to have the processors own the F's, and only add them to Problem->Trajectory when they are KF. This demands some re-design. It is possibly cleaner to work like this...
I don't know if multimap is overkilling. I assume that it would be enough since we only want a container that orders the frames by time (key).
Only KF in trajectory:
I like this alternative because in Trajectory there is no need to have the non-key frames (they are auxiliar for any processor). Actually, they disturb requiring some extra API (getLastFrame(), getLastKeyFrame()...) and handling.
I don't like if we implement it requiring that frames are not emplaced but instead created and then linked whenever they are set key... We strongly recommend that all nodes have to be emplaced (or installed in case of processors and sensors) because the whole machinery (linking, registration of state blocks) is automatically done. We almost forbid to use make_shared, we don't want to rely on the developer to remember linking a frame.
Registration of state blocks:
Thinking about this issue, it came to my mind a related issue. Currently, the registration of state blocks (HasStateBlocks::registerNewStateBlocks(prb)) is only called in FrameBase::setProblem() only for key frames. Has sense: state blocks of a non-key frame shouldn't be registered (so added to the solver). In FrameBase::setKey() they are registered.
However, in captures stateblocks (if any) are automatically registered when linking. Then, captures linked to a non-key frame will register state blocks anyway because FrameBase::getProblem() does provide the pointer.
This is a mess. Should the capture check if the frame is key? I don't think so... What's the sense of having the pointer to Problem for a non-key frame? None.
I propose (finally!):
To modify FrameBase::link(), FrameBase::setProblem() and FrameBase::setKey():
Only call setProblem() for KFs. This means that setProblem() always calls registerNewStateBlocks() (consistent with the rest of wolf nodes).
Only call Trajectory::addFrame() for KFs.
Call setTrajectory() for all frames. so, a partial linkage for non-key frames.
In setKey(): call Trajectory::addFrame() and setProblem() (complete the linkage).
This way, frames are emplaced (internally we redefined what emplace means for key frames and non-keyframes). Things get simplified and fixed (captures in non-key frames won't be registered) and automatic.
These would be the new functions:
voidFrameBase::link(TrajectoryBasePtr_trj_ptr){assert(!is_removing_&&"linking a removed frame");assert(this->getTrajectory()==nullptr&&"linking an already linked frame");if(_trj_ptr){this->setTrajectory(_trj_ptr);if(this->isKey()){_trj_ptr->addFrame(shared_from_this());this->setProblem(_trj_ptr->getProblem());}}else{WOLF_WARN("Linking with a nullptr");}}voidFrameBase::setProblem(ProblemPtr_problem){if(_problem==nullptr||_problem==this->getProblem())return;NodeBase::setProblem(_problem);registerNewStateBlocks(_problem);for(autocap:capture_list_)cap->setProblem(_problem);}voidFrameBase::setKey(){// register if previously not estimatedif(!isKeyOrAux())this->setProblem(getTrajectory()->getProblem());type_=KEY;// THIS WON'T BE NECESSARY ANYMORE WITH MAP/*if (getTrajectory()) { getTrajectory()->sortFrame(shared_from_this()); getTrajectory()->updateLastFrames(); }*/}
@joanvallve this is very interesting, and I agree on the design.
However, there is still a problem with the time stamps. This has two sides:
If Trajectory has a map<Time, Frame>, there is only space for one frame at a given time.
If we change the container (I propose multimap<Time, Frame>), then when Processors change the timestamp of frames F (and this is what processors do with their Frame), then F needs to be removed from the multimap and inserted again with a new timestamp. This is, in a way, stupid, although it can be done of course.
Note: multimaps have a special API for accessing the elements in the case of multiple instances with the same key. Because doing container[key] will return only the first value stored for a given key. If you want all the values, you have to use container.equal_range<key> to obtain a pair of iterators between which the requested values are.
So, 1. is a problem, and 2. is not very elegant. One option (ugly in my opinion) is
Keep the F with the timestamp as the key of the map, and only change its own time stamp. That is, do
Then, at the time of F->setKey(), remove F from the container, and insert it with the proper time stamp. This is ugly I know. It might lead to unordered Frames in the container.
Another option (it breaks the emplace-only paradigm) is
Since Non-Keyframes F are only used by the processors, simply do not have these Frames, and instead have Processors keep a state vector of their own (e.g. witn a Eigen::VectorXd, or with a VectorComposite, or even with a StateBlockComposite). This is good in some sense, but then Captures cannot be emplaced because there is no Frame. If instead we use a Frame out of Trajectory, then Captures can be emplaced but Frames cannot, as Joan says.
A tricky workaround is to add a tiny random time increment to the time stamp of the Capture, or the last KF, to place the new F:
Timet_key=capture->getTimeStamp()+1e-6*rand();// this optionTimet_key=KF->getTimeStamp()+1e-6*rand();// or this optionconstauto&F=Trajectory->emplaceFrame(trajectory,t_key,otherparamsblabla);// t_key is the key in the container `map<Time,Frame>`.
This way, keys are unique and we can use standard map<Time,Frame>.
At the time of setKey(), the KF is given a proper key exactly equal to its time stamp.
I think that with my proposal the timestamp is not a problem anymore. If non-key frames are not added to the map, this problem would be for two KF. This is not only unlikely, also it is a undesired behaviour and we could forbid it (assertion).
If there are two KF with exactly the same timestamp, it means that any processor didn't join to an existing KF that has the same timestamp (timetolerance 0!), so this is clearly a malfunction.
If I understand correctly, with your (@joanvallve ) proposal then non-key frames wouldn't have a pointer to the problem, right? Wouldn't they be kind of floating? is that ok?
Couldn't we move the registerStateBlocks functionality to the setKey function? Then we could setProblem of everybody and the tree would remain consistent. I see that the intention is to remain consistent with the rest of wolf nodes, which I agree, but we open the door to having dangling nodes in the tree that are actually relevant, not so sure about that.
Also, I'm thinking, is it not possible for a keyframe to become non-keyframe? How do we unregister the state blocks? Maybe this is not allowed behaviour, I don't know.
The whole thing is that if we have a F and a KF with the same timestamp, then either only the KF is in the trajectory, or the trajectory has to accept repeated timestamps. The registerStateBlocks() thing does not affect this.
When Processors create KF, they do not create a F yet. Instead, they wait until the next data arrives, with a new timestamp, and at this moment the F is created.
It seems at first sight that this can be implemented easily:
First of all, I want to clarify that in this issue, when we say "key" we actually mean "estimated" and "non-key" means "non-estimated".
The registeringStateBlocks() is something that I raised and I propose to change to avoid a malfunction. So I would keep it in the discussion.
What @jcasals proposes is the current implementation. I proposed changing it not only for consistency, also for avoiding captures of a non-keyframe to register their state blocks (now, since the problem pointer is given from frame to capture, they are registered).
I don't consider "floating" frames as a problem. I see them as auxiliary/provisional/unfinished frames that are responsability of the processor. They shouldn't be checked: we would be hampering the processors. Actually, it would allow to check more deeply the tree requiring that all (linked) nodes with state blocks have notified them to problem, for instance.
Also, printing these frames should be responsibility of the corresponding processors.
New function FrameBase::createNonKeyFrame(...) calling only make_shared().
Change FrameBase::setKey(ProblemPtr) added argument (can be either TrajectoryBasePtr or ProblemPtr, I think that is more accessible the second one from the processors).
Problem::emplaceFrame(...) -> Problem::emplaceKeyFrame(...) and remove the FrameType argument (in all 4 implementations).
Implementation:
voidFrameBase::link(TrajectoryBasePtr_trj_ptr){assert(!is_removing_&&"linking a removed frame");assert(isKey()&&"linking a non-key frame is forbiden");assert(this->getTrajectory()==nullptr&&"linking an already linked frame");if(_trj_ptr){this->setTrajectory(_trj_ptr);_trj_ptr->addFrame(shared_from_this());this->setProblem(_trj_ptr->getProblem());}else{WOLF_WARN("Linking with a nullptr");}}voidFrameBase::setProblem(ProblemPtr_problem){if(_problem==nullptr||_problem==this->getProblem())return;NodeBase::setProblem(_problem);registerNewStateBlocks(_problem);for(autocap:capture_list_)cap->setProblem(_problem);}voidFrameBase::setKey(ProblemPtrprb){assert(prb!=nullptr&&"setting Key frame with a null problem");type_=KEY;this->link(prb->getTrajectory());}template<typenameclassType,typename...T>std::shared_ptr<classType>FrameBase::createNonKeyFrame(T&&...all){returnstd::shared_ptr<classType>frm=std::make_shared<classType>(std::forward<T>(all)...);}
Sorry but this discussion should be reopened due to 2 things:
I wrongly said that "we forbid to move captures" in wolf. Well, there is CaptureBase::move(frm) and it is used in ProcessorTracker.
The solution is not working that easy. If non-key frames are not added to Trajectory in the current code (in ProcessorTrackers and ProcessorMotions), they are destroyed automatically since only a weak_ptr is pointing them (from captures).
Why are them relevant:
Discarded one solution pointed by @jsola. I don't remember exactly the details. In my opinion, the arguments I gave to justify "why moving captures is forbiden" (it's not) are still valid and we should forbit it. This is another issue.
The straight solution would be adding the frames as attributes. For instance in ProcessorTracker, last_frame_ptr_ and incoming_frame_ptr_ (updated in reset() and advance() analogously to the corresponding captures).
But I realized that, actually.. what are these non-key frames used for? I think that they are useless. Why do we have to work with a floating Frame if we can directly work with a floating Capture? Don't you think? Could we just avoid non-estimated frames? That would be great.
In PT, if you receive a KF precisely at your last, where you already have a Frame and a Capture... either you move this capture to the KF, or you have to regenerate a lot of members no?
This is one of the solutions I pointed: Processors contain these frames, they are never in the Trajectory. their captures are moved to the KF in Trajectory. This must be done before all the liniing and registration so that it is a safe operation.
After checking in the current code, I think we can get rid of the "floating" frames. But maybe it is good idea to get the last agreements working before doing that. Created #330 (closed) with this idea.
For now, let's get this agreement working with the "straight solution" I mentioned.
CaptureBase::move() should be restricted to the cases from a non-estimated frame to a key-frame.
Also an assertion to FactorBase constructor should be added checking if feature->getCapture()->getFrame() is estimated and frame_other is estimated as well (if not nullptr).
I have found a minor issue regarding the order of the parameters for the FrameBase constructor.
If we create a keyframe, via Problem, we might do something like this
auto my_frame = P->createKeyFrame(_frame_state, _time_stamp);
In this case, this function will just call another P->createKeyFrame adding more information, and at the end of the day we will call
auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _time_stamp, _frame_state);
Notice that we have flipped the order of the state and the timestamp. The problem is that if now we want to create my_frame as a non keyframe we have to flip the order of state and time stamp
auto my_frame = FrameBase::createNonKeyFrame<FrameBase>(_time_stamp, _frame_state);
because now we don't go through problem and so nobody flips the arguments to match the constructor.
This is a very small issue, but I think that since we are refactoring, might as well match the order of the arguments between Problem::createKeyFrame and FrameBase::constructor so that the only thing different when creating a keyframe or a non keyframe is the name of the function, but the order of the arguments remains the same. What I would do is just flip the order in Problem to match the order of the constructor. Thoughts? @jsola@joanvallve
So regardless of if you have more or less params, or if your state is not Eigen but Composite, let us try to keep this order.
This is not yet consistent with the constructors of FrameBase. But htese constructors are also a little messy.
I bet many of these variants will disappear soon. In Problem there are 4 or 5 already.
Obviously, Problem::createNonKeyframe() must follow the same order as emplaceKeyframe.
I think it starts being time to merge my branch to devel, or that Joaquim you develop this part on top of my branch.
I am almost ready to merge --- the problem is with the other plugins. Mut I keep merging devel to 287-quad, so I think working on top of my branch makes sense while I update all plugins (this can be easy, or it can also be a nightmare).
Then this branch is ready to merge. I will adapt the plugins before merging to devel. Meanwhile take a look at the changes to double check everything makes sense.
I want to write down the changes I have made so you can double check whether it makes sense or not:
Modified frame_list_ from TrajectoryBase to be a std::map<TimeStamp, FrameBasePtr>
Removed FrameBase::updateFrames, and the attributes last_key_frame_ptr_, last_key_or_aux_frame_ptr_, because now
all the frames in trajectory are key frames and they are ordered by the map.
Defined a custom iterator type. The reason is to be able to overload the operator*
I had to define this new iterator because from outside trajectory nobody should care about what data structure is used to store the trajectory. If we don't overload the operator* then, whenever somebody wants to iterate the trajectory, they get iterators to pairs <TimeStamp, FrameBasePtr> but we only care about the pointer. With these change, if you do *it with a FrameList iterator you directly get the frame pointer. In order to do this change, I implemented the following methods for TrajectoryBase
Now to iterate the trajectory one would do the following:
for( auto it: *(trajectory_ptr)){//code}
What happens is that TrajectoryBase is now iterable itself, no need to get the FrameList to iterate. This allows us to completely hide the internal details of the data structure used to store the trajectory.
I have made some more renames such as FrameList -> FrameMap and removed "Key" from some function names because they don't make sense anymore (since all frames in a trajectory are key frames now). The changes are ready to be merged, but since this is an important refactor, unless you tell me otherwise, I will wait until next Tuesday to merge (after the meeting)