diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 5c45c103c33b77542a1d136b870d6a0f88c177eb..91a94d274b36d0023a0ba267583f4b67726d8665 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -63,12 +63,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _v_ptr = nullptr);
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
- const SizeEigen _dim,
- const Eigen::VectorXd& _x);
-
FrameBase(const FrameType & _tp,
const TimeStamp& _ts,
const std::string _frame_structure,
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index d1b49a22e45ec0093d5027d852fb7d7e08f36d76..4147b98ca8dcd67d76fb732f11cbbee85cea9df8 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -89,65 +89,6 @@ FrameBase::FrameBase(const FrameType & _tp,
}
}
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
- const SizeEigen _dim,
- const Eigen::VectorXd& _x) :
- NodeBase("FRAME", "FrameBase"),
- HasStateBlocks(_frame_structure),
- trajectory_ptr_(),
- frame_id_(++frame_id_count_),
- type_(_tp),
- time_stamp_(_ts)
-{
- if(_frame_structure.compare("PO") == 0 and _dim == 2){
- assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for PO 2D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((double) _x(2) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- this->setType("PO 2d");
- }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
- assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- this->setType("PO 3d");
- }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXd::Zero(10);
- assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
- StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- addStateBlock("V", v_ptr);
- this->setType("POV 3d");
- }else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXd::Zero(10);
- assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3 ) ) );
- StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.segment <3> (7 ) ) );
- StateBlockPtr c_ptr ( std::make_shared<StateBlock> (_x.segment <3> (10) ) );
- StateBlockPtr cd_ptr ( std::make_shared<StateBlock> (_x.segment <3> (13) ) );
- StateBlockPtr Lc_ptr ( std::make_shared<StateBlock> (_x.segment <3> (16) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- addStateBlock("V", v_ptr);
- addStateBlock("C", c_ptr);
- addStateBlock("D", cd_ptr);
- addStateBlock("L", Lc_ptr);
- this->setType("POVCDL 3d");
- }else{
- std::cout << _frame_structure << " ^^ " << _dim << std::endl;
- throw std::runtime_error("Unknown frame structure");
- }
-
-
-}
FrameBase::~FrameBase()
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 7355c76e3856373db8c19c89c38e3fbe970a4c37..4d2462f72d4e9bbf2fc6cb2c453389ea02028309 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -331,12 +331,35 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
const SizeEigen _dim, //
const Eigen::VectorXd& _frame_state)
{
+ VectorComposite state;
+ SizeEigen index = 0;
+ SizeEigen size = 0;
+ for (const auto& ckey : _frame_structure)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+
+ if (key == "O")
+ {
+ if (_dim == 2) size = 1;
+ else size = 4;
+ }
+ else
+ size = _dim;
+
+ assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");
+
+ state.emplace(key, _frame_state.segment(index, size));
+
+ index += size;
+ }
+
+ assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
+
auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
_frame_key_type,
_time_stamp,
_frame_structure,
- _dim,
- _frame_state);
+ state);
return frm;
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 7b5d5dec09eadb5aa13b84d9340c760adba3f7c4..ef801eb6d525375272fd95d89a50eec9bed4dc1e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -201,7 +201,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// make F; append incoming to new F
FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getStateVector());
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
origin_ptr_ = last_ptr_;
@@ -250,7 +250,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Replace last frame for a new one in incoming
FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getStateVector());
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 713fd1c1b26624b84612a1e721eb4924db08777c..f2e8e5f6014811ea3b99c2843fa4b0e8e94c5b72 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -163,14 +163,12 @@ class FactorDiffDriveTest : public testing::Test
KEY,
0.0,
"PO",
- 2,
- Vector3d(0,0,0));
+ std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
F1 = FrameBase::emplace<FrameBase>(trajectory,
KEY,
1.0,
"PO",
- 2,
- Vector3d(1,0,0));
+ std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 8ec6ebcae42990451909902ee8f245c35fbf16c4..20e272cfa698f904e3bb63c6d7730b11e0fe4568 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -227,17 +227,13 @@ TEST(Problem, emplaceFrame_factory)
FrameBasePtr f1 = P->emplaceFrame(KEY, 1, "PO" , 3, VectorXd(7) );
FrameBasePtr f2 = P->emplaceFrame(KEY, 2, "POV", 3, VectorXd(10) );
- // std::cout << "f0: type PO 2d? " << f0->getType() << std::endl;
- // std::cout << "f1: type PO 3d? " << f1->getType() << std::endl;
- // std::cout << "f2: type POV 3d? " << f2->getType() << std::endl;
-
- ASSERT_EQ(f0->getType().compare("PO 2d"), 0);
- ASSERT_EQ(f1->getType().compare("PO 3d"), 0);
- ASSERT_EQ(f2->getType().compare("POV 3d"), 0);
-
// check that all frames are effectively in the trajectory
ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
+ ASSERT_EQ(f0->getStateVector().size(), 3 );
+ ASSERT_EQ(f1->getStateVector().size(), 7 );
+ ASSERT_EQ(f2->getStateVector().size(), 10);
+
// check that all frames are linked to Problem
ASSERT_EQ(f0->getProblem(), P);
ASSERT_EQ(f1->getProblem(), P);
@@ -497,5 +493,6 @@ TEST(Problem, check)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
return RUN_ALL_TESTS();
}