Skip to content
Snippets Groups Projects
Commit c85bbde2 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix lastCapture() and rename to lastKeyCapture()

parent 0c8ad97c
No related branches found
No related tags found
1 merge request!176Fix lastCapture() and rename to lastKeyCapture()
Pipeline #
......@@ -218,18 +218,21 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
noise_cov_ = _noise_cov;
}
CaptureBasePtr SensorBase::lastCapture(void)
CaptureBasePtr SensorBase::lastKeyCapture(void)
{
// we search for the most recent Capture of this sensor
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
CaptureBasePtr capture = nullptr;
FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
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)->isKey())
{
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;
......@@ -340,7 +343,7 @@ StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
CaptureBasePtr cap = lastCapture();
CaptureBasePtr cap = lastKeyCapture();
if (cap)
return cap->getStateBlockPtr(_i);
else
......
......@@ -102,7 +102,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
ProcessorBaseList& getProcessorList();
CaptureBasePtr lastCapture(void);
CaptureBasePtr lastKeyCapture(void);
CaptureBasePtr lastCapture(const TimeStamp& _ts);
bool process(const CaptureBasePtr capture_ptr);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment