Add cap->getStructure()

include/core/capture/capture_base.h

@@ -81,6 +81,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         const FactorBasePtrList& getConstrainedByList() const;
 
         // State blocks
+        std::string getStructure() const;
         StateBlockPtr getStateBlock(const std::string& _key) const;
         StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
         StateBlockPtr getSensorP() const;

src/capture/capture_base.cpp

@@ -135,6 +135,14 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
     constrained_by_list_.remove(_fac_ptr);
 }
 
+std::string CaptureBase::getStructure() const
+{
+    if (getSensor())
+        return getSensor()->getStructure();
+    else
+        return HasStateBlocks::getStructure();
+}
+
 StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
 {
     if (getSensor())
@@ -208,8 +216,7 @@ void CaptureBase::registerNewStateBlocks()
 SizeEigen CaptureBase::computeCalibSize() const
 {
     SizeEigen sz = 0;
-    const auto& structure = (getSensor() ? getSensor()->getStructure() : getStructure());
-    for (const auto& key : structure)
+    for (const auto& key : getStructure())
     {
         auto sb = getStateBlock(key);
         if (sb && !sb->isFixed())
@@ -222,8 +229,7 @@ Eigen::VectorXs CaptureBase::getCalibration() const
 {
     Eigen::VectorXs calib(calib_size_);
     SizeEigen index = 0;
-    const auto& structure = (getSensor() ? getSensor()->getStructure() : getStructure());
-    for (const auto& key : structure)
+    for (const auto& key : getStructure())
     {
         auto sb = getStateBlock(key);
         if (sb && !sb->isFixed())
@@ -240,8 +246,7 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
     updateCalibSize();
     assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
     SizeEigen index = 0;
-    const auto& structure = (getSensor() ? getSensor()->getStructure() : getStructure());
-    for (const auto& key : structure)
+    for (const auto& key : getStructure())
     {
         auto sb = getStateBlock(key);
         if (sb && !sb->isFixed())