diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index b1572a010a6594473b51aceb949f5b21c26e8eb6..bf9556bababfc52dbf40b6b9d5deec586f953e02 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -104,6 +104,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
Eigen::VectorXd noise_std_; // std of sensor noise
Eigen::MatrixXd noise_cov_; // cov matrix of noise
+ CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
+
void setProblem(ProblemPtr _problem) override final;
public:
@@ -166,9 +168,10 @@ 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;
+ CaptureBasePtr getLastCapture() const;
+ void setLastCapture(CaptureBasePtr);
+ void updateLastCapture();
+ CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts) const;
bool process(const CaptureBasePtr capture_ptr);
@@ -293,6 +296,11 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
return processor_list_;
}
+inline CaptureBasePtr SensorBase::getLastCapture() const
+{
+ return last_capture_;
+}
+
inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 47990c97d0190f0780a475780fc6c7294501a102..cc108e3a173542bcdcd3f2d106fb338a509d8423 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -70,6 +70,10 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
is_removing_ = true;
CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
+ // SensorBase::last_capture_
+ if (getSensor() and getSensor()->getLastCapture() == this_C)
+ getSensor()->updateLastCapture();
+
// remove downstream
while (!constrained_by_list_.empty())
{
@@ -228,6 +232,12 @@ void CaptureBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
registerNewStateBlocks(_problem);
+ // update SensorBase::last_capture_
+ if (getSensor() and
+ getSensor()->getLastCapture() and
+ getSensor()->getLastCapture()->getTimeStamp() < time_stamp_)
+ getSensor()->setLastCapture(shared_from_this());
+
for (auto ft : feature_list_)
ft->setProblem(_problem);
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 82fbe684558bd9791763b70c0bbedab68514b8f0..9e7adfd0b99009c41345826e6a3b8ad866e70957 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -23,7 +23,8 @@ SensorBase::SensorBase(const std::string& _type,
calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size)
+ noise_cov_(_noise_size, _noise_size),
+ last_capture_(nullptr)
{
assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
@@ -58,7 +59,8 @@ SensorBase::SensorBase(const std::string& _type,
calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size())
+ noise_cov_(_noise_std.size(), _noise_std.size()),
+ last_capture_(nullptr)
{
setNoiseStd(_noise_std);
@@ -214,74 +216,64 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
noise_cov_ = _noise_cov;
}
-CaptureBasePtr SensorBase::lastCapture(void) const
+void SensorBase::setLastCapture(CaptureBasePtr cap)
{
- // we search for the most recent Capture of this sensor which belongs to a KeyFrame
- CaptureBasePtr capture = nullptr;
- if (getProblem())
- {
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
- TrajectoryRevIter frame_rev_it = trajectory->rbegin();
- while (frame_rev_it != trajectory->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;
+ assert(cap);
+ assert(cap->getSensor() == shared_from_this());
+ assert(cap->getTimeStamp().ok());
+ assert(not last_capture_ or last_capture_->getTimeStamp() < cap->getTimeStamp());
+ last_capture_ = cap;
}
-CaptureBasePtr SensorBase::lastKeyCapture(void) const
+void SensorBase::updateLastCapture()
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
- CaptureBasePtr capture = nullptr;
if (getProblem())
{
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
auto trajectory = getProblem()->getTrajectory();
TrajectoryRevIter frame_rev_it = trajectory->rbegin();
while (frame_rev_it != trajectory->rend())
{
- if ((*frame_rev_it)->isKey())
+ auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture and not capture->isRemoving())
{
- capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
+ // found the most recent Capture made by this sensor !
+ last_capture_ = capture;
+ return;
}
frame_rev_it++;
}
}
- return capture;
+
+ // no captures found
+ last_capture_ = nullptr;
}
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
+CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) const
{
// we search for the most recent Capture of this sensor before _ts
- CaptureBasePtr capture = nullptr;
- if (getProblem())
- {
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
+ if (not getProblem())
+ return nullptr;
- // We iterate in reverse since we're likely to find it close to the rbegin() place.
- TrajectoryRevIter frame_rev_it = trajectory->rbegin();
- while (frame_rev_it != trajectory->rend())
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+ auto trajectory = getProblem()->getTrajectory();
+
+ // We iterate in reverse since we're likely to find it close to the rbegin() place.
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
+ {
+ if ((*frame_rev_it)->getTimeStamp() <= _ts)
{
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
- {
- capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
- }
- frame_rev_it++;
+ auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ return capture;
}
+ frame_rev_it++;
}
- return capture;
+
+ return nullptr;
}
StateBlockPtr SensorBase::getP(const TimeStamp _ts) const
@@ -405,8 +397,7 @@ bool SensorBase::isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap) c
{
if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
{
- _cap = lastCapture();
- // cap = lastKeyCapture();
+ _cap = last_capture_;
return _cap != nullptr;
}
@@ -418,7 +409,7 @@ bool SensorBase::isStateBlockInCapture(const char& _key, const TimeStamp& _ts, C
{
if (isStateBlockDynamic(_key))
{
- _cap = lastCapture(_ts);
+ _cap = findLastCaptureBefore(_ts);
return _cap != nullptr;
}
@@ -561,6 +552,12 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostr
<< " -X-> Sen" << id();
log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
}
+ // check last_capture_
+ if (getProblem()->getTimeStamp().ok())
+ {
+ inconsistency_explanation << "SensorBase::last_capture_ is not the actual last capture\n";
+ log.assertTrue((last_capture_ != findLastCaptureBefore(getProblem()->getTimeStamp())), inconsistency_explanation);
+ }
return log;
}