Skip to content
Snippets Groups Projects
Commit 3e9691e2 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Add problem::getTimeStamp()

parent bb77e506
No related branches found
No related tags found
1 merge request!366Resolve "Complete state vector new data structure?"
This commit is part of merge request !366. Comments created here will be created in the context of that merge request.
...@@ -312,6 +312,7 @@ class Problem : public std::enable_shared_from_this<Problem> ...@@ -312,6 +312,7 @@ class Problem : public std::enable_shared_from_this<Problem>
// State getters // State getters
VectorComposite getState ( ) const; VectorComposite getState ( ) const;
VectorComposite getState (const TimeStamp& _ts ) const; VectorComposite getState (const TimeStamp& _ts ) const;
TimeStamp getTimeStamp ( ) const;
// Zero state provider // Zero state provider
Eigen::VectorXd zeroState ( ) const; Eigen::VectorXd zeroState ( ) const;
......
...@@ -415,6 +415,25 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // ...@@ -415,6 +415,25 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
// _state = zeroState(); // _state = zeroState();
//} //}
TimeStamp Problem::getTimeStamp ( ) const
{
if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
{
auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!");
return last_kf_or_aux->getTimeStamp();
}
else
{
TimeStamp ts(0);
for (const auto& prc : processor_is_motion_list_)
if (prc->getTimeStamp() > ts)
ts = prc->getTimeStamp();
return ts;
}
}
VectorComposite Problem::getState() const VectorComposite Problem::getState() const
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment