Delete constructor with eigen vector

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,

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()
 {

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;
 }
 

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();
 

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,

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();
 }