diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 99dc9a25ec2875115901eabf2cfe9b39fc42890d..5b8cc7863c15b35143be567e5298b4732871ad32 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -95,7 +95,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
bool hasCalibration() {return calib_size_ > 0;}
SizeEigen getCalibSize() const;
- virtual Eigen::VectorXd getCalibration() const;
+ virtual VectorComposite getCalibration() const;
void setCalibration(const Eigen::VectorXd& _calib);
void move(FrameBasePtr);
void link(FrameBasePtr);
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 0ca665a661a83409d17540f8caa94a5dfd1b5603..7a4e2af97aedf58f7fa829ddcdde65ad3d73a46f 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -544,16 +544,18 @@ inline bool ProcessorMotion::voteForKeyFrame() const
inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
{
+ // TODO REMOVE function
// ensure integrity
assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
- // ensure proper size of the provided reference
- Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
- _x.resize( curr_x.size() );
+ Eigen::VectorXd x_origin = origin_ptr_->getFrame()->getState();
- // do get timestamp and state corrected by possible self-calibrations
- double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+// // do get timestamp and state corrected by possible self-calibrations
+// double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
+// statePlusDelta(x_origin, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+
+ // let it compile
+ _x = x_origin;
}
inline const MatrixComposite ProcessorMotion::getCurrentDeltaPreintCov() const
@@ -591,16 +593,17 @@ inline MotionBuffer& ProcessorMotion::getBuffer()
inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
{
+ // TODO check this code
return Motion(_ts,
VectorXd::Zero(data_size_), // data
Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
deltaZero(),
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
+ MatrixComposite(), // Cov delta
deltaZero(),
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
- Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
- Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib
+ MatrixComposite(), // Cov delta_integr
+ MatrixComposite(), // Jac delta
+ MatrixComposite(), // Jac delta_integr
+ MatrixComposite() // Jac calib
);
}
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index b03416837ec1b0a6a86a67ff0cf40fdf820b1370..e768e6ff0e95a7251ffbb1d9e635a292bd315315 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -52,26 +52,26 @@ class ProcessorOdom2d : public ProcessorMotion
protected:
virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
+ const VectorComposite& _calib,
const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
+ Eigen::VectorComposite& _delta,
+ MatrixComposite& _delta_cov,
+ MatrixComposite& _jacobian_calib) const override;
+ virtual void deltaPlusDelta(const VectorComposite& _delta1,
+ const VectorComposite& _delta2,
const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
- virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
+ VectorComposite& _delta1_plus_delta2) const override;
+ virtual void deltaPlusDelta(const VectorComposite& _delta1,
+ const VectorComposite& _delta2,
const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
- const Eigen::VectorXd& _delta,
+ VectorComposite& _delta1_plus_delta2,
+ MatrixComposite& _jacobian1,
+ MatrixComposite& _jacobian2) const override;
+ virtual void statePlusDelta(const VectorComposite& _x,
+ const VectorComposite& _delta,
const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const override;
- virtual Eigen::VectorXd deltaZero() const override;
+ VectorComposite& _x_plus_delta) const override;
+ virtual VectorComposite deltaZero() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 1b506aeaeb3498eeaf1b46293589f7364dc5aea9..631b3b18119ddcefc9761fd31a041df4e3ee6478 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -33,14 +33,17 @@ namespace Eigen
{
std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
-wolf::VectorComposite& operator + (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
-wolf::VectorComposite operator + (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
-wolf::VectorComposite& operator - (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
-wolf::VectorComposite operator - (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
}
namespace wolf{
+VectorComposite& operator + ( VectorComposite& _v1, const VectorComposite& _v2);
+VectorComposite operator + (const VectorComposite& _v1, const VectorComposite& _v2);
+VectorComposite& operator - ( VectorComposite& _v1, const VectorComposite& _v2);
+VectorComposite operator - (const VectorComposite& _v1, const VectorComposite& _v2);
+
+Eigen::VectorXd toVector(const VectorComposite& _v, const StateStructure& _s);
+
class StateBlockComposite
{
public:
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 949bf1740e13096a5ceafc2bb301a7f39d061fc4..ed4271be7c63622d634193b8bbbd4291ec5374c2 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -191,19 +191,43 @@ SizeEigen CaptureBase::computeCalibSize() const
return sz;
}
-Eigen::VectorXd CaptureBase::getCalibration() const
+VectorComposite CaptureBase::getCalibration() const
{
- Eigen::VectorXd calib(calib_size_);
- SizeEigen index = 0;
- for (const auto& key : getStructure())
+ VectorComposite calib;
+
+ // gather unfixed state blocks from capture, then from sensor
+ for (const auto& pair_key_sb : getStateBlockMap())
{
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
+ if (not sb->isFixed())
+ calib.emplace(key, sb->getState());
+ }
+
+ // gather unfixed state blocks from sensor that were not in capture
+ for (const auto& pair_key_sb : getSensor()->getStateBlockMap())
+ {
+ const auto& key = pair_key_sb.first;
+ if (calib.count(key) == 0)
{
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
+ const auto& sb = pair_key_sb.second;
+ // const auto& sb = getSensor()->getStateBlockDynamic(key);
+ if (not sb->isFixed())
+ calib.emplace(key, sb->getState());
}
}
+
+//// SizeEigen index = 0;
+// for (const auto& key : getStructure())
+// {
+// auto sb = getStateBlock(key);
+// if (sb && !sb->isFixed())
+// {
+// calib.segment(index, sb->getSize()) = sb->getState();
+// index += sb->getSize();
+// }
+// }
+
return calib;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index b31ae1e1b94dd7a33684684986c844fc0d09dc9f..2ca33d8d1a6844cec2097ca1b2f50ce07e284dae 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -25,24 +25,24 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
ProcessorBase(_type, _dim, _params_motion),
params_motion_(_params_motion),
processing_step_(RUNNING_WITHOUT_PACK),
- x_size_(_state_size),
+ x_size_(),
data_size_(_data_size),
- delta_size_(_delta_size),
- delta_cov_size_(_delta_cov_size),
- calib_size_(_calib_size),
+ delta_size_(),
+ delta_cov_size_(),
+ calib_size_(),
origin_ptr_(),
last_ptr_(),
incoming_ptr_(),
dt_(0.0),
- x_(_state_size),
- delta_(_delta_size),
- delta_cov_(_delta_cov_size, _delta_cov_size),
- delta_integrated_(_delta_size),
- delta_integrated_cov_(_delta_cov_size, _delta_cov_size),
- calib_preint_(_calib_size),
- jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
- jacobian_delta_(delta_cov_size_, delta_cov_size_),
- jacobian_calib_(delta_cov_size_, calib_size_)
+ x_(),
+ delta_(),
+ delta_cov_(),
+ delta_integrated_(),
+ delta_integrated_cov_(),
+ calib_preint_(),
+ jacobian_delta_preint_(),
+ jacobian_delta_(),
+ jacobian_calib_()
{
setStateStructure(_state_structure);
//
@@ -70,7 +70,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
_capture_source->setOriginCapture(_capture_target);
// Get optimized calibration params from 'origin' keyframe
- VectorXd calib_preint_optim = _capture_source->getOriginCapture()->getCalibration();
+ VectorComposite calib_preint_optim = _capture_source->getOriginCapture()->getCalibration();
// Write the calib params into the capture before re-integration
_capture_source->setCalibrationPreint(calib_preint_optim);
@@ -150,7 +150,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto capture_origin = capture_existing->getOriginCapture();
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXd calib_origin = capture_origin->getCalibration();
+ VectorComposite calib_origin = capture_origin->getCalibration();
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index e8e0ec769f1810bb4870fe508a708f314adad2ba..e3615683dd04e245fef6b15ede59ead685699bf7 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -16,6 +16,11 @@ std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
return _os;
}
+}
+
+namespace wolf
+{
+
wolf::VectorComposite& operator + (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2)
{
wolf::VectorComposite v;
@@ -68,10 +73,21 @@ wolf::VectorComposite operator - (const wolf::VectorComposite& _v1, const wolf::
return v;
}
+Eigen::VectorXd toVector(const VectorComposite &_v, const StateStructure &_s)
+{
+ Eigen::VectorXd x;
+ unsigned int index = 0;
+ unsigned int size = 0;
+ for (const auto& ckey : _s)
+ {
+ const auto key = std::string(1, ckey); // ckey is char
+ const auto& v = _v.at(key);
+ size = v.size();
+ x.segment(index, size) = v;
+ }
+ return x;
}
-namespace wolf
-{
bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, const Eigen::MatrixXd &_mat_blk)
{