Skip to content
Snippets Groups Projects
Commit 8b8b4549 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Cleanup and organization

parent 89862d1e
No related branches found
No related tags found
1 merge request!317Motion cleanup
Pipeline #4253 passed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    src/processor/processor_motion.cpp
    + 43
    − 37
    View file @ 8b8b4549
    ......@@ -118,28 +118,28 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
    // 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(ts_from_callback); // k
    auto existing_capture = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k
    // Find the frame acting as the capture's origin
    auto keyframe_origin = existing_capture->getOriginFrame(); // i
    auto keyframe_origin = existing_capture->getOriginFrame(); // i
    auto capture_origin = std::static_pointer_cast<CaptureMotion>(keyframe_origin->getCaptureOf(getSensor()));
    auto capture_origin = std::static_pointer_cast<CaptureMotion>(keyframe_origin->getCaptureOf(getSensor()));
    // Get calibration params for preintegration from origin, since it has chances to have optimized values
    VectorXs calib_origin = capture_origin->getCalibration();
    VectorXs calib_origin = capture_origin->getCalibration();
    // emplace a new motion capture to the new keyframe
    auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
    getSensor(),
    ts_from_callback,
    Eigen::VectorXs::Zero(data_size_),
    getSensor()->getNoiseCov(),
    calib_origin,
    calib_origin,
    keyframe_origin);
    TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
    auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
    getSensor(),
    ts_from_callback,
    Eigen::VectorXs::Zero(data_size_),
    getSensor()->getNoiseCov(),
    calib_origin,
    calib_origin,
    keyframe_origin);
    // split the buffer
    // and give the part of the buffer before the new keyframe to the capture for the KF callback
    ......@@ -202,15 +202,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
    // extract pack elements
    FrameBasePtr keyframe_from_callback = pack->key_frame;
    TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
    auto & existing_capture = last_ptr_;
    // Find the frame acting as the capture's origin
    auto keyframe_origin = last_ptr_->getOriginFrame();
    auto keyframe_origin = existing_capture->getOriginFrame();
    auto & capture_origin = origin_ptr_;
    // Get calibration params for preintegration from origin, since it has chances to have optimized values
    VectorXs calib_origin = origin_ptr_->getCalibration();
    VectorXs calib_origin = capture_origin->getCalibration();
    // emplace a new motion capture to the new keyframe
    TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
    auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
    getSensor(),
    ts_from_callback,
    ......@@ -222,7 +226,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
    // split the buffer
    // and give the part of the buffer before the new keyframe to the capture for the KF callback
    splitBuffer(last_ptr_, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
    splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
    // create motion feature and add it to the capture
    auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
    ......@@ -279,40 +283,40 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
    // Set the frame of last_ptr as key
    auto key_frame_ptr = last_ptr_->getFrame();
    key_frame_ptr->setKey();
    auto key_frame = last_ptr_->getFrame();
    key_frame ->setKey();
    // create motion feature and add it to the key_capture
    auto key_feature_ptr = emplaceFeature(last_ptr_);
    auto key_feature = emplaceFeature(last_ptr_);
    // create motion factor and link it to parent feature and other frame (which is origin's frame)
    auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
    auto factor = emplaceFactor(key_feature, origin_ptr_);
    // create a new frame
    auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
    auto new_frame = getProblem()->emplaceFrame(NON_ESTIMATED,
    getCurrentState(),
    getCurrentTimeStamp());
    // create a new capture
    auto new_capture_ptr = emplaceCapture(new_frame_ptr,
    getSensor(),
    key_frame_ptr->getTimeStamp(),
    Eigen::VectorXs::Zero(data_size_),
    getSensor()->getNoiseCov(),
    last_ptr_->getCalibration(),
    last_ptr_->getCalibration(),
    key_frame_ptr);
    auto new_capture = emplaceCapture(new_frame,
    getSensor(),
    key_frame->getTimeStamp(),
    Eigen::VectorXs::Zero(data_size_),
    getSensor()->getNoiseCov(),
    last_ptr_->getCalibration(),
    last_ptr_->getCalibration(),
    key_frame);
    // reset the new buffer
    new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
    new_capture->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ;
    // reset derived things
    resetDerived();
    // Update pointers
    origin_ptr_ = last_ptr_;
    last_ptr_ = new_capture_ptr;
    last_ptr_ = new_capture;
    // callback to other processors
    getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), params_motion_->time_tolerance);
    getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
    }
    resetDerived();
    ......@@ -389,9 +393,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
    // ---------- LAST ----------
    // Make non-key-frame for last Capture
    auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
    _origin_frame->getState(),
    _origin_frame->getTimeStamp());
    auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
    _origin_frame->getState(),
    _origin_frame->getTimeStamp());
    // emplace (emtpy) last Capture
    last_ptr_ = emplaceCapture(new_frame_ptr,
    getSensor(),
    ......@@ -437,9 +441,11 @@ void ProcessorMotion::integrateOneStep()
    // integrate Jacobian wrt calib
    if ( hasCalibration() )
    {
    jacobian_calib_.noalias()
    = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
    + jacobian_delta_ * jacobian_delta_calib_;
    }
    // Integrate covariance
    delta_integrated_cov_.noalias()
    ......@@ -502,7 +508,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
    // integrate Jacobian wrt calib
    if ( hasCalibration() )
    motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * jacobian_calib_
    + motion_it->jacobian_delta_ * jacobian_delta_calib_;
    + motion_it->jacobian_delta_ * jacobian_delta_calib_;
    // Integrate covariance
    motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose()
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment