diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 73d5145e89df89adf717dea14d8248263b340691..6d120c6298eb9c39b9234f2bc1ec2106480a042a 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -24,6 +24,7 @@ class FeatureIMU_test : public testing::Test
wolf::FrameBasePtr origin_frame;
Eigen::MatrixXd dD_db_jacobians;
wolf::ProcessorBasePtr processor_ptr_;
+ wolf::ProcessorMotionPtr processor_motion_ptr_;
//a new of this will be created for each new test
virtual void SetUp()
@@ -45,6 +46,7 @@ class FeatureIMU_test : public testing::Test
prc_imu_params->dist_traveled = 5;
prc_imu_params->angle_turned = 0.5;
processor_ptr_ = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
+ processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr_);
// Time and data variables
TimeStamp t;
@@ -80,16 +82,15 @@ class FeatureIMU_test : public testing::Test
sensor_ptr->process(imu_ptr);
//emplace Frame
- ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
- state_vec = problem->getProcessorMotion()->getCurrentState();
+ problem->getCurrentStateAndStamp(state_vec, ts);
last_frame = problem->emplaceFrame(KEY, state_vec, ts);
//emplace a feature
- delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
- delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
- VectorXd calib_preint = problem->getProcessorMotion()->getLast()->getCalibrationPreint();
- dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
- feat_imu = std::static_pointer_cast<FeatureIMU>(
+ delta_preint = processor_motion_ptr_->getMotion().delta_integr_;
+ delta_preint_cov = processor_motion_ptr_->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
+ VectorXd calib_preint = processor_motion_ptr_->getLast()->getCalibrationPreint();
+ dD_db_jacobians = processor_motion_ptr_->getMotion().jacobian_calib_;
+ feat_imu = std::static_pointer_cast<FeatureIMU>(
FeatureBase::emplace<FeatureIMU>(imu_ptr,
delta_preint,
delta_preint_cov,
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 08d8342b7f86399ec281d74272278a5206641dac..1f059b193f8e5fbf5f96f5024543223ffa0840c2 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -30,6 +30,7 @@ class ProcessorIMUt : public testing::Test
public: //These can be accessed in fixtures
wolf::ProblemPtr problem;
wolf::SensorBasePtr sensor_ptr;
+ wolf::ProcessorMotionPtr processor_motion;
wolf::TimeStamp t;
double mti_clock, tmp;
double dt;
@@ -55,6 +56,7 @@ class ProcessorIMUt : public testing::Test
Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished();
sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
data = Vector6d::Zero();
@@ -221,8 +223,8 @@ TEST_F(ProcessorIMUt, acc_x)
Vector6d b; b << 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
}
TEST_F(ProcessorIMUt, acc_y)
@@ -247,8 +249,8 @@ TEST_F(ProcessorIMUt, acc_y)
Vector6d b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 1000; //how many ms
@@ -261,8 +263,8 @@ TEST_F(ProcessorIMUt, acc_y)
// advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
x << 0,10,0, 0,0,0,1, 0,20,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
TEST_F(ProcessorIMUt, acc_z)
@@ -284,8 +286,8 @@ TEST_F(ProcessorIMUt, acc_z)
Vector6d b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 50; //how 0.1s
@@ -298,8 +300,8 @@ TEST_F(ProcessorIMUt, acc_z)
// advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
x << 0,0,25, 0,0,0,1, 0,0,10;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
TEST_F(ProcessorIMUt, check_Covariance)
@@ -315,8 +317,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
cap_imu_ptr->setTimeStamp(0.1);
sensor_ptr->process(cap_imu_ptr);
- VectorXd delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
-// Matrix<double,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov();
+ VectorXd delta_preint(processor_motion->getMotion().delta_integr_);
+// Matrix<double,9,9> delta_preint_cov = processor_motion->getCurrentDeltaPreintCov();
ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
@@ -382,8 +384,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things
problem->setPrior(x0, P0, t, 0.01);
- problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
- problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
+ processor_motion->getOrigin()->setCalibration(bias);
+ processor_motion->getLast()->setCalibrationPreint(bias);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// data
@@ -438,8 +440,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6d bias; bias << abx,aby,0, 0,0,0;
Vector3d acc_bias = bias.head(3);
- problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
- problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
+ processor_motion->getOrigin()->setCalibration(bias);
+ processor_motion->getLast()->setCalibrationPreint(bias);
double rate_of_turn = 5 * M_PI/180.0;
// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity