From f117b641bed1b87dd4472c75384b80a29fc823d3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 8 Feb 2019 14:44:35 +0100
Subject: [PATCH] specialized addPrior functions
---
src/sensor_base.cpp | 2 +-
src/sensor_base.h | 100 +++++++++++++++++++++++++++++++++++++++++++-
2 files changed, 100 insertions(+), 2 deletions(-)
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 1fb338827..a2a4fe226 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -144,7 +144,7 @@ void SensorBase::unfixIntrinsics()
updateCalibSize();
}
-void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+void SensorBase::addStaticParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
diff --git a/src/sensor_base.h b/src/sensor_base.h
index caffcf953..bd64a1e40 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -148,12 +148,56 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
* \param _size state segment size (-1: whole state) (not used in quaternions)
*
**/
- void addParameterPrior(const StateBlockPtr& _sb,
+ void addParameterStaticPrior(const StateBlockPtr& _sb,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+
+ void addParameterDynamicPrior(const CaptureBase& _cap,
+ const StateBlockPtr& _sb,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+
+ void addParameterPrior(const unsigned int _i,
const Eigen::VectorXs& _x,
const Eigen::MatrixXs& _cov,
unsigned int _start_idx = 0,
int _size = -1);
+ void addParameterPrior(const unsigned int _i,
+ const TimeStamp _ts,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+
+ void addPPrior(const TimeStamp _ts,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+ void addOPrior(const TimeStamp _ts,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov);
+ void addIntrinsicsPrior(const TimeStamp _ts,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+
+ void addPPrior(const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+ void addOPrior(const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov);
+ void addIntrinsicsPrior(const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
@@ -274,6 +318,60 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
hardware_ptr_ = _hw_ptr;
}
+inline void SensorBase::addParameterPrior(const unsigned int _i, const TimeStamp _ts, const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+ CaptureBasePtr cap;
+ // i is dynamic? //TODO
+ if (isDynamic(_i, _ts, cap))
+ addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
+ else
+ addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
+}
+
+inline void SensorBase::addParameterPrior(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx, int _size)
+{
+ CaptureBasePtr cap;
+ // i is dynamic? //TODO
+ if (isDynamic(_i, cap))
+ addParameterDynamicPrior(cap, cap->getStateBlockPtr(_i), _x, _cov, _start_idx, _size);
+ else
+ addParameterStaticPrior(getStateBlockPtrStatic(_i), _x, _cov, _start_idx, _size);
+}
+
+inline void SensorBase::addPPrior(const TimeStamp _ts, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx,
+ int _size)
+{
+ addParameterPrior(0, _ts, _x, _cov, _start_idx, _size);
+}
+
+inline void SensorBase::addOPrior(const TimeStamp _ts, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
+{
+ addParameterPrior(1, _ts, _x, _cov);
+}
+
+inline void SensorBase::addIntrinsicsPrior(const TimeStamp _ts, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx, int _size)
+{
+ addParameterPrior(2,_ts, _x, _cov);
+}
+
+inline void SensorBase::addPPrior(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+ addParameterPrior(0, _x, _cov, _start_idx, _size);
+}
+
+inline void SensorBase::addOPrior(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
+{
+ addParameterPrior(1, _x, _cov);
+}
+
+inline void SensorBase::addIntrinsicsPrior(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+ addParameterPrior(2, _x, _cov);
+}
+
inline SizeEigen SensorBase::getCalibSize() const
{
return calib_size_;
--
GitLab