diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp index a6a7c78e03ca80d2d89482f43f109f776c0822af..6ab3a7231c47e422c486a07dda2756ae52f3efb5 100644 --- a/src/processor/processor_inertial_kinematics.cpp +++ b/src/processor/processor_inertial_kinematics.cpp @@ -53,11 +53,11 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture) } // nothing to do if any of the two buffer is empty - if(buffer_pack_kf_.empty()){ + if(buffer_frame_.empty()){ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp()); return; } - if(buffer_pack_kf_.empty()){ + if(buffer_frame_.empty()){ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp()); return; } @@ -69,21 +69,21 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture) // 1. get corresponding KF FrameBasePtr kf; - auto buffer_pack_kf_it = buffer_pack_kf_.getContainer().begin(); + auto buffer_frame_it = buffer_frame_.getContainer().begin(); auto buffer_capture_it = buffer_capture_.getContainer().begin(); auto sensor_angvel = getProblem()->getSensor(params_ikin_->sensor_angvel_name); - while ((buffer_pack_kf_it != buffer_pack_kf_.getContainer().end()) + while ((buffer_frame_it != buffer_frame_.getContainer().end()) && (buffer_capture_it != buffer_capture_.getContainer().end())) { - bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_pack_kf_it->first, buffer_capture_it->first, buffer_pack_kf_it->second->time_tolerance); + bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_frame_it->first, buffer_capture_it->first, buffer_frame_it->second->getTimeTolerance()); if (time_ok) { - CaptureBasePtr cap_angvel = buffer_pack_kf_it->second->key_frame->getCaptureOf(sensor_angvel); - auto min_ts = (buffer_pack_kf_it->first < buffer_capture_it->first) ? buffer_pack_kf_it->first : buffer_capture_it->first; + CaptureBasePtr cap_angvel = buffer_frame_it->second->getCaptureOf(sensor_angvel); + auto min_ts = (buffer_frame_it->first < buffer_capture_it->first) ? buffer_frame_it->first : buffer_capture_it->first; if (cap_angvel && cap_angvel->getStateBlock('I')){ // TODO: or only cap_angvel? // cast incoming capture to the InertialKinematics type, add it to the keyframe - auto kf = buffer_pack_kf_it->second->key_frame; + auto kf = buffer_frame_it->second; auto cap_ikin = std::static_pointer_cast<CaptureInertialKinematics>(buffer_capture_it->second); cap_ikin->link(kf); createInertialKinematicsFactor(cap_ikin, @@ -92,27 +92,27 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture) // update pointer to origin capture (the previous one attached to a KF) if we have created a new factor cap_origin_ptr_ = buffer_capture_it->second; buffer_capture_it++; - buffer_pack_kf_it++; + buffer_frame_it++; } else { // if time ok but no capture angvel yet, there is not gonna be any in the next KF of the buffer break; buffer_capture_it++; - buffer_pack_kf_it++; + buffer_frame_it++; } //////////////// // remove everything before (Inclusive if equal) this timestamp -> the cap_angvel is yet to come - buffer_pack_kf_.removeUpTo(min_ts); + buffer_frame_.removeUpTo(min_ts); buffer_capture_.removeUpTo(min_ts); } else { // if a time difference between captures and KF pack, we increment the oldest iterator - if (buffer_capture_it->first < buffer_pack_kf_it->first){ + if (buffer_capture_it->first < buffer_frame_it->first){ buffer_capture_it++; } else { - buffer_pack_kf_it++; + buffer_frame_it++; } } } diff --git a/src/processor/processor_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp index d11d87acb96ce1885dde22ecf45d04c11bee5985..2ba76fd1feb93ade82fd71aca17e842a722ce02e 100644 --- a/src/processor/processor_point_feet_nomove.cpp +++ b/src/processor/processor_point_feet_nomove.cpp @@ -46,14 +46,14 @@ void ProcessorPointFeetNomove::configure(SensorBasePtr _sensor) void ProcessorPointFeetNomove::createFactorIfNecessary(){ auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor()); - while (buffer_pack_kf_.size() >= 2) + while (buffer_frame_.size() >= 2) { - auto kf1_it = buffer_pack_kf_.getContainer().begin(); + auto kf1_it = buffer_frame_.getContainer().begin(); auto kf2_it = std::next(kf1_it); TimeStamp t1 = kf1_it->first; TimeStamp t2 = kf2_it->first; - auto cap1_it = buffer_capture_.selectIterator(t1, kf1_it->second->time_tolerance); - auto cap2_it = buffer_capture_.selectIterator(t2, kf1_it->second->time_tolerance); + auto cap1_it = buffer_capture_.selectIterator(t1, kf1_it->second->getTimeTolerance()); + auto cap2_it = buffer_capture_.selectIterator(t2, kf1_it->second->getTimeTolerance()); // check that the first 2 KF have corresponding captures in the capture buffer // just quit and assume that you will someday have matching captures @@ -101,7 +101,7 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){ auto cap2 = std::static_pointer_cast<CapturePointFeetNomove>(cap2_it->second); // link (kind of arbitrarily) factor from KF1 to KF2 with a feature and capture on KF2 to mimic capture motion // however, this capture solely does not contain enough information to recreate the factor - cap2->link(kf2_it->second->key_frame); + cap2->link(kf2_it->second); auto kin_incontact_t2 = cap2->kin_incontact_; for (auto kin_pair1: kin_incontact_from_t1){ @@ -109,13 +109,13 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){ Vector6d meas; meas << kin_pair1.second, kin_pair2_it->second; FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, sensor_pfnm->getCovFootNomove()); - FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second->key_frame, nullptr, true); + FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true); } } // Once the factors are created, remove kf1 and all the captures until ts 2 NOT INCLUDING the one at ts 2 since we need it for the next // Note: erase by range does not include the end range iterator - buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), kf2_it); // !! works only if getContainer returns a non const reference + buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), kf2_it); // !! works only if getContainer returns a non const reference buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), cap2_it); } } @@ -130,11 +130,11 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture) return; } // nothing to do if any of the two buffer is empty - if(buffer_pack_kf_.empty()){ + if(buffer_frame_.empty()){ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp()); return; } - if(buffer_pack_kf_.empty()){ + if(buffer_frame_.empty()){ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp()); return; } @@ -151,11 +151,11 @@ inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr return; } // nothing to do if any of the two buffer is empty - if(buffer_pack_kf_.empty()){ + if(buffer_frame_.empty()){ WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp()); return; } - if(buffer_pack_kf_.empty()){ + if(buffer_frame_.empty()){ WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp()); return; }