Move calibration stuff to CaptureBase

src/capture_base.cpp

@@ -3,6 +3,8 @@
 
 namespace wolf{
 
+using namespace Eigen;
+
 unsigned int CaptureBase::capture_id_count_ = 0;
 
 CaptureBase::CaptureBase(const std::string& _type,
@@ -60,6 +62,7 @@ CaptureBase::CaptureBase(const std::string& _type,
     {
         WOLF_ERROR("Provided sensor parameters but no sensor pointer");
     }
+    updateCalilbSize();
 }
 
 
@@ -144,6 +147,7 @@ void CaptureBase::fix()
                 getProblem()->updateStateBlockPtr(sbp);
         }
     }
+    updateCalilbSize();
 }
 
 void CaptureBase::unfix()
@@ -158,6 +162,7 @@ void CaptureBase::unfix()
                 getProblem()->updateStateBlockPtr(sbp);
         }
     }
+    updateCalilbSize();
 }
 
 void CaptureBase::fixExtrinsics()
@@ -172,6 +177,7 @@ void CaptureBase::fixExtrinsics()
                 getProblem()->updateStateBlockPtr(sbp);
         }
     }
+    updateCalilbSize();
 }
 
 void CaptureBase::unfixExtrinsics()
@@ -186,6 +192,7 @@ void CaptureBase::unfixExtrinsics()
                 getProblem()->updateStateBlockPtr(sbp);
         }
     }
+    updateCalilbSize();
 }
 
 void CaptureBase::fixIntrinsics()
@@ -200,6 +207,7 @@ void CaptureBase::fixIntrinsics()
                 getProblem()->updateStateBlockPtr(sbp);
         }
     }
+    updateCalilbSize();
 }
 
 void CaptureBase::unfixIntrinsics()
@@ -214,6 +222,7 @@ void CaptureBase::unfixIntrinsics()
                 getProblem()->updateStateBlockPtr(sbp);
         }
     }
+    updateCalilbSize();
 }
 
 
@@ -228,6 +237,50 @@ void CaptureBase::registerNewStateBlocks()
     }
 }
 
+wolf::Size CaptureBase::computeCalibSize() const
+{
+    Size sz = 0;
+    for (Size i = 0; i < state_block_vec_.size(); i++)
+    {
+        auto sb = getStateBlockPtr(i);
+        if (sb && !sb->isFixed())
+            sz += sb->getSize();
+    }
+    return sz;
+}
+
+Eigen::VectorXs CaptureBase::getCalibration() const
+{
+    Eigen::VectorXs calib(calib_size_);
+    Size index = 0;
+    for (Size i = 0; i < getStateBlockVec().size(); i++)
+    {
+        auto sb = getStateBlockPtr(i);
+        if (sb && !sb->isFixed())
+        {
+            calib.segment(index, sb->getSize()) = sb->getState();
+            index += sb->getSize();
+        }
+    }
+    return calib;
+}
+
+void CaptureBase::setCalibration(const VectorXs& _calib)
+{
+    updateCalilbSize();
+    assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
+    Size index = 0;
+    for (Size i = 0; i < getStateBlockVec().size(); i++)
+    {
+        auto sb = getStateBlockPtr(i);
+        if (sb && !sb->isFixed())
+        {
+            sb->setState(_calib.segment(index, sb->getSize()));
+            index += sb->getSize();
+        }
+    }
+}
+
 
 
 

src/capture_base.h

@@ -32,6 +32,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
     protected:
         unsigned int capture_id_;
         TimeStamp time_stamp_; ///< Time stamp
+        Size calib_size_;
 
     public:
 
@@ -64,6 +65,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         void getConstraintList(ConstraintBaseList& _ctr_list);
 
         SensorBasePtr getSensorPtr() const;
+        virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+
         // State blocks
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
@@ -83,9 +86,25 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         void fixIntrinsics();
         void unfixIntrinsics();
 
-        virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+        Size getCalibSize() const;
+        Size computeCalibSize() const;
+        void updateCalilbSize();
+
+        virtual Eigen::VectorXs getCalibration() const;
+        void setCalibration(const Eigen::VectorXs& _calib);
+
 };
 
+inline wolf::Size CaptureBase::getCalibSize() const
+{
+    return calib_size_;
+}
+
+inline void CaptureBase::updateCalilbSize()
+{
+    calib_size_ = computeCalibSize();
+}
+
 }
 
 #include "sensor_base.h"

src/capture_motion.cpp

@@ -22,7 +22,6 @@ CaptureMotion::CaptureMotion(const TimeStamp& _ts,
                 CaptureBase("MOTION", _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
                 data_(_data),
                 data_cov_(_data_cov),
-                calib_size_(_calib_size),
                 buffer_(_data.size(), _delta_size, _delta_cov_size, _calib_size),
                 origin_frame_ptr_(_origin_frame_ptr)
 {
@@ -37,37 +36,6 @@ CaptureMotion::~CaptureMotion()
     //
 }
 
-VectorXs CaptureMotion::getCalibration() const
-{
-    VectorXs calib(calib_size_);
-    Size index = 0;
-    for (Size i = 0; i < getStateBlockVec().size(); i++)
-    {
-        auto sb = getStateBlockPtr(i);
-        if (sb && !sb->isFixed())
-        {
-            calib.segment(index, sb->getSize()) = sb->getState();
-            index += sb->getSize();
-        }
-    }
-    return calib;
-}
-
-void CaptureMotion::setCalibration(const VectorXs& _calib)
-{
-    assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
-    Size index = 0;
-    for (Size i = 0; i < getStateBlockVec().size(); i++)
-    {
-        auto sb = getStateBlockPtr(i);
-        if (sb && !sb->isFixed())
-        {
-            sb->setState(_calib.segment(index, sb->getSize()));
-            index += sb->getSize();
-        }
-    }
-}
-
 
 Eigen::VectorXs CaptureMotion::getDelta(const VectorXs& _calib_current)
 {

src/capture_motion.h

@@ -74,8 +74,6 @@ class CaptureMotion : public CaptureBase
         // Buffer's initial conditions for pre-integration
         VectorXs getCalibrationPreint() const;
         void setCalibrationPreint(const VectorXs& _calib_preint);
-        virtual VectorXs getCalibration() const;
-        void setCalibration(const VectorXs& _calib);
         MatrixXs getJacobianCalib();
         MatrixXs getJacobianCalib(const TimeStamp& _ts);
 
@@ -92,7 +90,6 @@ class CaptureMotion : public CaptureBase
     private:
         Eigen::VectorXs data_;          ///< Motion data in form of vector mandatory
         Eigen::MatrixXs data_cov_;      ///< Motion data covariance
-        Size calib_size_;
         MotionBuffer buffer_;           ///< Buffer of motions between this Capture and the next one.
         FrameBasePtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
 };
@@ -105,7 +102,6 @@ inline CaptureMotion::CaptureMotion(const TimeStamp& _ts,
         CaptureBase("MOTION", _ts, _sensor_ptr),
         data_(_data),
         data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
-        calib_size_(_calib_size),
         buffer_(_data.size(), _delta_size, _delta_cov_size, _calib_size),
         origin_frame_ptr_(_origin_frame_ptr)
 {