diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp index 3bc14edc32c1222dc3e4bb387cdd3364dd15c26e..1d763b2a5785ddaf5b8ed2bb3c5107f15154c126 100644 --- a/src/feature_imu.cpp +++ b/src/feature_imu.cpp @@ -4,8 +4,8 @@ namespace wolf { FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance) : FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance), - dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.segment<3>(7)), dq_preint_(_delta_preintegrated.segment<4>(3)), - acc_bias_preint_(_delta_preintegrated.segment<3>(10)), gyro_bias_preint_(_delta_preintegrated.tail<3>()) + dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.segment<3>(7)), dq_preint_(_delta_preintegrated.segment<4>(3)) + //acc_bias_preint_(_delta_preintegrated.segment<3>(10)), gyro_bias_preint_(_delta_preintegrated.tail<3>()) FIXME { //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl; } diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp index ea9df027bd052235959e15eda6e6464f510df450..824ff059098e3a321ac6f26abaed3b3e46079833 100644 --- a/src/test/gtest_feature_imu.cpp +++ b/src/test/gtest_feature_imu.cpp @@ -26,6 +26,7 @@ class FeatureIMU_test : public testing::Test std::shared_ptr<wolf::FeatureIMU> feat_imu; wolf::FrameIMUPtr last_frame; wolf::FrameBasePtr origin_frame; + Eigen::Matrix<wolf::Scalar,9,6> dD_db_jacobians; //a new of this will be created for each new test virtual void SetUp() @@ -79,11 +80,14 @@ class FeatureIMU_test : public testing::Test state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState(); last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame); + //create a feature delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov(); delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_; feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); + //wolf_problem_ptr_->getProcessorMotionPtr()->getJacobians(dD_db_jacobians); FIXME + //feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov, dD_db_jacobians); feat_imu->setCapturePtr(imu_ptr); //associate the feature to a capture }