From ce59de0e4243ad4c5b39f897759e603403d89f84 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Tue, 13 Dec 2016 16:55:26 +0100
Subject: [PATCH] added FIXMEs
---
src/feature_imu.cpp | 4 ++--
src/test/gtest_feature_imu.cpp | 4 ++++
2 files changed, 6 insertions(+), 2 deletions(-)
diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp
index 3bc14edc3..1d763b2a5 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 ea9df027b..824ff0590 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
}
--
GitLab