Add setCalibration() and setCalibrationPreint()

src/capture_motion.cpp

@@ -53,6 +53,22 @@ VectorXs CaptureMotion::getCalibration() const
     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()
 {
     VectorXs calib_preint   = getCalibrationPreint();

src/capture_motion.h

@@ -73,7 +73,9 @@ 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);
 
@@ -174,6 +176,11 @@ inline VectorXs CaptureMotion::getCalibrationPreint() const
     return getBuffer().getCalibrationPreint();
 }
 
+inline void CaptureMotion::setCalibrationPreint(const VectorXs& _calib_preint)
+{
+    getBuffer().setCalibrationPreint(_calib_preint);
+}
+
 } // namespace wolf
 
 #endif /* SRC_CAPTURE_MOTION_H_ */