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

Merge branch 'lastCapture_fix' into 'master'

Fix lastCapture() and rename to lastKeyCapture()

See merge request mobile_robotics/wolf!176
parents 0c8ad97c c85bbde2
No related branches found
No related tags found
1 merge request!176Fix lastCapture() and rename to lastKeyCapture()
...@@ -218,18 +218,21 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) { ...@@ -218,18 +218,21 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
noise_cov_ = _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; CaptureBasePtr capture = nullptr;
FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList(); FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
FrameBaseRevIter frame_rev_it = frame_list.rbegin(); FrameBaseRevIter frame_rev_it = frame_list.rbegin();
while (frame_rev_it != frame_list.rend()) while (frame_rev_it != frame_list.rend())
{ {
CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); if ((*frame_rev_it)->isKey())
if (capture) {
// found the most recent Capture made by this sensor ! capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
break; if (capture)
// found the most recent Capture made by this sensor !
break;
}
frame_rev_it++; frame_rev_it++;
} }
return capture; return capture;
...@@ -340,7 +343,7 @@ StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i) ...@@ -340,7 +343,7 @@ StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
{ {
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures())) if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{ {
CaptureBasePtr cap = lastCapture(); CaptureBasePtr cap = lastKeyCapture();
if (cap) if (cap)
return cap->getStateBlockPtr(_i); return cap->getStateBlockPtr(_i);
else else
......
...@@ -102,7 +102,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa ...@@ -102,7 +102,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr); ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
ProcessorBaseList& getProcessorList(); ProcessorBaseList& getProcessorList();
CaptureBasePtr lastCapture(void); CaptureBasePtr lastKeyCapture(void);
CaptureBasePtr lastCapture(const TimeStamp& _ts); CaptureBasePtr lastCapture(const TimeStamp& _ts);
bool process(const CaptureBasePtr capture_ptr); 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