diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp index 01b3cbceb074632927a6b3f1ec4b5798ffd4d83e..1cb86165fdd7b19cdfdbf305cc3e2be9810087a0 100644 --- a/demos/hello_wolf/processor_range_bearing.cpp +++ b/demos/hello_wolf/processor_range_bearing.cpp @@ -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,