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

Fix motion buffer first motion zero

parent d8e8d5d1
No related branches found
No related tags found
1 merge request!315Resolve "Remove all methods `ProcessorMotion::interpolate(...)`"
Pipeline #4244 passed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    src/processor/processor_motion.cpp
    + 26
    − 25
    View file @ f6c1b75e
    ......@@ -29,7 +29,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
    calib_preint_(_calib_size),
    jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
    jacobian_delta_(delta_cov_size_, delta_cov_size_),
    jacobian_calib_(delta_size_, calib_size_)
    jacobian_calib_(delta_cov_size_, calib_size_)
    {
    //
    }
    ......@@ -48,21 +48,9 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
    // and give the part of the buffer before the new keyframe to the capture for the KF callback
    _capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
    // // interpolate individual delta which has been cut by the split timestamp
    // if (!_capture_source->getBuffer().get().empty()
    // && _capture_target->getBuffer().get().back().ts_ != _ts_split)
    // {
    // // interpolate Motion at the new time stamp
    // Motion motion_interpolated = interpolate(_capture_target->getBuffer().get().back(), // last Motion of old buffer
    // _capture_source->getBuffer().get().front(), // first motion of new buffer
    // _ts_split,
    // _capture_source->getBuffer().get().front());
    //
    // // add to old buffer
    // _capture_target->getBuffer().get().push_back(motion_interpolated);
    // }
    // start with empty motion
    _capture_source->getBuffer().get().push_front(motionZero(_keyframe_target->getTimeStamp()));
    TimeStamp t_zero = _capture_target->getBuffer().get().back().ts_; // last tick of previous buffer is zero tick of current buffer
    _capture_source->getBuffer().get().push_front(motionZero(t_zero));
    // Update the existing capture
    _capture_source->setOriginFrame(_keyframe_target);
    ......@@ -409,13 +397,20 @@ void ProcessorMotion::integrateOneStep()
    void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
    {
    // // start with empty motion
    // _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFrame()->getTimeStamp()));
    //
    VectorXs calib = _capture_ptr->getCalibrationPreint();
    VectorXs calib = _capture_ptr->getCalibrationPreint();
    // some temporaries for integration
    delta_integrated_ =deltaZero();
    delta_integrated_cov_.setZero();
    jacobian_calib_ .setZero();
    // check that first motion in buffer is zero!
    assert(_capture_ptr->getBuffer().get().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
    assert(_capture_ptr->getBuffer().get().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
    assert(_capture_ptr->getBuffer().get().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
    // Iterate all the buffer
    auto motion_it = _capture_ptr->getBuffer().get().begin();
    auto motion_it = _capture_ptr->getBuffer().get().begin();
    auto prev_motion_it = motion_it;
    motion_it++;
    while (motion_it != _capture_ptr->getBuffer().get().end())
    ......@@ -433,7 +428,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
    jacobian_delta_calib_);
    // integrate delta into delta_integr, and rewrite the buffer
    deltaPlusDelta(prev_motion_it->delta_integr_,
    deltaPlusDelta(delta_integrated_,
    motion_it->delta_,
    dt,
    motion_it->delta_integr_,
    ......@@ -442,11 +437,17 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
    // integrate Jacobian wrt calib
    if ( hasCalibration() )
    motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * prev_motion_it->jacobian_calib_ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
    motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * jacobian_calib_
    + motion_it->jacobian_delta_ * jacobian_delta_calib_;
    // Integrate covariance
    motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * prev_motion_it->delta_integr_cov_ * motion_it->jacobian_delta_integr_.transpose()
    + motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
    motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose()
    + motion_it->jacobian_delta_ * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
    // update temporaries
    delta_integrated_ = motion_it->delta_integr_;
    delta_integrated_cov_ = motion_it->delta_integr_cov_;
    jacobian_calib_ = motion_it->jacobian_calib_;
    // advance in buffer
    motion_it++;
    ......@@ -593,7 +594,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
    // Note: since the buffer goes from a KF in the past until the next KF, we need to:
    // 1. See that the KF contains a CaptureMotion
    // 2. See that the TS is smaller than the KF's TS
    // 3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer
    // 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer
    FrameBasePtr frame = nullptr;
    CaptureBasePtr capture = nullptr;
    CaptureMotionPtr capture_motion = nullptr;
    ......
    test/gtest_odom_2D.cpp
    + 4
    − 3
    View file @ f6c1b75e
    ......@@ -476,15 +476,16 @@ TEST(Odom2D, KF_callback)
    for (int n=1; n<=N; n++)
    {
    t += dt;
    // WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
    // WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
    EXPECT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
    WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
    WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
    ASSERT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
    }
    }
    int main(int argc, char **argv)
    {
    testing::InitGoogleTest(&argc, argv);
    testing::GTEST_FLAG(filter) = "Odom2D.KF_callback";
    return RUN_ALL_TESTS();
    }
    test/gtest_processor_motion.cpp
    + 80
    − 15
    View file @ f6c1b75e
    ......@@ -18,20 +18,43 @@ using namespace Eigen;
    using namespace wolf;
    using std::static_pointer_cast;
    class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
    class ProcessorOdom2DPublic : public ProcessorOdom2D
    {
    public:
    Scalar dt;
    ProblemPtr problem;
    SensorOdom2DPtr sensor;
    ProcessorOdom2DPtr processor;
    CaptureMotionPtr capture;
    Vector2s data;
    Matrix2s data_cov;
    ProcessorMotion_test() :
    ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
    dt(0)
    { }
    ProcessorOdom2DPublic(ProcessorParamsOdom2DPtr params) : ProcessorOdom2D(params)
    {
    //
    }
    virtual ~ProcessorOdom2DPublic(){}
    void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
    TimeStamp ts_split,
    const FrameBasePtr& keyframe_target,
    const wolf::CaptureMotionPtr& capture_target)
    {
    ProcessorOdom2D::splitBuffer(capture_source,
    ts_split,
    keyframe_target,
    capture_target);
    }
    };
    WOLF_PTR_TYPEDEFS(ProcessorOdom2DPublic);
    class ProcessorMotion_test : public testing::Test{
    public:
    Scalar dt;
    ProblemPtr problem;
    SensorOdom2DPtr sensor;
    ProcessorOdom2DPublicPtr processor;
    CaptureMotionPtr capture;
    Vector2s data;
    Matrix2s data_cov;
    // ProcessorMotion_test() :
    // ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
    // dt(0)
    // { }
    virtual void SetUp()
    {
    ......@@ -39,6 +62,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
    dt = 1.0;
    problem = Problem::create("PO", 2);
    sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
    ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
    params->time_tolerance = 0.25;
    params->dist_traveled = 100;
    ......@@ -46,8 +70,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
    params->max_time_span = 2.5*dt; // force KF at every third process()
    params->cov_det = 100;
    params->unmeasured_perturbation_std = 0.001;
    sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
    processor = static_pointer_cast<ProcessorOdom2D>(problem->installProcessor("ODOM 2D", "odom", sensor, params));
    processor = ProcessorBase::emplace<ProcessorOdom2DPublic>(sensor, params);
    capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
    Vector3s x0; x0 << 0, 0, 0;
    ......@@ -55,6 +78,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
    problem->setPrior(x0, P0, 0.0, 0.01);
    }
    // Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
    // {
    // return ProcessorMotion::interpolate(_ref, _second, _ts);
    ......@@ -108,6 +132,47 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
    ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8);
    }
    TEST_F(ProcessorMotion_test, splitBuffer)
    {
    data << 1, 2*M_PI/10; // advance in circle
    data_cov.setIdentity();
    TimeStamp t(0.0);
    for (int i = 0; i<10; i++) // one full turn exactly
    {
    t += dt;
    capture->setTimeStamp(t);
    capture->setData(data);
    capture->setDataCovariance(data_cov);
    processor->captureCallback(capture);
    WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
    }
    SensorBasePtr S = processor->getSensor();
    TimeStamp t_target = 8.5;
    FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
    CaptureBasePtr C_last = processor->getLast();
    CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(C_last);
    FrameBasePtr F_origin = C_source->getOriginFrame();
    CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
    "ODOM 2D",
    t_target,
    sensor,
    data,
    3,
    3,
    F_origin);
    processor->splitBuffer(C_source,
    t_target,
    F_target,
    C_target);
    C_target->getBuffer().print(1,1,1,0);
    C_source->getBuffer().print(1,1,1,0);
    }
    //TEST_F(ProcessorMotion_test, Interpolate)
    //{
    // data << 1, 2*M_PI/10; // advance in turn
    ......
    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