-
Joan Solà Ortega authoredJoan Solà Ortega authored
processor_motion.cpp 13.49 KiB
#include "processor_motion.h"
namespace wolf
{
ProcessorMotion::ProcessorMotion(ProcessorType _tp, const std::string& _type, Size _state_size, Size _delta_size,
Size _delta_cov_size, Size _data_size, const Scalar& _time_tolerance) :
ProcessorBase(_tp, _type, _time_tolerance), x_size_(_state_size), delta_size_(_delta_size), delta_cov_size_(
_delta_cov_size), data_size_(_data_size), origin_ptr_(), last_ptr_(), incoming_ptr_(), dt_(0.0), x_(
_state_size), delta_(_delta_size), delta_cov_(_delta_cov_size, _delta_cov_size), delta_integrated_(
_delta_size), delta_integrated_cov_(_delta_cov_size, _delta_cov_size), data_(_data_size), jacobian_delta_preint_(
delta_cov_size_, delta_cov_size_), jacobian_delta_(delta_cov_size_, delta_cov_size_)
{
//
}
ProcessorMotion::~ProcessorMotion()
{
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG_HERE
incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
preProcess();
integrate();
if (voteForKeyFrame() && permittedKeyFrame())
{
// key_capture
CaptureMotion::Ptr key_capture_ptr = last_ptr_;
FrameBasePtr key_frame_ptr = key_capture_ptr->getFramePtr();
if (key_frame_ptr == nullptr) {WOLF_DEBUG_HERE; std::runtime_error("bad pointer");}
// Set the frame as key
key_frame_ptr->setState(getCurrentState());
key_frame_ptr->setTimeStamp(getBuffer().get().back().ts_);
key_frame_ptr->setKey();
// create motion feature and add it to the capture
FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>(
FEATURE_MOTION,
"MOTION",
key_capture_ptr->getBuffer().get().back().delta_integr_,
key_capture_ptr->getBuffer().get().back().delta_integr_cov_.determinant() > 0 ?
key_capture_ptr->getBuffer().get().back().delta_integr_cov_ :
Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_) * 1e-8);
key_capture_ptr->addFeature(key_feature_ptr);
// create motion constraint and link it to parent feature and other frame (which is origin's frame)
auto ctr_ptr = createConstraint(key_feature_ptr, origin_ptr_->getFramePtr());
key_feature_ptr -> addConstraint(ctr_ptr);
origin_ptr_->getFramePtr() -> addConstrainedBy(ctr_ptr);
// new last capture
last_ptr_ = std::make_shared<CaptureMotion>(key_frame_ptr->getTimeStamp(), getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_), key_frame_ptr);
// create a new last frame
FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME, key_frame_ptr->getState(), last_ptr_->getTimeStamp());
new_frame_ptr->addCapture(last_ptr_); // Add Capture to the new Frame
// reset processor origin
origin_ptr_ = key_capture_ptr;
getBuffer().get().push_back(Motion( {key_frame_ptr->getTimeStamp(), deltaZero(), deltaZero(),
Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_),
Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_)}));
delta_integrated_ = deltaZero();
delta_integrated_cov_.setZero();
// reset derived things
resetDerived();
getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
//// debug cout
//Eigen::VectorXs interpolated_state(3);
//xPlusDelta(origin_ptr_->getFramePtr()->getState(), key_capture_ptr->getBufferPtr()->get().back().delta_integr_, interpolated_state);
//std::cout << "\tinterpolated state: " << interpolated_state.transpose() << std::endl;
}
postProcess();
}
CaptureMotion::Ptr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
//std::cout << "ProcessorMotion::findCaptureContainingTimeStamp: ts = " << _ts.getSeconds() << "." << _ts.getNanoSeconds() << std::endl;
auto capture_ptr = last_ptr_;
while (capture_ptr != nullptr)
{
// capture containing motion previous than the ts found:
if (capture_ptr->getBuffer().get().front().ts_ < _ts)
return capture_ptr;
else
{
// go to the previous motion capture
if (capture_ptr == last_ptr_)
capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
else if (capture_ptr->getOriginFramePtr() == nullptr)
return nullptr;
else
{
CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
if (capture_base_ptr == nullptr)
return nullptr;
else
capture_ptr = std::static_pointer_cast<CaptureMotion>(capture_base_ptr);
}
}
}
return capture_ptr;
}
void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
{
WOLF_DEBUG_HERE
assert(_origin_frame->getTrajectoryPtr() != nullptr
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
// make (empty) origin Capture
origin_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(), getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_), nullptr);
// Add origin capture to origin frame
_origin_frame->addCapture(origin_ptr_);
// make (emtpy) last Capture
last_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(),
getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
_origin_frame);
// Make frame at last Capture
// makeFrame(last_ptr_, _origin_frame->getState(), NON_KEY_FRAME);
FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME,
_origin_frame->getState(),
last_ptr_->getTimeStamp());
new_frame_ptr->addCapture(last_ptr_); // Add last Capture to the new Frame
// Reset deltas
delta_ = deltaZero();
delta_integrated_ = deltaZero();
delta_cov_.setZero();
delta_integrated_cov_.setZero();
// clear and reset buffer
getBuffer().get().clear();
getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
// Reset derived things
resetDerived();
}
bool ProcessorMotion::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
{
WOLF_DEBUG_HERE
Scalar time_tol = std::min(time_tolerance_, _time_tol_other);
std::cout << " Time tol this " << time_tolerance_ << std::endl;
std::cout << " Time tol other " << _time_tol_other << std::endl;
std::cout << " Time tol eff " << time_tol << std::endl;
std::cout << " Time stamp input F " << _keyframe_ptr->getTimeStamp().get() << std::endl;
std::cout << " Time stamp orig F " << getOriginPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
std::cout << " Time stamp orig C " << getOriginPtr()->getTimeStamp().get() << std::endl;
std::cout << " Time stamp last F " << getLastPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
std::cout << " Time stamp last C " << getLastPtr()->getTimeStamp().get() << std::endl;
assert(_keyframe_ptr->getTrajectoryPtr() != nullptr
&& "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
// get time stamp
TimeStamp ts = _keyframe_ptr->getTimeStamp();
// find capture in which the new keyframe is interpolated
CaptureMotion::Ptr capture_ptr = findCaptureContainingTimeStamp(ts);
assert(capture_ptr != nullptr
&& "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found");
FrameBasePtr key_frame_origin = capture_ptr->getOriginFramePtr();
// create motion capture
CaptureMotion::Ptr key_capture_ptr = std::make_shared<CaptureMotion>(ts, getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
key_frame_origin);
// add motion capture to keyframe
_keyframe_ptr->addCapture(key_capture_ptr);
// split the buffer
// and give old buffer to new key capture
capture_ptr->getBuffer().split(ts, key_capture_ptr->getBuffer());
// interpolate individual delta
Motion mot = interpolate(key_capture_ptr->getBuffer().get().back(), // last Motion of old buffer
capture_ptr->getBuffer().get().front(), // first motion of new buffer
ts);
// add to old buffer
key_capture_ptr->getBuffer().get().push_back(mot);
// create motion feature and add it to the capture
FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>(
FEATURE_MOTION,
"MOTION",
key_capture_ptr->getBuffer().get().back().delta_integr_,
key_capture_ptr->getBuffer().get().back().delta_integr_cov_.determinant() > 0 ?
key_capture_ptr->getBuffer().get().back().delta_integr_cov_ :
Eigen::MatrixXs::Identity(delta_size_, delta_size_) * 1e-8);
key_capture_ptr->addFeature(key_feature_ptr);
// create motion constraint and add it to the feature, and link it to the other frame (origin)
auto key_ctr_ptr = createConstraint(key_feature_ptr, key_frame_origin);
key_feature_ptr->addConstraint(key_ctr_ptr);
key_frame_origin->addConstrainedBy(key_ctr_ptr);
// Fix the remaining capture
if (capture_ptr == last_ptr_)
// reset processor origin
origin_ptr_ = key_capture_ptr;
capture_ptr->setOriginFramePtr(_keyframe_ptr);
// reintegrate own buffer // XXX: where is the result of re-integration stored?
reintegrate(capture_ptr);
// modify feature and constraint (if they exist)
if (!capture_ptr->getFeatureList().empty())
{
FeatureBasePtr feature_ptr = capture_ptr->getFeatureList().front();
// modify feature
feature_ptr->setMeasurement(capture_ptr->getBuffer().get().back().delta_integr_);
feature_ptr->setMeasurementCovariance(
capture_ptr->getBuffer().get().back().delta_integr_cov_.determinant() > 0 ?
capture_ptr->getBuffer().get().back().delta_integr_cov_ :
Eigen::MatrixXs::Identity(delta_size_, delta_size_) * 1e-8);
// modify constraint
if (!feature_ptr->getConstraintList().empty())
{
auto ctr = feature_ptr->getConstraintList().front();
feature_ptr->getConstraintList().remove(ctr);
// feature_ptr->getConstraintList().front()->remove(); // XXX What happens here?
feature_ptr->addConstraint(createConstraint(feature_ptr, _keyframe_ptr));
}
}
return true;
}
void ProcessorMotion::integrate()
{
WOLF_DEBUG_HERE
// Set dt
updateDt();
// get data and convert it to delta, and obtain also the delta covariance
data2delta(incoming_ptr_->getData(), incoming_ptr_->getDataCovariance(), dt_);
// then integrate the current delta to pre-integrated measurements, and get Jacobians
deltaPlusDelta(delta_integrated_, delta_, dt_, delta_integrated_, jacobian_delta_preint_, jacobian_delta_);
// and covariance
delta_integrated_cov_ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_
* jacobian_delta_preint_.transpose() + jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
// then push it into buffer
getBuffer().get().push_back(Motion( {incoming_ptr_->getTimeStamp(), delta_, delta_integrated_, delta_cov_,
delta_integrated_cov_}));
}
void ProcessorMotion::reintegrate(CaptureMotion::Ptr _capture_ptr)
{
WOLF_DEBUG_HERE
// start with empty motion
_capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFramePtr()->getTimeStamp()));
auto motion_it = _capture_ptr->getBuffer().get().begin();
auto prev_motion_it = motion_it;
motion_it++;
while (motion_it != _capture_ptr->getBuffer().get().end())
{
// get dt
const Scalar dt = motion_it->ts_ - prev_motion_it->ts_;
// integrate delta into delta_integr
deltaPlusDelta(prev_motion_it->delta_integr_, motion_it->delta_, dt, motion_it->delta_integr_,
jacobian_delta_preint_, jacobian_delta_);
// integrate covariances
delta_integrated_cov_ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_
* jacobian_delta_preint_.transpose() + jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
// XXX: Are we not pushing into buffer?
// advance in buffer
motion_it++;
prev_motion_it++;
}
}
}