diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index b1f951be056199ae586d77627d4212a7b1ddeb63..63d55436c2c2f0ccd02f8a33cbbc378900718922 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -215,10 +215,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
+ // Helper functions:
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
- // Helper functions:
protected:
/** \brief process an incoming capture
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 847c01e36d9986827e8fd16a1a4ba8d32ebb9338..9e63f71a1b2fa1a257c209cee391ab84f352a264 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -401,7 +401,9 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
// an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors
std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered
- for (auto proc: processor_is_motion_list_){
+
+ for (auto proc: processor_is_motion_list_)
+ {
Eigen::VectorXd proc_state = proc->getState(_ts);
int idx = 0;
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 1e928a56b9b4b938319f4a5e4dac141e793918d5..6945b82ea7391816cd4cf924b42d682662263cf9 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -385,11 +385,15 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
{
CaptureMotionPtr capture_motion;
if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
+ {
// timestamp found in the current processor buffer
capture_motion = last_ptr_;
+ }
else
+ {
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
capture_motion = findCaptureContainingTimeStamp(_ts);
+ }
if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
{
@@ -437,11 +441,13 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
+ TimeStamp origin_ts = _origin_frame->getTimeStamp();
+
// -------- ORIGIN ---------
// emplace (empty) origin Capture
origin_ptr_ = emplaceCapture(_origin_frame,
getSensor(),
- _origin_frame->getTimeStamp(),
+ origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
@@ -450,7 +456,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- TimeStamp origin_ts = _origin_frame->getTimeStamp();
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
_origin_frame->getState(),
origin_ts);
@@ -458,7 +463,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
- _origin_frame->getTimeStamp(),
+ origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
@@ -619,15 +624,6 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
PackKeyFramePtr ProcessorMotion::computeProcessingStep()
{
-// if (!getProblem()->priorIsSet())
-// {
-// WOLF_WARN ("||*||");
-// WOLF_INFO (" ... It seems you missed something!");
-// WOLF_ERROR("ProcessorMotion received data before being initialized.");
-// WOLF_INFO ("Did you forget to issue a Problem::setPrior()?");
-// throw std::runtime_error("ProcessorMotion received data before being initialized.");
-// }
-
// Origin not set
if (!origin_ptr_)
{
@@ -636,21 +632,24 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
if (pack)
{
if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ {
+ WOLF_DEBUG("First time with a KF compatible.")
processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
+ }
else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
{
- WOLF_INFO("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
+ WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
}
else
{
- WOLF_INFO("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
}
}
else
{
- WOLF_INFO("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
processing_step_ = FIRST_TIME_WITHOUT_KF;
}
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index e3a1c2c8a284dac10c08b4d9ec0f941d7ee448ed..fd82dd83817fc2fe6991f0e8ba6bea09bf8c431e 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -441,7 +441,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
-
TimeStamp t(0.0);
double dt = 1.0;
Vector3d x0(0,0,0);
@@ -449,9 +448,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d x2(3.0, -3.0, 0.0);
Matrix3d P0; P0.setIdentity();
- auto F0 = problem->setPriorFactor(x0, P0, t, 0.1); // set prior at t=0
- // process a dummy capture to join F0 at t=t0
- std::make_shared<CaptureDiffDrive>(t, sensor, Vector3d::Zero(), Matrix2d::Identity(), nullptr)->process();
+ // set prior at t=0 and processor origin
+ auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+ processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -461,7 +460,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
-
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
@@ -582,9 +580,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Vector3d x2(3.0, -3.0, 0.0);
Matrix3d P0; P0.setIdentity();
+ // set prior at t=0 and processor origin
auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
- // process a dummy capture to join F0 at t=t0
- std::make_shared<CaptureDiffDrive>(t, sensor, Vector3d::Zero(), Matrix2d::Identity(), nullptr)->process();
+ processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 50fb5f4273aec851be8d6616ce6938c509eee07b..33026ee4b5f72b7ce06cdf79c3c765963e0cb64c 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -355,6 +355,7 @@ TEST(Odom2d, KF_callback)
// Origin Key Frame (in t1 to let processor motion to join the KF)
FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0+dt, dt/2);
+ processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
Eigen::Vector3d integrated_pose = x0;
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index a58457ce3762128e4ff02bfcec8536e8d7292580..7704ec8c4e720ae0060f22dfdccaed512383692e 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -312,6 +312,7 @@ TEST_F(ProcessorDiffDriveTest, process)
Matrix3d P; P.setIdentity();
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
int N = 9;
@@ -322,10 +323,10 @@ TEST_F(ProcessorDiffDriveTest, process)
for (int n = 1; n <= N; n++)
{
+ t += dt;
C->setTimeStamp(t);
C->process();
WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
- t += dt;
}
problem->print(4,1,1,1);
@@ -340,6 +341,7 @@ TEST_F(ProcessorDiffDriveTest, linear)
Matrix3d P; P.setIdentity();
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
data(0) = 100.0 ; // one turn of the wheels
@@ -365,6 +367,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
Matrix3d P; P.setIdentity();
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 9302b1c2ae8d78592de1806933aa9a0541b3a342..8f4738dae79311399a88bf15487c729741eba888 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -78,12 +78,11 @@ class ProcessorMotion_test : public testing::Test{
};
-TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
+TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
{
// Prior
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
- problem->setPriorOptions("factor", 0.01, x0, P0);
data << 1, 0; // advance straight
data_cov.setIdentity();
@@ -102,16 +101,43 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
// Prior
+ TimeStamp t(0.0);
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
- problem->setPriorOptions("fix", 0.01, x0);
+ auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ processor->setOrigin(KF_0);
data << 1, 0; // advance straight
data_cov.setIdentity();
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
+{
+ // Prior
TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
for (int i = 0; i<9; i++)
{
@@ -126,12 +152,11 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
+TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
{
// Prior
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
- problem->setPriorOptions("factor", 0.01, x0, P0);
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
@@ -150,16 +175,42 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
+TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
// Prior
+ TimeStamp t(0.0);
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
- problem->setPriorOptions("fix", 0.01, x0);
+ auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
+
+ 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());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
+{
+ // Prior
TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
for (int i = 0; i<10; i++) // one full turn exactly
{
@@ -174,12 +225,11 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
+TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
{
// Prior
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
- problem->setPriorOptions("factor", 0.01, x0, P0);
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
@@ -218,16 +268,62 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
C_source->getBuffer().print(1,1,1,0);
}
-TEST_F(ProcessorMotion_test, splitBufferFixPrior)
+TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
// Prior
+ TimeStamp t(0.0);
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
- problem->setPriorOptions("fix", 0.01, x0);
+ auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
+ processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
+
+ 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);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ 3,
+ 3,
+ nullptr);
+
+ 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, splitBufferFixPrior)
+{
+ // Prior
TimeStamp t(0.0);
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
for (int i = 0; i<10; i++) // one full turn exactly
{