diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 69e346ac755a4db3086b878fcc0ee61cf8979811..b1f951be056199ae586d77627d4212a7b1ddeb63 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -204,11 +204,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
- MotionBuffer& getBuffer();
- const MotionBuffer& getBuffer() const;
-
- // Helper functions:
- protected:
/** Set the origin of all motion for this processor
* \param _origin_frame the keyframe to be the origin
*/
@@ -220,6 +215,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
+ MotionBuffer& getBuffer();
+ const MotionBuffer& getBuffer() const;
+
+ // Helper functions:
+ protected:
+
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 6dfb753acd6d4e2693a6c99be5e14be8239bdd06..1e928a56b9b4b938319f4a5e4dac141e793918d5 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -102,14 +102,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
{
case FIRST_TIME_WITHOUT_KF :
{
- // There is no KF: create own origin (before this capture: 2 x time_tolerance before)
- setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
+ // There is no KF: create own origin
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
{
- // cannot joint to the KF: create own origin (before this capture: 2 x time_tolerance before)
- setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_ON_INCOMING :
@@ -120,8 +120,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
}
case FIRST_TIME_WITH_KF_AFTER_INCOMING :
{
- // cannot joint to the KF: create own origin (before this capture: 2 x time_tolerance before)
- setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
break;
}
case RUNNING_WITHOUT_KF :
@@ -296,9 +296,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// NOW on with the received data
// integrate data
- WOLF_INFO("before integrating, getCurrentState = ", getCurrentState().transpose());
integrateOneStep();
- WOLF_INFO("after integrating, getCurrentState = ", getCurrentState().transpose());
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 72f8d2529751a2ae634d93203120d5a14e766490..e3a1c2c8a284dac10c08b4d9ec0f941d7ee448ed 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -449,8 +449,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+dt, 0.1);
- problem->setPriorOptions("factor", 0.1, x0, P0);
+ 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();
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -460,8 +461,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;
- // initial dummy capture process for seting prior at t=0
- std::make_shared<CaptureDiffDrive>(t, sensor, Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()*0.01, nullptr)->process();
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
@@ -484,7 +483,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
}
auto F2 = problem->getLastKeyFrame();
- auto F0 = problem->getTrajectory()->getFrameList().front();
// Fix boundaries
F0->fix();
@@ -584,7 +582,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Vector3d x2(3.0, -3.0, 0.0);
Matrix3d P0; P0.setIdentity();
- problem->setPriorOptions("factor", 0.1, x0, P0);
+ 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();
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -594,9 +594,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
- // initial dummy capture process for seting prior at t=0
- std::make_shared<CaptureDiffDrive>(t, sensor, Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()*0.01, nullptr)->process();
-
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
@@ -619,7 +616,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
C->process();
}
- auto F0 = problem->getTrajectory()->getFrameList().front();
auto F2 = problem->getLastKeyFrame();
F2->setState(x2); // Impose known final state regardless of integrated value.
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 93b1ed685f976e09dd467305e7d2c2472fc37617..50fb5f4273aec851be8d6616ce6938c509eee07b 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -201,7 +201,7 @@ TEST(Odom2d, VoteForKfAndSolve)
params->voting_active = true;
params->dist_traveled = 100;
params->angle_turned = data(1)*2.5; // force KF at every third process()
- params->max_time_span = 1e4; // since prior is automatically set at first capture, first KF is not in t=0
+ params->max_time_span = 100;
params->cov_det = 100;
params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.00;
@@ -217,8 +217,8 @@ TEST(Odom2d, VoteForKfAndSolve)
// Ceres wrapper
CeresManager ceres_manager(problem);
- // Origin Key Frame
- problem->setPriorOptions("factor", dt/2, x0, P0);
+ // Origin Key Frame (in t1 to let processor motion to join the KF)
+ problem->setPriorFactor(x0, P0, t+dt, dt/2);
ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
@@ -340,9 +340,10 @@ TEST(Odom2d, KF_callback)
SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->dist_traveled = 100;
- params->angle_turned = 6.28;
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
params->max_time_span = 100;
params->cov_det = 100;
+ params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.000001;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
@@ -352,10 +353,8 @@ TEST(Odom2d, KF_callback)
// Ceres wrapper
CeresManager ceres_manager(problem);
- // Origin Key Frame
- //FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
- problem->setPriorOptions("factor", dt/2, x0, x0_cov);
-
+ // 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);
// Check covariance values
Eigen::Vector3d integrated_pose = x0;
@@ -468,7 +467,6 @@ TEST(Odom2d, KF_callback)
// solve
// cout << "===== full ====" << endl;
- FrameBasePtr keyframe_0 = problem->getTrajectory()->getFrameList().front();
keyframe_0->setState(Vector3d(1,2,3));
keyframe_1->setState(Vector3d(2,3,1));
keyframe_2->setState(Vector3d(3,1,2));
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 92d985bfdd05576be50f862de33f13df5056eb23..a58457ce3762128e4ff02bfcec8536e8d7292580 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -325,7 +325,6 @@ TEST_F(ProcessorDiffDriveTest, process)
C->setTimeStamp(t);
C->process();
WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
-
t += dt;
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 8b80be06846217ed28016b1c827b31aa5c81dfb7..9302b1c2ae8d78592de1806933aa9a0541b3a342 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -52,7 +52,7 @@ class ProcessorMotion_test : public testing::Test{
Matrix2d data_cov;
// ProcessorMotion_test() :
-// ProcessorOdom2d(std::make_shared<ParamsProcessorOdom2d>()),
+// ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
// dt(0)
// { }