WIP: Resolve "Complete state vector new data structure?"

Closes #287 (closed) Closes #293 (closed)

include/core/capture/capture_base.h

@@ -83,7 +83,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
 
 
         // State blocks
-        const std::string& getStructure() const;
+        const StateStructure& getStructure() const;
         StateBlockPtr getStateBlock(const std::string& _key) const;
         StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
         StateBlockPtr getSensorP() const;
@@ -95,13 +95,17 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
 
         bool hasCalibration() const {return calib_size_ > 0;}
         SizeEigen getCalibSize() const;
-        virtual Eigen::VectorXd getCalibration() const;
+        Eigen::VectorXd getCalibration() const;
         void setCalibration(const Eigen::VectorXd& _calib);
         void move(FrameBasePtr);
         void link(FrameBasePtr);
         template<typename classType, typename... T>
         static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
 
+
+        VectorComposite getCalibrationComposite() const;
+        void getCalibrationComposite(VectorComposite& _calib) const;
+
         virtual void printHeader(int depth, //
                                  bool constr_by, //
                                  bool metric, //
@@ -116,6 +120,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
                    std::ostream& stream = std::cout,
                    std::string _tabs = "") const;
 
+
     protected:
         virtual SizeEigen computeCalibSize() const;
 
@@ -149,6 +154,26 @@ inline SizeEigen CaptureBase::getCalibSize() const
     return calib_size_;
 }
 
+inline VectorComposite CaptureBase::getCalibrationComposite() const
+{
+    VectorComposite calib;
+    getCalibrationComposite(calib);
+    return calib;
+}
+
+inline void CaptureBase::getCalibrationComposite(VectorComposite &_calib) const
+{
+    for (const auto& ckey : getStructure())
+    {
+        std::string key(1,ckey);
+        const auto& sb = getStateBlock(key);
+        if (sb and not sb->isFixed())
+            _calib[key] = sb->getState();
+        else
+            _calib.erase(key);
+    }
+}
+
 inline void CaptureBase::updateCalibSize()
 {
     calib_size_ = computeCalibSize();