new constructor FeatureIMU

src/feature_imu.cpp

@@ -5,21 +5,41 @@ 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>()) FIXME
 {
     //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) :
+FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Vector3s& _acc_bias, const Eigen::Vector3s& _gyro_bias, 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>()),
+    acc_bias_preint_(_acc_bias), gyro_bias_preint_(_gyro_bias),
     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(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const wolf::CaptureIMUPtr _cap_imu_ptr, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians): 
+FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance)
+{
+    //TODO : SIZE ASSERTIONS : make sure _delta_preintegrated and _delta_preintegrated_covariance sizes are correct !
+
+    this->setCapturePtr(_cap_imu_ptr);
+    //TODO : we should make sure here that there is a frame in the capture
+    dp_preint_ = _delta_preintegrated.head<3>();
+    dv_preint_ = _delta_preintegrated.segment<3>(7);
+    dq_preint_ = _delta_preintegrated.segment<4>(3);
+
+    acc_bias_preint_ = std::static_pointer_cast<wolf::FrameIMU>(_cap_imu_ptr->getFramePtr())->getAccBiasPtr()->getVector();
+    gyro_bias_preint_ = std::static_pointer_cast<wolf::FrameIMU>(_cap_imu_ptr->getFramePtr())->getGyroBiasPtr()->getVector();
+
+    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);
+}
+
 FeatureIMU::~FeatureIMU()
 {
     //

src/feature_imu.h

@@ -3,12 +3,16 @@
 
 //Wolf includes
 #include "feature_base.h"
+#include "capture_imu.h"
+#include "frame_imu.h"
+#include "wolf.h"
 
 //std includes
 
 
 namespace wolf {
-    
+
+WOLF_PTR_TYPEDEFS(CaptureIMU);
 WOLF_PTR_TYPEDEFS(FeatureIMU);
 
 //class FeatureIMU
@@ -29,9 +33,22 @@ class FeatureIMU : public FeatureBase
          * \param _measurement the measurement
          * \param _meas_covariance the noise of the measurement
          * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
+         * \param acc_bias accelerometer bias of origin frame
+         * \param gyro_bias gyroscope bias of origin frame
+         *
+         */
+        FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, 
+        const Eigen::Vector3s& _acc_bias, const Eigen::Vector3s& _gyro_bias, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians);
+
+        /** \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
+         * \param _cap_imu_ptr is the CaptureIMUPtr pointing to desired capture containing the origin frame
          *
          */
-        FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians);
+        FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const wolf::CaptureIMUPtr _cap_imu_ptr, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians);
 
         virtual ~FeatureIMU();