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

Minor cleanup

parent 0ad9ff88
No related branches found
No related tags found
3 merge requests!436Release to start wolf public,!433After 2nd RA-L submission,!431Resolve "Add time_tolerance field in Frame and remove PackKeyFrame"
Pipeline #7848 failed
This commit is part of merge request !431. Comments created here will be created in the context of that merge request.
...@@ -50,30 +50,27 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) ...@@ -50,30 +50,27 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
} }
// 1. get KF // 1. get KF
FrameBasePtr kf(nullptr); FrameBasePtr keyframe(nullptr);
if ( !buffer_frame_.empty() ) if ( !buffer_frame_.empty() )
{ {
// KeyFrame Callback received // KeyFrame Callback received
FrameBasePtr keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance ); keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
if (keyframe!=nullptr)
kf = keyframe;
buffer_frame_.removeUpTo( _capture->getTimeStamp() ); buffer_frame_.removeUpTo( _capture->getTimeStamp() );
assert( kf && "Callback KF is not close enough to _capture!"); assert( keyframe && "Callback KF is not close enough to _capture!");
} }
if (!kf) if (!keyframe)
{ {
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below) // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp()); keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!"); assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
} }
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture); CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
capture_rb->link(kf); capture_rb->link(keyframe);
// 3. explore all observations in the capture // 3. explore all observations in the capture
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++) for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
...@@ -103,14 +100,14 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) ...@@ -103,14 +100,14 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
} }
// 5. create feature // 5. create feature
Vector2d rb(range,bearing); Vector2d measurement_rb(range,bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb, auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
rb, measurement_rb,
getSensor()->getNoiseCov()); getSensor()->getNoiseCov());
// 6. create factor // 6. create factor
auto prc = shared_from_this(); auto prc = shared_from_this();
auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, auto fac = FactorBase::emplace<FactorRangeBearing>(ftr,
capture_rb, capture_rb,
ftr, ftr,
lmk, lmk,
......
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