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

Upgrade to new IsMotion API

parent 2ecb4add
No related branches found
No related tags found
2 merge requests!39release after RAL,!38After 2nd RAL submission
...@@ -24,6 +24,7 @@ class FeatureIMU_test : public testing::Test ...@@ -24,6 +24,7 @@ class FeatureIMU_test : public testing::Test
wolf::FrameBasePtr origin_frame; wolf::FrameBasePtr origin_frame;
Eigen::MatrixXd dD_db_jacobians; Eigen::MatrixXd dD_db_jacobians;
wolf::ProcessorBasePtr processor_ptr_; wolf::ProcessorBasePtr processor_ptr_;
wolf::ProcessorMotionPtr processor_motion_ptr_;
//a new of this will be created for each new test //a new of this will be created for each new test
virtual void SetUp() virtual void SetUp()
...@@ -45,6 +46,7 @@ class FeatureIMU_test : public testing::Test ...@@ -45,6 +46,7 @@ class FeatureIMU_test : public testing::Test
prc_imu_params->dist_traveled = 5; prc_imu_params->dist_traveled = 5;
prc_imu_params->angle_turned = 0.5; prc_imu_params->angle_turned = 0.5;
processor_ptr_ = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params); 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 // Time and data variables
TimeStamp t; TimeStamp t;
...@@ -80,16 +82,15 @@ class FeatureIMU_test : public testing::Test ...@@ -80,16 +82,15 @@ class FeatureIMU_test : public testing::Test
sensor_ptr->process(imu_ptr); sensor_ptr->process(imu_ptr);
//emplace Frame //emplace Frame
ts = problem->getProcessorMotion()->getBuffer().get().back().ts_; problem->getCurrentStateAndStamp(state_vec, ts);
state_vec = problem->getProcessorMotion()->getCurrentState();
last_frame = problem->emplaceFrame(KEY, state_vec, ts); last_frame = problem->emplaceFrame(KEY, state_vec, ts);
//emplace a feature //emplace a feature
delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_; delta_preint = processor_motion_ptr_->getMotion().delta_integr_;
delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08; delta_preint_cov = processor_motion_ptr_->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
VectorXd calib_preint = problem->getProcessorMotion()->getLast()->getCalibrationPreint(); VectorXd calib_preint = processor_motion_ptr_->getLast()->getCalibrationPreint();
dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_; dD_db_jacobians = processor_motion_ptr_->getMotion().jacobian_calib_;
feat_imu = std::static_pointer_cast<FeatureIMU>( feat_imu = std::static_pointer_cast<FeatureIMU>(
FeatureBase::emplace<FeatureIMU>(imu_ptr, FeatureBase::emplace<FeatureIMU>(imu_ptr,
delta_preint, delta_preint,
delta_preint_cov, delta_preint_cov,
......
...@@ -30,6 +30,7 @@ class ProcessorIMUt : public testing::Test ...@@ -30,6 +30,7 @@ class ProcessorIMUt : public testing::Test
public: //These can be accessed in fixtures public: //These can be accessed in fixtures
wolf::ProblemPtr problem; wolf::ProblemPtr problem;
wolf::SensorBasePtr sensor_ptr; wolf::SensorBasePtr sensor_ptr;
wolf::ProcessorMotionPtr processor_motion;
wolf::TimeStamp t; wolf::TimeStamp t;
double mti_clock, tmp; double mti_clock, tmp;
double dt; double dt;
...@@ -55,6 +56,7 @@ class ProcessorIMUt : public testing::Test ...@@ -55,6 +56,7 @@ class ProcessorIMUt : public testing::Test
Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished(); 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"); 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"); 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 // Time and data variables
data = Vector6d::Zero(); data = Vector6d::Zero();
...@@ -221,8 +223,8 @@ TEST_F(ProcessorIMUt, acc_x) ...@@ -221,8 +223,8 @@ TEST_F(ProcessorIMUt, acc_x)
Vector6d b; b << 0,0,0, 0,0,0; 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->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->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()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
} }
TEST_F(ProcessorIMUt, acc_y) TEST_F(ProcessorIMUt, acc_y)
...@@ -247,8 +249,8 @@ TEST_F(ProcessorIMUt, acc_y) ...@@ -247,8 +249,8 @@ TEST_F(ProcessorIMUt, acc_y)
Vector6d b; b<< 0,0,0, 0,0,0; 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->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->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()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s //do so for 5s
const unsigned int iter = 1000; //how many ms const unsigned int iter = 1000; //how many ms
...@@ -261,8 +263,8 @@ TEST_F(ProcessorIMUt, acc_y) ...@@ -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 // 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; 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->getCurrentState().head(10) , x, wolf::Constants::EPS);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
} }
TEST_F(ProcessorIMUt, acc_z) TEST_F(ProcessorIMUt, acc_z)
...@@ -284,8 +286,8 @@ TEST_F(ProcessorIMUt, acc_z) ...@@ -284,8 +286,8 @@ TEST_F(ProcessorIMUt, acc_z)
Vector6d b; b<< 0,0,0, 0,0,0; 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->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->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()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s //do so for 5s
const unsigned int iter = 50; //how 0.1s const unsigned int iter = 50; //how 0.1s
...@@ -298,8 +300,8 @@ TEST_F(ProcessorIMUt, acc_z) ...@@ -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 // 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; 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->getCurrentState().head(10) , x, wolf::Constants::EPS);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
} }
TEST_F(ProcessorIMUt, check_Covariance) TEST_F(ProcessorIMUt, check_Covariance)
...@@ -315,8 +317,8 @@ TEST_F(ProcessorIMUt, check_Covariance) ...@@ -315,8 +317,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
cap_imu_ptr->setTimeStamp(0.1); cap_imu_ptr->setTimeStamp(0.1);
sensor_ptr->process(cap_imu_ptr); sensor_ptr->process(cap_imu_ptr);
VectorXd delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_); VectorXd delta_preint(processor_motion->getMotion().delta_integr_);
// Matrix<double,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov(); // Matrix<double,9,9> delta_preint_cov = processor_motion->getCurrentDeltaPreintCov();
ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); 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 // 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) ...@@ -382,8 +384,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things // init things
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t, 0.01);
problem->getProcessorMotion()->getOrigin()->setCalibration(bias); processor_motion->getOrigin()->setCalibration(bias);
problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias); processor_motion->getLast()->setCalibrationPreint(bias);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose()); // WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// data // data
...@@ -438,8 +440,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy) ...@@ -438,8 +440,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6d bias; bias << abx,aby,0, 0,0,0; Vector6d bias; bias << abx,aby,0, 0,0,0;
Vector3d acc_bias = bias.head(3); Vector3d acc_bias = bias.head(3);
problem->getProcessorMotion()->getOrigin()->setCalibration(bias); processor_motion->getOrigin()->setCalibration(bias);
problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias); processor_motion->getLast()->setCalibrationPreint(bias);
double rate_of_turn = 5 * M_PI/180.0; 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 // data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment