FIXME getJacobian fixed

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);
 };
 

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