diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 6bc88b701aa909e2fa94141aa7993080bf3332b8..ab55d341b5edbf2bcf4200678c2eb5e5e19dea79 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -96,9 +96,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
unsigned int sensor_id_; // sensor ID
- bool extrinsic_dynamic_;// extrinsic parameters vary with time? If so, they will be taken from the Capture nodes.
- bool intrinsic_dynamic_;// intrinsic parameters vary with time? If so, they will be taken from the Capture nodes.
- bool has_capture_; // indicates this sensor took at least one capture
+ std::map<std::string, bool> state_block_dynamic_; // mark dynamic state blocks
std::map<std::string, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
@@ -125,7 +123,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const unsigned int _noise_size,
- const bool _extr_dyn = false,
+ const bool _p_dyn = false,
+ const bool _o_dyn = false,
const bool _intr_dyn = false);
/** \brief Constructor with noise std vector
@@ -144,7 +143,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn = false,
+ const bool _p_dyn = false,
+ const bool _o_dyn = false,
const bool _intr_dyn = false);
virtual ~SensorBase();
@@ -161,25 +161,25 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
public:
const ProcessorBasePtrList& getProcessorList() const;
+ CaptureBasePtr lastCapture(void) const;
CaptureBasePtr lastKeyCapture(void) const;
CaptureBasePtr lastCapture(const TimeStamp& _ts) const;
bool process(const CaptureBasePtr capture_ptr);
// State blocks
- StateBlockPtr getStateBlockStatic(const std::string& _key) const;
- StateBlockPtr getStateBlockStatic(const char& _key) const {return getStateBlockStatic(std::string(1, _key));}
void setStateBlockStatic(const std::string& _key, const StateBlockPtr _sb_ptr);
void setStateBlockStatic(const char& _key, const StateBlockPtr _sb_ptr){setStateBlockStatic(std::string(1, _key), _sb_ptr);}
- StateBlockPtr getStateBlock(const std::string& _key) const;
- StateBlockPtr getStateBlock(const std::string& _key, const TimeStamp& _ts) const;
- StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1,_key)); }
- StateBlockPtr getStateBlock(const char _key, const TimeStamp& _ts) const { return getStateBlock(std::string(1,_key), _ts); }
-
- bool isStateBlockDynamic(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& cap) const;
- bool isStateBlockDynamic(const std::string& _key, CaptureBasePtr& cap) const;
- bool isStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const;
+ StateBlockPtr getStateBlockAuto(const std::string& _key) const;
+ StateBlockPtr getStateBlockAuto(const std::string& _key, const TimeStamp& _ts) const;
+ StateBlockPtr getStateBlockAuto(const char _key) const { return getStateBlockAuto(std::string(1,_key)); }
+ StateBlockPtr getStateBlockAuto(const char _key, const TimeStamp& _ts) const { return getStateBlockAuto(std::string(1,_key), _ts); }
+
bool isStateBlockDynamic(const std::string& _key) const;
+ bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& cap) const;
+ bool isStateBlockInCapture(const std::string& _key, CaptureBasePtr& cap) const;
+ bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const;
+ bool isStateBlockInCapture(const std::string& _key) const;
StateBlockPtr getP(const TimeStamp _ts) const;
StateBlockPtr getO(const TimeStamp _ts) const;
@@ -192,6 +192,10 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
protected:
void removeStateBlocks();
virtual void registerNewStateBlocks();
+ void setStateBlockDynamic(const std::string& _key, bool _dynamic = true)
+ {
+ state_block_dynamic_[_key] = _dynamic;
+ }
public:
@@ -229,13 +233,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
- bool isExtrinsicDynamic() const;
- bool isIntrinsicDynamic() const;
- bool hasCapture() const {return has_capture_;}
- void setHasCapture() {has_capture_ = true;}
- bool extrinsicsInCaptures() const { return extrinsic_dynamic_ && has_capture_; }
- bool intrinsicsInCaptures() const { return intrinsic_dynamic_ && has_capture_; }
-
void setNoiseStd(const Eigen::VectorXs & _noise_std);
void setNoiseCov(const Eigen::MatrixXs & _noise_std);
Eigen::VectorXs getNoiseStd() const;
@@ -281,27 +278,15 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
return processor_list_;
}
-inline StateBlockPtr SensorBase::getStateBlockStatic(const std::string& _key) const
-{
- return HasStateBlocks::getStateBlock(_key);
-}
-
inline void SensorBase::setStateBlockStatic(const std::string& _key, const StateBlockPtr _sb_ptr)
{
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
HasStateBlocks::setStateBlock(_key, _sb_ptr);
}
-inline bool SensorBase::isExtrinsicDynamic() const
-{
- // If this Sensor has no Capture yet, we'll consider it static
- return extrinsic_dynamic_;
-}
-
-inline bool SensorBase::isIntrinsicDynamic() const
+inline bool SensorBase::isStateBlockDynamic(const std::string& _key) const
{
- // If this Sensor has no Capture yet, we'll consider it static
- return intrinsic_dynamic_;
+ return state_block_dynamic_.at(_key);
}
inline Eigen::VectorXs SensorBase::getNoiseStd() const
@@ -354,16 +339,6 @@ inline void SensorBase::updateCalibSize()
calib_size_ = computeCalibSize();
}
-inline void SensorBase::setExtrinsicDynamic(bool _extrinsic_dynamic)
-{
- extrinsic_dynamic_ = _extrinsic_dynamic;
-}
-
-inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic)
-{
- intrinsic_dynamic_ = _intrinsic_dynamic;
-}
-
} // namespace wolf
#endif
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index d30785c88b81f25900dd6bd4b42af8f490f8e678..a63308ba975bf6d5dbb91b9a86d70d4aa60d7747 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -24,41 +24,41 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr)
{
- if (_sensor_ptr->isExtrinsicDynamic())
+ if (_sensor_ptr->isStateBlockDynamic("P"))
{
assert(_p_ptr && "Pointer to dynamic position params is null!");
setStateBlock("P", _p_ptr);
appendToStructure("P");
+ }
+ else
+ assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static");
+
+ if (_sensor_ptr->isStateBlockDynamic("O"))
+ {
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
setStateBlock("O", _o_ptr);
appendToStructure("O");
}
- else if (_p_ptr || _o_ptr)
- {
- WOLF_ERROR("Provided dynamic sensor extrinsics but the sensor extrinsics are static");
- }
+ else
+ assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static");
- if (_sensor_ptr->isIntrinsicDynamic())
+ if (_sensor_ptr->isStateBlockDynamic("I"))
{
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
setStateBlock("I", _intr_ptr);
appendToStructure("I");
}
- else if (_intr_ptr)
- {
- WOLF_ERROR("Provided dynamic sensor intrinsics but the sensor intrinsics are static");
- }
+ else
+ assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static");
- getSensor()->setHasCapture();
}
else if (_p_ptr || _o_ptr || _intr_ptr)
{
WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
- updateCalibSize();
-// WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
+ updateCalibSize();
}
CaptureBase::~CaptureBase()
@@ -147,29 +147,10 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
{
if (getSensor())
{
- if (_key == "P") // extrinsics
- {
- if (getSensor()->extrinsicsInCaptures())
- return HasStateBlocks::getStateBlock(_key);
- else
- return getSensor()->getStateBlockStatic("P");
- }
-
- if (_key == "O") // extrinsics
- {
- if (getSensor()->extrinsicsInCaptures())
- return HasStateBlocks::getStateBlock(_key);
- else
- return getSensor()->getStateBlockStatic("O");
- }
-
- else // intrinsics
- {
- if (getSensor()->intrinsicsInCaptures())
- return HasStateBlocks::getStateBlock(_key);
- else
- return getSensor()->getStateBlockStatic("I");
- }
+ if (getSensor()->isStateBlockDynamic(_key))
+ return HasStateBlocks::getStateBlock(_key);
+ else
+ return getSensor()->getStateBlock(_key);
}
else // No sensor associated: assume sensor params are here
return HasStateBlocks::getStateBlock(_key);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 05fdb412870ae0048388eb38cd48beb628e1b93d..c3d90e385523955001f0e4d60bfb00c38a2911a0 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -868,45 +868,39 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
for (auto S : getHardware()->getSensorList())
{
cout << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
if (depth < 2)
cout << " -- " << S->getProcessorList().size() << "p";
cout << endl;
if (metric && state_blocks)
{
+ cout << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
- if (key == "P") cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- if (key == "I") cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlockStatic(key);
- if (sb)
- {
- cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
- }
- if (key == "O") cout << " ]" << endl;
+ auto sb = S->getStateBlock(key);
+ cout << key << "[" << (S->isStateBlockInCapture(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
- if (S->getStateBlockVec().size() > 2) cout << " ]" << endl;
+ cout << endl;
}
else if (metric)
{
- cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
- if (S->getP())
- cout << S->getP()->getState().transpose();
- if (S->getO())
- cout << " " << S->getO()->getState().transpose();
- cout << " )";
- if (S->getIntrinsic())
- cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
- cout << endl;
+ cout << " ( ";
+ for (auto& _key : S->getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ cout << sb->getState().transpose() << " ";
+ }
+ cout << ")" << endl;
}
else if (state_blocks)
{
- cout << " sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
- for (auto& sb : S->getStateBlockVec())
+ cout << " sb: ";
+ for (auto& _key : S->getStructure())
{
- if (sb != nullptr)
- cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ cout << key << "[" << (S->isStateBlockInCapture(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
cout << endl;
}
@@ -988,8 +982,6 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
if(C->getSensor() != nullptr)
{
cout << " -> S" << C->getSensor()->id();
- cout << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
- cout << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
}
else
cout << " -> S-";
@@ -1135,46 +1127,44 @@ std::string Problem::printToString(int depth, bool constr_by, bool metric, bool
for (auto S : getHardware()->getSensorList())
{
result << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (!metric && !state_blocks) result << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
if (depth < 2)
result << " -- " << S->getProcessorList().size() << "p";
result << endl;
if (metric && state_blocks)
{
- for (const auto& _key : S->getStructure())
+ result << " sb: ";
+ for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
- if (key == "P") cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- if (key == "I") cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlockStatic(key);
- if (sb)
- {
- cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
- }
- if (key == "O") cout << " ]" << endl;
+ auto sb = S->getStateBlock(key);
+ result << key << "[" << (S->isStateBlockInCapture(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
- if (S->getStateBlockVec().size() > 2) result << " ]" << endl;
+ result << endl;
}
else if (metric)
{
- result << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
- if (S->getP())
- result << S->getP()->getState().transpose();
- if (S->getO())
- result << " " << S->getO()->getState().transpose();
- result << " )";
- if (S->getIntrinsic())
- result << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
- result << endl;
+ result << " ( ";
+ for (auto& _key : S->getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ result << sb->getState().transpose() << " ";
+ }
+ result << ")" << endl;
}
else if (state_blocks)
{
- result << " sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
- for (auto sb : S->getStateBlockVec())
- if (sb != nullptr)
- result << " " << (sb->isFixed() ? "Fix" : "Est");
+ result << " sb: ";
+ for (auto& _key : S->getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = S->getStateBlock(key);
+ result << key << "[" << (S->isStateBlockInCapture(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
result << endl;
}
+
+
if (depth >= 2)
{
// Processors =======================================================================================
@@ -1252,8 +1242,6 @@ std::string Problem::printToString(int depth, bool constr_by, bool metric, bool
if(C->getSensor() != nullptr)
{
result << " -> S" << C->getSensor()->id();
- result << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
- result << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
}
else
result << " -> S-";
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index f73e644aed5aa7f294dcd5008ec6d3b781427359..bd43440e1e7ec5061c27bb98f303f2d97ba32c87 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -14,16 +14,14 @@ SensorBase::SensorBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const unsigned int _noise_size,
- const bool _extr_dyn,
+ const bool _p_dyn,
+ const bool _o_dyn,
const bool _intr_dyn) :
NodeBase("SENSOR", _type),
HasStateBlocks(""),
hardware_ptr_(),
calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
noise_std_(_noise_size),
noise_cov_(_noise_size, _noise_size)
{
@@ -35,16 +33,22 @@ SensorBase::SensorBase(const std::string& _type,
setStateBlock("P", _p_ptr);
appendToStructure("P");
}
+ setStateBlockDynamic("P", _p_dyn);
+
if (_o_ptr)
{
setStateBlock("O", _o_ptr);
appendToStructure("O");
}
+ setStateBlockDynamic("O", _o_dyn);
+
if (_intr_ptr)
{
setStateBlock("I", _intr_ptr);
appendToStructure("I");
}
+ setStateBlockDynamic("I", _intr_dyn);
+
updateCalibSize();
}
@@ -53,16 +57,14 @@ SensorBase::SensorBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn,
+ const bool _p_dyn,
+ const bool _o_dyn,
const bool _intr_dyn) :
NodeBase("SENSOR", _type),
HasStateBlocks(""),
hardware_ptr_(),
calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
noise_std_(_noise_std),
noise_cov_(_noise_std.size(), _noise_std.size())
{
@@ -73,16 +75,22 @@ SensorBase::SensorBase(const std::string& _type,
setStateBlock("P", _p_ptr);
appendToStructure("P");
}
+ setStateBlockDynamic("P", _p_dyn);
+
if (_o_ptr)
{
setStateBlock("O", _o_ptr);
appendToStructure("O");
}
+ setStateBlockDynamic("O", _o_dyn);
+
if (_intr_ptr)
{
setStateBlock("I", _intr_ptr);
appendToStructure("I");
}
+ setStateBlockDynamic("I", _intr_dyn);
+
updateCalibSize();
}
@@ -94,12 +102,13 @@ SensorBase::~SensorBase()
void SensorBase::removeStateBlocks()
{
- for (const auto& key : getStructure())
+ for (const auto& _key : getStructure())
{
- auto sbp = getStateBlockStatic(key);
+ auto key = std::string(1,_key);
+ auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
- if (getProblem() != nullptr && !extrinsic_dynamic_)
+ if (getProblem() != nullptr && !isStateBlockInCapture(key))
{
getProblem()->notifyStateBlock(sbp,REMOVE);
}
@@ -110,9 +119,9 @@ void SensorBase::removeStateBlocks()
void SensorBase::fixExtrinsics()
{
- for (const auto& key : "PO")
+ for (const auto& key : std::string("PO"))
{
- auto sbp = getStateBlockStatic(key);
+ auto sbp = getStateBlockAuto(key);
if (sbp != nullptr)
sbp->fix();
}
@@ -121,9 +130,9 @@ void SensorBase::fixExtrinsics()
void SensorBase::unfixExtrinsics()
{
- for (const auto& key : "PO")
+ for (const auto& key : std::string("PO"))
{
- auto sbp = getStateBlockStatic(key);
+ auto sbp = getStateBlockAuto(key);
if (sbp != nullptr)
sbp->unfix();
}
@@ -137,7 +146,7 @@ void SensorBase::fixIntrinsics()
auto key = std::string(1,_key);
if (key != "P" and key != "O") // exclude extrinsics
{
- auto sbp = getStateBlock(key);
+ auto sbp = getStateBlockAuto(key);
if (sbp != nullptr)
sbp->fix();
}
@@ -152,7 +161,7 @@ void SensorBase::unfixIntrinsics()
auto key = std::string(1,_key);
if (key != "P" and key != "O") // exclude extrinsics
{
- auto sbp = getStateBlock(key);
+ auto sbp = getStateBlockAuto(key);
if (sbp != nullptr)
sbp->unfix();
}
@@ -162,9 +171,9 @@ void SensorBase::unfixIntrinsics()
void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
- assert(!isStateBlockDynamic(_key) && "SensorBase::addPriorParameter only allowed for static parameters");
+ assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters");
- StateBlockPtr _sb = getStateBlockStatic(_key);
+ StateBlockPtr _sb = getStateBlock(_key);
bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
@@ -211,13 +220,9 @@ void SensorBase::registerNewStateBlocks()
{
auto key = pair_key_sbp.first;
auto sbp = pair_key_sbp.second;
- if (key == "P" or key == "O")
- {
- if(sbp and !isExtrinsicDynamic())
- getProblem()->notifyStateBlock(sbp, ADD);
- }
- else
- if (sbp and !isIntrinsicDynamic()) getProblem()->notifyStateBlock(sbp, ADD);
+
+ if (sbp && !isStateBlockInCapture(key))
+ getProblem()->notifyStateBlock(sbp, ADD);
}
}
}
@@ -232,22 +237,45 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
noise_cov_ = _noise_cov;
}
-CaptureBasePtr SensorBase::lastKeyCapture(void) const
+CaptureBasePtr SensorBase::lastCapture(void) const
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ if (getProblem())
{
- if ((*frame_rev_it)->isKey())
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+ FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+ while (frame_rev_it != frame_list.rend())
{
capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
break;
+ frame_rev_it++;
+ }
+ }
+ return capture;
+}
+
+CaptureBasePtr SensorBase::lastKeyCapture(void) const
+{
+ // we search for the most recent Capture of this sensor which belongs to a KeyFrame
+ CaptureBasePtr capture = nullptr;
+ if (getProblem())
+ {
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+ FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+ while (frame_rev_it != frame_list.rend())
+ {
+ if ((*frame_rev_it)->isKey())
+ {
+ capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ break;
+ }
+ frame_rev_it++;
}
- frame_rev_it++;
}
return capture;
}
@@ -256,52 +284,55 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
{
// we search for the most recent Capture of this sensor before _ts
CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-
- // We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ if (getProblem())
{
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+
+ // We iterate in reverse since we're likely to find it close to the rbegin() place.
+ FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+ while (frame_rev_it != frame_list.rend())
{
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
+ if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ {
+ CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ break;
+ }
+ frame_rev_it++;
}
- frame_rev_it++;
}
return capture;
}
StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
{
- return getStateBlock("P", _ts);
+ return getStateBlockAuto("P", _ts);
}
StateBlockPtr SensorBase::getO(const TimeStamp _ts) const
{
- return getStateBlock("O", _ts);
+ return getStateBlockAuto("O", _ts);
}
StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts) const
{
- return getStateBlock("I", _ts);
+ return getStateBlockAuto("I", _ts);
}
StateBlockPtr SensorBase::getP() const
{
- return getStateBlock("P");
+ return getStateBlockAuto("P");
}
StateBlockPtr SensorBase::getO() const
{
- return getStateBlock("O");
+ return getStateBlockAuto("O");
}
StateBlockPtr SensorBase::getIntrinsic() const
{
- return getStateBlock("I");
+ return getStateBlockAuto("I");
}
SizeEigen SensorBase::computeCalibSize() const
@@ -323,7 +354,7 @@ Eigen::VectorXs SensorBase::getCalibration() const
Eigen::VectorXs calib(sz);
for (const auto& key : getStructure())
{
- auto sb = getStateBlock(key);
+ auto sb = getStateBlockAuto(key);
if (sb && !sb->isFixed())
{
calib.segment(index, sb->getSize()) = sb->getState();
@@ -356,31 +387,32 @@ void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
processor_list_.remove(_proc_ptr);
}
-StateBlockPtr SensorBase::getStateBlock(const std::string& _key) const
+StateBlockPtr SensorBase::getStateBlockAuto(const std::string& _key) const
{
CaptureBasePtr cap;
- if (isStateBlockDynamic(_key, cap))
+ if (isStateBlockInCapture(_key, cap))
return cap->getStateBlock(_key);
- return getStateBlockStatic(_key);
+ return getStateBlock(_key);
}
-StateBlockPtr SensorBase::getStateBlock(const std::string& _key, const TimeStamp& _ts) const
+StateBlockPtr SensorBase::getStateBlockAuto(const std::string& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
- if (isStateBlockDynamic(_key, _ts, cap))
+ if (isStateBlockInCapture(_key, _ts, cap))
return cap->getStateBlock(_key);
- return getStateBlockStatic(_key);
+ return getStateBlock(_key);
}
-bool SensorBase::isStateBlockDynamic(const std::string& _key, CaptureBasePtr& cap) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& cap) const
{
- if (((_key == "P" or _key == "O") && this->extrinsicsInCaptures()) || ((_key != "P" and _key != "O") && intrinsicsInCaptures()))
+ if (isStateBlockDynamic(_key))
{
- cap = lastKeyCapture();
+ cap = lastCapture();
+ // cap = lastKeyCapture();
return cap != nullptr;
}
@@ -388,9 +420,9 @@ bool SensorBase::isStateBlockDynamic(const std::string& _key, CaptureBasePtr& ca
return false;
}
-bool SensorBase::isStateBlockDynamic(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& cap) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& cap) const
{
- if (((_key == "P" or _key == "O") && this->extrinsicsInCaptures()) || ((_key != "P" and _key != "O") && intrinsicsInCaptures()))
+ if (isStateBlockDynamic(_key))
{
cap = lastCapture(_ts);
@@ -400,18 +432,18 @@ bool SensorBase::isStateBlockDynamic(const std::string& _key, const TimeStamp& _
return false;
}
-bool SensorBase::isStateBlockDynamic(const std::string& _key) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key) const
{
CaptureBasePtr cap;
- return isStateBlockDynamic(_key,cap);
+ return isStateBlockInCapture(_key,cap);
}
-bool SensorBase::isStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const
+bool SensorBase::isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts) const
{
CaptureBasePtr cap;
- return isStateBlockDynamic(_key,_ts,cap);
+ return isStateBlockInCapture(_key,_ts,cap);
}
void SensorBase::setProblem(ProblemPtr _problem)
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 15be1ff455614ce95b116e865b1666a1a728d24b..7750ddc1c8f07af56a953b4e806bdff0f27294ee 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -59,7 +59,7 @@ TEST(CaptureBase, Dynamic_sensor_params)
StateBlockPtr p(std::make_shared<StateBlock>(2));
StateBlockPtr o(std::make_shared<StateAngle>() );
StateBlockPtr i(std::make_shared<StateBlock>(2));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2, true, true)); // last 2 'true' mark dynamic
+ SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2, true, true, true)); // last 3 'true' mark dynamic
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params