diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index 327cfb213aef75f328f761b6d875500d23123751..785cccc5e14383d41774d6ea322b0dd131d681a6 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -74,14 +74,6 @@ void ProcessorBase::remove()
}
}
-KFPackPtr ProcessorBase::selectPack(const CaptureBasePtr & _cap)
-{
- if (_cap)
- return kf_pack_buffer_.selectPack(_cap->getTimeStamp(), time_tolerance_);
-
- return nullptr;
-}
-
/////////////////////////////////////////////////////////////////////////////////////////
@@ -136,6 +128,10 @@ KFPackPtr KFPackBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _
return nullptr;
}
+KFPackPtr KFPackBuffer::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+{
+ return selectPack(_capture->getTimeStamp(), _time_tolerance);
+}
KFPackPtr KFPackBuffer::selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
{
@@ -161,6 +157,10 @@ KFPackPtr KFPackBuffer::selectPackBefore(const TimeStamp& _time_stamp, const Sca
return nullptr;
}
+KFPackPtr KFPackBuffer::selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+{
+ return selectPackBefore(_capture->getTimeStamp(), _time_tolerance);
+}
void KFPackBuffer::print(void)
{
diff --git a/src/processor_base.h b/src/processor_base.h
index a063e66a4f116ff9b76d8c43a844951c8a107eb1..ce506925f2bb06118633912d70dfbfd44f5c9237 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -53,8 +53,10 @@ class KFPackBuffer
* respecting a defined time tolerances
*/
KFPackPtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+ KFPackPtr selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
KFPackPtr selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+ KFPackPtr selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
/**\brief Buffer size
*
@@ -176,10 +178,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setTimeTolerance(Scalar _time_tolerance);
- protected:
- KFPackPtr selectPack(const CaptureBasePtr & _cap);
-
-
};
}
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 5e526257413fca4756edfaabd558bb4228f8889e..731cb7f2f635dc405c0e6ff50cb1e06fc1f8d451 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -253,140 +253,9 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// 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::process2(CaptureBasePtr _incoming_ptr)
-{
- if (_incoming_ptr == nullptr)
- {
- WOLF_ERROR("Received capture is nullptr.");
- return;
- }
-
- if ( !kf_pack_buffer_.empty() )
- {
- KFPackPtr pack;
-
- // Select using last_ptr
- if (last_ptr_ != nullptr)
- {
- pack = kf_pack_buffer_.selectPack( last_ptr_->getTimeStamp(), time_tolerance_ );
- if (pack!=nullptr)
- {
- keyFrameCallback(pack->key_frame,pack->time_tolerance);
- kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
- }
- }
-
- // Select using incoming_ptr
- pack = kf_pack_buffer_.selectPack( _incoming_ptr->getTimeStamp(), time_tolerance_ );
- if (pack!=nullptr)
- {
- keyFrameCallback(pack->key_frame,pack->time_tolerance);
- kf_pack_buffer_.removeUpTo( _incoming_ptr->getTimeStamp() );
- }
- }
-
-
- if (status_ == IDLE)
- {
- TimeStamp t0 = _incoming_ptr->getTimeStamp();
-
- if (origin_ptr_ == nullptr)
- {
- auto frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t0);
- if (frm && fabs(frm->getTimeStamp() - t0) < time_tolerance_)
- {
- std::cout << "PM: join KF" << std::endl;
- // Join existing KF
- setOrigin(frm);
- }
- else
- {
- // Create new KF for origin
- std::cout << "PM: make KF" << std::endl;
- VectorXs x0 = getProblem()->zeroState();
- setOrigin(x0, t0);
- }
- }
- status_ = RUNNING;
- }
-
- incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
-
- /// @todo Anything else to do ?
- if (incoming_ptr_ == nullptr) return;
-
- preProcess();
-
- // integrate data
- integrateOneStep();
-
- // Update state and time stamps
- last_ptr_->setTimeStamp(incoming_ptr_->getTimeStamp());
- last_ptr_->getFramePtr()->setTimeStamp(last_ptr_->getTimeStamp());
- 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->setState(getCurrentState());
- key_frame_ptr->setTimeStamp(getCurrentTimeStamp());
- 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 processor origin to the new keyframe's capture
- origin_ptr_ = last_ptr_;
- last_ptr_ = new_capture_ptr;
-
- // reset derived things
- resetDerived();
-
- // callback to other processors
- getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
- }
-
-
- postProcess();
-
- // clear incoming just in case
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
-}
void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
{
@@ -499,107 +368,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
resetDerived();
}
-bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& _time_tol_other)
-{
-
- ProcessorBase::keyFrameCallback(_new_keyframe, _time_tol_other);
- return true;
-
- assert(_new_keyframe->getTrajectoryPtr() != nullptr
- && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
-
- // get keyframe's time stamp
- TimeStamp new_ts = _new_keyframe->getTimeStamp();
-
- // find the capture whose buffer is affected by the new keyframe
- auto existing_capture = findCaptureContainingTimeStamp(new_ts);
-
-
- if(existing_capture == nullptr) // Keyframe without Capture --> first time
- {
- CaptureMotionPtr new_capture = createCapture(new_ts,
- getSensorPtr(),
- Eigen::VectorXs::Zero(data_size_),
- getSensorPtr()->getNoiseCov(),
- _new_keyframe);
-
- new_capture->setCalibration(getSensorPtr()->getCalibration());
- new_capture->setCalibrationPreint(getSensorPtr()->getCalibration());
-
- emplaceFrame(NON_KEY_FRAME, new_capture);
- }
- 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_keyframe,
- 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());
-
- // interpolate individual delta
- if (!existing_capture->getBuffer().get().empty() && new_capture->getBuffer().get().back().ts_ != new_ts)
- {
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(new_capture->getBuffer().get().back(), // last Motion of old buffer
- existing_capture->getBuffer().get().front(), // first motion of new buffer
- new_ts);
- // add to old buffer
- new_capture->getBuffer().get().push_back(motion_interpolated);
- }
-
- // create motion feature and add it to the capture
- auto new_feature = emplaceFeature(new_capture);
- // reintegrateBuffer(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 existing capture
- if (existing_capture == last_ptr_)
- // reset processor origin
- origin_ptr_ = new_capture;
-
- existing_capture->setOriginFramePtr(_new_keyframe);
-
- // reintegrate 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.)
- }
-
- }
-
- return true;
-}
-
void ProcessorMotion::integrateOneStep()
{
// Set dt
@@ -794,14 +562,6 @@ FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion)
return feature;
}
-KFPackPtr ProcessorMotion::selectPackBefore(const CaptureBasePtr & _cap)
-{
- if (_cap)
- return kf_pack_buffer_.selectPackBefore(_cap->getTimeStamp(), time_tolerance_); // ignore time tolerance here
-
- return nullptr;
-}
-
KFPackPtr ProcessorMotion::computeProcessingStep()
{
if (!getProblem()->priorIsSet())
@@ -813,7 +573,7 @@ KFPackPtr ProcessorMotion::computeProcessingStep()
throw std::runtime_error("ProcessorMotion received data before being initialized.");
}
- KFPackPtr pack = selectPackBefore(last_ptr_);
+ KFPackPtr pack = kf_pack_buffer_.selectPackBefore(last_ptr_, time_tolerance_);
if (pack)
{
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 88ec1764040305aee3fc7e9db4e75c0a1788d600..93d1af04a43ee77e02661331d1beb8031e2ed49a 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -194,8 +194,6 @@ class ProcessorMotion : public ProcessorBase
*/
FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
- virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol);
-
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
@@ -233,8 +231,6 @@ class ProcessorMotion : public ProcessorBase
*/
virtual void postProcess() { };
- KFPackPtr selectPackBefore(const CaptureBasePtr & _cap);
-
KFPackPtr computeProcessingStep();
diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index 57b9d871938325f74efc0c787c264c06fcf75d20..1268c2debc5c2a7320e1479009657820f728ddcb 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -50,7 +50,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
case FIRST_TIME_WITH_PACK :
{
- KFPackPtr pack = selectPack( incoming_ptr_);
+ KFPackPtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, time_tolerance_);
kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
WOLF_DEBUG( "PT: KF" , pack->key_frame->id() , " callback received with ts= " , pack->key_frame->getTimeStamp().get() );
@@ -109,7 +109,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
case RUNNING_WITH_PACK :
{
- KFPackPtr pack = selectPack( last_ptr_ );
+ KFPackPtr pack = kf_pack_buffer_.selectPack( last_ptr_ , time_tolerance_);
kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
WOLF_DEBUG( "PT: KF" , pack->key_frame->id() , " callback received with ts= " , pack->key_frame->getTimeStamp().get() );
@@ -217,7 +217,7 @@ void ProcessorTracker::computeProcessingStep()
{
case FIRST_TIME :
- if (selectPack(incoming_ptr_))
+ if (kf_pack_buffer_.selectPack(incoming_ptr_, time_tolerance_))
processing_step_ = FIRST_TIME_WITH_PACK;
else // ! last && ! pack(incoming)
processing_step_ = FIRST_TIME_WITHOUT_PACK;
@@ -225,7 +225,7 @@ void ProcessorTracker::computeProcessingStep()
case SECOND_TIME :
- if (selectPack(last_ptr_))
+ if (kf_pack_buffer_.selectPack(last_ptr_, time_tolerance_))
processing_step_ = SECOND_TIME_WITH_PACK;
else
processing_step_ = SECOND_TIME_WITHOUT_PACK;
@@ -234,7 +234,7 @@ void ProcessorTracker::computeProcessingStep()
case RUNNING :
default :
- if (selectPack(last_ptr_))
+ if (kf_pack_buffer_.selectPack(last_ptr_, time_tolerance_))
{
if (last_ptr_->getFramePtr()->isKey())
{