diff --git a/src/processor_imu.h b/src/processor_imu.h index dd958230807a03f1b48eb0755502609885dfd6b2..98619262cb12e661da65fb5faca22721d17cfe93 100644 --- a/src/processor_imu.h +++ b/src/processor_imu.h @@ -79,10 +79,10 @@ class ProcessorIMU : public ProcessorMotion{ virtual void remapPQV(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, Eigen::VectorXs& _delta_out); virtual void remapDelta(Eigen::VectorXs& _delta_out); virtual void remapData(const Eigen::VectorXs& _data); - void getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb); - void getJacobians(Eigen::Matrix<wolf::Scalar,9,6>& _dD_db); public: + void getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb); + void getJacobians(Eigen::Matrix<wolf::Scalar,9,6>& _dD_db); static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); }; diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp index 824ff059098e3a321ac6f26abaed3b3e46079833..93974c8dfdc87c7feb38de864fde89ccbac75f41 100644 --- a/src/test/gtest_feature_imu.cpp +++ b/src/test/gtest_feature_imu.cpp @@ -86,7 +86,7 @@ class FeatureIMU_test : public testing::Test 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 + std::static_pointer_cast<wolf::ProcessorIMU>(wolf_problem_ptr_->getProcessorMotionPtr())->getJacobians(dD_db_jacobians); //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