Skip to content
Snippets Groups Projects

Resolve "Add time_tolerance field in Frame and remove PackKeyFrame"

1 file
+ 10
13
Compare changes
  • Side-by-side
  • Inline
@@ -50,30 +50,27 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 1. get KF
FrameBasePtr kf(nullptr);
FrameBasePtr keyframe(nullptr);
if ( !buffer_frame_.empty() )
{
// KeyFrame Callback received
FrameBasePtr keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
if (keyframe!=nullptr)
kf = keyframe;
keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
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)
kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
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
CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
capture_rb->link(kf);
capture_rb->link(keyframe);
// 3. explore all observations in the capture
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
@@ -103,14 +100,14 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 5. create feature
Vector2d rb(range,bearing);
Vector2d measurement_rb(range,bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
rb,
measurement_rb,
getSensor()->getNoiseCov());
// 6. create factor
auto prc = shared_from_this();
auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr,
auto fac = FactorBase::emplace<FactorRangeBearing>(ftr,
capture_rb,
ftr,
lmk,
Loading