From 1d1526801071b52a53edc40ed040055a6dbd7926 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Tue, 13 Dec 2016 16:41:50 +0100
Subject: [PATCH] new constructor : initialize jacobians
---
src/feature_imu.cpp | 10 ++++++++++
src/feature_imu.h | 9 +++++++++
2 files changed, 19 insertions(+)
diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp
index cd1bec557..3bc14edc3 100644
--- a/src/feature_imu.cpp
+++ b/src/feature_imu.cpp
@@ -10,6 +10,16 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen:
//std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
}
+FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians) :
+ 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>()),
+ dDp_dab_(_dD_db_jacobians.block(3,3,0,0)), dDv_dab_(_dD_db_jacobians.block(3,3,6,0)),
+ dDp_dwb_(_dD_db_jacobians.block(3,3,0,3)), dDv_dwb_(_dD_db_jacobians.block(3,3,6,3)), dDq_dwb_(_dD_db_jacobians.block(3,3,3,3))
+{
+ //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
+}
+
FeatureIMU::~FeatureIMU()
{
//
diff --git a/src/feature_imu.h b/src/feature_imu.h
index a19336857..781db5c56 100644
--- a/src/feature_imu.h
+++ b/src/feature_imu.h
@@ -24,6 +24,15 @@ class FeatureIMU : public FeatureBase
*/
FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance);
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
+ *
+ */
+ FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians);
+
virtual ~FeatureIMU();
public: // TODO eventually produce getters for these and then go private
--
GitLab