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

Fix frame structure upon emplaceFrame

parent 12bef0d9
No related branches found
No related tags found
1 merge request!403Resolve "Merge Aux/KeyFrames into Estimated Frames"
Pipeline #6201 passed
......@@ -347,6 +347,10 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
state.emplace(key, _frame_state.segment(index, size));
// append new key to frame structure
if (frame_structure_.find(key) == std::string::npos) // not found
frame_structure_ += std::string(1,key);
index += size;
}
......@@ -356,6 +360,7 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
_time_stamp,
_frame_structure,
state);
return frm;
}
......@@ -364,36 +369,44 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
const SizeEigen _dim)
{
return emplaceFrame(_time_stamp,
_frame_structure,
getState(_time_stamp));
_frame_structure,
getState(_time_stamp));
}
FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const VectorComposite& _frame_state)
const StateStructure& _frame_structure, //
const VectorComposite& _frame_state)
{
return FrameBase::emplaceFrame<FrameBase>(getTrajectory(),
_time_stamp,
_frame_structure,
_frame_state);
_time_stamp,
_frame_structure,
_frame_state);
}
FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
const VectorComposite& _frame_state)
{
// append new keys to frame structure
for (const auto& pair_key_vec : _frame_state)
{
const auto& key = pair_key_vec.first;
if (frame_structure_.find(key) == std::string::npos) // not found
frame_structure_ += std::string(1,key);
}
return FrameBase::emplaceFrame<FrameBase>(getTrajectory(),
_time_stamp,
getFrameStructure(),
_frame_state);
_time_stamp,
getFrameStructure(),
_frame_state);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
const Eigen::VectorXd& _frame_state)
{
return emplaceFrame(_time_stamp,
this->getFrameStructure(),
this->getDim(),
_frame_state);
this->getFrameStructure(),
this->getDim(),
_frame_state);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
......
......@@ -228,6 +228,7 @@ TEST(Problem, emplaceFrame_factory)
FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, VectorXd(7) );
FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, VectorXd(10) );
// check that all frames are effectively in the trajectory
ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
......@@ -239,6 +240,8 @@ TEST(Problem, emplaceFrame_factory)
ASSERT_EQ(f0->getProblem(), P);
ASSERT_EQ(f1->getProblem(), P);
ASSERT_EQ(f2->getProblem(), P);
ASSERT_EQ(P->getFrameStructure(), "POV");
}
TEST(Problem, StateBlocks)
......@@ -565,6 +568,6 @@ TEST(Problem, getState)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
//::testing::GTEST_FLAG(filter) = "Problem.getState";
// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
return RUN_ALL_TESTS();
}
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