diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp index fbaf83cafcb3efc3edc37c5b5199bb00a5f8f1bc..bb438060795ddfb88581b1216ca1ca0064f9e93f 100644 --- a/src/processor_motion.cpp +++ b/src/processor_motion.cpp @@ -10,7 +10,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, Scalar _time_tolerance, Size _calib_size) : ProcessorBase(_type, _time_tolerance), - processing_step_(FIRST_TIME_WITHOUT_PACK), + processing_step_(RUNNING_WITHOUT_PACK), x_size_(_state_size), data_size_(_data_size), delta_size_(_delta_size), @@ -39,7 +39,7 @@ ProcessorMotion::~ProcessorMotion() // std::cout << "destructed -p-Mot" << id() << std::endl; } -void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr) +void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) { if (_incoming_ptr == nullptr) { @@ -51,180 +51,216 @@ void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr) preProcess(); // Derived class operations - computeProcessingStep(); + KFPackPtr pack = computeProcessingStep(); + if (pack) + kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() ); switch(processing_step_) { - case FIRST_TIME_WITH_PACK : - case FIRST_TIME_WITHOUT_PACK : - case SECOND_TIME_WITH_PACK : - case SECOND_TIME_WITHOUT_PACK : - { - WOLF_WARN("ProcessorMotion received data before being initialized."); - WOLF_INFO("Did you forget to issue a Problem::setPrior()?"); - throw std::runtime_error("ProcessorMotion received data before being initialized."); - } - case RUNNING_WITH_PACK : - { - //////////////////////////////////////////////////// - // FIRST open pack and join KF - KFPackPtr pack = selectPack( last_ptr_); - kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() ); + case RUNNING_WITHOUT_PACK : + case RUNNING_WITH_PACK_ON_ORIGIN : + break; - // extract pack elements TODO rename _new_keyframe - FrameBasePtr new_kf = pack->key_frame; - TimeStamp new_ts = new_kf->getTimeStamp(); + case RUNNING_WITH_PACK_BEFORE_ORIGIN : + { + // extract pack elements + FrameBasePtr keyframe_from_callback = pack->key_frame; + TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp(); // find the capture whose buffer is affected by the new keyframe - auto existing_capture = findCaptureContainingTimeStamp(new_ts); + auto existing_capture = findCaptureContainingTimeStamp(ts_from_callback); + + // Find the frame acting as the capture's origin + auto keyframe_origin = existing_capture->getOriginFramePtr(); + + // emplace a new motion capture to the new keyframe + auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, + getSensorPtr(), + ts_from_callback, + Eigen::VectorXs::Zero(data_size_), + existing_capture->getDataCovariance(), + existing_capture->getCalibration(), + existing_capture->getCalibration(), + keyframe_origin); + + // split the buffer + // and give the part of the buffer before the new keyframe to the capture for the KF callback + existing_capture->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer()); - if(existing_capture == nullptr) // Keyframe without Capture --> first time + + // interpolate individual delta + if (!existing_capture->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback) { - // TODO should be an error + // interpolate Motion at the new time stamp + Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer + existing_capture->getBuffer().get().front(), // first motion of new buffer + ts_from_callback); + // add to old buffer + capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated); } - else // Normal operation - { - // Find the frame acting as the capture's origin - auto keyframe_origin = existing_capture->getOriginFramePtr(); - - // emplace a new motion capture to the new keyframe - auto new_capture = emplaceCapture(new_kf, - getSensorPtr(), - new_ts, - Eigen::VectorXs::Zero(data_size_), - existing_capture->getDataCovariance(), - existing_capture->getCalibration(), - existing_capture->getCalibration(), - keyframe_origin); - - // split the buffer - // and give the part of the buffer before the new keyframe to the key_capture - existing_capture->getBuffer().split(new_ts, new_capture->getBuffer()); - - // TODO add interpolation code ... - - // create motion feature and add it to the capture - auto new_feature = emplaceFeature(new_capture); - - // create motion constraint and add it to the feature, and constrain to the other capture (origin) - emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) ); - - // Update the processor pointers - if (existing_capture == last_ptr_) - // reset processor origin - origin_ptr_ = new_capture; - - // Update the existing capture - existing_capture->setOriginFramePtr(new_kf); - - // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer! - reintegrateBuffer(existing_capture); - - // modify existing feature and constraint (if they exist in the existing capture) - if (!existing_capture->getFeatureList().empty()) - { - auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature! - - // Modify existing feature -------- - existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_); - existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_); - - // Modify existing constraint -------- - // Instead of modifying, we remove one ctr, and create a new one. - auto ctr_to_remove = existing_feature->getConstraintList().back(); // there is only one constraint! - auto new_ctr = emplaceConstraint(existing_feature, new_capture); - ctr_to_remove ->remove(); // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.) - } - } - //////////////////////////////////////////////////// - // NOW on with the received data + // create motion feature and add it to the capture + auto new_feature = emplaceFeature(capture_for_keyframe_callback); - // integrate data - integrateOneStep(); + // create motion constraint and add it to the feature, and constrain to the other capture (origin) + emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) ); - // Update state and time stamps - last_ptr_->setTimeStamp(getCurrentTimeStamp()); - last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp()); - last_ptr_->getFramePtr()->setState(getCurrentState()); + // Update the existing capture + existing_capture->setOriginFramePtr(keyframe_from_callback); - resetDerived(); // TODO see where to put this + // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer! + reintegrateBuffer(existing_capture); + // modify existing feature and constraint (if they exist in the existing capture) + if (!existing_capture->getFeatureList().empty()) + { + auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature! + + // Modify existing feature -------- + existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_); + existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_); + + // Modify existing constraint -------- + // Instead of modifying, we remove one ctr, and create a new one. + auto ctr_to_remove = existing_feature->getConstraintList().back(); // there is only one constraint! + auto new_ctr = emplaceConstraint(existing_feature, capture_for_keyframe_callback); + ctr_to_remove ->remove(); // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.) + } break; } - case RUNNING_WITHOUT_PACK : - { - // integrate data - integrateOneStep(); - // Update state and time stamps - last_ptr_->setTimeStamp(getCurrentTimeStamp()); - last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp()); - last_ptr_->getFramePtr()->setState(getCurrentState()); - if (voteForKeyFrame() && permittedKeyFrame()) + case RUNNING_WITH_PACK_AFTER_ORIGIN : + { + // extract pack elements + FrameBasePtr keyframe_from_callback = pack->key_frame; + TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp(); + + // Find the frame acting as the capture's origin + auto keyframe_origin = last_ptr_->getOriginFramePtr(); + + // emplace a new motion capture to the new keyframe + auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, + getSensorPtr(), + ts_from_callback, + Eigen::VectorXs::Zero(data_size_), + last_ptr_->getDataCovariance(), + last_ptr_->getCalibration(), + last_ptr_->getCalibration(), + keyframe_origin); + + // split the buffer + // and give the part of the buffer before the new keyframe to the capture for the KF callback + last_ptr_->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer()); + + // interpolate individual delta + if (!last_ptr_->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback) { - // Set the frame of last_ptr as key - auto key_frame_ptr = last_ptr_->getFramePtr(); - key_frame_ptr->setKey(); - - // create motion feature and add it to the key_capture - auto key_feature_ptr = emplaceFeature(last_ptr_); - - // create motion constraint and link it to parent feature and other frame (which is origin's frame) - auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_); - - // create a new frame - auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, - getCurrentState(), - getCurrentTimeStamp()); - // create a new capture - auto new_capture_ptr = emplaceCapture(new_frame_ptr, - getSensorPtr(), - key_frame_ptr->getTimeStamp(), - Eigen::VectorXs::Zero(data_size_), - Eigen::MatrixXs::Zero(data_size_, data_size_), - last_ptr_->getCalibration(), - last_ptr_->getCalibration(), - key_frame_ptr); - // reset the new buffer - new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ; - - // reset integrals - delta_ = deltaZero(); - delta_cov_ . setZero(); - delta_integrated_ = deltaZero(); - delta_integrated_cov_ . setZero(); - jacobian_calib_ . setZero(); - - // reset derived things - resetDerived(); - - // Update pointers - origin_ptr_ = last_ptr_; - last_ptr_ = new_capture_ptr; - - // callback to other processors - getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_); + // interpolate Motion at the new time stamp + Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer + last_ptr_->getBuffer().get().front(), // first motion of new buffer + ts_from_callback); + // add to old buffer + capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated); } + // create motion feature and add it to the capture + auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback); - // clear incoming just in case - incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer. + // create motion constraint and add it to the feature, and constrain to the other capture (origin) + emplaceConstraint(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensorPtr()) ); + + // reset processor origin + origin_ptr_ = capture_for_keyframe_callback; + + // Update the existing capture + last_ptr_->setOriginFramePtr(keyframe_from_callback); + + // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer! + reintegrateBuffer(last_ptr_); break; } + + + default : break; } + //////////////////////////////////////////////////// + // NOW on with the received data + + // integrate data + integrateOneStep(); + + // Update state and time stamps + last_ptr_->setTimeStamp(getCurrentTimeStamp()); + last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp()); + last_ptr_->getFramePtr()->setState(getCurrentState()); + + if (voteForKeyFrame() && permittedKeyFrame()) + { + // Set the frame of last_ptr as key + auto key_frame_ptr = last_ptr_->getFramePtr(); + key_frame_ptr->setKey(); + + // create motion feature and add it to the key_capture + auto key_feature_ptr = emplaceFeature(last_ptr_); + + // create motion constraint and link it to parent feature and other frame (which is origin's frame) + auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_); + + // create a new frame + auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, + getCurrentState(), + getCurrentTimeStamp()); + // create a new capture + auto new_capture_ptr = emplaceCapture(new_frame_ptr, + getSensorPtr(), + key_frame_ptr->getTimeStamp(), + Eigen::VectorXs::Zero(data_size_), + Eigen::MatrixXs::Zero(data_size_, data_size_), + last_ptr_->getCalibration(), + last_ptr_->getCalibration(), + key_frame_ptr); + // reset the new buffer + new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ; + + // reset integrals + delta_ = deltaZero(); + delta_cov_ . setZero(); + delta_integrated_ = deltaZero(); + delta_integrated_cov_ . setZero(); + jacobian_calib_ . setZero(); + + // reset derived things + resetDerived(); + + // Update pointers + origin_ptr_ = last_ptr_; + last_ptr_ = new_capture_ptr; + + // callback to other processors + getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_); + } + + resetDerived(); // TODO see where to put this + + // clear incoming just in case + incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer. + +// WOLF_DEBUG("Buffer length: ", getBuffer().get().size(), +// " from ts=", getBuffer().get().front().ts_, +// " to ts=", getBuffer().get().back().ts_); + postProcess(); } -void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) +void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr) { if (_incoming_ptr == nullptr) { @@ -465,8 +501,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& _time_tol_other) { -// ProcessorBase::keyFrameCallback(_new_keyframe, _time_tol_other); -// return true; + ProcessorBase::keyFrameCallback(_new_keyframe, _time_tol_other); + return true; assert(_new_keyframe->getTrajectoryPtr() != nullptr && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory."); @@ -757,61 +793,48 @@ FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion) return feature; } -KFPackPtr ProcessorMotion::selectPack(const CaptureBasePtr & _cap) +KFPackPtr ProcessorMotion::selectPackBefore(const CaptureBasePtr & _cap) { if (_cap) - return kf_pack_buffer_.selectPack(_cap->getTimeStamp(), time_tolerance_); + return kf_pack_buffer_.selectPackBefore(_cap->getTimeStamp(), time_tolerance_); // ignore time tolerance here return nullptr; } -void ProcessorMotion::computeProcessingStep() +KFPackPtr ProcessorMotion::computeProcessingStep() { - // First determine the processing phase by checking the status of the tracker pointers - enum {FIRST_TIME, SECOND_TIME, RUNNING} step; - if (origin_ptr_ == nullptr && last_ptr_ == nullptr) - step = FIRST_TIME; - else if (origin_ptr_ == last_ptr_) - step = SECOND_TIME; - else - step = RUNNING; + if (!getProblem()->priorIsSet()) + { + WOLF_WARN ("||*||"); + WOLF_INFO (" ... It seems you missed something!"); + WOLF_ERROR("ProcessorMotion received data before being initialized."); + WOLF_INFO ("Did you forget to issue a Problem::setPrior()?"); + throw std::runtime_error("ProcessorMotion received data before being initialized."); + } + KFPackPtr pack = selectPackBefore(last_ptr_); - // Then combine with the existence (or not) of a keyframe callback pack - switch (step) + if (pack) { - case FIRST_TIME : - case SECOND_TIME : - WOLF_WARN ("||*||"); - WOLF_INFO (" ... It seems you missed something!"); - WOLF_INFO ("ProcessorMotion received data before being initialized."); - WOLF_INFO ("Did you forget to issue a Problem::setPrior()?"); - WOLF_ERROR("ProcessorMotion received data before being initialized."); - throw std::runtime_error("ProcessorMotion received data before being initialized."); - - case RUNNING : - default : + if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), time_tolerance_)) + { + WOLF_WARN("||*||"); + WOLF_INFO(" ... It seems you missed something!"); + WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)"); + // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)"); + processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN; + } + else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp() - time_tolerance_) + processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN; + + else + processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN; - if (selectPack(last_ptr_)) - { - if (last_ptr_->getFramePtr()->isKey()) - { - WOLF_WARN("||*||"); - WOLF_INFO(" ... It seems you missed something!"); - WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)"); - WOLF_INFO("Check the following for correctness:"); - WOLF_INFO(" - You have all processors installed before starting receiving any data"); - WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed ---> ", (getProblem()->priorIsSet() ? "OK" : "NOK")); - WOLF_INFO(" - You have configured all your processors with compatible time tolerances"); - WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)."); - throw std::runtime_error("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)."); - } - processing_step_ = RUNNING_WITH_PACK; - } - else - processing_step_ = RUNNING_WITHOUT_PACK; - break; } + else + processing_step_ = RUNNING_WITHOUT_PACK; + + return pack; } } diff --git a/src/processor_motion.h b/src/processor_motion.h index d2c211d2f273202f3dd3896da32372a127a3edf4..88ec1764040305aee3fc7e9db4e75c0a1788d600 100644 --- a/src/processor_motion.h +++ b/src/processor_motion.h @@ -101,12 +101,10 @@ class ProcessorMotion : public ProcessorBase { public: typedef enum { - FIRST_TIME_WITH_PACK, - FIRST_TIME_WITHOUT_PACK, - SECOND_TIME_WITH_PACK, - SECOND_TIME_WITHOUT_PACK, - RUNNING_WITH_PACK, - RUNNING_WITHOUT_PACK + RUNNING_WITHOUT_PACK, + RUNNING_WITH_PACK_BEFORE_ORIGIN, + RUNNING_WITH_PACK_ON_ORIGIN, + RUNNING_WITH_PACK_AFTER_ORIGIN } ProcessingStep ; protected: @@ -235,9 +233,9 @@ class ProcessorMotion : public ProcessorBase */ virtual void postProcess() { }; - KFPackPtr selectPack(const CaptureBasePtr & _cap); + KFPackPtr selectPackBefore(const CaptureBasePtr & _cap); - void computeProcessingStep(); + KFPackPtr computeProcessingStep(); // These are the pure virtual functions doing the mathematics