diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 44561aeb1e6a2b2d813f78ba0f34d41d20b8301f..5d2c9032e962c2268f0adae9cb314537477779de 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -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; diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index 4970ddc97e0e8f63079c9b55b82e2d75ec243c25..83e9ea62baf912954f94bb135408969398a37201 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -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(); } diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 7e27fb1758188d4b9038d94c733788f94b92c3a6..e92eaedbdba94dd659400ee27f566fce493767db 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -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