From c85bbde28a7fe886c5269c7066a119034fc1e848 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Tue, 13 Mar 2018 16:20:13 +0100
Subject: [PATCH] Fix lastCapture() and rename to lastKeyCapture()
---
src/sensor_base.cpp | 17 ++++++++++-------
src/sensor_base.h | 2 +-
2 files changed, 11 insertions(+), 8 deletions(-)
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 83578afef..942fff7ee 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -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
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 8c6b6c168..3a2de9957 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -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);
--
GitLab