From bfa42056729cb10223802c5800ef7455767cab5a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 26 May 2020 16:57:11 +0200
Subject: [PATCH] Access calibration from processor, not capture
---
include/core/capture/capture_base.h | 29 ++++-------
include/core/processor/processor_diff_drive.h | 14 +++++-
include/core/processor/processor_motion.h | 11 ++---
include/core/processor/processor_odom_2d.h | 10 ++++
include/core/processor/processor_odom_3d.h | 2 +
src/capture/capture_base.cpp | 49 -------------------
src/capture/capture_motion.cpp | 14 +++---
src/processor/processor_diff_drive.cpp | 3 +-
src/processor/processor_motion.cpp | 14 +++---
src/processor/processor_odom_2d.cpp | 2 +-
src/processor/processor_odom_3d.cpp | 11 ++++-
test/gtest_factor_diff_drive.cpp | 13 ++---
12 files changed, 68 insertions(+), 104 deletions(-)
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 42fe6af5e..35372c103 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -29,7 +29,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
FeatureBasePtrList feature_list_;
FactorBasePtrList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
- SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
static unsigned int capture_id_count_;
@@ -93,10 +92,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
virtual void fix() override;
virtual void unfix() override;
- bool hasCalibration() const {return calib_size_ > 0;}
- SizeEigen getCalibSize() const;
- virtual Eigen::VectorXd getCalibration() const;
- void setCalibration(const Eigen::VectorXd& _calib);
void move(FrameBasePtr);
void link(FrameBasePtr);
template<typename classType, typename... T>
@@ -116,15 +111,9 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
std::ostream& stream = std::cout,
std::string _tabs = "") const;
- protected:
- virtual SizeEigen computeCalibSize() const;
-
private:
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
void removeFeature(FeatureBasePtr _ft_ptr);
-
- private:
- void updateCalibSize();
};
}
@@ -144,15 +133,15 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
return cpt;
}
-inline SizeEigen CaptureBase::getCalibSize() const
-{
- return calib_size_;
-}
-
-inline void CaptureBase::updateCalibSize()
-{
- calib_size_ = computeCalibSize();
-}
+//inline SizeEigen CaptureBase::getCalibSize() const
+//{
+// return calib_size_;
+//}
+//
+//inline void CaptureBase::updateCalibSize()
+//{
+// calib_size_ = computeCalibSize();
+//}
inline StateBlockPtr CaptureBase::getSensorP() const
{
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index bcbf3f603..e3e504f36 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -60,7 +60,8 @@ class ProcessorDiffDrive : public ProcessorOdom2d
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
-
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
@@ -68,6 +69,17 @@ class ProcessorDiffDrive : public ProcessorOdom2d
};
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
+{
+ return _capture->getStateBlock("I")->getState();
+}
+
+inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ _capture->getStateBlock("I")->setState(_calibration);
+}
+
+
}
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index d668792ae..46fe7058a 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -174,14 +174,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
virtual VectorComposite getState() const override;
virtual VectorComposite getState(const TimeStamp& _ts) const override;
-
-// /** \brief Fill the state corresponding to the provided time-stamp
-// * \param _ts the time stamp
-// * \param _x the returned state
-// * \return if state in the provided time-stamp could be resolved
-// */
-// virtual Eigen::VectorXd getStateVector(const TimeStamp& _ts) const;
-
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
*/
@@ -457,6 +449,9 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const = 0;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
+
Motion motionZero(const TimeStamp& _ts) const;
bool hasCalibration() const {return calib_size_ > 0;}
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 9926fe781..7875c7974 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -88,6 +88,8 @@ class ProcessorOdom2d : public ProcessorMotion
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom2dPtr params_odom_2d_;
@@ -107,6 +109,14 @@ inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& del
return delta_corrected;
}
+inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+
+inline void ProcessorOdom2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+}
} // namespace wolf
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 2fa981158..7f76c128e 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -109,6 +109,8 @@ class ProcessorOdom3d : public ProcessorMotion
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom3dPtr params_odom_3d_;
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index a6dde95f9..b98002d5a 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -17,7 +17,6 @@ CaptureBase::CaptureBase(const std::string& _type,
HasStateBlocks(""),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
- calib_size_(0),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
{
@@ -52,8 +51,6 @@ CaptureBase::CaptureBase(const std::string& _type,
{
WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
-
- updateCalibSize();
}
CaptureBase::~CaptureBase()
@@ -170,57 +167,11 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
void CaptureBase::fix()
{
HasStateBlocks::fix();
- updateCalibSize();
}
void CaptureBase::unfix()
{
HasStateBlocks::unfix();
- updateCalibSize();
-}
-
-SizeEigen CaptureBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXd CaptureBase::getCalibration() const
-{
- Eigen::VectorXd calib(calib_size_);
- 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;
-}
-
-void CaptureBase::setCalibration(const VectorXd& _calib)
-{
- updateCalibSize();
- assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
- SizeEigen index = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- {
- sb->setState(_calib.segment(index, sb->getSize()));
- index += sb->getSize();
- }
- }
}
void CaptureBase::move(FrameBasePtr _frm_ptr)
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index a61ae5b55..d3bae9855 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -137,13 +137,13 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
if ( _metric && ! getBuffer().empty())
{
_stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
- if (hasCalibration())
- {
- _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
- _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
- _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
- _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
- }
+// if (hasCalibration())
+// {
+// _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
+// _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
+//// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
+//// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+// }
}
}
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 55841ede9..43f15c52d 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -138,7 +138,7 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
_data,
_data_cov,
_capture_origin);
- cap_motion->setCalibration (_calib);
+ setCalibration (cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -168,7 +168,6 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
return fac_odom;
}
-
} /* namespace wolf */
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index f7f39a673..5087f744b 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -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();
+ VectorXd calib_preint_optim = getCalibration(_capture_source->getOriginCapture());
// Write the calib params into the capture before re-integration
_capture_source->setCalibrationPreint(calib_preint_optim);
@@ -175,7 +175,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();
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
@@ -260,7 +260,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto & capture_origin = origin_ptr_;
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXd calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
@@ -359,8 +359,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
key_frame->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- last_ptr_->getCalibration(),
- last_ptr_->getCalibration(),
+ getCalibration(last_ptr_),
+ getCalibration(last_ptr_),
last_ptr_);
// reset the new buffer
capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
@@ -416,7 +416,7 @@ VectorComposite ProcessorMotion::getState() const
if ( calib_preint.size() > 0 )
{
// Get current calibration -- from origin capture
- const auto& calib = origin_ptr_->getCalibration();
+ const auto& calib = getCalibration(origin_ptr_);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
@@ -488,7 +488,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
if ( calib_preint.size() > 0 )
{
// Get current calibration -- from origin capture
- const auto& calib = cap_orig->getCalibration();
+ const auto& calib = getCalibration(cap_orig);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 6b11628e9..2a8eaaf94 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -141,7 +141,7 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
const CaptureBasePtr& _capture_origin_ptr)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom2d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
- cap_motion->setCalibration(_calib);
+ setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 51f0e26de..9d67f6777 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -206,7 +206,7 @@ CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own,
const CaptureBasePtr& _capture_origin)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom3d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
- cap_motion->setCalibration(_calib);
+ setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -246,12 +246,21 @@ FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, Cap
return fac_odom;
}
+VectorXd ProcessorOdom3d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+void ProcessorOdom3d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
}
+} // namespace wolf
+
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorOdom3d);
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom3d);
} // namespace wolf
+
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 827ca5f73..579875a8b 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -63,13 +63,10 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
-// virtual void statePlusDelta(const Eigen::VectorXd& _x,
-// const Eigen::VectorXd& _delta,
-// const double _dt,
-// Eigen::VectorXd& _x_plus_delta) const
-// {
-// Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
-// }
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const
+ {
+ return Base::getCalibration(_capture);
+ }
virtual Eigen::VectorXd deltaZero() const
{
@@ -201,7 +198,7 @@ class FactorDiffDriveTest : public testing::Test
"FeatureDiffDrive",
delta1,
delta1_cov,
- C0->getCalibration(),
+ processor->getCalibration(C0),
Matrix3d::Identity()); // TODO Jacobian?
--
GitLab